New PID with CIC integrator (#519)

firmware/controllers/math/pid.cpp

@@ -38,29 +38,23 @@ float Pid::getValue(float target, float input) {
 	return getValue(target, input, 1);
 }
 
-float Pid::getValue(float target, float input, float dTime) {
+float Pid::getRawValue(float target, float input, float dTime) {
 	float error = target - input;
 	prevTarget = target;
 	prevInput = input;
 
 	float pTerm = pid->pFactor * error;
-	iTerm += pid->iFactor * dTime * error;
+	updateITerm(pid->iFactor * dTime * error);
 	dTerm = pid->dFactor / dTime * (error - prevError);
 
 	prevError = error;
 
-	/**
+	return pTerm + iTerm + dTerm + pid->offset;
-	 * If we have exceeded the ability of the controlled device to hit target, the I factor will keep accumulating and approach infinity.
+}
-	 * Here we limit the I-term #353
-	 */
-	if (iTerm > pid->maxValue)
-		iTerm = pid->maxValue;
 
-	// this is kind of a hack. a proper fix would be having separate additional settings 'maxIValue' and 'minIValue'
+float Pid::getValue(float target, float input, float dTime) {
-	if (iTerm < -pid->maxValue)
+	float result = getRawValue(target, input, dTime);
-		iTerm = -pid->maxValue;
 
-	float result = pTerm + iTerm + dTerm + pid->offset;
 	if (result > pid->maxValue) {
 		result = pid->maxValue;
 	} else if (result < pid->minValue) {
@@ -151,3 +145,61 @@ void Pid::showPidStatus(Logging *logging, const char*msg) {
 			iTerm, dTerm);
 
 }
+
+void Pid::updateITerm(float value) {
+	iTerm += value;
+	/**
+	 * If we have exceeded the ability of the controlled device to hit target, the I factor will keep accumulating and approach infinity.
+	 * Here we limit the I-term #353
+	 */
+	if (iTerm > pid->maxValue * 100)
+		iTerm = pid->maxValue * 100;
+
+	// this is kind of a hack. a proper fix would be having separate additional settings 'maxIValue' and 'minIValye'
+	if (iTerm < -pid->maxValue * 100)
+		iTerm = -pid->maxValue * 100;
+}
+
+
+PidCic::PidCic() {
+	// call our derived reset()
+	reset();
+}
+
+PidCic::PidCic(pid_s *pid) : Pid(pid) {
+	// call our derived reset()
+	reset();
+}
+
+void PidCic::reset(void) {
+	Pid::reset();
+
+	totalItermCnt = 0;
+	for (int i = 0; i < PID_AVG_BUF_SIZE; i++)
+		iTermBuf[i] = 0;
+	iTermInvNum = 1.0f / (float)PID_AVG_BUF_SIZE;
+}
+
+float PidCic::getValue(float target, float input, float dTime) {
+	return getRawValue(target, input, dTime);
+}
+
+void PidCic::updateITerm(float value) {
+	// use a variation of cascaded integrator-comb (CIC) filtering to get non-overflow iTerm
+	totalItermCnt++;
+	int localBufPos = (totalItermCnt >> PID_AVG_BUF_SIZE_SHIFT) % PID_AVG_BUF_SIZE;
+	int localPrevBufPos = ((totalItermCnt - 1) >> PID_AVG_BUF_SIZE_SHIFT) % PID_AVG_BUF_SIZE;
+	
+	// reset old buffer cell
+	if (localPrevBufPos != localBufPos)
+		iTermBuf[localBufPos] = 0;
+	// integrator stage
+	iTermBuf[localBufPos] += value;
+	
+	// return moving average of all sums, to smoothen the result
+	float iTermSum = 0;
+	for (int i = 0; i < PID_AVG_BUF_SIZE; i++) {
+		iTermSum += iTermBuf[i];
+	}
+	iTerm = iTermSum * iTermInvNum;
+}

firmware/controllers/math/pid.h

@@ -16,6 +16,10 @@
 #include "tunerstudio_configuration.h"
 #endif
 
+// See PidCic below
+#define PID_AVG_BUF_SIZE_SHIFT 5
+#define PID_AVG_BUF_SIZE (1<<PID_AVG_BUF_SIZE_SHIFT) // 32*sizeof(float)
+
 class Pid {
 
 public:
@@ -26,9 +30,11 @@ public:
 
 	float getValue(float target, float input);
 	// todo: dTime should be taken from pid_s
-	float getValue(float target, float input, float dTime);
+	virtual float getValue(float target, float input, float dTime);
+	// doesn't limit the result (used in incremental CIC PID, see below)
+	float getRawValue(float target, float input, float dTime);
 	void updateFactors(float pFactor, float iFactor, float dFactor);
-	void reset(void);
+	virtual void reset(void);
 	float getP(void);
 	float getI(void);
 	float getD(void);
@@ -54,6 +60,35 @@ private:
 	float prevTarget;
 	float prevInput;
 	float prevResult;
+
+private:
+	virtual void updateITerm(float value);
+};
+
+
+/**
+ * A PID impl. with a modified cascaded integrator-comb (CIC) filtering.
+ * Used for incremental auto-IAC control. See autoIdle() in idle_thread.cpp
+ */
+class PidCic : public Pid {
+
+public:
+	PidCic();
+	PidCic(pid_s *pid);
+
+	virtual void reset(void);
+	virtual float getValue(float target, float input, float dTime);
+	
+private:
+	// Circular running-average buffer for I-term, used by CIC-like filter
+	float iTermBuf[PID_AVG_BUF_SIZE];
+	// Needed by averaging (smoothing) of iTerm sums
+	float iTermInvNum;
+	// Total PID iterations (>240 days max. for 10ms update period)
+	int totalItermCnt;
+
+private:
+	virtual void updateITerm(float value);
 };
 
 #endif /* PID_H_ */