Industrial PID Controller (#1002)

* Implement Industrial PID controller

* Test-Use PidIndustrial in alternator

* Meaningful unit-tests for PidIndustrial

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));
 

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

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;
+}

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_ */

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));
+
+}