Mercurial > mbsePi-apps / file diff

--- a/thermferm/pid.c	Thu May 14 22:03:35 2015 +0200
+++ b/thermferm/pid.c	Sat May 16 17:39:30 2015 +0200
@@ -24,27 +24,63 @@
 #include "pid.h"
 
 
-double UpdatePID(pid_var *pid, double error, double position)
+void InitPID(pid_var *pid, int type)
 {
-    double	pTerm, dTerm, iTerm;
-
-    pTerm = pid->pGain * error;
-
-    /*
-     * Calculate the integral state with appopriate limiting
-     */
-    pid->iState += error;
-    if (pid->iState > pid->iMax)
-	pid->iState = pid->iMax;
-    else if (pid->iState < pid->iMin)
-	pid->iState = pid->iMin;
-
-    iTerm = pid->iGain * pid->iState;
-
-    dTerm = pid->dGain * (pid->dState - position);
-    pid->dState = position;
-
-    return pTerm + dTerm + iTerm;
+    pid->Err = pid->ErrLast = pid->ErrLastLast = 0.0;
+    pid->Input = pid->InputD = pid->OutP = pid->InputLast = pid->SetP = 0.0;
+    pid->pGain = pid->iGain = pid->dGain = 0.0;
+    pid->idleRange = 0.4;
+    pid->Mode = PID_MODE_NONE;
+    pid->Type = type;
+    pid->iMax = 100.0;
 }
 
 
+
+void UpdatePID(pid_var *pid)
+{
+    if (pid->Mode == PID_MODE_AUTO) {
+
+	pid->InputD = pid->Input + (pid->Input - pid->InputLast) * pid->dGain * PID_TIMES;
+	pid->InputLast = pid->Input;
+	if (pid->Type == PID_TYPE_HEAT)
+	    pid->Err = pid->InputD - pid->SetP;
+	else
+	    pid->Err = pid->SetP - pid->InputD;
+
+	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;
+	pid->ErrLast = pid->Err;
+
+    } if (pid->Mode == PID_MODE_BOO) {
+	/*
+	 * Mode Bang On Off
+	 */
+	pid->InputLast = pid->Input;
+	if (pid->Type == PID_TYPE_HEAT)
+	    pid->ErrLastLast = pid->ErrLast = pid->Err = pid->SetP - pid->Input;
+	else
+	    pid->ErrLastLast = pid->ErrLast = pid->Err = pid->Input - pid->SetP;
+
+	if (pid->Err > 0.0)
+	    pid->OutP = pid->iMax;
+	else
+	    pid->OutP = 0.0;
+
+    } else {
+	/*
+	 * While in manual mode, stay ready for bumpless switch to
+	 * auto.
+	 */
+	pid->InputLast = pid->Input;
+	pid->ErrLastLast = pid->ErrLast = pid->Err;
+    }
+
+    if (pid->OutP > pid->iMax)
+	pid->OutP = pid->iMax;
+    if (pid->OutP < 0.0)
+	pid->OutP = 0.0;
+
+}
+
+