multirotor_pos_control: some refactoring and cleanup, attitude-thrust correction moved to thrust_pid

5 files changed, 54 insertions, 58 deletions

diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index b49186ee9..6252178d1 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -235,7 +235,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 	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.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 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(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
@@ -259,7 +259,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 				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);
-					pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max);
+					pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max);
 				}
 
 				pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
@@ -416,12 +416,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 			}
 
 			/* run position & altitude controllers, calculate velocity setpoint */
-			global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt);
+			global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
 
 			if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) {
 				/* calculate velocity set point in NED frame */
-				global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt);
-				global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt);
+				global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
+				global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];
 
 				/* limit horizontal speed */
 				float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
@@ -439,9 +439,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 			/* publish new velocity setpoint */
 			orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
 
-			/* run velocity controllers, calculate thrust vector */
+			/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
 			float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
-			thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt);
+			thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
 
 			if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) {
 				/* calculate velocity set point in NED frame */
@@ -452,33 +452,24 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 			/* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
 			/* limit horizontal part of thrust */
 			float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
-			float thrust_xy_norm = norm(thrust_sp[0], thrust_sp[1]);
+			/* assuming that vertical component of thrust is g,
+			 * horizontal component = g * tan(alpha) */
+			float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
 
-			if (thrust_xy_norm > params.slope_max) {
-				thrust_xy_norm = params.slope_max;
+			if (tilt > params.tilt_max) {
+				tilt = params.tilt_max;
 			}
 
-			/* use approximation: slope ~ sin(slope) = force */
 			/* convert direction to body frame */
 			thrust_xy_dir -= att.yaw;
 
 			if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) {
 				/* calculate roll and pitch */
-				att_sp.roll_body = sinf(thrust_xy_dir) * thrust_xy_norm;
-				att_sp.pitch_body = -cosf(thrust_xy_dir) * thrust_xy_norm / cosf(att_sp.roll_body);	// reverse pitch
+				att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
+				att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
 			}
 
-			/* attitude-thrust compensation */
-			float att_comp;
-
-			if (att.R[2][2] > 0.8f)
-				att_comp = 1.0f / att.R[2][2];
-			else if (att.R[2][2] > 0.0f)
-				att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f;
-			else
-				att_comp = 1.0f;
-
-			att_sp.thrust = -thrust_sp[2] * att_comp;
+			att_sp.thrust = -thrust_sp[2];
 			att_sp.timestamp = hrt_absolute_time();
 
 			if (status.flag_control_manual_enabled) {
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
index f8a982c6c..1094fd23b 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f);
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f);
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f);
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f);
-PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f);
+PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
 
 int parameters_init(struct multirotor_position_control_param_handles *h)
 {
@@ -74,7 +74,7 @@ int parameters_init(struct multirotor_position_control_param_handles *h)
 	h->xy_vel_i 	=	param_find("MPC_XY_VEL_I");
 	h->xy_vel_d 	=	param_find("MPC_XY_VEL_D");
 	h->xy_vel_max 	=	param_find("MPC_XY_VEL_MAX");
-	h->slope_max 	=	param_find("MPC_SLOPE_MAX");
+	h->tilt_max 	=	param_find("MPC_TILT_MAX");
 
 	h->rc_scale_pitch    =   param_find("RC_SCALE_PITCH");
 	h->rc_scale_roll    =   param_find("RC_SCALE_ROLL");
@@ -99,7 +99,7 @@ int parameters_update(const struct multirotor_position_control_param_handles *h,
 	param_get(h->xy_vel_i, &(p->xy_vel_i));
 	param_get(h->xy_vel_d, &(p->xy_vel_d));
 	param_get(h->xy_vel_max, &(p->xy_vel_max));
-	param_get(h->slope_max, &(p->slope_max));
+	param_get(h->tilt_max, &(p->tilt_max));
 
 	param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
 	param_get(h->rc_scale_roll, &(p->rc_scale_roll));
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
index e9b1f5c39..14a3de2e5 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -56,7 +56,7 @@ struct multirotor_position_control_params {
 	float xy_vel_i;
 	float xy_vel_d;
 	float xy_vel_max;
-	float slope_max;
+	float tilt_max;
 
 	float rc_scale_pitch;
 	float rc_scale_roll;
@@ -78,7 +78,7 @@ struct multirotor_position_control_param_handles {
 	param_t xy_vel_i;
 	param_t xy_vel_d;
 	param_t xy_vel_max;
-	param_t slope_max;
+	param_t tilt_max;
 
 	param_t rc_scale_pitch;
 	param_t rc_scale_roll;
diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c
index 89efe1334..51dcd7df2 100644
--- a/src/modules/multirotor_pos_control/thrust_pid.c
+++ b/src/modules/multirotor_pos_control/thrust_pid.c
@@ -1,11 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
- *           Julian Oes <joes@student.ethz.ch>
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,13 +35,9 @@
 /**
  * @file thrust_pid.c
  *
- * Implementation of generic PID control interface.
+ * Implementation of thrust control PID.
  *
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
- * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #include "thrust_pid.h"
@@ -108,16 +100,18 @@ __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, fl
 	return ret;
 }
 
-__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt)
+__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22)
 {
 	/* Alternative integral component calculation
-	 error = setpoint - actual_position
-	 integral = integral + (Ki*error*dt)
-	 derivative = (error - previous_error)/dt
-	 output = (Kp*error) + integral + (Kd*derivative)
-	 previous_error = error
-	 wait(dt)
-	 goto start
+	 *
+	 * 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)) {
@@ -147,21 +141,32 @@ __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, floa
 		d = 0.0f;
 	}
 
-	// Calculate the error integral and check for saturation
+	/* calculate the error integral */
 	i = pid->integral + (pid->ki * error * dt);
 
-	float output = (error * pid->kp) + i + (d * pid->kd);
+	/* attitude-thrust compensation
+	 * r22 is (2, 2) componet of rotation matrix for current attitude */
+	float att_comp;
+
+	if (r22 > 0.8f)
+		att_comp = 1.0f / r22;
+	else if (r22 > 0.0f)
+		att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f;
+	else
+		att_comp = 1.0f;
+
+	/* calculate output */
+	float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp;
+
+	/* check for saturation */
 	if (output < pid->limit_min || output > pid->limit_max) {
-		i = pid->integral;		// If saturated then do not update integral value
-		// recalculate output with old integral
-		output = (error * pid->kp) + i + (d * pid->kd);
+		/* saturated, recalculate output with old integral */
+		output = (error * pid->kp) + pid->integral + (d * pid->kd);
 
 	} else {
-		if (!isfinite(i)) {
-			i = 0.0f;
+		if (isfinite(i)) {
+			pid->integral = i;
 		}
-
-		pid->integral = i;
 	}
 
 	if (isfinite(output)) {
diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h
index 65ee33c51..551c032fe 100644
--- a/src/modules/multirotor_pos_control/thrust_pid.h
+++ b/src/modules/multirotor_pos_control/thrust_pid.h
@@ -68,7 +68,7 @@ typedef struct {
 
 __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 float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt);
+__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22);
 
 __END_DECLS