From b2eca4e729305925a851f5ebeb3b079aca6a2a37 Mon Sep 17 00:00:00 2001
From: jsphuebner <dev@johanneshuebner.com>
Date: Tue, 15 Aug 2023 11:02:09 +0200
Subject: [PATCH] FOC: When surpassing fmax start derating field weakening
 current as well to prevent runaway

---
 include/pwmgeneration.h   |  1 +
 src/pwmgeneration-foc.cpp |  8 +++++++-
 src/stm32_sine.cpp        |  1 -
 src/vehiclecontrol.cpp    | 10 +++++++++-
 4 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/include/pwmgeneration.h b/include/pwmgeneration.h
index 4a91200..f79726a 100644
--- 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
index 18eaa9f..a46abe3 100644
--- 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
index 2f4d0b4..99c5e91 100644
--- 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
index 7094248..0961e24 100644
--- a/src/vehiclecontrol.cpp
+++ b/src/vehiclecontrol.cpp
@@ -161,6 +161,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);
@@ -178,9 +179,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))
@@ -217,6 +219,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
    }