multirotor_pos_control: position controller implemented

3 files changed, 126 insertions, 34 deletions

diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 6fe129d84..ad5670edc 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -42,6 +42,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <math.h>
 #include <stdbool.h>
 #include <unistd.h>
 #include <fcntl.h>
@@ -59,7 +60,6 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_vicon_position.h>
 #include <systemlib/systemlib.h>
 #include <mavlink/mavlink_log.h>
 
@@ -82,6 +82,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]);
  */
 static void usage(const char *reason);
 
+static float scale_control(float ctl, float end, float dz);
+
+static float limit_value(float v, float limit);
+
+static float norm(float x, float y);
+
 static void usage(const char *reason) {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
@@ -137,9 +143,32 @@ int multirotor_pos_control_main(int argc, char *argv[]) {
 	exit(1);
 }
 
+static float scale_control(float ctl, float end, float dz) {
+	if (ctl > dz) {
+		return (ctl - dz) / (end - dz);
+	} else if (ctl < -dz) {
+		return (ctl + dz) / (end - dz);
+	} else {
+		return 0.0f;
+	}
+}
+
+static float limit_value(float v, float limit) {
+	if (v > limit) {
+		v = limit;
+	} else if (v < -limit) {
+		v = -limit;
+	}
+	return v;
+}
+
+static float norm(float x, float y) {
+	return sqrtf(x * x + y * y);
+}
+
 static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 	/* welcome user */
-	printf("[multirotor_pos_control] started\n");
+	warnx("started.");
 	static int mavlink_fd;
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 	mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started");
@@ -175,7 +204,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 	bool reset_sp_pos = true;
 	hrt_abstime t_prev = 0;
 	float alt_integral = 0.0f;
-	float alt_ctl_dz = 0.2f;
+	/* integrate in NED frame to estimate wind but not attitude offset */
+	float pos_x_integral = 0.0f;
+	float pos_y_integral = 0.0f;
+	const float alt_ctl_dz = 0.2f;
+	const float pos_ctl_dz = 0.05f;
 
 	thread_running = true;
 
@@ -235,64 +268,97 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 				reset_sp_alt = false;
 				local_pos_sp.z = local_pos.z;
 				alt_integral = manual.throttle;
-				char str[80];
-				sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle);
-				mavlink_log_info(mavlink_fd, str);
+				mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle);
 			}
 
 			if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) {
 				reset_sp_pos = false;
 				local_pos_sp.x = local_pos.x;
 				local_pos_sp.y = local_pos.y;
-				char str[80];
-				sprintf(str, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y);
-				mavlink_log_info(mavlink_fd, str);
+				pos_x_integral = 0.0f;
+				pos_y_integral = 0.0f;
+				mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y);
 			}
 
-			float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max;
+			float alt_err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max;
+			float pos_err_linear_limit = params.pos_d / params.pos_p * params.pos_rate_max;
 
 			if (status.flag_control_manual_enabled) {
 				/* move altitude setpoint with manual controls */
-				float alt_ctl = manual.throttle - 0.5f;
-				if (fabs(alt_ctl) < alt_ctl_dz) {
-					alt_ctl = 0.0f;
-				} else {
-					if (alt_ctl > 0.0f)
-						alt_ctl = (alt_ctl - alt_ctl_dz) / (0.5f - alt_ctl_dz);
-					else
-						alt_ctl = (alt_ctl + alt_ctl_dz) / (0.5f - alt_ctl_dz);
-					local_pos_sp.z -= alt_ctl * params.alt_rate_max * dt;
-					if (local_pos_sp.z > local_pos.z + err_linear_limit)
-						local_pos_sp.z = local_pos.z + err_linear_limit;
-					else if (local_pos_sp.z < local_pos.z - err_linear_limit)
-						local_pos_sp.z = local_pos.z - err_linear_limit;
+				float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
+				if (alt_sp_ctl != 0.0f) {
+					local_pos_sp.z -= alt_sp_ctl * params.alt_rate_max * dt;
+					if (local_pos_sp.z > local_pos.z + alt_err_linear_limit) {
+						local_pos_sp.z = local_pos.z + alt_err_linear_limit;
+					} else if (local_pos_sp.z < local_pos.z - alt_err_linear_limit) {
+						local_pos_sp.z = local_pos.z - alt_err_linear_limit;
+					}
 				}
 
 				if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) {
-					// TODO move position setpoint with manual controls
+					/* move position setpoint with manual controls */
+					float pos_x_sp_ctl = scale_control(-manual.pitch, 1.0f, pos_ctl_dz);
+					float pos_y_sp_ctl = scale_control(manual.roll, 1.0f, pos_ctl_dz);
+					if (pos_x_sp_ctl != 0.0f || pos_y_sp_ctl != 0.0f) {
+						/* calculate direction and increment of control in NED frame */
+						float dir_sp_ctl = att.yaw + atan2f(pos_y_sp_ctl, pos_x_sp_ctl);
+						float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max * dt;
+						local_pos_sp.x += cosf(dir_sp_ctl) * d_sp_ctl;
+						local_pos_sp.y += sinf(dir_sp_ctl) * d_sp_ctl;
+						/* limit maximum setpoint from position offset and preserve direction */
+						float pos_vec_x = local_pos_sp.x - local_pos.x;
+						float pos_vec_y = local_pos_sp.y - local_pos.y;
+						float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / pos_err_linear_limit;
+						if (pos_vec_norm > 1.0f) {
+							local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
+							local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
+						}
+					}
 				}
 			}
 
 			/* PID for altitude */
-			float alt_err = local_pos.z - local_pos_sp.z;
 			/* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */
-			if (alt_err > err_linear_limit) {
-				alt_err = err_linear_limit;
-			} else if (alt_err < -err_linear_limit) {
-				alt_err = -err_linear_limit;
-			}
+			float alt_err = limit_value(local_pos.z - local_pos_sp.z, alt_err_linear_limit);
 			/* P and D components */
 			float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d;
 			/* add I component */
 			float thrust_ctl = thrust_ctl_pd + alt_integral;
+			/* integrate */
 			alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt;
+			if (alt_integral < params.thr_min) {
+				alt_integral = params.thr_min;
+			} else if (alt_integral > params.thr_max) {
+				alt_integral = params.thr_max;
+			}
 			if (thrust_ctl < params.thr_min) {
 				thrust_ctl = params.thr_min;
 			} else if (thrust_ctl > params.thr_max) {
 				thrust_ctl = params.thr_max;
 			}
 			if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) {
-				// TODO add position controller
+				/* PID for position */
+				/* don't accelerate more than POS_RATE_MAX, limit error to corresponding value */
+				float pos_x_err = limit_value(local_pos.x - local_pos_sp.x, pos_err_linear_limit);
+				float pos_y_err = limit_value(local_pos.y - local_pos_sp.y, pos_err_linear_limit);
+				/* P and D components */
+				float pos_x_ctl_pd = - pos_x_err * params.pos_p - local_pos.vx * params.pos_d;
+				float pos_y_ctl_pd = - pos_y_err * params.pos_p - local_pos.vy * params.pos_d;
+				/* add I component */
+				float pos_x_ctl = pos_x_ctl_pd + pos_x_integral;
+				float pos_y_ctl = pos_y_ctl_pd + pos_y_integral;
+				/* integrate */
+				pos_x_integral = limit_value(pos_x_integral + pos_x_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max);
+				pos_y_integral = limit_value(pos_y_integral + pos_y_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max);
+				/* calculate direction and slope in NED frame */
+				float dir = atan2f(pos_y_ctl, pos_x_ctl);
+				/* use approximation: slope ~ sin(slope) = force */
+				float slope = limit_value(sqrtf(pos_x_ctl * pos_x_ctl + pos_y_ctl * pos_y_ctl), params.slope_max);
+				/* convert direction to body frame */
+				dir -= att.yaw;
+				/* calculate roll and pitch */
+				att_sp.pitch_body = -cosf(dir) * slope;	// reverse pitch
+				att_sp.roll_body = sinf(dir) * slope;
 			} else {
 				reset_sp_pos = true;
 			}
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 90dd820f4..9610ef9f7 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -45,19 +45,29 @@
 PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f);
 PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f);
 PARAM_DEFINE_FLOAT(MPC_ALT_P, 0.1f);
-PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.01f);
-PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.1f);
 PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f);
+PARAM_DEFINE_FLOAT(MPC_POS_P, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 3.0f);
+PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.3f);
 
 int parameters_init(struct multirotor_position_control_param_handles *h)
 {
 	h->thr_min 	=	param_find("MPC_THR_MIN");
 	h->thr_max 	=	param_find("MPC_THR_MAX");
-	/* PID parameters */
 	h->alt_p 	=	param_find("MPC_ALT_P");
 	h->alt_i 	=	param_find("MPC_ALT_I");
 	h->alt_d 	=	param_find("MPC_ALT_D");
 	h->alt_rate_max 	=	param_find("MPC_ALT_RATE_MAX");
+	h->pos_p 	=	param_find("MPC_POS_P");
+	h->pos_i 	=	param_find("MPC_POS_I");
+	h->pos_d 	=	param_find("MPC_POS_D");
+	h->pos_rate_max 	=	param_find("MPC_POS_RATE_MAX");
+	h->slope_max 	=	param_find("MPC_SLOPE_MAX");
+
 	return OK;
 }
 
@@ -69,5 +79,11 @@ int parameters_update(const struct multirotor_position_control_param_handles *h,
 	param_get(h->alt_i, &(p->alt_i));
 	param_get(h->alt_d, &(p->alt_d));
 	param_get(h->alt_rate_max, &(p->alt_rate_max));
+	param_get(h->pos_p, &(p->pos_p));
+	param_get(h->pos_i, &(p->pos_i));
+	param_get(h->pos_d, &(p->pos_d));
+	param_get(h->pos_rate_max, &(p->pos_rate_max));
+	param_get(h->slope_max, &(p->slope_max));
+
 	return OK;
 }
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 849055cd1..589ee92a1 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -48,6 +48,11 @@ struct multirotor_position_control_params {
 	float alt_i;
 	float alt_d;
 	float alt_rate_max;
+	float pos_p;
+	float pos_i;
+	float pos_d;
+	float pos_rate_max;
+	float slope_max;
 };
 
 struct multirotor_position_control_param_handles {
@@ -57,6 +62,11 @@ struct multirotor_position_control_param_handles {
 	param_t alt_i;
 	param_t alt_d;
 	param_t alt_rate_max;
+	param_t pos_p;
+	param_t pos_i;
+	param_t pos_d;
+	param_t pos_rate_max;
+	param_t slope_max;
 };
 
 /**