Merge branch 'mpc_fix' into vector_control2

7 files changed, 124 insertions, 185 deletions

diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
index 8245aa560..b7df0433a 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.c
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c
@@ -194,8 +194,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, 0.0f);
-		pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
+		pid_init(&pitch_controller, PID_MODE_DERIVATIV_SET, 0.0f);
+		pid_init(&roll_controller, PID_MODE_DERIVATIV_SET, 0.0f);
 
 		initialized = true;
 	}
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index 86ac0e4ff..eb41bf93e 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -161,9 +161,9 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 		parameters_update(&h, &p);
 		initialized = true;
 
-		pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
-		pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
-		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
+		pid_init(&pitch_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
+		pid_init(&roll_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
+		pid_init(&yaw_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
 	}
 
 	/* load new parameters with lower rate */
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 3d23d0c09..834df9655 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -244,39 +244,41 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	parameters_init(&params_h);
 	parameters_update(&params_h, &params);
 
-
 	for (int i = 0; i < 2; i++) {
-		pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
-		pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
+		pid_init(&(xy_pos_pids[i]), PID_MODE_DERIVATIV_SET, 0.02f);
+		pid_init(&(xy_vel_pids[i]), PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
 	}
 
-	pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
-	thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
+	pid_init(&z_pos_pid, PID_MODE_DERIVATIV_SET, 0.02f);
+	thrust_pid_init(&z_vel_pid, 0.02f);
+
+	bool param_updated = true;
 
 	while (!thread_should_exit) {
 
-		bool param_updated;
-		orb_check(param_sub, &param_updated);
+		if (!param_updated)
+			orb_check(param_sub, &param_updated);
 
 		if (param_updated) {
 			/* clear updated flag */
 			struct parameter_update_s ps;
 			orb_copy(ORB_ID(parameter_update), param_sub, &ps);
+
 			/* update params */
 			parameters_update(&params_h, &params);
 
-			for (int i = 0; i < 2; i++) {
-				pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
-				/* use integral_limit_out = tilt_max / 2 */
-				float i_limit;
+			/* integral_limit * ki = tilt_max / 2 */
+			float i_limit;
 
-				if (params.xy_vel_i > 0.0f) {
-					i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
+			if (params.xy_vel_i > 0.0f) {
+				i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
 
-				} else {
-					i_limit = 0.0f;	// not used
-				}
+			} else {
+				i_limit = 0.0f;	// not used
+			}
 
+			for (int i = 0; i < 2; i++) {
+				pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 0.0f, 0.0f);
 				pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
 			}
 
@@ -471,6 +473,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 							} else {
 								local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
 							}
+
 							/* update yaw setpoint only if value is valid */
 							if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
 								att_sp.yaw_body = global_pos_sp.yaw;
diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c
index b985630ae..39a715322 100644
--- a/src/modules/multirotor_pos_control/thrust_pid.c
+++ b/src/modules/multirotor_pos_control/thrust_pid.c
@@ -43,22 +43,22 @@
 #include "thrust_pid.h"
 #include <math.h>
 
-__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min)
+#define COS_TILT_MAX	0.7f
+
+__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min)
 {
-	pid->kp = kp;
-	pid->ki = ki;
-	pid->kd = kd;
-	pid->limit_min = limit_min;
-	pid->limit_max = limit_max;
-	pid->mode = mode;
 	pid->dt_min = dt_min;
-	pid->last_output = 0.0f;
-	pid->sp = 0.0f;
-	pid->error_previous = 0.0f;
+	pid->kp = 0.0f;
+	pid->ki = 0.0f;
+	pid->kd = 0.0f;
 	pid->integral = 0.0f;
+	pid->output_min = 0.0f;
+	pid->output_max = 0.0f;
+	pid->error_previous = 0.0f;
+	pid->last_output = 0.0f;
 }
 
-__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max)
+__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_max)
 {
 	int ret = 0;
 
@@ -83,15 +83,15 @@ __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, fl
 		ret = 1;
 	}
 
-	if (isfinite(limit_min)) {
-		pid->limit_min = limit_min;
+	if (isfinite(output_min)) {
+		pid->output_min = output_min;
 
 	}  else {
 		ret = 1;
 	}
 
-	if (isfinite(limit_max)) {
-		pid->limit_max = limit_max;
+	if (isfinite(output_max)) {
+		pid->output_max = output_max;
 
 	}  else {
 		ret = 1;
@@ -102,40 +102,18 @@ __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, fl
 
 __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22)
 {
-	/* Alternative integral component calculation
-	 *
-	 * start:
-	 * error = setpoint - current_value
-	 * integral = integral + (Ki * error * dt)
-	 * derivative = (error - previous_error) / dt
-	 * previous_error = error
-	 * output = (Kp * error) + integral + (Kd * derivative)
-	 * wait(dt)
-	 * goto start
-	 */
-
 	if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) {
 		return pid->last_output;
 	}
 
 	float i, d;
-	pid->sp = sp;
-
-	// Calculated current error value
-	float error = pid->sp - val;
 
-	// Calculate or measured current error derivative
-	if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) {
-		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
-		pid->error_previous = error;
+	/* error value */
+	float error = sp - val;
 
-	} else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) {
-		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
-		pid->error_previous = -val;
-
-	} else {
-		d = 0.0f;
-	}
+	/* error derivative */
+	d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
+	pid->error_previous = -val;
 
 	if (!isfinite(d)) {
 		d = 0.0f;
@@ -145,36 +123,41 @@ __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, floa
 	i = pid->integral + (pid->ki * error * dt);
 
 	/* attitude-thrust compensation
-	 * r22 is (2, 2) componet of rotation matrix for current attitude */
+	 * r22 is (2, 2) component of rotation matrix for current attitude */
 	float att_comp;
 
-	if (r22 > 0.8f)
+	if (r22 > COS_TILT_MAX) {
 		att_comp = 1.0f / r22;
-	else if (r22 > 0.0f)
-		att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f;
-	else
+
+	} else if (r22 > 0.0f) {
+		att_comp = ((1.0f / COS_TILT_MAX - 1.0f) / COS_TILT_MAX) * r22 + 1.0f;
+
+	} else {
 		att_comp = 1.0f;
+	}
 
-	/* calculate output */
-	float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp;
+	/* calculate PD output */
+	float output = ((error * pid->kp) + (d * pid->kd)) * att_comp;
 
 	/* check for saturation */
-	if (output < pid->limit_min || output > pid->limit_max) {
-		/* saturated, recalculate output with old integral */
-		output = (error * pid->kp) + pid->integral + (d * pid->kd);
-
-	} else {
-		if (isfinite(i)) {
+	if (isfinite(i)) {
+		float i_comp = i * att_comp;
+		if ((output + i_comp) >= pid->output_min || (output + i_comp) <= pid->output_max) {
+			/* not saturated, use new integral value */
 			pid->integral = i;
 		}
 	}
 
+	/* add I component to output */
+	output += pid->integral * att_comp;
+
+	/* limit output */
 	if (isfinite(output)) {
-		if (output > pid->limit_max) {
-			output = pid->limit_max;
+		if (output > pid->output_max) {
+			output = pid->output_max;
 
-		} else if (output < pid->limit_min) {
-			output = pid->limit_min;
+		} else if (output < pid->output_min) {
+			output = pid->output_min;
 		}
 
 		pid->last_output = output;
diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h
index 5e169c1ba..71c3704d0 100644
--- a/src/modules/multirotor_pos_control/thrust_pid.h
+++ b/src/modules/multirotor_pos_control/thrust_pid.h
@@ -47,27 +47,20 @@
 
 __BEGIN_DECLS
 
-/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */
-#define THRUST_PID_MODE_DERIVATIV_CALC	0
-/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */
-#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP	1
-
 typedef struct {
+	float dt_min;
 	float kp;
 	float ki;
 	float kd;
-	float sp;
 	float integral;
+	float output_min;
+	float output_max;
 	float error_previous;
 	float last_output;
-	float limit_min;
-	float limit_max;
-	float dt_min;
-	uint8_t mode;
 } thrust_pid_t;
 
-__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min);
-__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max);
+__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min);
+__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_max);
 __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22);
 __EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i);
 
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 77c952f52..0bec72495 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -39,7 +39,7 @@
 /**
  * @file pid.c
  *
- * Implementation of generic PID control interface.
+ * Implementation of generic PID controller.
  *
  * @author Laurens Mackay <mackayl@student.ethz.ch>
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
@@ -53,24 +53,21 @@
 
 #define SIGMA 0.000001f
 
-__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
-		       float limit, uint8_t mode, float dt_min)
+__EXPORT void pid_init(PID_t *pid, uint8_t mode, float dt_min)
 {
-	pid->kp = kp;
-	pid->ki = ki;
-	pid->kd = kd;
-	pid->intmax = intmax;
-	pid->limit = limit;
 	pid->mode = mode;
 	pid->dt_min = dt_min;
-	pid->count = 0.0f;
-	pid->saturated = 0.0f;
-	pid->last_output = 0.0f;
-	pid->sp = 0.0f;
-	pid->error_previous = 0.0f;
+	pid->kp = 0.0f;
+	pid->ki = 0.0f;
+	pid->kd = 0.0f;
 	pid->integral = 0.0f;
+	pid->integral_limit = 0.0f;
+	pid->output_limit = 0.0f;
+	pid->error_previous = 0.0f;
+	pid->last_output = 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 integral_limit, float output_limit)
 {
 	int ret = 0;
 
@@ -95,15 +92,15 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
 		ret = 1;
 	}
 
-	if (isfinite(intmax)) {
-		pid->intmax = intmax;
+	if (isfinite(integral_limit)) {
+		pid->integral_limit = integral_limit;
 
 	}  else {
 		ret = 1;
 	}
 
-	if (isfinite(limit)) {
-		pid->limit = limit;
+	if (isfinite(output_limit)) {
+		pid->output_limit = output_limit;
 
 	}  else {
 		ret = 1;
@@ -112,42 +109,18 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
 	return ret;
 }
 
-//void pid_set(PID_t *pid, float sp)
-//{
-//	pid->sp = sp;
-//	pid->error_previous = 0;
-//	pid->integral = 0;
-//}
-
-/**
- *
- * @param pid
- * @param val
- * @param dt
- * @return
- */
 __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
 {
-	/*  error = setpoint - actual_position
-	 integral = integral + (error*dt)
-	 derivative = (error - previous_error)/dt
-	 output = (Kp*error) + (Ki*integral) + (Kd*derivative)
-	 previous_error = error
-	 wait(dt)
-	 goto start
-	 */
-
 	if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) {
 		return pid->last_output;
 	}
 
 	float i, d;
-	pid->sp = sp;
 
-	// Calculated current error value
-	float error = pid->sp - val;
+	/* current error value */
+	float error = sp - val;
 
-	// Calculate or measured current error derivative
+	/* current error derivative */
 	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
 		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
 		pid->error_previous = error;
@@ -167,39 +140,29 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		d = 0.0f;
 	}
 
-	if (pid->ki > 0.0f) {
-		// Calculate the error integral and check for saturation
-		i = pid->integral + (error * dt);
-
-		if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) ||
-		    fabsf(i) > pid->intmax) {
-			i = pid->integral;		// If saturated then do not update integral value
-			pid->saturated = 1;
-
-		} else {
-			if (!isfinite(i)) {
-				i = 0.0f;
-			}
+	/* calculate PD output */
+	float output = (error * pid->kp) + (d * pid->kd);
 
+	/* check for saturation */
+	if (isfinite(i)) {
+		if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
+		    fabsf(i) <= pid->integral_limit) {
+			/* not saturated, use new integral value */
 			pid->integral = i;
-			pid->saturated = 0;
 		}
-
-	} else {
-		i = 0.0f;
-		pid->saturated = 0;
 	}
 
-	// Calculate the output.  Limit output magnitude to pid->limit
-	float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
+	/* add I component to output */
+	output += pid->integral * pid-> ki;
 
+	/* limit output */
 	if (isfinite(output)) {
-		if (pid->limit > SIGMA) {
-			if (output > pid->limit) {
-				output = pid->limit;
+		if (pid->output_limit > SIGMA) {
+			if (output > pid->output_limit) {
+				output = pid->output_limit;
 
-			} else if (output < -pid->limit) {
-				output = -pid->limit;
+			} else if (output < -pid->output_limit) {
+				output = -pid->output_limit;
 			}
 		}
 
@@ -212,5 +175,5 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 
 __EXPORT void pid_reset_integral(PID_t *pid)
 {
-	pid->integral = 0;
+	pid->integral = 0.0f;
 }
diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h
index eca228464..e8b1aac4f 100644
--- a/src/modules/systemlib/pid/pid.h
+++ b/src/modules/systemlib/pid/pid.h
@@ -39,7 +39,7 @@
 /**
  * @file pid.h
  *
- * Definition of generic PID control interface.
+ * Definition of generic PID controller.
  *
  * @author Laurens Mackay <mackayl@student.ethz.ch>
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
@@ -55,38 +55,35 @@
 
 __BEGIN_DECLS
 
-/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error
- * val_dot in pid_calculate() will be ignored */
-#define PID_MODE_DERIVATIV_CALC	0
-/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored
- * val_dot in pid_calculate() will be ignored */
-#define PID_MODE_DERIVATIV_CALC_NO_SP	1
-/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
-#define PID_MODE_DERIVATIV_SET	2
-// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
-#define PID_MODE_DERIVATIV_NONE 9
+typedef enum PID_MODE {
+	/* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */
+	PID_MODE_DERIVATIV_NONE = 0,
+	/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
+	 * val_dot in pid_calculate() will be ignored */
+	PID_MODE_DERIVATIV_CALC,
+	/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value,
+	 * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */
+	PID_MODE_DERIVATIV_CALC_NO_SP,
+	/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
+	PID_MODE_DERIVATIV_SET
+} pid_mode_t;
 
 typedef struct {
+	pid_mode_t mode;
+	float dt_min;
 	float kp;
 	float ki;
 	float kd;
-	float intmax;
-	float sp;
 	float integral;
+	float integral_limit;
+	float output_limit;
 	float error_previous;
 	float last_output;
-	float limit;
-	float dt_min;
-	uint8_t mode;
-	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, float dt_min);
-__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 void pid_init(PID_t *pid, pid_mode_t mode, float dt_min);
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);
 __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
-
 __EXPORT void pid_reset_integral(PID_t *pid);
 
 __END_DECLS