2 files changed, 34 insertions, 50 deletions

diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c
index cff5e6bbe..f9b50d030 100644
--- a/apps/systemlib/pid/pid.c
+++ b/apps/systemlib/pid/pid.c
@@ -43,12 +43,13 @@
 #include <math.h>
 
 __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
-	      uint8_t mode)
+	      float limit, uint8_t mode)
 {
 	pid->kp = kp;
 	pid->ki = ki;
 	pid->kd = kd;
 	pid->intmax = intmax;
+	pid->limit = limit;
 	pid->mode = mode;
 	pid->count = 0;
 	pid->saturated = 0;
@@ -58,7 +59,7 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
 	pid->error_previous = 0;
 	pid->integral = 0;
 }
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax)
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
 {
 	int ret = 0;
 
@@ -85,8 +86,13 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
 	}  else {
 		ret = 1;
 	}
+	
+	if (isfinite(limit)) {
+		pid->limit = limit;
+	}  else {
+		ret = 1;
+	}
 
-	// pid->limit = limit;
 	return ret;
 }
 
@@ -122,72 +128,48 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 
 	float i, d;
 	pid->sp = sp;
+	
+	// Calculated current error value
 	float error = pid->sp - val;
-
-	if (pid->saturated && (pid->integral * error > 0)) {
-		//Output is saturated and the integral would get bigger (positive or negative)
-		i = pid->integral;
-
-		//Reset saturation. If we are still saturated this will be set again at output limit check.
-		pid->saturated = 0;
-
-	} else {
-		i = pid->integral + (error * dt);
+	
+	if (isfinite(error)) {				// Why is this necessary?  DEW
+		pid->error_previous = error;
 	}
 
-	// Anti-Windup. Needed if we don't use the saturation above.
-	if (pid->intmax != 0.0f) {
-		if (i > pid->intmax) {
-			pid->integral = pid->intmax;
-
-		} else if (i < -pid->intmax) {
-
-			pid->integral = -pid->intmax;
-
-		} else {
-			pid->integral = i;
-		}
-	}
+	// Calculate or measured current error derivative
 
 	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
 		d = (error - pid->error_previous) / dt;
-
 	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
 		d = -val_dot;
-
 	} else {
 		d = 0.0f;
 	}
 
-	if (pid->kd == 0.0f) {
-		d = 0.0f;
-	}
-
-	if (pid->ki == 0.0f) {
-		i = 0;
-	}
-
-	float p;
-
-	if (pid->kp == 0.0f) {
-		p = 0.0f;
+	// Calculate the error integral and check for saturation
+	i = pid->integral + (error * dt);
+	if( fabs((error * pid->kp) + (i * pid->ki) +  (d * pid->kd)) > pid->limit ||
+		fabs(i) > pid->intmax )
+	{
+		i = pid->integral;		// If saturated then do not update integral value
+		pid->saturated = 1;
 	} else {
-		p = error;
-	}
-
-	if (isfinite(error)) {
-		pid->error_previous = error;
+		if (!isfinite(i)) {
+			i = 0;
+		}
+		pid->integral = i;
+		pid->saturated = 0;
 	}
 
+	// Calculate the output.  Limit output magnitude to pid->limit
 	float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
+	if (output > pid->limit) output = pid->limit;
+	if (output < -pid->limit) output = -pid->limit;
 
 	if (isfinite(output)) {
 		pid->last_output = output;
 	}
 
-	if (!isfinite(pid->integral)) {
-		pid->integral = 0;
-	}
 
 	return pid->last_output;
 }
diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h
index 098b2bef8..b51afef9e 100644
--- a/apps/systemlib/pid/pid.h
+++ b/apps/systemlib/pid/pid.h
@@ -49,6 +49,8 @@
 #define PID_MODE_DERIVATIV_CALC	0
 /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
 #define PID_MODE_DERIVATIV_SET	1
+// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
+#define PID_MODE_DERIVATIV_NONE 9
 
 typedef struct {
 	float kp;
@@ -65,8 +67,8 @@ typedef struct {
 	uint8_t saturated;
 } PID_t;
 
-__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode);
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax);
+__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode);
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit);
 //void pid_set(PID_t *pid, float sp);
 __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);