From a006eb6d7149653943071309012e9d5dfe3bedd9 Mon Sep 17 00:00:00 2001 From: jsphuebner Date: Thu, 6 Feb 2020 12:15:22 +0100 Subject: [PATCH] Added regen throttle limit parameter throtmin Removed voltage limiting field weakening controller Made curki frequency dependent by curkifrqgain parameter --- include/param_prj.h | 15 ++--- include/pwmgeneration.h | 2 +- include/throttle.h | 1 + libopeninv | 2 +- src/pwmgeneration-foc.cpp | 28 +++++----- src/stm32_sine.cpp | 112 +++++++++++++++++++++----------------- src/throttle.cpp | 6 +- 7 files changed, 89 insertions(+), 77 deletions(-) diff --git a/include/param_prj.h b/include/param_prj.h index 3fd9de8..aa19eb5 100644 --- a/include/param_prj.h +++ b/include/param_prj.h @@ -17,7 +17,7 @@ * along with this program. If not, see . */ -#define VER 4.71.R +#define VER 4.73.R /* Entries must be ordered as follows: 1. Saveable parameters (id != 0) @@ -146,14 +146,14 @@ VALUE_ENTRY(cpuload, "%", 2035 ) #elif CONTROL == CTRL_FOC -//Next param id (increase when adding new parameter!): 119 +//Next param id (increase when adding new parameter!): 121 /* category name unit min max default id */ #define PARAM_LIST \ PARAM_ENTRY(CAT_MOTOR, curkp, "", 0, 20000, 64, 107 ) \ - PARAM_ENTRY(CAT_MOTOR, curki, "", 0, 100000, 20000, 108 ) \ - PARAM_ENTRY(CAT_MOTOR, fwkp, "", 0, 10000, 100, 114 ) \ - PARAM_ENTRY(CAT_MOTOR, fwkp2, "", -10000, 0, -80, 118 ) \ - PARAM_ENTRY(CAT_MOTOR, dmargin, "Hz", -10000, 0, -1000, 113 ) \ + PARAM_ENTRY(CAT_MOTOR, curki, "", 0, 100000, 20000, 108 ) \ + PARAM_ENTRY(CAT_MOTOR, curkifrqgain,"dig/Hz", 0, 1000, 0, 120 ) \ + PARAM_ENTRY(CAT_MOTOR, fwkp, "", -10000, 0, -100, 118 ) \ + PARAM_ENTRY(CAT_MOTOR, dmargin, "Hz", -10000, 0, -2000, 113 ) \ PARAM_ENTRY(CAT_MOTOR, polepairs, "", 1, 16, 2, 32 ) \ PARAM_ENTRY(CAT_MOTOR, respolepairs,"", 1, 16, 1, 93 ) \ PARAM_ENTRY(CAT_MOTOR, encmode, ENCMODES, 0, 5, 0, 75 ) \ @@ -162,7 +162,7 @@ PARAM_ENTRY(CAT_MOTOR, dirchrpm, "rpm", 0, 2000, 100, 87 ) \ PARAM_ENTRY(CAT_MOTOR, dirmode, DIRMODES, 0, 3, 1, 95 ) \ PARAM_ENTRY(CAT_MOTOR, syncofs, "dig", 0, 65535, 0, 70 ) \ - PARAM_ENTRY(CAT_MOTOR, syncadv, "dig/hz",-100, 100, 0, 101 ) \ + PARAM_ENTRY(CAT_MOTOR, syncadv, "dig/hz",-100, 100, 10, 101 ) \ PARAM_ENTRY(CAT_MOTOR, snsm, SNS_M, 12, 14, 12, 46 ) \ PARAM_ENTRY(CAT_INVERTER,pwmfrq, PWMFRQS, 0, 2, 1, 13 ) \ PARAM_ENTRY(CAT_INVERTER,pwmpol, PWMPOLS, 0, 1, 0, 52 ) \ @@ -182,6 +182,7 @@ PARAM_ENTRY(CAT_DERATE, idcmax, "A", 0, 5000, 5000, 96 ) \ PARAM_ENTRY(CAT_DERATE, idcmin, "A", -5000, 0, -5000, 98 ) \ PARAM_ENTRY(CAT_DERATE, throtmax, "%", 0, 100, 100, 97 ) \ + PARAM_ENTRY(CAT_DERATE, throtmin, "%", -100, 0, -100, 119 ) \ PARAM_ENTRY(CAT_CHARGER, chargemode, CHARGEMODS,0, 4, 0, 74 ) \ PARAM_ENTRY(CAT_CHARGER, chargecur, "A", 0, 50, 0, 71 ) \ PARAM_ENTRY(CAT_CHARGER, chargekp, "dig", 0, 100, 80, 72 ) \ diff --git a/include/pwmgeneration.h b/include/pwmgeneration.h index 4fc0eab..fc967e0 100644 --- a/include/pwmgeneration.h +++ b/include/pwmgeneration.h @@ -35,7 +35,7 @@ class PwmGeneration static void SetTorquePercent(s32fp torque); static void SetCurrentOffset(int offset1, int offset2); static void SetCurrentLimitThreshold(s32fp ocurlim); - static void SetControllerGains(int kp, int ki, int fwkp, int fwkp2); + static void SetControllerGains(int kp, int ki, int fwkp); static int GetCpuLoad(); //static void SetCurrentLimit(s32fp limit); private: diff --git a/include/throttle.h b/include/throttle.h index fd8b15a..4063f14 100644 --- a/include/throttle.h +++ b/include/throttle.h @@ -44,6 +44,7 @@ class Throttle static s32fp brknompedal; static s32fp brkmax; static s32fp throtmax; + static s32fp throtmin; static int idleSpeed; static int cruiseSpeed; static s32fp speedkp; diff --git a/libopeninv b/libopeninv index 530c569..daa6887 160000 --- a/libopeninv +++ b/libopeninv @@ -1 +1 @@ -Subproject commit 530c56929f88af7e3a9e7e4c34f4f7044e583985 +Subproject commit daa6887e07570ef511328a273f72832f93540e4b diff --git a/src/pwmgeneration-foc.cpp b/src/pwmgeneration-foc.cpp index c1dac30..fc8565b 100644 --- a/src/pwmgeneration-foc.cpp +++ b/src/pwmgeneration-foc.cpp @@ -36,19 +36,19 @@ static int initwait = 0; static s32fp idref = 0; +static int curki = 0; static PiController qController; static PiController dController; static PiController fwController; -static PiController fwController2; void PwmGeneration::Run() { if (opmode == MOD_MANUAL || opmode == MOD_RUN) { int dir = Param::GetInt(Param::dir); + int kifrqgain = Param::GetInt(Param::curkifrqgain); uint16_t dc[3]; s32fp id, iq; - static int32_t fwIdRef = 0; Encoder::UpdateRotorAngle(dir); @@ -57,12 +57,17 @@ void PwmGeneration::Run() else CalcNextAngleAsync(dir); + int moddedKi = curki + kifrqgain * FP_TOINT(frq); + + qController.SetIntegralGain(moddedKi); + dController.SetIntegralGain(moddedKi); + ProcessCurrents(id, iq); if (opmode == MOD_RUN) { - s32fp fwIdRef2 = fwController2.Run(iq); - dController.SetRef(idref + fwIdRef + fwIdRef2); + s32fp fwIdRef = fwController.Run(iq); + dController.SetRef(idref + fwIdRef); } else if (opmode == MOD_MANUAL) { @@ -76,8 +81,6 @@ void PwmGeneration::Run() qController.SetMinMaxY(-qlimit, qlimit); int32_t uq = qController.Run(iq); FOC::InvParkClarke(ud, uq, angle); - //Calculate extra field weakening current for the next cycle - fwIdRef = fwController.Run(FOC::GetTotalVoltage(ud, uq) >> 5); s32fp idc = (iq * uq) / FOC::GetMaximumModulationIndex(); @@ -96,7 +99,6 @@ void PwmGeneration::Run() dController.ResetIntegrator(); qController.ResetIntegrator(); fwController.ResetIntegrator(); - fwController2.ResetIntegrator(); } else { @@ -156,16 +158,16 @@ void PwmGeneration::SetTorquePercent(s32fp torquePercent) FOC::Mtpa(is, id, iq); qController.SetRef(FP_FROMINT(iq)); - fwController2.SetRef(FP_FROMINT(iq)); + fwController.SetRef(FP_FROMINT(iq)); idref = FP_FROMINT(id); } -void PwmGeneration::SetControllerGains(int kp, int ki, int fwkp, int fwkp2) +void PwmGeneration::SetControllerGains(int kp, int ki, int fwkp) { qController.SetGains(kp, ki); dController.SetGains(kp, ki); fwController.SetGains(fwkp, 0); - fwController2.SetGains(fwkp2, 0); + curki = ki; } void PwmGeneration::PwmInit() @@ -183,11 +185,7 @@ void PwmGeneration::PwmInit() dController.SetMinMaxY(-maxVd, maxVd); fwController.ResetIntegrator(); fwController.SetCallingFrequency(pwmfrq); - fwController.SetMinMaxY(-FP_FROMINT(500), 0); - fwController.SetRef(1024); //We right shift the modulation index by 5 to effectively have less gain - fwController2.ResetIntegrator(); - fwController2.SetCallingFrequency(pwmfrq); - fwController2.SetMinMaxY(-FP_FROMINT(500), 0); + fwController.SetMinMaxY(-50 * Param::Get(Param::throtcur), 0); //allow 50% of max current for extra field weakening if (opmode == MOD_ACHEAT) AcHeatTimerSetup(); diff --git a/src/stm32_sine.cpp b/src/stm32_sine.cpp index f019213..92d1f1e 100644 --- a/src/stm32_sine.cpp +++ b/src/stm32_sine.cpp @@ -707,59 +707,69 @@ static void Ms1Task(void) /** This function is called when the user changes a parameter */ extern void parm_Change(Param::PARAM_NUM paramNum) { + switch (paramNum) + { #if CONTROL == CTRL_SINE - if (Param::fslipspnt == paramNum) - PwmGeneration::SetFslip(Param::Get(Param::fslipspnt)); - else if (Param::ampnom == paramNum) - PwmGeneration::SetAmpnom(Param::Get(Param::ampnom)); - else + case Param::fslipspnt: + PwmGeneration::SetFslip(Param::Get(Param::fslipspnt)); + break; + case Param::ampnom: + PwmGeneration::SetAmpnom(Param::Get(Param::ampnom)); + break; #endif - if (Param::canspeed == paramNum) - Can::SetBaudrate((enum Can::baudrates)Param::GetInt(Param::canspeed)); - else - { - PwmGeneration::SetCurrentLimitThreshold(Param::Get(Param::ocurlim)); - - #if CONTROL == CTRL_FOC - PwmGeneration::SetControllerGains(Param::GetInt(Param::curkp), Param::GetInt(Param::curki), - Param::GetInt(Param::fwkp), Param::GetInt(Param::fwkp2)); - Encoder::SwapSinCos((Param::GetInt(Param::pinswap) & SWAP_RESOLVER) > 0); - #elif CONTROL == CTRL_SINE - MotorVoltage::SetMinFrq(Param::Get(Param::fmin)); - SineCore::SetMinPulseWidth(Param::GetInt(Param::minpulse)); - #endif // CONTROL - - Encoder::SetMode((enum Encoder::mode)Param::GetInt(Param::encmode)); - Encoder::SetImpulsesPerTurn(Param::GetInt(Param::numimp)); - - Throttle::potmin[0] = Param::GetInt(Param::potmin); - Throttle::potmax[0] = Param::GetInt(Param::potmax); - Throttle::potmin[1] = Param::GetInt(Param::pot2min); - Throttle::potmax[1] = Param::GetInt(Param::pot2max); - Throttle::brknom = Param::Get(Param::brknom); - Throttle::brknompedal = Param::Get(Param::brknompedal); - Throttle::regenRamp = Param::Get(Param::regenramp); - Throttle::brkmax = Param::Get(Param::brkmax); - Throttle::throtmax = Param::Get(Param::throtmax); - Throttle::idleSpeed = Param::GetInt(Param::idlespeed); - 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); - Throttle::fmax = Param::Get(Param::fmax); - - if (hwRev != HW_BLUEPILL) - { - if (Param::GetInt(Param::pwmfunc) == PWM_FUNC_SPEEDFRQ) - gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO9); - else - gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO9); - } + case Param::canspeed: + Can::SetBaudrate((enum Can::baudrates)Param::GetInt(Param::canspeed)); + break; + case Param::throtmax: + case Param::throtmin: + //These are candidates to be frequently set by CAN, so we handle them separately + Throttle::throtmax = Param::Get(Param::throtmax); + Throttle::throtmin = Param::Get(Param::throtmin); + break; + default: + PwmGeneration::SetCurrentLimitThreshold(Param::Get(Param::ocurlim)); + + #if CONTROL == CTRL_FOC + PwmGeneration::SetControllerGains(Param::GetInt(Param::curkp), Param::GetInt(Param::curki), Param::GetInt(Param::fwkp)); + Encoder::SwapSinCos((Param::GetInt(Param::pinswap) & SWAP_RESOLVER) > 0); + #elif CONTROL == CTRL_SINE + MotorVoltage::SetMinFrq(Param::Get(Param::fmin)); + SineCore::SetMinPulseWidth(Param::GetInt(Param::minpulse)); + #endif // CONTROL + + Encoder::SetMode((enum Encoder::mode)Param::GetInt(Param::encmode)); + Encoder::SetImpulsesPerTurn(Param::GetInt(Param::numimp)); + + Throttle::potmin[0] = Param::GetInt(Param::potmin); + Throttle::potmax[0] = Param::GetInt(Param::potmax); + Throttle::potmin[1] = Param::GetInt(Param::pot2min); + Throttle::potmax[1] = Param::GetInt(Param::pot2max); + Throttle::brknom = Param::Get(Param::brknom); + Throttle::brknompedal = Param::Get(Param::brknompedal); + Throttle::regenRamp = Param::Get(Param::regenramp); + Throttle::brkmax = Param::Get(Param::brkmax); + Throttle::throtmax = Param::Get(Param::throtmax); + Throttle::throtmin = Param::Get(Param::throtmin); + Throttle::idleSpeed = Param::GetInt(Param::idlespeed); + 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); + Throttle::fmax = Param::Get(Param::fmax); + + if (hwRev != HW_BLUEPILL) + { + if (Param::GetInt(Param::pwmfunc) == PWM_FUNC_SPEEDFRQ) + gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO9); + else + gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO9); + } + break; } } diff --git a/src/throttle.cpp b/src/throttle.cpp index de66b61..281f104 100644 --- a/src/throttle.cpp +++ b/src/throttle.cpp @@ -35,6 +35,7 @@ int Throttle::speedFiltered; s32fp Throttle::idleThrotLim; s32fp Throttle::potnomFiltered; s32fp Throttle::throtmax; +s32fp Throttle::throtmin; s32fp Throttle::regenRamp; s32fp Throttle::throttleRamp; s32fp Throttle::throttleRamped; @@ -120,13 +121,14 @@ s32fp Throttle::CalcThrottle(int potval, int pot2val, bool brkpedal) } } - potnom = MIN(potnom, throtmax); - return potnom; } s32fp Throttle::RampThrottle(s32fp potnom) { + potnom = MIN(potnom, throtmax); + potnom = MAX(potnom, throtmin); + if (potnom >= throttleRamped) { throttleRamped = RAMPUP(throttleRamped, potnom, throttleRamp);