From d9db929ad4eec165466b50e01c9588b9d6ffaf53 Mon Sep 17 00:00:00 2001 From: jsphuebner Date: Sat, 22 Feb 2020 13:58:17 +0100 Subject: [PATCH] Moved deraters behind throttle ramp, add filtering in different places --- Makefile | 2 +- include/param_prj.h | 4 ++-- src/pwmgeneration-foc.cpp | 11 ++++++++--- src/stm32_sine.cpp | 12 +++++------- src/throttle.cpp | 14 +++++++++----- 5 files changed, 25 insertions(+), 18 deletions(-) diff --git a/Makefile b/Makefile index 68a40ab..9f093c0 100644 --- a/Makefile +++ b/Makefile @@ -53,7 +53,7 @@ ifeq ($(CONTROL), FOC) OBJSL += pwmgeneration-foc.o foc.o picontroller.o endif -OBJS = $(subst $(space),$(space)$(OUT_DIR)/, $(OBJSL)) +OBJS = $(patsubst %.o,obj/%.o, $(OBJSL)) vpath %.c src/ libopeninv/src vpath %.cpp src/ libopeninv/src diff --git a/include/param_prj.h b/include/param_prj.h index b026b37..10bf0f3 100644 --- a/include/param_prj.h +++ b/include/param_prj.h @@ -17,7 +17,7 @@ * along with this program. If not, see . */ -#define VER 4.73.R +#define VER 4.76.R /* Entries must be ordered as follows: 1. Saveable parameters (id != 0) @@ -152,7 +152,7 @@ #define PARAM_LIST \ PARAM_ENTRY(CAT_MOTOR, curkp, "", 0, 20000, 64, 107 ) \ PARAM_ENTRY(CAT_MOTOR, curki, "", 0, 100000, 20000, 108 ) \ - PARAM_ENTRY(CAT_MOTOR, curkifrqgain,"dig/Hz", 0, 1000, 0, 120 ) \ + PARAM_ENTRY(CAT_MOTOR, curkifrqgain,"dig/Hz", 0, 1000, 50, 120 ) \ PARAM_ENTRY(CAT_MOTOR, fwkp, "", -10000, 0, -100, 118 ) \ PARAM_ENTRY(CAT_MOTOR, dmargin, "Hz", -10000, 0, -2000, 113 ) \ PARAM_ENTRY(CAT_MOTOR, polepairs, "", 1, 16, 2, 32 ) \ diff --git a/src/pwmgeneration-foc.cpp b/src/pwmgeneration-foc.cpp index fc8565b..3295102 100644 --- a/src/pwmgeneration-foc.cpp +++ b/src/pwmgeneration-foc.cpp @@ -45,6 +45,7 @@ void PwmGeneration::Run() { if (opmode == MOD_MANUAL || opmode == MOD_RUN) { + static s32fp iqFiltered, frqFiltered; int dir = Param::GetInt(Param::dir); int kifrqgain = Param::GetInt(Param::curkifrqgain); uint16_t dc[3]; @@ -57,17 +58,21 @@ void PwmGeneration::Run() else CalcNextAngleAsync(dir); - int moddedKi = curki + kifrqgain * FP_TOINT(frq); + frqFiltered = IIRFILTER(frqFiltered, frq, 8); + int moddedKi = curki + kifrqgain * FP_TOINT(frqFiltered); qController.SetIntegralGain(moddedKi); dController.SetIntegralGain(moddedKi); + Param::SetInt(Param::ud, moddedKi); ProcessCurrents(id, iq); if (opmode == MOD_RUN) { + iqFiltered = IIRFILTER(iqFiltered, iq, 4); s32fp fwIdRef = fwController.Run(iq); dController.SetRef(idref + fwIdRef); + Param::SetFlt(Param::dspnt, fwIdRef); } else if (opmode == MOD_MANUAL) { @@ -86,10 +91,10 @@ void PwmGeneration::Run() Param::SetFlt(Param::fstat, frq); Param::SetFlt(Param::angle, DIGIT_TO_DEGREE(angle)); - Param::SetInt(Param::ud, ud); + //Param::SetInt(Param::ud, ud); Param::SetInt(Param::uq, uq); Param::SetFlt(Param::idc, idc); - Param::SetFlt(Param::dspnt, dController.GetRef()); + //Param::SetFlt(Param::dspnt, dController.GetRef()); Param::SetFlt(Param::qspnt, qController.GetRef()); /* Shut down PWM on stopped motor, neutral gear or init phase */ diff --git a/src/stm32_sine.cpp b/src/stm32_sine.cpp index 92d1f1e..97fca42 100644 --- a/src/stm32_sine.cpp +++ b/src/stm32_sine.cpp @@ -484,6 +484,7 @@ static s32fp ProcessThrottle() throtSpnt = GetUserThrottleCommand(); GetCruiseCreepCommand(finalSpnt, throtSpnt); + finalSpnt = Throttle::RampThrottle(finalSpnt); if (hwRev != HW_TESLA) Throttle::BmsLimitCommand(finalSpnt, Param::GetBool(Param::din_bms)); @@ -491,7 +492,6 @@ static s32fp ProcessThrottle() Throttle::UdcLimitCommand(finalSpnt, Param::Get(Param::udc)); Throttle::IdcLimitCommand(finalSpnt, Param::Get(Param::idc)); Throttle::FrequencyLimitCommand(finalSpnt, Param::Get(Param::fstat)); - finalSpnt = Throttle::RampThrottle(finalSpnt); if (Throttle::TemperatureDerate(Param::Get(Param::tmphs), finalSpnt)) { @@ -613,12 +613,6 @@ static void Ms10Task(void) PwmGeneration::SetOpmode(MOD_OFF); Throttle::cruiseSpeed = -1; runChargeControl = false; - /*Param::SetInt(Param::speed, 0); - Param::SetInt(Param::fstat, 0); - Param::SetInt(Param::il1, 0); - Param::SetInt(Param::il2, 0); - Param::SetInt(Param::il1rms, 0); - Param::SetInt(Param::il2rms, 0);*/ } else if (0 == initWait) { @@ -722,9 +716,13 @@ extern void parm_Change(Param::PARAM_NUM paramNum) break; case Param::throtmax: case Param::throtmin: + case Param::idcmin: + case Param::idcmax: //These are candidates to be frequently set by CAN, so we handle them separately Throttle::throtmax = Param::Get(Param::throtmax); Throttle::throtmin = Param::Get(Param::throtmin); + Throttle::idcmin = Param::Get(Param::idcmin); + Throttle::idcmax = Param::Get(Param::idcmax); break; default: PwmGeneration::SetCurrentLimitThreshold(Param::Get(Param::ocurlim)); diff --git a/src/throttle.cpp b/src/throttle.cpp index 281f104..66cc3a5 100644 --- a/src/throttle.cpp +++ b/src/throttle.cpp @@ -217,7 +217,7 @@ void Throttle::IdcLimitCommand(s32fp& finalSpnt, s32fp idc) if (finalSpnt >= 0) { s32fp idcerr = idcmax - idc; - s32fp res = idcerr * 10; + s32fp res = idcerr * 5; res = MAX(0, res); finalSpnt = MIN(res, finalSpnt); @@ -225,7 +225,7 @@ void Throttle::IdcLimitCommand(s32fp& finalSpnt, s32fp idc) else { s32fp idcerr = idcmin - idc; - s32fp res = idcerr * 10; + s32fp res = idcerr * 5; res = MIN(0, res); finalSpnt = MAX(res, finalSpnt); @@ -234,12 +234,16 @@ void Throttle::IdcLimitCommand(s32fp& finalSpnt, s32fp idc) void Throttle::FrequencyLimitCommand(s32fp& finalSpnt, s32fp frequency) { + static s32fp frqFiltered = 0; + + frqFiltered = IIRFILTER(frqFiltered, frequency, 4); + if (finalSpnt > 0) { - s32fp frqerr = fmax - frequency; - s32fp res = frqerr * 10; + s32fp frqerr = fmax - frqFiltered; + s32fp res = frqerr * 4; - res = MAX(0, res); + res = MAX(1, res); finalSpnt = MIN(res, finalSpnt); } }