From 35735008fb1a712edb73897bc9cf18fa1c7740b5 Mon Sep 17 00:00:00 2001 From: Charlie Smurthwaite Date: Sun, 15 Jan 2023 12:53:13 +0000 Subject: [PATCH] sine current control prototype --- include/param_prj.h | 32 +------ include/pwmgeneration.h | 3 +- src/pwmgeneration-sine.cpp | 166 ++++--------------------------------- src/pwmgeneration.cpp | 13 +-- src/vehiclecontrol.cpp | 36 +------- 5 files changed, 24 insertions(+), 226 deletions(-) diff --git a/include/param_prj.h b/include/param_prj.h index 730d4ec..2d18daa 100644 --- a/include/param_prj.h +++ b/include/param_prj.h @@ -37,20 +37,14 @@ PARAM_ENTRY(CAT_MOTOR, numimp, "ppr", 8, 8192, 60, 15 ) \ PARAM_ENTRY(CAT_MOTOR, dirchrpm, "rpm", 0, 20000, 100, 87 ) \ PARAM_ENTRY(CAT_MOTOR, dirmode, DIRMODES, 0, 4, 1, 95 ) \ - PARAM_ENTRY(CAT_MOTOR, snsm, SNS_M, 12, 23, 12, 46 ) + PARAM_ENTRY(CAT_MOTOR, throtcur, "A/%", 0, 10, 1, 105 ) \ + PARAM_ENTRY(CAT_MOTOR, snsm, SNS_M, 12, 23, 12, 46 ) \ + PARAM_ENTRY(CAT_MOTOR, curkp, "", 0, 20000, 32, 107 ) \ #define MOTOR_PARAMETERS_SINE \ - PARAM_ENTRY(CAT_MOTOR, boost, "dig", 0, 37813, 1700, 1 ) \ - PARAM_ENTRY(CAT_MOTOR, fweak, "Hz", 0, 1000, 90, 2 ) \ - PARAM_ENTRY(CAT_MOTOR, fweakstrt, "Hz", 0, 1000, 400, 134 ) \ - PARAM_ENTRY(CAT_MOTOR, fconst, "Hz", 0, 1000, 180, 99 ) \ - PARAM_ENTRY(CAT_MOTOR, udcnom, "V", 0, 1000, 0, 78 ) \ - PARAM_ENTRY(CAT_MOTOR, fslipmin, "Hz", 0.3, 10, 1, 37 ) \ PARAM_ENTRY(CAT_MOTOR, fslipmax, "Hz", 0.3, 10, 3, 33 ) \ - PARAM_ENTRY(CAT_MOTOR, fslipconstmax,"Hz", 0, 10, 5, 100 ) #define MOTOR_PARAMETERS_FOC \ - PARAM_ENTRY(CAT_MOTOR, curkp, "", 0, 20000, 32, 107 ) \ PARAM_ENTRY(CAT_MOTOR, curki, "", 0, 100000, 20000, 108 ) \ PARAM_ENTRY(CAT_MOTOR, vlimflt, "", 0, 16, 10, 145 ) \ PARAM_ENTRY(CAT_MOTOR, vlimmargin, "dig", 0, 10000, 2500, 141 ) \ @@ -89,11 +83,6 @@ PARAM_ENTRY(CAT_DERATE, throtmax, "%", 0, 100, 100, 97 ) \ PARAM_ENTRY(CAT_DERATE, throtmin, "%", -100, 0, -100, 119 ) -#define DERATE_PARAMETERS_SINE \ - PARAM_ENTRY(CAT_DERATE, iacmax, "A", 0, 5000, 5000, 89 ) \ - PARAM_ENTRY(CAT_DERATE, ifltrise, "dig", 0, 32, 10, 91 ) \ - PARAM_ENTRY(CAT_DERATE, ifltfall, "dig", 0, 32, 3, 92 ) \ - #define CHARGER_PARAMETERS \ PARAM_ENTRY(CAT_CHARGER, chargemode, CHARGEMODS,0, 4, 0, 74 ) \ PARAM_ENTRY(CAT_CHARGER, chargecur, "A", 0, 50, 0, 71 ) \ @@ -112,14 +101,6 @@ PARAM_ENTRY(CAT_THROTTLE,throtramp, "%/10ms", 0.1, 100, 100, 81 ) \ PARAM_ENTRY(CAT_THROTTLE,throtramprpm,"rpm", 0, 20000, 20000, 85 ) -#define THROTTLE_PARAMETERS_SINE \ - PARAM_ENTRY(CAT_THROTTLE,ampmin, "%", 0, 100, 10, 4 ) \ - PARAM_ENTRY(CAT_THROTTLE,slipstart, "%", 10, 100, 50, 90 ) \ - PARAM_ENTRY(CAT_THROTTLE,throtfilter, "dig", 0, 10, 4, 147 ) - -#define THROTTLE_PARAMETERS_FOC \ - PARAM_ENTRY(CAT_THROTTLE,throtcur, "A/%", 0, 10, 1, 105 ) - #define REGEN_PARAMETERS \ PARAM_ENTRY(CAT_REGEN, brakeregen, "%", -100, 0, -50, 38 ) \ PARAM_ENTRY(CAT_REGEN, regenramp, "%/10ms", 0.1, 100, 100, 68 ) \ @@ -191,10 +172,6 @@ #define VALUES_SINE \ VALUE_ENTRY(ilmax, "A", 2005 ) \ VALUE_ENTRY(uac, "V", 2006 ) \ - VALUE_ENTRY(il1rms, "A", 2007 ) \ - VALUE_ENTRY(il2rms, "A", 2008 ) \ - VALUE_ENTRY(boostcalc, "dig", 2009 ) \ - VALUE_ENTRY(fweakcalc, "Hz", 2010 ) \ #define VALUES_FOC \ VALUE_ENTRY(id, "A", 2003 ) \ @@ -209,10 +186,8 @@ MOTOR_PARAMETERS_COMMON \ INVERTER_PARAMETERS_COMMON \ THROTTLE_PARAMETERS_COMMON \ - THROTTLE_PARAMETERS_SINE \ REGEN_PARAMETERS \ DERATE_PARAMETERS_COMMON \ - DERATE_PARAMETERS_SINE \ CHARGER_PARAMETERS \ AUTOMATION_CONTACT_PWM_COMM_PARAMETERS \ PARAM_ENTRY(CAT_TEST, fslipspnt, "Hz", -100, 1000, 0, 0 ) \ @@ -328,7 +303,6 @@ enum _modes MOD_MANUAL, MOD_BOOST, MOD_BUCK, - MOD_SINE, MOD_ACHEAT, MOD_LAST }; diff --git a/include/pwmgeneration.h b/include/pwmgeneration.h index a90619d..553dfeb 100644 --- a/include/pwmgeneration.h +++ b/include/pwmgeneration.h @@ -52,8 +52,7 @@ class PwmGeneration static s32fp ProcessCurrents(); static s32fp ProcessCurrents(s32fp& id, s32fp& iq); static void CalcNextAngleSync(); - static void CalcNextAngleAsync(int dir); - static void CalcNextAngleConstant(int dir); + static void CalcNextAngleAsync(); static void Charge(); static void AcHeat(); static s32fp GetIlMax(s32fp il1, s32fp il2); diff --git a/src/pwmgeneration-sine.cpp b/src/pwmgeneration-sine.cpp index a1f5ac2..c426c10 100644 --- a/src/pwmgeneration-sine.cpp +++ b/src/pwmgeneration-sine.cpp @@ -37,19 +37,25 @@ void PwmGeneration::Run() { - if (opmode == MOD_MANUAL || opmode == MOD_RUN || opmode == MOD_SINE) + if (opmode == MOD_MANUAL || opmode == MOD_RUN) { + static uint32_t amp = 0; int dir = Param::GetInt(Param::dir); Encoder::UpdateRotorAngle(dir); - s32fp ampNomLimited = LimitCurrent(); + ProcessCurrents(); + CalcNextAngleAsync(); - if (opmode == MOD_SINE) - CalcNextAngleConstant(dir); - else - CalcNextAngleAsync(dir); + // Adjust amplitude according to requested and measured current + uint32_t curkp = Param::GetInt(Param::curkp); + s32fp ilmax = Param::Get(Param::ilmax); + s32fp throtcur = Param::Get(Param::throtcur); + s32fp ilmaxtarget = FP_MUL(throtcur, ampnom); - uint32_t amp = MotorVoltage::GetAmpPerc(frq, ampNomLimited); + if (ilmax > ilmaxtarget && amp > 0) + amp -= curkp; + else if (ilmax < ilmaxtarget && amp < 37813) + amp += curkp; SineCore::SetAmp(amp); Param::SetInt(Param::amp, amp); @@ -84,60 +90,11 @@ void PwmGeneration::Run() void PwmGeneration::SetTorquePercent(float torque) { - int filterConst = Param::GetInt(Param::throtfilter); - float roundingError = FP_TOFLOAT((float)((1 << filterConst) - 1)); - float fslipmin = Param::GetFloat(Param::fslipmin); - float ampmin = Param::GetFloat(Param::ampmin); - float slipstart = Param::GetFloat(Param::slipstart); - float ampnomLocal; - float fslipspnt = 0; - - if (torque >= 0) - { - /* In async mode first X% throttle commands amplitude, X-100% raises slip */ - ampnomLocal = ampmin + (100.0f - ampmin) * torque / slipstart; + float fslipmax = Param::GetFloat(Param::fslipmax); - if (torque >= slipstart) - { - float fstat = Param::GetFloat(Param::fstat); - float fweak = Param::GetFloat(Param::fweakcalc); - float fslipmax = Param::GetFloat(Param::fslipmax); - - if (fstat > fweak) - { - float fconst = Param::GetFloat(Param::fconst); - float fslipconstmax = Param::GetFloat(Param::fslipconstmax); - //Basically, for every Hz above fweak we get a fraction of - //the difference between fslipconstmax and fslipmax - //of additional slip - fslipmax += (fstat - fweak) / (fconst - fweak) * (fslipconstmax - fslipmax); - fslipmax = MIN(fslipmax, fslipconstmax); //never exceed fslipconstmax! - } + ampnom = FP_FROMFLT(ABS(torque)); + fslip = FP_FROMFLT(torque / 100.f * fslipmax); - float fslipdiff = fslipmax - fslipmin; - fslipspnt = roundingError + fslipmin + (fslipdiff * (torque - slipstart)) / (100.0f - slipstart); - } - else - { - fslipspnt = fslipmin + roundingError; - } - } - else if (Encoder::GetRotorDirection() != Param::GetInt(Param::dir)) - { - // Do not apply negative torque if we are already traveling backwards. - fslipspnt = 0; - ampnomLocal = 0; - } - else - { - ampnomLocal = -torque; - fslipspnt = -fslipmin; - } - - ampnomLocal = MIN(ampnomLocal, 100.0f); - //anticipate sudden changes by filtering - ampnom = IIRFILTER(ampnom, FP_FROMFLT(ampnomLocal), filterConst); - fslip = IIRFILTER(fslip, FP_FROMFLT(fslipspnt), filterConst); Param::Set(Param::ampnom, ampnom); Param::Set(Param::fslipspnt, fslip); @@ -155,36 +112,6 @@ void PwmGeneration::PwmInit() AcHeatTimerSetup(); } -s32fp PwmGeneration::LimitCurrent() -{ - static s32fp curLimSpntFiltered = 0, slipFiltered = 0; - s32fp slipmin = Param::Get(Param::fslipmin); - s32fp imax = Param::Get(Param::iacmax); - s32fp ilMax = ProcessCurrents(); - - //setting of 0 disables current limiting - if (imax == 0) return ampnom; - - s32fp a = imax / 20; //Start acting at 80% of imax - s32fp imargin = imax - ilMax; - s32fp curLimSpnt = FP_DIV(100 * imargin, a); - s32fp slipSpnt = FP_DIV(FP_MUL(fslip, imargin), a); - slipSpnt = MAX(slipmin, slipSpnt); - curLimSpnt = MAX(FP_FROMINT(40), curLimSpnt); //Never go below 40% - int filter = Param::GetInt(curLimSpnt < curLimSpntFiltered ? Param::ifltfall : Param::ifltrise); - curLimSpntFiltered = IIRFILTER(curLimSpntFiltered, curLimSpnt, filter); - slipFiltered = IIRFILTER(slipFiltered, slipSpnt, 1); - - s32fp ampNomLimited = MIN(ampnom, curLimSpntFiltered); - slipSpnt = MIN(fslip, slipFiltered); - slipIncr = FRQ_TO_ANGLE(slipSpnt); - - if (curLimSpnt < ampnom) - ErrorMessage::Post(ERR_CURRENTLIMIT); - - return ampNomLimited; -} - s32fp PwmGeneration::GetIlMax(s32fp il1, s32fp il2) { s32fp il3 = -il1 - il2; @@ -195,71 +122,12 @@ s32fp PwmGeneration::GetIlMax(s32fp il1, s32fp il2) return ilMax; } -PwmGeneration::EdgeType PwmGeneration::CalcRms(s32fp il, EdgeType& lastEdge, s32fp& max, s32fp& rms, int& samples, s32fp prevRms) -{ - const s32fp oneOverSqrt2 = FP_FROMFLT(0.707106781187); - int minSamples = pwmfrq / (4 * FP_TOINT(frq)); - EdgeType edgeType = NoEdge; - - minSamples = MAX(10, minSamples); - - if (samples > minSamples) - { - if (lastEdge == NegEdge && il > 0) - edgeType = PosEdge; - else if (lastEdge == PosEdge && il < 0) - edgeType = NegEdge; - } - - if (edgeType != NoEdge) - { - rms = (FP_MUL(oneOverSqrt2, max) + prevRms) / 2; // average with previous rms reading - - max = 0; - samples = 0; - lastEdge = edgeType; - } - - il = ABS(il); - max = MAX(il, max); - samples++; - - return edgeType; -} - s32fp PwmGeneration::ProcessCurrents() { - static s32fp currentMax[2]; - static int samples[2] = { 0 }; - static int sign = 1; - static EdgeType lastEdge[2] = { PosEdge, PosEdge }; - s32fp il1 = GetCurrent(AnaIn::il1, ilofs[0], Param::Get(Param::il1gain)); s32fp il2 = GetCurrent(AnaIn::il2, ilofs[1], Param::Get(Param::il2gain)); - s32fp rms; - s32fp il1PrevRms = Param::Get(Param::il1rms); - s32fp il2PrevRms = Param::Get(Param::il2rms); - EdgeType edge = CalcRms(il1, lastEdge[0], currentMax[0], rms, samples[0], il1PrevRms); - - if (edge != NoEdge) - { - Param::SetFixed(Param::il1rms, rms); - - if (opmode != MOD_BOOST || opmode != MOD_BUCK) - { - //rough approximation as we do not take power factor into account - s32fp idc = (SineCore::GetAmp() * rms) / SineCore::MAXAMP; - idc = FP_MUL(idc, FP_FROMFLT(1.2247)); //multiply by sqrt(3)/sqrt(2) - idc *= fslip < 0 ? -1 : 1; - Param::SetFixed(Param::idc, idc); - } - } - if (CalcRms(il2, lastEdge[1], currentMax[1], rms, samples[1], il2PrevRms)) - { - Param::SetFixed(Param::il2rms, rms); - } - s32fp ilMax = sign * GetIlMax(il1, il2); + s32fp ilMax = GetIlMax(il1, il2); Param::SetFixed(Param::il1, il1); Param::SetFixed(Param::il2, il2); diff --git a/src/pwmgeneration.cpp b/src/pwmgeneration.cpp index 139bb7e..7cf373c 100644 --- a/src/pwmgeneration.cpp +++ b/src/pwmgeneration.cpp @@ -149,7 +149,6 @@ void PwmGeneration::SetOpmode(int _opmode) break; case MOD_MANUAL: case MOD_RUN: - case MOD_SINE: EnableOutput(); break; } @@ -272,27 +271,19 @@ void PwmGeneration::SetChargeCurrent(float cur) /*----- Private methods ----------------------------------------- */ -void PwmGeneration::CalcNextAngleAsync(int dir) +void PwmGeneration::CalcNextAngleAsync() { static uint16_t slipAngle = 0; uint16_t rotorAngle = Encoder::GetRotorAngle(); frq = polePairRatio * Encoder::GetRotorFrequency() + fslip; - slipAngle += dir * slipIncr; + slipAngle += slipIncr; if (frq < 0) frq = 0; angle = polePairRatio * rotorAngle + slipAngle; } -void PwmGeneration::CalcNextAngleConstant(int dir) -{ - frq = fslip; - angle += dir * slipIncr; - - if (frq < 0) frq = 0; -} - void PwmGeneration::Charge() { static s32fp iFlt; diff --git a/src/vehiclecontrol.cpp b/src/vehiclecontrol.cpp index 9502c64..5eb3048 100644 --- a/src/vehiclecontrol.cpp +++ b/src/vehiclecontrol.cpp @@ -214,12 +214,10 @@ float VehicleControl::ProcessThrottle() finalSpnt = (rotorfreq / brkrampstr) * finalSpnt; } -#if CONTROL == CTRL_FOC if (finalSpnt < 0) finalSpnt *= Encoder::GetRotorDirection(); - else //inconsistency here: in slip control negative always means regen + else finalSpnt *= Param::GetInt(Param::dir); -#endif // CONTROL } return finalSpnt; @@ -379,38 +377,6 @@ float VehicleControl::ProcessUdc() ErrorMessage::Post(ERR_PRECHARGE); } - #if CONTROL == CTRL_SINE - { - float udcnom = Param::GetFloat(Param::udcnom); - float boost = Param::GetFloat(Param::boost); - float fweak; - - if (Param::GetInt(Param::potnom) > 35) - { - fweak = MAP(Param::GetFloat(Param::potnom), 36, 100, (Param::GetFloat(Param::fweakstrt)), (Param::GetFloat(Param::fweak))); - } - else - { - fweak = Param::GetFloat(Param::fweakstrt); - } - - if (udcnom > 0) - { - float udcdiff = udcfp - udcnom; - float factor = 1.0 + udcdiff / udcnom; - //increase fweak on voltage above nominal - fweak = fweak * factor; - //decrease boost on voltage above nominal - boost = boost / factor; - } - - Param::SetFloat(Param::fweakcalc, fweak); - Param::SetFloat(Param::boostcalc, boost); - MotorVoltage::SetWeakeningFrq(fweak); - MotorVoltage::SetBoost(boost); - } - #endif // CONTROL - Param::SetFloat(Param::udc, udcfp); return udcfp;