WIP on HIL

2 files changed, 41 insertions, 20 deletions

diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c
index 807373c15..fbf3edc33 100644
--- a/apps/systemlib/pid/pid.c
+++ b/apps/systemlib/pid/pid.c
@@ -40,18 +40,19 @@
  */
 
 #include "pid.h"
+#include <math.h>
 
 __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
-	      uint8_t mode, uint8_t plot_i)
+	      uint8_t mode)
 {
 	pid->kp = kp;
 	pid->ki = ki;
 	pid->kd = kd;
 	pid->intmax = intmax;
 	pid->mode = mode;
-	pid->plot_i = plot_i;
 	pid->count = 0;
 	pid->saturated = 0;
+	pid->last_output = 0;
 
 	pid->sp = 0;
 	pid->error_previous = 0;
@@ -63,11 +64,7 @@ __EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
 	pid->ki = ki;
 	pid->kd = kd;
 	pid->intmax = intmax;
-	//	pid->mode = mode;
-
-	//	pid->sp = 0;
-	//	pid->error_previous = 0;
-	//	pid->integral = 0;
+	// pid->limit = limit;
 }
 
 //void pid_set(PID_t *pid, float sp)
@@ -95,6 +92,11 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	 goto start
 	 */
 
+	if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt))
+	{
+		return pid->last_output;
+	}
+
 	float i, d;
 	pid->sp = sp;
 	float error = pid->sp - val;
@@ -111,7 +113,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	}
 
 	// Anti-Windup. Needed if we don't use the saturation above.
-	if (pid->intmax != 0.0) {
+	if (pid->intmax != 0.0f) {
 		if (i > pid->intmax) {
 			pid->integral = pid->intmax;
 
@@ -122,14 +124,6 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		} else {
 			pid->integral = i;
 		}
-
-		//Send Controller integrals
-		//		Disabled because of new possibilities with debug_vect.
-		//		Now sent in Main Loop at 5 Hz. 26.06.2010 Laurens
-		//		if (pid->plot_i && (pid->count++ % 16 == 0)&&(global_data.param[PARAM_SEND_SLOT_DEBUG_2] == 1))
-		//		{
-		//			mavlink_msg_debug_send(MAVLINK_COMM_1, pid->plot_i, pid->integral);
-		//		}
 	}
 
 	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
@@ -142,7 +136,33 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		d = 0;
 	}
 
-	pid->error_previous = error;
+	if (pid->kd == 0) {
+		d = 0;
+	}
+
+	if (pid->ki == 0) {
+		i = 0;
+	}
+
+	if (pid->kp == 0) {
+		p = 0;
+	} else {
+		p = error;
+	}
+
+	if (isfinite(error)) {
+		pid->error_previous = error;
+	}
+
+	float val = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
+
+	if (isfinite(val)) {
+		last_output = val;
+	}
+
+	if (!isfinite(pid->integral)) {
+		pid->integral = 0;
+	}
 
-	return (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
+	return pid->last_output;
 }
diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h
index 83bf09b59..d4bbcaf31 100644
--- a/apps/systemlib/pid/pid.h
+++ b/apps/systemlib/pid/pid.h
@@ -58,13 +58,14 @@ typedef struct {
 	float sp;
 	float integral;
 	float error_previous;
+	float last_output;
+	float limit;
 	uint8_t mode;
-	uint8_t plot_i;
 	uint8_t count;
 	uint8_t saturated;
 } PID_t;
 
-__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i);
+__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode);
 __EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax);
 //void pid_set(PID_t *pid, float sp);
 __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);