Merge branch 'master' into safer_can2

jsphuebner · Aug 22, 2023 · dacd3ea · dacd3ea
2 parents 358f66f + b2eca4e
commit dacd3ea
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 3 deletions.
diff --git a/include/pwmgeneration.h b/include/pwmgeneration.h
@@ -39,6 +39,7 @@ class PwmGeneration
       static int GetCpuLoad();
       static void SetChargeCurrent(float cur);
       static void SetPolePairRatio(int ratio) { polePairRatio = ratio; }
+      static void SetFwCurMax(float c);
 
    private:
       enum EdgeType { NoEdge, PosEdge, NegEdge };

diff --git a/src/pwmgeneration-foc.cpp b/src/pwmgeneration-foc.cpp
@@ -47,6 +47,7 @@ static s32fp idMtpa = 0, iqMtpa = 0;
 static tim_oc_id ocChannels[3];
 static PiController qController;
 static PiController dController;
+static s32fp fwCurMax = 0;
 
 void PwmGeneration::Run()
 {
@@ -74,7 +75,7 @@ void PwmGeneration::Run()
          if (0 == frq) amplitudeErrFiltered = fwOutMax << shiftForFilter;
 
          int vlim = amplitudeErrFiltered >> shiftForFilter;
-         s32fp ifw = ((fwOutMax - vlim) * Param::Get(Param::fwcurmax)) / fwOutMax;
+         s32fp ifw = ((fwOutMax - vlim) * fwCurMax) / fwOutMax;
          Param::SetFixed(Param::ifw, ifw);
 
          s32fp limitedIq = (vlim * iqMtpa) / fwOutMax;
@@ -143,6 +144,11 @@ void PwmGeneration::Run()
    }
 }
 
+void PwmGeneration::SetFwCurMax(float cur)
+{
+   fwCurMax = FP_FROMFLT(cur);
+}
+
 void PwmGeneration::SetTorquePercent(float torquePercent)
 {
    float is = Param::GetFloat(Param::throtcur) * torquePercent;

diff --git a/src/stm32_sine.cpp b/src/stm32_sine.cpp
@@ -331,7 +331,6 @@ void Param::Change(Param::PARAM_NUM paramNum)
          Throttle::idcmin = Param::GetFloat(Param::idcmin);
          Throttle::idcmax = Param::GetFloat(Param::idcmax);
          Throttle::idckp = Param::GetFloat(Param::idckp);
-         Throttle::fmax = Param::GetFloat(Param::fmax);
          Throttle::accelflt = Param::GetInt(Param::accelflt);
          Throttle::accelmax = Param::GetInt(Param::accelmax);
 

diff --git a/src/vehiclecontrol.cpp b/src/vehiclecontrol.cpp
@@ -256,6 +256,7 @@ void VehicleControl::SelectDirection()
 float VehicleControl::ProcessThrottle()
 {
    float throtSpnt = 0, finalSpnt;
+   const float fstat = Param::GetFloat(Param::fstat);
 
    if ((int)Encoder::GetSpeed() < Param::GetInt(Param::throtramprpm))
       Throttle::throttleRamp = Param::GetFloat(Param::throtramp);
@@ -273,9 +274,10 @@ float VehicleControl::ProcessThrottle()
    if (hwRev != HW_TESLA)
       Throttle::BmsLimitCommand(finalSpnt, Param::GetBool(Param::din_bms));
 
+   Throttle::fmax = Param::GetFloat(Param::fmax);
    Throttle::UdcLimitCommand(finalSpnt, Param::GetFloat(Param::udc));
    Throttle::IdcLimitCommand(finalSpnt, Param::GetFloat(Param::idc));
-   Throttle::FrequencyLimitCommand(finalSpnt, Param::GetFloat(Param::fstat));
+   Throttle::FrequencyLimitCommand(finalSpnt, fstat);
    Throttle::AccelerationLimitCommand(finalSpnt, Encoder::GetSpeed());
 
    if (Throttle::TemperatureDerate(Param::GetFloat(Param::tmphs), Param::GetFloat(Param::tmphsmax), finalSpnt))
@@ -312,6 +314,12 @@ float VehicleControl::ProcessThrottle()
          finalSpnt *= Encoder::GetRotorDirection();
       else //inconsistency here: in slip control negative always means regen
          finalSpnt *= Param::GetInt(Param::dir);
+
+      //At 110% fmax start derating field weakening current just in case it has a torque producing current
+      Throttle::fmax = Param::GetFloat(Param::fmax) * 1.1f;
+      float fwPercent = 100;
+      Throttle::FrequencyLimitCommand(fwPercent, fstat);
+      PwmGeneration::SetFwCurMax(fwPercent * Param::GetFloat(Param::fwcurmax));
 #endif // CONTROL
    }