Moved deraters behind throttle ramp, add filtering in different places

jsphuebner · Feb 22, 2020 · d9db929 · d9db929
1 parent c04949d
commit d9db929
Show file tree

Hide file tree

Showing 5 changed files with 25 additions and 18 deletions.
diff --git 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
@@ -17,7 +17,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#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
@@ -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
@@ -484,14 +484,14 @@ static s32fp ProcessThrottle()
 
    throtSpnt = GetUserThrottleCommand();
    GetCruiseCreepCommand(finalSpnt, throtSpnt);
+   finalSpnt = Throttle::RampThrottle(finalSpnt);
 
    if (hwRev != HW_TESLA)
       Throttle::BmsLimitCommand(finalSpnt, Param::GetBool(Param::din_bms));
 
    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
@@ -217,15 +217,15 @@ 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);
    }
    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);
    }
 }