diff --git a/include/throttle.h b/include/throttle.h index 4a7d23c..2331431 100644 --- 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 index 6792f80..5730256 100644 --- 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 index bab4c4f..1d21ccd 100644 --- 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 index b0c3a02..7409c5b 100644 --- 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 index 6726920..4620644 100644 --- 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