Added a filter parameter to the pid function

Conflicts:
	apps/multirotor_pos_control/multirotor_pos_control.c

7 files changed, 40 insertions, 31 deletions

diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
index 769b8b0a8..a226757e0 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
@@ -128,8 +128,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	if (!initialized) {
 		parameters_init(&h);
 		parameters_update(&h, &p);
-		pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
-		pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
+		pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, 0, PID_MODE_DERIVATIV_NONE); //P Controller
+		pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, 0, PID_MODE_DERIVATIV_NONE); //P Controller
 		initialized = true;
 	}
 
@@ -137,8 +137,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	if (counter % 100 == 0) {
 		/* update parameters from storage */
 		parameters_update(&h, &p);
-		pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim);
-		pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim);
+		pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, 0);
+		pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, 0);
 	}
 
 	/* Roll (P) */
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
index 4eccc118c..79194e515 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
@@ -179,9 +179,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	if (!initialized) {
 		parameters_init(&h);
 		parameters_update(&h, &p);
-		pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller
-		pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
-		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
+		pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller
+		pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
+		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
 		initialized = true;
 	}
 
@@ -189,9 +189,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	if (counter % 100 == 0) {
 		/* update parameters from storage */
 		parameters_update(&h, &p);
-		pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1);
-		pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1);
-		pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1);
+		pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, 0);
+		pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, 0);
+		pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, 0);
 	}
 
 
diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
index 71c78f5b8..48ec3603f 100644
--- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
+++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
@@ -239,10 +239,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 
 	parameters_init(&h);
 	parameters_update(&h, &p);
-	pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit
-	pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE);
-	pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE);
-	pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value
+	pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, 0.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit
+	pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, 0.0f, PID_MODE_DERIVATIV_NONE);
+	pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, 0.0f, PID_MODE_DERIVATIV_NONE);
+	pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD_F, 0.0f, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value
 
 	/* error and performance monitoring */
 	perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");
@@ -268,10 +268,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 
 				/* update parameters from storage */
 				parameters_update(&h, &p);
-				pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
-				pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
-				pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
-				pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value
+				pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f, 0.0f); //arbitrary high limit
+				pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim, 0.0f);
+				pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim, 0.0f);
+				pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD_F, 0.0f); //TODO: remove hardcoded value
 			}
 
 			/* only run controller if attitude changed */
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
index 1053ac7a3..d7e1a3adc 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.c
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c
@@ -155,8 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		parameters_init(&h);
 		parameters_update(&h, &p);
 
-		pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET);
-		pid_init(&roll_controller,  p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET);
+		pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET);
+		pid_init(&roll_controller,  p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET);
 
 		initialized = true;
 	}
@@ -167,8 +167,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		parameters_update(&h, &p);
 
 		/* apply parameters */
-		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
-		pid_set_parameters(&roll_controller,  p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
+		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f);
+		pid_set_parameters(&roll_controller,  p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f);
 	}
 
 	/* reset integral if on ground */
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index 4ab92b955..57aea726a 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -182,17 +182,17 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 		initialized = true;
 
 		pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f,
-					 1000.0f, PID_MODE_DERIVATIV_CALC);
+					 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC);
 		pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f,
-					 1000.0f, PID_MODE_DERIVATIV_CALC);
+					 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC);
 	}
 
 	/* load new parameters with lower rate */
 	if (motor_skip_counter % 2500 == 0) {
 		/* update parameters from storage */
 		parameters_update(&h, &p);
-		pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f);
-		pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f);
+		pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f);
+		pid_set_parameters(&roll_rate_controller,  p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f);
 	}
 
 	/* control pitch (forward) output */
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 08e0ca46f..d0e67d3ea 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -43,7 +43,7 @@
 #include <math.h>
 
 __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
-		       float limit, uint8_t mode)
+		       float limit, float diff_filter_factor, uint8_t mode)
 {
 	pid->kp = kp;
 	pid->ki = ki;
@@ -51,6 +51,7 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
 	pid->intmax = intmax;
 	pid->limit = limit;
 	pid->mode = mode;
+	pid->diff_filter_factor = diff_filter_factor;
 	pid->count = 0.0f;
 	pid->saturated = 0.0f;
 	pid->last_output = 0.0f;
@@ -60,7 +61,7 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
 	pid->control_previous = 0.0f;
 	pid->integral = 0.0f;
 }
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor)
 {
 	int ret = 0;
 
@@ -99,6 +100,13 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
 		ret = 1;
 	}
 
+	if (isfinite(diff_filter_factor)) {
+		pid->limit = diff_filter_factor;
+
+	}  else {
+		ret = 1;
+	}
+
 	return ret;
 }
 
@@ -137,7 +145,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	// Calculated current error value
 	float error = pid->sp - val;
 
-	float error_filtered = 0.2f*error + 0.8f*pid->error_previous_filtered;
+	float error_filtered = pid->diff_filter_factor*error + (1.0f-pid->diff_filter_factor)*pid->error_previous_filtered;
 
 	// Calculate or measured current error derivative
 
diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h
index 5790459c8..f89c36d75 100644
--- a/src/modules/systemlib/pid/pid.h
+++ b/src/modules/systemlib/pid/pid.h
@@ -64,12 +64,13 @@ typedef struct {
 	float last_output;
 	float limit;
 	uint8_t mode;
+	float diff_filter_factor;
 	uint8_t count;
 	uint8_t saturated;
 } PID_t;
 
-__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);
+__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor, uint8_t mode);
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor);
 //void pid_set(PID_t *pid, float sp);
 __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);