Mercurial > mbsePi-apps / file diff

--- a/thermferm/pid.c	Sat May 16 17:39:30 2015 +0200
+++ b/thermferm/pid.c	Sun May 17 19:34:55 2015 +0200
@@ -26,8 +26,8 @@
 
 void InitPID(pid_var *pid, int type)
 {
-    pid->Err = pid->ErrLast = pid->ErrLastLast = 0.0;
-    pid->Input = pid->InputD = pid->OutP = pid->InputLast = pid->SetP = 0.0;
+    pid->Err = pid->ErrLast = pid->iState = 0.0;
+    pid->Input = pid->OutP = pid->SetP = 0.0;
     pid->pGain = pid->iGain = pid->dGain = 0.0;
     pid->idleRange = 0.4;
     pid->Mode = PID_MODE_NONE;
@@ -41,39 +41,55 @@
 {
     if (pid->Mode == PID_MODE_AUTO) {
 
-	pid->InputD = pid->Input + (pid->Input - pid->InputLast) * pid->dGain * PID_TIMES;
-	pid->InputLast = pid->Input;
+	double	pTerm, dTerm, iTerm;
+
 	if (pid->Type == PID_TYPE_HEAT)
-	    pid->Err = pid->InputD - pid->SetP;
+	    pid->Err = pid->SetP - pid->Input;
 	else
-	    pid->Err = pid->SetP - pid->InputD;
+	    pid->Err = pid->Input - pid->SetP;
 
-	pid->OutP = pid->OutP + pid->pGain * (pid->Err - pid->ErrLast + pid->iGain * pid->Err + pid->dGain * (pid->Err - pid->ErrLast * 2 + pid->ErrLastLast));
-	pid->ErrLastLast = pid->ErrLast;
+	/*
+	 * Calculate the integral state with appopriate limiting.
+	 * Use ErrLastLast as iState
+	 */
+	pid->iState += pid->Err;
+	if (pid->iState > PID_WINDUP_GUARD)
+	    pid->iState = PID_WINDUP_GUARD;
+	else if (pid->iState < -PID_WINDUP_GUARD)
+	    pid->iState = -PID_WINDUP_GUARD;
+
+	pTerm = pid->pGain * pid->Err;
+	iTerm = pid->iGain * pid->iState;
+	dTerm = pid->dGain * (pid->Err - pid->ErrLast);
+
+	pid->OutP = pTerm + dTerm + iTerm;
 	pid->ErrLast = pid->Err;
 
-    } if (pid->Mode == PID_MODE_BOO) {
+    } else if (pid->Mode == PID_MODE_BOO) {
 	/*
 	 * Mode Bang On Off
 	 */
-	pid->InputLast = pid->Input;
+	pid->ErrLast = pid->Err;
+
 	if (pid->Type == PID_TYPE_HEAT)
-	    pid->ErrLastLast = pid->ErrLast = pid->Err = pid->SetP - pid->Input;
+	    pid->Err = pid->SetP - pid->Input;
 	else
-	    pid->ErrLastLast = pid->ErrLast = pid->Err = pid->Input - pid->SetP;
+	    pid->Err = pid->Input - pid->SetP;
 
-	if (pid->Err > 0.0)
+	if (pid->OutP && (pid->Err <= 0.0))
+	    pid->OutP = 0.0;
+	else if ((pid->OutP == 0.0) && (pid->Err > pid->idleRange))
 	    pid->OutP = pid->iMax;
-	else
-	    pid->OutP = 0.0;
+
+	pid->iState = 0.0;
 
     } else {
 	/*
 	 * While in manual mode, stay ready for bumpless switch to
 	 * auto.
 	 */
-	pid->InputLast = pid->Input;
-	pid->ErrLastLast = pid->ErrLast = pid->Err;
+	pid->ErrLast = pid->Err = 0.0;
+	pid->OutP = pid->iState = 0.0;
     }
 
     if (pid->OutP > pid->iMax)