Changes to the PID controller. Adds "limit" to the parameter set. Implements an output limit where the output magnitude is limited by the parameter value "limit". Also changes the integrator saturation such that the integrator is not updated (added to) if either updating it will cause the integrator values magnitude to exceed "intmax" or if the output magnitude would exceed "limit" with an updated integrator value.

Arbitrary large limit values were hard coded into multirotor_attitude_control.c.  These should be changed to parametric values or something sensible.

This commit will temporarily break fixedwing_control.c.  A following commit will repair it along with significant changes to the inner loop control.

This commit has been tested to compile with fixedwing_control.c temporarily removed.  No other testing has been completed.

3 files changed, 42 insertions, 58 deletions

diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index 2129915d1..cb34209b1 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -212,13 +212,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		parameters_update(&h, &p);
 
 		// pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu,
-		// 	PID_MODE_DERIVATIV_SET, 154);
+		// 	90.0f, PID_MODE_DERIVATIV_SET, 154);	// Arbitrarily large limit added
 		pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu,
-			PID_MODE_DERIVATIV_SET);
+			90.0f, PID_MODE_DERIVATIV_SET);	// Arbitrarily large limit added
 		pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
-			PID_MODE_DERIVATIV_SET);
+			90.0f, PID_MODE_DERIVATIV_SET);	// Arbitrarily large limit added
 		pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
-			PID_MODE_DERIVATIV_SET);
+			90.0f, PID_MODE_DERIVATIV_SET);	// Arbitrarily large limit added
 
 		initialized = true;
 	}
@@ -228,10 +228,10 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		/* update parameters from storage */
 		parameters_update(&h, &p);
 		/* apply parameters */
-		// pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu);
-		pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu);
-		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
-		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
+		// pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu, 90.0f);
+		pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu, 90.0f);
+		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, 90.0f);
+		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, 90.0f);
 	}
 
 	/* calculate current control outputs */
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);