Merge custom calcThrottle changes.

jsphuebner · Nov 5, 2024 · e5f2590 · e5f2590
1 parent 9e7d887
commit e5f2590
Show file tree

Hide file tree

Showing 5 changed files with 79 additions and 37 deletions.
diff --git a/include/throttle.h b/include/throttle.h
@@ -27,7 +27,7 @@ class Throttle
    public:
       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 CalcThrottle(float potval, float pot2val, bool brkpedal, float rotorHz);
       static float CalcThrottleBiDir(float potval, bool brkpedal);
       static float CalcIdleSpeed(int speed);
       static float CalcCruiseSpeed(int speed);
@@ -39,7 +39,6 @@ class Throttle
       static void AccelerationLimitCommand(float& finalSpnt, int speed);
       static void FrequencyLimitCommand(float& finalSpnt, float frequency);
       static float RampThrottle(float finalSpnt);
-      static void UpdateDynamicRegenTravel(float regenTravelMax, float frequency);
       static bool IsThrottlePressed(int pot1);
       static int potmin[2];
       static int potmax[2];
@@ -57,6 +56,7 @@ class Throttle
       static float idleThrotLim;
       static float cruiseThrotLim;
       static float regenRamp;
+      static float regenrampstr;
       static float throttleRamp;
       static int bmslimhigh;
       static int bmslimlow;

diff --git a/src/stm32_sine.cpp b/src/stm32_sine.cpp
@@ -305,6 +305,7 @@ void Param::Change(Param::PARAM_NUM paramNum)
          Throttle::brknom = Param::GetFloat(Param::regentravel);
          Throttle::brknompedal = Param::GetFloat(Param::brakeregen);
          Throttle::regenRamp = Param::GetFloat(Param::regenramp);
+         Throttle::regenrampstr = Param::GetFloat(Param::regenrampstr);
          Throttle::maxregentravelhz = Param::GetFloat(Param::maxregentravelhz);
          Throttle::brkmax = Param::GetFloat(Param::offthrotregen);
          Throttle::brkcruise = Param::GetFloat(Param::cruiseregen);

diff --git a/src/throttle.cpp b/src/throttle.cpp
@@ -40,6 +40,7 @@ float Throttle::potnomFiltered;
 float Throttle::throtmax;
 float Throttle::throtmin;
 float Throttle::regenRamp;
+float Throttle::regenrampstr;
 float Throttle::throttleRamp;
 float Throttle::throttleRamped;
 int Throttle::bmslimhigh;
@@ -90,45 +91,87 @@ float Throttle::DigitsToPercent(int potval, int potidx)
    return (100 * (potval - potmin[potidx])) / (potmax[potidx] - potmin[potidx]);
 }
 
-void Throttle::UpdateDynamicRegenTravel(float regenTravelMax, float frequency)
+/**
+ * Calculate throttle and regen. 
+ * - Increase regentravel when lifting accelerator using historical pedal values.
+ * - Decrease regentravel when speed goes below regenrampstr.
+ * 
+ * Parameters:
+ * potnom = accelerator pedal pressed percentage
+ * pot2nom = optional potentiometer for adjusting regen braking
+ * brkpedal = is brake pedal pressed
+ * rotorfreq = rotor rotation freqyency in hz
+ *
+ * Used variables:
+ * brkmax (offthrotregen) = max regen percentage to apply, when throttle is lifted
+ * brknompedal (brakeregen) = regen percentage to apply, when brakes are applied
+ * brknom (regentravel) = percentage of how much of accelerator is assigned for adjusting regen
+ * regenrampstr = rotor hz where regen strenght start to decrease linearly
+*/
+float Throttle::CalcThrottle(float potnom, float pot2nom, bool brkpedal, float rotorfreq)
 {
-   if (maxregentravelhz == 0) //dynamic travel turned off
-   {
-      brknom = regenTravelMax;
-      return;
-   }
+   // variables for rolling average calculation
+   static const int history = 20;         // 20 events, 10ms delta = 200ms of history to calculate
+   static int currentArrayIndex = 0;      // pointer where to pu the next value
+   static float prevPotnom[history] = {}; // array holding previous potnom value
+   static float potnomSums = 0.0f;           // sum of values in array
 
-   // increase speedBasedRegenTravel linearly until rotorfreq reaches maxregentravelhz, then stay at max.
-   // Don't go over max regentravel
-   float speedBasedRegenTravel = 3 + regenTravelMax * frequency / maxregentravelhz;
-   speedBasedRegenTravel = MIN(regenTravelMax, speedBasedRegenTravel);
+   // current dynamic regentravel
+   static float currentRegentravel = 0.0;
 
-   brknom = speedBasedRegenTravel;
-}
-
-float Throttle::CalcThrottle(float potnom, float pot2nom, bool brkpedal)
-{
    float scaledBrkMax = brkpedal ? brknompedal : brkmax;
-
-   //Never reach 0, because that can spin up the motor
+   // Never reach 0, because that can spin up the motor
    scaledBrkMax = -0.1 + (scaledBrkMax * pot2nom) / 100.0f;
 
-   if (brkpedal)
+   // increase speedBasedRegenTravel linearly until rotorfreq reaches maxregentravelhz, then stay at max.
+   // Don't go over max regentravel (brknom).
+   float speedBasedRegenTravel = MIN(brknom * rotorfreq / maxregentravelhz, brknom);
+   // decreasing regentravel when rotor hz is goes below "regen ramp start"
+   if (rotorfreq < regenrampstr)
    {
-      potnom = scaledBrkMax;
+      float decreasingRegenTravel = brknom * rotorfreq / regenrampstr;
+      // decrease only if it's been higher before (don't start to increase on low speeds to keep linear curve)
+      if (currentRegentravel > decreasingRegenTravel)
+      {
+         currentRegentravel = decreasingRegenTravel;
+      }
    }
-   else if (potnom < brknom)
+
+   // increase currentRegenTravel when lifting accelerator OR when foot is completely off throttle
+   if (currentRegentravel < speedBasedRegenTravel)
    {
-      potnom -= brknom;
-      potnom = -(potnom * scaledBrkMax / brknom);
+      float acceleratorDelta = potnom - (potnomSums / (float)history);
+      if (acceleratorDelta < 0) // accelerator is being lifted
+      {
+        currentRegentravel = RAMPUP(currentRegentravel, speedBasedRegenTravel, -acceleratorDelta);
+      }
+      else if (potnom < 0.1f)  // Added check for foot completely off throttle
+      {
+         currentRegentravel = speedBasedRegenTravel;
+      }
    }
-   else
+
+   //keep short pedal history 
+   potnomSums -= prevPotnom[currentArrayIndex];           // remove old potnom value from average calculation sums
+   potnomSums += potnom;                                  // replace it with new potnom value
+   prevPotnom[currentArrayIndex] = potnom;                // put current potnom to array
+   currentArrayIndex = (currentArrayIndex + 1) % history; // loop array index from 0 - history, start again from 0
+
+   if (brkpedal) // give max brake regen
    {
-      potnom -= brknom;
-      potnom = 100.0f * potnom / (100.0f - brknom);
+      return scaledBrkMax;
    }
 
-   return potnom;
+   if (potnom < currentRegentravel) // calculate regen
+   {
+      potnom -= currentRegentravel;
+      return -(potnom * scaledBrkMax / currentRegentravel);
+   }
+   else // calculate thorttle
+   {
+      potnom -= currentRegentravel;
+      return 100.0f * potnom / (100.0f - currentRegentravel);
+   }
 }
 
 float Throttle::CalcThrottleBiDir(float potval, bool brkpedal)

diff --git a/src/vehiclecontrol.cpp b/src/vehiclecontrol.cpp
@@ -673,8 +673,6 @@ float VehicleControl::GetUserThrottleCommand()
    bool inRange1 = Throttle::CheckAndLimitRange(&potval, 0);
    bool inRange2 = Throttle::CheckAndLimitRange(&pot2val, 1);
 
-   Throttle::UpdateDynamicRegenTravel(Param::GetFloat(Param::regentravel), FP_TOFLOAT(Encoder::GetRotorFrequency()));
-
    if (!inRange1)
    {
       DigIo::err_out.Set();
@@ -749,7 +747,7 @@ float VehicleControl::GetUserThrottleCommand()
       return 0;
    }
 
-   return Throttle::CalcThrottle(potnom1, regenPreset, brake);
+   return Throttle::CalcThrottle(potnom1, regenPreset, brake, FP_TOFLOAT(Encoder::GetRotorFrequency()));
 }
 
 bool VehicleControl::GetCruiseCreepCommand(float& finalSpnt, float throtSpnt)

diff --git a/test/test_throttle.cpp b/test/test_throttle.cpp
@@ -48,20 +48,20 @@ static void TestSetup()
 
 static void TestBrkPedal()
 {
-   int percent = Throttle::CalcThrottle(1500, 3000, true);
+   int percent = Throttle::CalcThrottle(1500, 3000, true, 1000);
    ASSERT(percent == -25)
-   percent = Throttle::CalcThrottle(1500, 3000, true);
+   percent = Throttle::CalcThrottle(1500, 3000, true, 1000);
    ASSERT(percent == -50)
-   percent = Throttle::CalcThrottle(1500, 3000, true);
+   percent = Throttle::CalcThrottle(1500, 3000, true, 1000);
    ASSERT(percent == -60)
-   percent = Throttle::CalcThrottle(1500, 3000, true);
+   percent = Throttle::CalcThrottle(1500, 3000, true, 1000);
    ASSERT(percent == -60)
 }
 
 static void TestRegen()
 {
-   Throttle::CalcThrottle(2000, 3000, false);
-   int percent = Throttle::CalcThrottle(1000, 3000, false);
+   Throttle::CalcThrottle(2000, 3000, false, 1000);
+   int percent = Throttle::CalcThrottle(1000, 3000, false, 1000);
    ASSERT(percent == -25)
 }
 #if 0