Merge branch 'anticogging' into syncadv_test

Author: jsphuebner

include/hwdefs.h

@@ -45,7 +45,7 @@
 
 typedef enum
 {
-   HW_REV1, HW_REV2, HW_REV3, HW_TESLA, HW_BLUEPILL, HW_PRIUS, HW_MINI, HW_LEAF2, HW_LEAF3, HW_BMWI3
+   HW_REV1, HW_REV2, HW_REV3, HW_TESLA, HW_BLUEPILL, HW_PRIUS, HW_MINI, HW_LEAF2, HW_LEAF3, HW_BMWI3, HW_ZOE
 } HWREV;
 
 extern HWREV hwRev;

include/param_prj.h

@@ -17,15 +17,15 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#define VERSION 5.36
+#define VERSION 5.38
 
 /* Entries should be ordered as follows:
    1. Saveable parameters
    2. Temporary parameters
    3. Display values
  */
-//Next param id (increase when adding new parameter!): 159
-//Next value Id: 2055
+//Next param id (increase when adding new parameter!): 164
+//Next value Id: 2057
 /*              category     name         unit       min     max     default id */
 
 #define MOTOR_PARAMETERS_COMMON \
@@ -53,9 +53,14 @@
     PARAM_ENTRY(CAT_MOTOR,   iqkp,        "",        0,      20000,  32,     107 ) \
     PARAM_ENTRY(CAT_MOTOR,   idkp,        "",        0,      20000,  32,     149 ) \
     PARAM_ENTRY(CAT_MOTOR,   curki,       "",        0,      100000, 20000,  108 ) \
+    PARAM_ENTRY(CAT_MOTOR,   exckp,       "",        0,      20000,  3000,   162 ) \
+    PARAM_ENTRY(CAT_MOTOR,   cogkp,       "",        -1000,  1000,   0,      159 ) \
+    PARAM_ENTRY(CAT_MOTOR,   cogph,       "",        0,      65535,  0,      160 ) \
+    PARAM_ENTRY(CAT_MOTOR,   cogmax,       "",       0,      30000,  0,      161 ) \
     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,   excurmax,    "A",       0,      10,     0,      163 ) \
     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 ) \
@@ -68,7 +73,7 @@
     PARAM_ENTRY(CAT_INVERTER,ocurlim,     "A",       -65536, 65536,  100,    22  ) \
     PARAM_ENTRY(CAT_INVERTER,il1gain,     "dig/A",   -100,   100,    4.7,    27  ) \
     PARAM_ENTRY(CAT_INVERTER,il2gain,     "dig/A",   -100,   100,    4.7,    28  ) \
-    PARAM_ENTRY(CAT_INVERTER,udcgain,     "dig/V",   0,      4095,   6.175,  29  ) \
+    PARAM_ENTRY(CAT_INVERTER,udcgain,     "dig/V",   -100,   100,    6.175,  29  ) \
     PARAM_ENTRY(CAT_INVERTER,udcofs,      "dig",     0,      4095,   0,      77  ) \
     PARAM_ENTRY(CAT_INVERTER,udclim,      "V",       0,      1000,   540,    48  ) \
     PARAM_ENTRY(CAT_INVERTER,snshs,       SNS_HS,    0,      7,      0,      45  ) \
@@ -216,6 +221,8 @@
     VALUE_ENTRY(ifw,     "A",     2048 ) \
     VALUE_ENTRY(ud,      "dig",   2046 ) \
     VALUE_ENTRY(uq,      "dig",   2047 ) \
+    VALUE_ENTRY(uexc,    "dig",   2056 ) \
+    VALUE_ENTRY(anticog, "dig",   2055 ) \
 
 #if CONTROL == CTRL_SINE
 #define PARAM_LIST \
@@ -264,7 +271,7 @@
 #define TRIPMODES    "0=AllOff, 1=DcSwOn, 2=PrechargeOn, 3=AutoResume"
 #define SNS_HS       "0=JCurve, 1=Semikron, 2=MBB600, 3=KTY81, 4=PT1000, 5=NTCK45_2k2, 6=Leaf, 7=BMW-i3"
 #define SNS_M        "12=KTY83-110, 13=KTY84-130, 14=Leaf, 15=KTY81-110, 16=Toyota, 21=OutlanderFront, 22=EpcosB57861-S, 23=ToyotaGen2"
-#define PWMFUNCS     "0=tmpm, 1=tmphs, 2=speed, 3=speedfrq"
+#define PWMFUNCS     "0=tmpm, 1=tmphs, 2=speed, 3=exciter"
 #define SINECURVES   "0=VoltageSlip, 1=Simultaneous"
 #define CRUISEMODS   "0=Off, 1=Switch, 2=CAN, 3=ThrottlePot, 4=Limiter"
 #define DIRMODES     "0=Button, 1=Switch, 2=ButtonReversed, 3=SwitchReversed, 4=DefaultForward"
@@ -277,7 +284,7 @@
 #define CANSPEEDS    "0=125k, 1=250k, 2=500k, 3=800k, 4=1M"
 #define CANIOS       "1=Cruise, 2=Start, 4=Brake, 8=Fwd, 16=Rev, 32=Bms"
 #define CANPERIODS   "0=100ms, 1=10ms"
-#define HWREVS       "0=Rev1, 1=Rev2, 2=Rev3, 3=Tesla, 4=BluePill, 5=Prius, 6=MiniMainboard, 7=Leaf2, 8=Leaf3, 9=BMWi3"
+#define HWREVS       "0=Rev1, 1=Rev2, 2=Rev3, 3=Tesla, 4=BluePill, 5=Prius, 6=MiniMainboard, 7=Leaf2, 8=Leaf3, 9=BMWi3, 10=Zoe"
 #define SWAPS        "0=None, 1=Currents12, 2=SinCos, 4=PWMOutput13, 8=PWMOutput23"
 #define OUTMODES     "0=DcSw, 1=TmpmThresh, 2=TmphsThresh"
 #define STATUS       "0=None, 1=UdcLow, 2=UdcHigh, 4=UdcBelowUdcSw, 8=UdcLim, 16=EmcyStop, 32=MProt, 64=PotPressed, 128=TmpHs, 256=WaitStart, 512=BrakeCheck"
@@ -344,7 +351,7 @@ enum _pwmfuncs
    PWM_FUNC_TMPM = 0,
    PWM_FUNC_TMPHS,
    PWM_FUNC_SPEED,
-   PWM_FUNC_SPEEDFRQ
+   PWM_FUNC_EXCITER
 };
 
 enum _idlemodes

include/pwmgeneration.h

@@ -36,11 +36,11 @@ class PwmGeneration
       static void SetTorquePercent(float torque);
       static void SetCurrentOffset(int offset1, int offset2);
       static void SetCurrentLimitThreshold(s32fp ocurlim);
-      static void SetControllerGains(int iqkp, int idkp, int ki);
+      static void SetControllerGains(int iqkp, int idkp, int exckp, int ki);
       static int GetCpuLoad();
       static void SetChargeCurrent(float cur);
       static void SetPolePairRatio(int ratio) { polePairRatio = ratio; }
-      static void SetFwCurMax(float c);
+      static void SetFwExcCurMax(float fwcur, float excur);
 
    private:
       enum EdgeType { NoEdge, PosEdge, NegEdge };

include/throttle.h

@@ -25,7 +25,7 @@
 class Throttle
 {
    public:
-      static bool CheckAndLimitRange(int* potval, uint8_t potIdx);
+      static bool CheckAndLimitRange(int& potval, uint8_t potIdx);
       static float DigitsToPercent(int potval, int potidx);
       static float CalcThrottle(float potval, float pot2val, bool brkpedal);
       static float CalcThrottleBiDir(float potval, bool brkpedal);

src/hwinit.cpp

@@ -139,7 +139,8 @@ static HWREV ReadVariantResistor()
    //See here for variants: https://openinverter.org/wiki/Mini_Mainboard#Hardware_detection
    if ((result1 + result2) < 30) return HW_BMWI3; //connected to MISO so always low
    else if (result2 > 3700) return HW_MINI; //might have to compare this against result1 later
-   else if (result1 > 510 && result1 < 630) return HW_LEAF3;
+   else if (result1 > 510 && result1 < 616) return HW_LEAF3;
+   else if (result1 > 624 && result1 < 670) return HW_ZOE;
    else return HW_MINI;
 }
 

src/pwmgeneration-foc.cpp

@@ -33,11 +33,14 @@
 
 #define FRQ_TO_ANGLE(frq) FP_TOINT((frq << SineCore::BITS) / pwmfrq)
 #define DIGIT_TO_DEGREE(a) FP_FROMINT(angle) / (65536 / 360)
+#define DEGREE_TO_DIGIT(a) (((a) * 65536) / 360)
 
 #ifndef QLIMIT_FREQUENCY
 #define QLIMIT_FREQUENCY FP_FROMINT(30)
 #endif // QLIMIT_FREQUENCY
 
+static s32fp MeasureCoggingCurrent(uint16_t angle, s32fp id);
+static int32_t GenerateAntiCoggingSignal(uint16_t angle, s32fp coggingCurrent);
 static int initwait = 0;
 static bool isIdle = false;
 static const s32fp dcCurFac = FP_FROMFLT(0.81649658092772603273 * 1.05); //sqrt(2/3)*1.05 (inverter losses)
@@ -46,7 +49,9 @@ static const uint32_t shiftForFilter = 8;
 static s32fp idMtpa = 0, iqMtpa = 0;
 static PiController qController;
 static PiController dController;
+static PiController excController;
 static s32fp fwCurMax = 0;
+static s32fp excCurMax = 0;
 
 void PwmGeneration::Run()
 {
@@ -75,19 +80,38 @@ void PwmGeneration::Run()
          if (0 == frq) amplitudeErrFiltered = fwOutMax << shiftForFilter;
 
          int vlim = amplitudeErrFiltered >> shiftForFilter;
-         s32fp ifw = ((fwOutMax - vlim) * fwCurMax) / fwOutMax;
-         Param::SetFixed(Param::ifw, ifw);
 
-         s32fp limitedIq = (vlim * iqMtpa) / fwOutMax;
-         qController.SetRef(limitedIq + Param::Get(Param::manualiq));
-
-         s32fp limitedId = -2 * ABS(limitedIq); //ratio between idMtpa and iqMtpa never > 2
-         limitedId = MAX(idMtpa, limitedId);
-         limitedId = MIN(ifw, limitedId);
-         dController.SetRef(limitedId + Param::Get(Param::manualid));
+         if (hwRev == HW_ZOE)
+         {
+            s32fp exciterSpnt = (excCurMax * vlim) / fwOutMax;
+            s32fp iexc = FP_DIV((AnaIn::udc.Get() - Param::GetInt(Param::udcofs)), Param::GetInt(Param::udcgain));
+            Param::SetFixed(Param::ifw, iexc);
+            dController.SetRef(idMtpa + Param::Get(Param::manualid));
+            excController.SetRef(exciterSpnt);
+            qController.SetRef(iqMtpa + Param::Get(Param::manualiq));
+            uint16_t pwm = excController.Run(iexc);
+            Param::SetInt(Param::uexc, pwm);
+            timer_set_oc_value(OVER_CUR_TIMER, TIM_OC4, pwm);
+         }
+         else
+         {
+            s32fp ifw = ((fwOutMax - vlim) * fwCurMax) / fwOutMax;
+            Param::SetFixed(Param::ifw, ifw);
+
+            s32fp limitedIq = (vlim * iqMtpa) / fwOutMax;
+            qController.SetRef(limitedIq + Param::Get(Param::manualiq));
+
+            s32fp limitedId = -2 * ABS(limitedIq); //ratio between idMtpa and iqMtpa never > 2
+            limitedId = MAX(idMtpa, limitedId);
+            limitedId = MIN(ifw, limitedId);
+            dController.SetRef(limitedId + Param::Get(Param::manualid));
+         }
       }
 
-      int32_t ud = dController.Run(id);
+      s32fp coggingCurrent = MeasureCoggingCurrent(angle, id);
+      int32_t antiCogScaled = GenerateAntiCoggingSignal(angle, coggingCurrent);
+      Param::SetInt(Param::anticog, antiCogScaled);
+      int32_t ud = dController.Run(id, antiCogScaled);
       int32_t qlimit = FOC::GetQLimit(ud);
 
       if (frqFiltered < QLIMIT_FREQUENCY)
@@ -117,8 +141,10 @@ void PwmGeneration::Run()
       if (isIdle || initwait > 0)
       {
          timer_disable_break_main_output(PWM_TIMER);
+         //timer_set_oc_value(OVER_CUR_TIMER, TIM_OC4, 0); //stop rotor excitation
          dController.ResetIntegrator();
          qController.ResetIntegrator();
+         //excController.ResetIntegrator();
          RunOffsetCalibration();
          amplitudeErrFiltered = fwOutMax << shiftForFilter;
       }
@@ -144,9 +170,10 @@ void PwmGeneration::Run()
    }
 }
 
-void PwmGeneration::SetFwCurMax(float cur)
+void PwmGeneration::SetFwExcCurMax(float fwcur, float excur)
 {
-   fwCurMax = FP_FROMFLT(cur);
+   fwCurMax = FP_FROMFLT(fwcur);
+   excCurMax = FP_FROMFLT(excur);
 }
 
 void PwmGeneration::SetTorquePercent(float torquePercent)
@@ -167,10 +194,11 @@ void PwmGeneration::SetTorquePercent(float torquePercent)
    idMtpa = FP_FROMFLT(id);
 }
 
-void PwmGeneration::SetControllerGains(int iqkp, int idkp, int ki)
+void PwmGeneration::SetControllerGains(int iqkp, int idkp, int exckp, int ki)
 {
    qController.SetGains(iqkp, ki);
    dController.SetGains(idkp, ki);
+   excController.SetGains(exckp, 1000);
 }
 
 void PwmGeneration::PwmInit()
@@ -185,6 +213,8 @@ void PwmGeneration::PwmInit()
    dController.ResetIntegrator();
    dController.SetCallingFrequency(pwmfrq);
    dController.SetMinMaxY(-maxVd, maxVd);
+   excController.SetCallingFrequency(pwmfrq);
+   excController.SetMinMaxY(0, 2048);
 
    if (opmode == MOD_ACHEAT)
       AcHeatTimerSetup();
@@ -217,7 +247,7 @@ s32fp PwmGeneration::ProcessCurrents(s32fp& id, s32fp& iq)
 
 void PwmGeneration::CalcNextAngleSync()
 {
-   if (Encoder::SeenNorthSignal())
+   if (true)
    {
       uint16_t syncOfs = Param::GetInt(Param::syncofs);
       uint16_t rotorAngle = Encoder::GetRotorAngle();
@@ -250,3 +280,54 @@ void PwmGeneration::RunOffsetCalibration()
       samples = 0;
    }
 }
+
+static s32fp MeasureCoggingCurrent(uint16_t angle, s32fp id)
+{
+   static uint16_t previousAngle = 0;
+   static s32fp minId = 0x7fffffff, maxId = -0x7fffffff;
+   static s32fp coggingCurrent = 0;
+
+   if (previousAngle < 32767 && angle > 32767)
+   {
+      coggingCurrent = ABS(minId - maxId);
+      minId = 0x7fffffff;
+      maxId = -minId;
+   }
+   else
+   {
+      minId = MIN(id, minId);
+      maxId = MAX(id, maxId);
+   }
+   previousAngle = angle;
+   return coggingCurrent;
+}
+
+/** \brief Generates a trapezoidal wave form to counter the cogging current of IPM motors
+ *
+ * \param angle rotor angle
+ * \param coggingCurrent magnitude of cogging current in A
+ * \return int32_t counter waveform as integer
+ *
+ */
+static int32_t GenerateAntiCoggingSignal(uint16_t angle, s32fp coggingCurrent)
+{
+   angle += Param::GetInt(Param::cogph);
+
+   if (angle < DEGREE_TO_DIGIT(90))
+      angle = angle; //no change
+   else if (angle < DEGREE_TO_DIGIT(180)) //90 to 180°
+      angle = 32767 - angle;
+   else if (angle < DEGREE_TO_DIGIT(270)) //180 to 270°
+      angle = angle - 32767;
+   else //270 to 360°
+      angle = 65535 - angle;
+
+   uint16_t antiCog = 4 * angle;
+   int32_t antiCogScaled = ((int32_t)antiCog) - 32767;
+   int32_t antiCogMax = Param::GetInt(Param::cogmax);
+   antiCogScaled = (antiCogScaled * FP_TOINT(coggingCurrent) * Param::GetInt(Param::cogkp)) / 65536;
+   antiCogScaled = MIN(antiCogScaled, antiCogMax);
+   antiCogScaled = MAX(antiCogScaled, -antiCogMax);
+
+   return antiCogScaled;
+}

src/stm32_sine.cpp

@@ -310,7 +310,10 @@ 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::iqkp), Param::GetInt(Param::idkp), Param::GetInt(Param::curki));
+         PwmGeneration::SetControllerGains(Param::GetInt(Param::iqkp),
+                                           Param::GetInt(Param::idkp),
+                                           Param::GetInt(Param::exckp),
+                                           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);
          FOC::SetMaximumModulationIndex(Param::GetInt(Param::modmax));
@@ -350,10 +353,7 @@ void Param::Change(Param::PARAM_NUM paramNum)
 
          if (hwRev != HW_BLUEPILL)
          {
-            if (Param::GetInt(Param::pwmfunc) == PWM_FUNC_SPEEDFRQ)
-               gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO9);
-            else
-               gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO9);
+            gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO9);
          }
          break;
    }

src/terminal_prj.cpp

@@ -57,25 +57,20 @@ extern "C" const TERM_CMD TermCmds[] =
   { NULL, NULL }
 };
 
-static void LoadDefaults(Terminal* term, char *arg)
+static void LoadDefaults(Terminal* , char *)
 {
-   arg = arg;
-   term = term;
    Param::LoadDefaults();
    printf("Defaults loaded\r\n");
 }
 
-static void StopInverter(Terminal* term, char *arg)
+static void StopInverter(Terminal*, char *)
 {
-   arg = arg;
-   term = term;
    Param::SetInt(Param::opmode, 0);
    printf("Inverter halted.\r\n");
 }
 
-static void StartInverter(Terminal* term, char *arg)
+static void StartInverter(Terminal* , char *arg)
 {
-   term = term;
    arg = my_trim(arg);
    #if CONTROL == CTRL_SINE
    int val = my_atoi(arg);
@@ -94,17 +89,13 @@ static void StartInverter(Terminal* term, char *arg)
    #endif
 }
 
-static void PrintErrors(Terminal* term, char *arg)
+static void PrintErrors(Terminal* , char *)
 {
-   term = term;
-   arg = arg;
    ErrorMessage::PrintAllErrors();
 }
 
-static void PrintSerial(Terminal* term, char *arg)
+static void PrintSerial(Terminal* , char *)
 {
-   arg = arg;
-   term = term;
    printf("%X:%X:%X\r\n", DESIG_UNIQUE_ID2, DESIG_UNIQUE_ID1, DESIG_UNIQUE_ID0);
 }