diff --git a/firmware/controllers/actuators/alternator_controller.cpp b/firmware/controllers/actuators/alternator_controller.cpp index eb1d626fa8..48f459a7ec 100644 --- a/firmware/controllers/actuators/alternator_controller.cpp +++ b/firmware/controllers/actuators/alternator_controller.cpp @@ -31,8 +31,7 @@ EXTERN_ENGINE static Logging *logger; static SimplePwm alternatorControl("alt"); -static pid_s *altPidS = &persistentState.persistentConfiguration.engineConfiguration.alternatorControl; -Pid alternatorPid(altPidS); +PidIndustrial alternatorPid; static percent_t currentAltDuty; @@ -56,6 +55,10 @@ class AlternatorController : public PeriodicTimerController { } #endif + // todo: move this to pid_s one day + alternatorPid.antiwindupFreq = engineConfiguration->alternator_antiwindupFreq; + alternatorPid.derivativeFilterLoss = engineConfiguration->alternator_derivativeFilterLoss; + if (engineConfiguration->debugMode == DBG_ALTERNATOR_PID) { // this block could be executed even in on/off alternator control mode // but at least we would reflect latest state @@ -148,12 +151,14 @@ void onConfigurationChangeAlternatorCallback(engine_configuration_s *previousCon shouldResetPid = !alternatorPid.isSame(&previousConfiguration->alternatorControl); } -void initAlternatorCtrl(Logging *sharedLogger) { +void initAlternatorCtrl(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX) { logger = sharedLogger; addConsoleAction("altinfo", showAltInfo); if (CONFIGB(alternatorControlPin) == GPIO_UNASSIGNED) return; + alternatorPid.initPidClass(&engineConfiguration->alternatorControl); + if (CONFIGB(onOffAlternatorLogic)) { enginePins.alternatorPin.initPin("on/off alternator", CONFIGB(alternatorControlPin)); diff --git a/firmware/controllers/engine_controller.cpp b/firmware/controllers/engine_controller.cpp index c10e8b7505..f6a64ffe8b 100644 --- a/firmware/controllers/engine_controller.cpp +++ b/firmware/controllers/engine_controller.cpp @@ -775,7 +775,7 @@ void initEngineContoller(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX) initMalfunctionCentral(); #if EFI_ALTERNATOR_CONTROL - initAlternatorCtrl(sharedLogger); + initAlternatorCtrl(sharedLogger PASS_ENGINE_PARAMETER_SUFFIX); #endif #if EFI_AUX_PID diff --git a/firmware/util/math/pid.cpp b/firmware/util/math/pid.cpp index 5fb3cc5dc8..5e09c2544b 100644 --- a/firmware/util/math/pid.cpp +++ b/firmware/util/math/pid.cpp @@ -235,3 +235,51 @@ void PidCic::updateITerm(float value) { } iTerm = iTermSum * iTermInvNum; } + +float PidIndustrial::getOutput(float target, float input, float dTime) { + float ad, bd; + float error = (target - input) * errorAmplificationCoef; + float pTerm = parameters->pFactor * error; + + // update the I-term + iTerm += parameters->iFactor * dTime * error; + + // calculate dTerm coefficients + if (fabsf(derivativeFilterLoss) > DBL_EPSILON) { + // restore Td in the Standard form from the Parallel form: Td = Kd / Kc + float Td = parameters->dFactor / parameters->pFactor; + // calculate the backward differences approximation of the derivative term + ad = Td / (Td + dTime / derivativeFilterLoss); + bd = parameters->pFactor * ad / derivativeFilterLoss; + } else { + // According to the Theory of limits, if p.derivativeFilterLoss -> 0, then + // lim(ad) = 0; lim(bd) = p.pFactor * Td / dTime = p.dFactor / dTime + // i.e. dTerm becomes equal to Pid's + ad = 0.0f; + bd = parameters->dFactor / dTime; + } + + // (error - previousError) = (target-input) - (target-prevousInput) = -(input - prevousInput) + dTerm = dTerm * ad + (error - previousError) * bd; + + // calculate output and apply the limits + float output = pTerm + iTerm + dTerm + parameters->offset; + float limitedOutput = limitOutput(output); + + // apply the integrator anti-windup + // If p.antiwindupFreq = 0, then iTerm is equal to PidParallelController's + iTerm += dTime * antiwindupFreq * (limitedOutput - output); + + // update the state + previousError = error; + + return limitedOutput; +} + +float PidIndustrial::limitOutput(float v) const { + if (v < parameters->minValue) + v = parameters->minValue; + if (v > parameters->maxValue) + v = parameters->maxValue; + return v; +} diff --git a/firmware/util/math/pid.h b/firmware/util/math/pid.h index 289f183acf..1a43fae1b8 100644 --- a/firmware/util/math/pid.h +++ b/firmware/util/math/pid.h @@ -105,4 +105,24 @@ private: void updateITerm(float value) override; }; +/** + * A PID with derivative filtering (backward differences) and integrator anti-windup. + * See: Wittenmark B., Astrom K., Arzen K. IFAC Professional Brief. Computer Control: An Overview. + * Two additional parameters used: derivativeFilterLoss and antiwindupFreq + * (If both are 0, then this controller is identical to PidParallelController) + */ +class PidIndustrial : public Pid { +public: + using Pid::getOutput; + float getOutput(float target, float input, float dTime) override; + +public: + // todo: move this to pid_s one day + float_t antiwindupFreq = 0.0f; // = 1/ResetTime + float_t derivativeFilterLoss = 0.0f; // = 1/Gain + +private: + float limitOutput(float v) const; +}; + #endif /* PID_H_ */ diff --git a/unit_tests/tests/test_pid.cpp b/unit_tests/tests/test_pid.cpp index 43165ce670..479faf3208 100644 --- a/unit_tests/tests/test_pid.cpp +++ b/unit_tests/tests/test_pid.cpp @@ -88,3 +88,73 @@ TEST(util, pidLimits) { ASSERT_EQ( 40.0, pid.getOutput(/*target*/50, /*input*/0)) << "target=50, input=0 #3"; } + +TEST(util, pidIndustrial) { + pid_s pidS; + pidS.pFactor = 1.0; + pidS.iFactor = 1.0; + pidS.dFactor = 1.0; + pidS.offset = 0; + pidS.minValue = 0; + pidS.maxValue = 100; + pidS.periodMs = 1; + + PidIndustrial pid; + pid.initPidClass(&pidS); + + // we want to compare with the "normal" PID controller + Pid pid0(&pidS); + + // no additional features + pid.derivativeFilterLoss = 0; + pid.antiwindupFreq = 0; + + float industValue = pid.getOutput(/*target*/1, /*input*/0); + // check if the first output is clamped because of large deviative + ASSERT_EQ(100.0, industValue); + + // check if all output of the 'zeroed' PidIndustrial (w/o new features) is the same as our "normal" Pid + for (int i = 0; i < 10; i++) { + float normalValue = pid0.getOutput(1, 0); + ASSERT_EQ(normalValue, industValue) << "[" << i << "]"; + industValue = pid.getOutput(1, 0); + } + + pid.reset(); + + // now test the "derivative filter loss" param (some small value) + pid.derivativeFilterLoss = 0.01; + + // now the first value is less (and not clipped!) due to the derivative filtering + ASSERT_EQ(67.671669f, pid.getOutput(1, 0)); + // here we still have some leftovers of the initial D-term + ASSERT_EQ(45.4544487f, pid.getOutput(1, 0)); + // but the value is quickly fading + ASSERT_EQ(30.6446342f, pid.getOutput(1, 0)); + + pid.reset(); + + // now test much stronger "derivative filter loss" + pid.derivativeFilterLoss = 0.1; + + // now the first value is much less due to the derivative filtering + ASSERT_EQ(10.5288095f, pid.getOutput(1, 0)); + // here we still have some leftovers of the initial D-term + ASSERT_EQ(10.0802946f, pid.getOutput(1, 0)); + // but the fading is slower than with 'weaker' derivative filter above + ASSERT_EQ(9.65337563f, pid.getOutput(1, 0)); + + pid.reset(); + pid.derivativeFilterLoss = 0; + + // now test "anti-windup" param + pid.antiwindupFreq = 0.1; + + // the first value is clipped, and that's when the anti-windup comes into effect + ASSERT_EQ(100.0f, pid.getOutput(1, 0)); + // it stores a small negative offset in the I-term to avoid it's saturation! + ASSERT_EQ(-0.0455025025f, pid.getIntegration()); + // and that's why the second output is smaller then that of normal PID (=1.00999999) + ASSERT_EQ(0.959497511f, pid.getOutput(1, 0)); + +}