diff --git a/include/param_prj.h b/include/param_prj.h index 3a12ffe..cbd985b 100644 --- a/include/param_prj.h +++ b/include/param_prj.h @@ -132,7 +132,7 @@ PARAM_ENTRY(CAT_REGEN, offthrotregen,"%", -100, 0, -30, 49 ) \ PARAM_ENTRY(CAT_REGEN, cruiseregen, "%", -100, 0, -30, 124 ) \ PARAM_ENTRY(CAT_REGEN, regenrampstr,"Hz", 0, 400, 10, 39 ) \ - PARAM_ENTRY(CAT_REGEN, maxregentravelhz,"Hz", 0, 1000, 90, 158 ) \ + PARAM_ENTRY(CAT_REGEN, maxregentravelhz,"Hz", 0, 1000, 0, 158 ) \ PARAM_ENTRY(CAT_REGEN, brklightout, "%", -100, -1, -50, 67 ) #define AUTOMATION_CONTACT_PWM_COMM_PARAMETERS \ diff --git a/include/throttle.h b/include/throttle.h index 58c4082..ae5f1f9 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, float rotorHz); + static float CalcThrottle(float potval, float pot2val, bool brkpedal); static float CalcThrottleBiDir(float potval, bool brkpedal); static float CalcIdleSpeed(int speed); static float CalcCruiseSpeed(int speed); @@ -39,6 +39,7 @@ 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 int potmin[2]; static int potmax[2]; static float brknom; @@ -55,7 +56,6 @@ 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/include/vehiclecontrol.h b/include/vehiclecontrol.h index e5929cf..807dad2 100644 --- a/include/vehiclecontrol.h +++ b/include/vehiclecontrol.h @@ -57,7 +57,7 @@ class VehicleControl static bool GetCruiseCreepCommand(float& finalSpnt, float throtSpnt); static void BmwAdcAcquire(); static void CanClear(); - static bool CanReceive(uint32_t id, uint32_t data[2]); + static bool CanReceive(uint32_t id, uint32_t data[2], uint8_t); }; #endif // VCU_H diff --git a/src/stm32_sine.cpp b/src/stm32_sine.cpp index 97a0156..6e0e373 100644 --- a/src/stm32_sine.cpp +++ b/src/stm32_sine.cpp @@ -147,7 +147,7 @@ static void Ms10Task(void) RunCharger(udc); } - stt |= DigIo::emcystop_in.Get() ? STAT_NONE : STAT_EMCYSTOP; + stt |= DigIo::emcystop_in.Get() || hwRev == HW_REV3 ? STAT_NONE : STAT_EMCYSTOP; stt |= DigIo::mprot_in.Get() ? STAT_NONE : STAT_MPROT; stt |= Param::GetInt(Param::potnom) <= 0 ? STAT_NONE : STAT_POTPRESSED; stt |= udc >= Param::GetFloat(Param::udcsw) ? STAT_NONE : STAT_UDCBELOWUDCSW; @@ -303,7 +303,6 @@ 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 8483af7..a015a01 100644 --- a/src/throttle.cpp +++ b/src/throttle.cpp @@ -53,7 +53,6 @@ float Throttle::fmax; int Throttle::accelmax; int Throttle::accelflt; float Throttle::maxregentravelhz; -float Throttle::regenrampstr; bool Throttle::CheckAndLimitRange(int* potval, uint8_t potIdx) { @@ -85,87 +84,44 @@ float Throttle::DigitsToPercent(int potval, int potidx) return (100 * (potval - potmin[potidx])) / (potmax[potidx] - potmin[potidx]); } - -/** - * 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 = regen percentage to apply, when brake is pressed - * brknompedal = regen percentage to apply, when accelerator is lifted - * brknom = 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) +void Throttle::UpdateDynamicRegenTravel(float regenTravelMax, float frequency) { - // 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; // sum of values in array - - // current dynamic regentravel - static float currentRegentravel = 0.0; - - float scaledBrkMax = brkpedal ? brknompedal : brkmax; - // Never reach 0, because that can spin up the motor - scaledBrkMax = -0.1 + (scaledBrkMax * pot2nom) / 100.0f; + if (maxregentravelhz == 0) //dynamic travel turned off + { + brknom = regenTravelMax; + return; + } // 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); + // Don't go over max regentravel + float speedBasedRegenTravel = 3 + regenTravelMax * frequency / maxregentravelhz; + speedBasedRegenTravel = MIN(regenTravelMax, speedBasedRegenTravel); - // decreasing regentravel when rotor hz is goes below "regen ramp start" - if (rotorfreq < regenrampstr) - { - 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; - } - } + brknom = speedBasedRegenTravel; +} - // increase currentRegenTravel when lifting accelerator - if (currentRegentravel < speedBasedRegenTravel) // if currentregentravel is smaller than speedBaseRegenTravel - { - // current pedal position delta compared against average of last 200ms - float acceleratorDelta = potnom - potnomSums / history; - if (acceleratorDelta < 0) // accelerator is being lifted (acceleratorDelta is negative) - { - currentRegentravel = RAMPUP(currentRegentravel, speedBasedRegenTravel, -acceleratorDelta); - } - } +float Throttle::CalcThrottle(float potnom, float pot2nom, bool brkpedal) +{ + float scaledBrkMax = brkpedal ? brknompedal : brkmax; + + //Never reach 0, because that can spin up the motor + scaledBrkMax = -0.1 + (scaledBrkMax * pot2nom) / 100.0f; - if (brkpedal) // give max brake regen + if (brkpedal) { potnom = scaledBrkMax; } - else if (potnom < currentRegentravel) // calculate regen + else if (potnom < brknom) { - potnom -= currentRegentravel; - potnom = -(potnom * scaledBrkMax / currentRegentravel); + potnom -= brknom; + potnom = -(potnom * scaledBrkMax / brknom); } - else // calculate thorttle + else { - potnom -= currentRegentravel; - potnom = 100.0f * potnom / (100.0f - currentRegentravel); + potnom -= brknom; + potnom = 100.0f * potnom / (100.0f - brknom); } - // there next 4 lines are for calculating rolling average for historical pedal (potnom) position - // and saving current potnom - 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 - return potnom; } diff --git a/src/vehiclecontrol.cpp b/src/vehiclecontrol.cpp index f7798d6..c322851 100644 --- a/src/vehiclecontrol.cpp +++ b/src/vehiclecontrol.cpp @@ -67,7 +67,7 @@ void VehicleControl::CanClear() can->RegisterUserMessage(Param::GetInt(Param::controlid)); } -bool VehicleControl::CanReceive(uint32_t canId, uint32_t data[2]) +bool VehicleControl::CanReceive(uint32_t canId, uint32_t data[2], uint8_t) { const int maxErrors = 5; @@ -659,6 +659,8 @@ 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(); @@ -733,7 +735,7 @@ float VehicleControl::GetUserThrottleCommand() return 0; } - return Throttle::CalcThrottle(potnom1, regenPreset, brake, FP_TOFLOAT(Encoder::GetRotorFrequency())); + return Throttle::CalcThrottle(potnom1, regenPreset, brake); } bool VehicleControl::GetCruiseCreepCommand(float& finalSpnt, float throtSpnt)