Limiting currents and calculating FW with IIR filtering

jsphuebner · Oct 31, 2022 · 701875d · 701875d
1 parent b0d4356
commit 701875d
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 21 deletions.
diff --git a/include/param_prj.h b/include/param_prj.h
@@ -17,14 +17,14 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#define VER 5.22.R
+#define VER 5.23.R
 
 /* Entries must be ordered as follows:
    1. Saveable parameters (id != 0)
    2. Temporary parameters (id = 0)
    3. Display values
  */
-//Next param id (increase when adding new parameter!): 145
+//Next param id (increase when adding new parameter!): 146
 //Next value Id: 2049
 /*              category     name         unit       min     max     default id */
 
@@ -52,10 +52,9 @@
 #define MOTOR_PARAMETERS_FOC \
     PARAM_ENTRY(CAT_MOTOR,   curkp,       "",        0,      20000,  32,     107 ) \
     PARAM_ENTRY(CAT_MOTOR,   curki,       "",        0,      100000, 20000,  108 ) \
-    PARAM_ENTRY(CAT_MOTOR,   fwkp,        "",        0,      1000,   5,      142 ) \
-    PARAM_ENTRY(CAT_MOTOR,   fwki,        "",        0,      1000,   300,    143 ) \
+    PARAM_ENTRY(CAT_MOTOR,   vlimflt,     "",        0,      16,     10,     145 ) \
+    PARAM_ENTRY(CAT_MOTOR,   vlimmargin,  "dig",     0,      10000,  2500,   141 ) \
     PARAM_ENTRY(CAT_MOTOR,   fwcurmax,    "A",       -1000,  0,     -100,    144 ) \
-    PARAM_ENTRY(CAT_MOTOR,   fwmargin,    "dig",     0,      10000,  2500,   141 ) \
     PARAM_ENTRY(CAT_MOTOR,   syncofs,     "dig",     0,      65535,  0,      70  ) \
     PARAM_ENTRY(CAT_MOTOR,   lqminusld,   "mH",      0,      1000,   0,      139 ) \
     PARAM_ENTRY(CAT_MOTOR,   fluxlinkage, "mWeber",  0,      1000,   90,     140 ) \

diff --git a/include/pwmgeneration.h b/include/pwmgeneration.h
@@ -35,7 +35,7 @@ class PwmGeneration
       static void SetTorquePercent(float torque);
       static void SetCurrentOffset(int offset1, int offset2);
       static void SetCurrentLimitThreshold(s32fp ocurlim);
-      static void SetControllerGains(int kp, int ki, int fwkp, int fwki, int fwmargin);
+      static void SetControllerGains(int kp, int ki);
       static int GetCpuLoad();
       static void SetChargeCurrent(float cur);
       static void SetPolePairRatio(int ratio) { polePairRatio = ratio; }

diff --git a/src/pwmgeneration-foc.cpp b/src/pwmgeneration-foc.cpp
@@ -41,27 +41,30 @@ static s32fp idMtpa = 0, iqMtpa = 0;
 static tim_oc_id ocChannels[3];
 static PiController qController;
 static PiController dController;
-static PiController fwController;
 
 void PwmGeneration::Run()
 {
    if (opmode == MOD_MANUAL || opmode == MOD_RUN)
    {
       static s32fp idcFiltered = 0;
-      int dir = Encoder::GetRotorDirection(); //Param::GetInt(Param::dir);
+      static int amplitudeErrFiltered;
+      int dir = Param::GetInt(Param::dir);
       s32fp id, iq;
 
       Encoder::UpdateRotorAngle(0);
       CalcNextAngleSync();
       FOC::SetAngle(angle);
-
       ProcessCurrents(id, iq);
 
       frqFiltered = IIRFILTER(frqFiltered, frq, 4);
 
       if (opmode == MOD_RUN && initwait == 0)
       {
-         int fwPermille = fwController.Run(Param::GetInt(Param::amp));
+         int amplitudeErr = (FOC::GetMaximumModulationIndex() - Param::GetInt(Param::vlimmargin)) - Param::GetInt(Param::amp);
+         amplitudeErr = MIN(fwOutMax, amplitudeErr);
+         amplitudeErr = MAX(fwOutMax / 10, amplitudeErr);
+         amplitudeErrFiltered = IIRFILTER(amplitudeErrFiltered, amplitudeErr << 8, Param::GetInt(Param::vlimflt));
+         int fwPermille = amplitudeErrFiltered >> 8;
          s32fp ifw = Param::Get(Param::manualifw) + ((fwOutMax - fwPermille) * Param::Get(Param::fwcurmax)) / fwOutMax;
          Param::SetFixed(Param::ifw, ifw);
          idMtpa = (fwPermille * idMtpa) / fwOutMax;
@@ -80,7 +83,7 @@ void PwmGeneration::Run()
       int32_t qlimit = FOC::GetQLimit(ud);
 
       if (frqFiltered < FP_FROMINT(30))
-         qController.SetMinMaxY(dir < 0 ? -qlimit : 0, dir > 0 ? qlimit : 0);
+         qController.SetMinMaxY(dir <= 0 ? -qlimit : 0, dir => 0 ? qlimit : 0);
       else
          qController.SetMinMaxY(-qlimit, qlimit);
 
@@ -108,8 +111,8 @@ void PwmGeneration::Run()
          timer_disable_break_main_output(PWM_TIMER);
          dController.ResetIntegrator();
          qController.ResetIntegrator();
-         fwController.PreloadIntegrator(fwOutMax);
          RunOffsetCalibration();
+         amplitudeErrFiltered = fwOutMax;
       }
       else
       {
@@ -144,12 +147,10 @@ void PwmGeneration::SetTorquePercent(float torquePercent)
    idMtpa = FP_FROMFLT(id);
 }
 
-void PwmGeneration::SetControllerGains(int kp, int ki, int fwkp, int fwki, int fwmargin)
+void PwmGeneration::SetControllerGains(int kp, int ki)
 {
    qController.SetGains(kp, ki);
    dController.SetGains(kp, ki);
-   fwController.SetGains(fwkp, fwki);
-   fwController.SetRef(FOC::GetMaximumModulationIndex() - fwmargin);
 }
 
 void PwmGeneration::PwmInit()
@@ -164,9 +165,6 @@ void PwmGeneration::PwmInit()
    dController.ResetIntegrator();
    dController.SetCallingFrequency(pwmfrq);
    dController.SetMinMaxY(-maxVd, maxVd);
-   fwController.ResetIntegrator();
-   fwController.SetCallingFrequency(pwmfrq);
-   fwController.SetMinMaxY(0, fwOutMax); //outputs permille of FW current/torque reduction
 
    if ((Param::GetInt(Param::pinswap) & SWAP_PWM13) > 0)
    {

diff --git a/src/stm32_sine.cpp b/src/stm32_sine.cpp
@@ -285,9 +285,7 @@ void Param::Change(Param::PARAM_NUM paramNum)
          PwmGeneration::SetPolePairRatio(Param::GetInt(Param::polepairs) / Param::GetInt(Param::respolepairs));
 
          #if CONTROL == CTRL_FOC
-         PwmGeneration::SetControllerGains(Param::GetInt(Param::curkp), Param::GetInt(Param::curki),
-                                           Param::GetInt(Param::fwkp), Param::GetInt(Param::fwki),
-                                           Param::GetInt(Param::fwmargin));
+         PwmGeneration::SetControllerGains(Param::GetInt(Param::curkp), Param::GetInt(Param::curki));
          Encoder::SwapSinCos((Param::GetInt(Param::pinswap) & SWAP_RESOLVER) > 0);
          FOC::SetMotorParameters(Param::GetFloat(Param::lqminusld) / 1000.0f, Param::GetFloat(Param::fluxlinkage) / 1000.0f);
          #endif // CONTROL