From 15b42789f4e3c0df83940b6fb388105bda147d84 Mon Sep 17 00:00:00 2001 From: jsphuebner Date: Sat, 6 Apr 2019 20:42:44 +0200 Subject: [PATCH] Moved derating functions to throttle --- include/project/param_prj.h | 5 ++- include/project/throttle.h | 11 ++++++ src/project/stm32_sine.cpp | 74 ++++++++----------------------------- src/project/throttle.cpp | 60 +++++++++++++++++++++++++++++- 4 files changed, 88 insertions(+), 62 deletions(-) diff --git a/include/project/param_prj.h b/include/project/param_prj.h index 325e16a..8ff28c7 100644 --- a/include/project/param_prj.h +++ b/include/project/param_prj.h @@ -66,7 +66,7 @@ #define POTMODE_DUALCHANNEL 1 #define POTMODE_CAN 2 -#define VER 4.15.B +#define VER 4.17.R #define VERSTR STRINGIFY(4=VER) enum _modes @@ -171,7 +171,7 @@ extern const char* errorListString; PARAM_ENTRY(CAT_REGEN, brknompedal, "%", -100, 0, -50, 38 ) \ PARAM_ENTRY(CAT_REGEN, brkpedalramp,"%/10ms", 1, 100, 100, 68 ) \ PARAM_ENTRY(CAT_REGEN, brknom, "%", 0, 100, 30, 19 ) \ - PARAM_ENTRY(CAT_REGEN, brkmax, "%", 0, 100, 30, 49 ) \ + PARAM_ENTRY(CAT_REGEN, brkmax, "%", -100, 0, -30, 49 ) \ PARAM_ENTRY(CAT_REGEN, brkrampstr, "Hz", 0, 400, 10, 39 ) \ PARAM_ENTRY(CAT_REGEN, brkout, "%", -100, -1, -50, 67 ) \ PARAM_ENTRY(CAT_AUTOM, idlespeed, "rpm", -100, 1000, -100, 54 ) \ @@ -216,6 +216,7 @@ extern const char* errorListString; VALUE_ENTRY(tmphs, "°C", 2019 ) \ VALUE_ENTRY(tmpm, "°C", 2020 ) \ VALUE_ENTRY(uaux, "V", 2021 ) \ + VALUE_ENTRY(pwmio, "", 12022 ) \ VALUE_ENTRY(canio, CANIOS, 2022 ) \ VALUE_ENTRY(din_cruise, ONOFF, 2023 ) \ VALUE_ENTRY(din_start, ONOFF, 2024 ) \ diff --git a/include/project/throttle.h b/include/project/throttle.h index af434fc..9e160f1 100644 --- a/include/project/throttle.h +++ b/include/project/throttle.h @@ -33,6 +33,9 @@ class Throttle static int CalcIdleSpeed(int speed); static int CalcCruiseSpeed(int speed); static bool TemperatureDerate(s32fp tmphs, int& finalSpnt); + static void BmsLimitCommand(int& finalSpnt, bool dinbms); + static void UdcLimitCommand(int& finalSpnt, s32fp udc); + static void IdcLimitCommand(int& finalSpnt, s32fp idc); static int potmin[2]; static int potmax[2]; static int brknom; @@ -46,6 +49,14 @@ class Throttle static s32fp idleThrotLim; static int brkPedalRamp; static int throttleRamp; + static int bmslimhigh; + static int bmslimlow; + static int accelmax; + static int accelflt; + static s32fp udcmin; + static s32fp udcmax; + static s32fp idcmin; + static s32fp idcmax; private: static int speedFiltered; diff --git a/src/project/stm32_sine.cpp b/src/project/stm32_sine.cpp index 8f08012..4d86fa4 100644 --- a/src/project/stm32_sine.cpp +++ b/src/project/stm32_sine.cpp @@ -526,61 +526,6 @@ static void GetCruiseCreepCommand(int& finalSpnt, int throtSpnt) } } -static void BmsLimitCommand(int& finalSpnt) -{ - if (hwRev != HW_TESLA && Param::GetBool(Param::din_bms)) - { - if (finalSpnt >= 0) - finalSpnt = (finalSpnt * Param::GetInt(Param::bmslimhigh)) / 100; - else - finalSpnt = -(finalSpnt * Param::GetInt(Param::bmslimlow)) / 100; - } -} - -static void UdcLimitCommand(int& finalSpnt) -{ - s32fp udc = Param::Get(Param::udc); - - if (finalSpnt >= 0) - { - s32fp udcErr = udc - FP_MUL(Param::Get(Param::udcmin), FP_FROMFLT(0.95)); - int res = FP_TOINT(udcErr * 5); - res = MAX(0, res); - finalSpnt = MIN(finalSpnt, res); - } - else - { - s32fp udcErr = udc - FP_MUL(Param::Get(Param::udcmax), FP_FROMFLT(1.05)); - int res = FP_TOINT(udcErr * 5); - res = MIN(0, res); - finalSpnt = MAX(finalSpnt, res); - } -} - -static void IdcLimitCommand(int& finalSpnt) -{ - s32fp idc = Param::Get(Param::idc); - - if (finalSpnt >= 0) - { - s32fp idcmax = Param::Get(Param::idcmax); - s32fp idcerr = idcmax - idc; - int res = FP_TOINT(idcerr * 10); - - res = MAX(0, res); - finalSpnt = MIN(res, finalSpnt); - } - else - { - s32fp idcmin = Param::Get(Param::idcmin); - s32fp idcerr = idcmin - idc; - int res = FP_TOINT(idcerr * 10); - - res = MIN(0, res); - finalSpnt = MAX(res, finalSpnt); - } -} - static void ProcessThrottle() { int throtSpnt, finalSpnt; @@ -592,9 +537,12 @@ static void ProcessThrottle() throtSpnt = GetUserThrottleCommand(); GetCruiseCreepCommand(finalSpnt, throtSpnt); - BmsLimitCommand(finalSpnt); - UdcLimitCommand(finalSpnt); - IdcLimitCommand(finalSpnt); + + if (hwRev != HW_TESLA) + Throttle::BmsLimitCommand(finalSpnt, Param::GetBool(Param::din_bms)); + + Throttle::UdcLimitCommand(finalSpnt, Param::Get(Param::udc)); + Throttle::IdcLimitCommand(finalSpnt, Param::Get(Param::idc)); if (Throttle::TemperatureDerate(Param::Get(Param::tmphs), finalSpnt)) { @@ -824,6 +772,12 @@ extern void parm_Change(Param::PARAM_NUM paramNum) Throttle::speedkp = Param::Get(Param::speedkp); Throttle::speedflt = Param::GetInt(Param::speedflt); Throttle::idleThrotLim = Param::Get(Param::idlethrotlim); + Throttle::bmslimlow = Param::GetInt(Param::bmslimlow); + Throttle::bmslimhigh = Param::GetInt(Param::bmslimhigh); + Throttle::udcmin = FP_MUL(Param::Get(Param::udcmin), FP_FROMFLT(0.95)); //Leave some room for the notification light + Throttle::udcmax = FP_MUL(Param::Get(Param::udcmax), FP_FROMFLT(1.05)); + Throttle::idcmin = Param::Get(Param::idcmin); + Throttle::idcmax = Param::Get(Param::idcmax); if (Param::GetInt(Param::pwmfunc) == PWM_FUNC_SPEEDFRQ) gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO9); @@ -837,6 +791,8 @@ static void InitPWMIO() uint8_t outputMode = Param::GetInt(Param::pwmpol) == 0 ? GPIO_MODE_OUTPUT_50_MHZ : GPIO_MODE_INPUT; uint8_t outputConf = Param::GetInt(Param::pwmpol) == 0 ? GPIO_CNF_OUTPUT_ALTFN_PUSHPULL : GPIO_CNF_INPUT_FLOAT; + Param::SetInt(Param::pwmio, gpio_get(GPIOA, GPIO8 | GPIO9 | GPIO10) | gpio_get(GPIOB, GPIO13 | GPIO14 | GPIO15)); + gpio_set_mode(GPIOA, outputMode, outputConf, GPIO8 | GPIO9 | GPIO10); gpio_set_mode(GPIOB, outputMode, outputConf, GPIO13 | GPIO14 | GPIO15); } @@ -905,6 +861,8 @@ extern "C" int main(void) if (Param::GetInt(Param::snsm) < 12) Param::SetInt(Param::snsm, Param::GetInt(Param::snsm) + 10); //upgrade parameter + if (Param::Get(Param::brkmax) > 0) + Param::Set(Param::brkmax, -Param::Get(Param::brkmax)); term_Run(); diff --git a/src/project/throttle.cpp b/src/project/throttle.cpp index b908286..a2bd4f8 100644 --- a/src/project/throttle.cpp +++ b/src/project/throttle.cpp @@ -38,6 +38,12 @@ int Throttle::brkPedalRamp; int Throttle::brkRamped; int Throttle::throttleRamp; int Throttle::throttleRamped; +int Throttle::bmslimhigh; +int Throttle::bmslimlow; +s32fp Throttle::udcmin; +s32fp Throttle::udcmax; +s32fp Throttle::idcmin; +s32fp Throttle::idcmax; bool Throttle::CheckAndLimitRange(int* potval, int potIdx) { @@ -88,7 +94,7 @@ bool Throttle::CheckDualThrottle(int* potval, int pot2val) int Throttle::CalcThrottle(int potval, int pot2val, bool brkpedal) { int potnom; - int scaledBrkMax = brkpedal ? brknompedal : -brkmax; + int scaledBrkMax = brkpedal ? brknompedal : brkmax; if (pot2val > potmin[1]) { @@ -139,7 +145,7 @@ int Throttle::CalcCruiseSpeed(int speed) { speedFiltered = IIRFILTER(speedFiltered, speed, speedflt); int speederr = cruiseSpeed - speedFiltered; - return FP_TOINT(MAX(FP_FROMINT(-brkmax), MIN(FP_FROMINT(100), speedkp * speederr))); + return FP_TOINT(MAX(FP_FROMINT(brkmax), MIN(FP_FROMINT(100), speedkp * speederr))); } bool Throttle::TemperatureDerate(s32fp tmphs, int& finalSpnt) @@ -158,3 +164,53 @@ bool Throttle::TemperatureDerate(s32fp tmphs, int& finalSpnt) return limit < 100; } + +void Throttle::BmsLimitCommand(int& finalSpnt, bool dinbms) +{ + if (dinbms) + { + if (finalSpnt >= 0) + finalSpnt = (finalSpnt * bmslimhigh) / 100; + else + finalSpnt = -(finalSpnt * bmslimlow) / 100; + } +} + +void Throttle::UdcLimitCommand(int& finalSpnt, s32fp udc) +{ + if (finalSpnt >= 0) + { + s32fp udcErr = udc - udcmin; + int res = FP_TOINT(udcErr * 5); + res = MAX(0, res); + finalSpnt = MIN(finalSpnt, res); + } + else + { + s32fp udcErr = udc - udcmax; + int res = FP_TOINT(udcErr * 5); + res = MIN(0, res); + finalSpnt = MAX(finalSpnt, res); + } +} + +void Throttle::IdcLimitCommand(int& finalSpnt, s32fp idc) +{ + if (finalSpnt >= 0) + { + s32fp idcerr = idcmax - idc; + int res = FP_TOINT(idcerr * 10); + + res = MAX(0, res); + finalSpnt = MIN(res, finalSpnt); + } + else + { + s32fp idcerr = idcmin - idc; + int res = FP_TOINT(idcerr * 10); + + res = MIN(0, res); + finalSpnt = MAX(res, finalSpnt); + } +} +