multirotor_pos_control: use separate PID controllers for position and velocity

6 files changed, 420 insertions, 135 deletions

diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk
index d04847745..bc4b48fb4 100644
--- a/src/modules/multirotor_pos_control/module.mk
+++ b/src/modules/multirotor_pos_control/module.mk
@@ -38,4 +38,5 @@
 MODULE_COMMAND	= multirotor_pos_control
 
 SRCS		= multirotor_pos_control.c \
-		  multirotor_pos_control_params.c
+		  multirotor_pos_control_params.c \
+		  thrust_pid.c
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index acae03fae..d56c3d58f 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -61,9 +61,11 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/pid/pid.h>
 #include <mavlink/mavlink_log.h>
 
 #include "multirotor_pos_control_params.h"
+#include "thrust_pid.h"
 
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
@@ -84,8 +86,6 @@ 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) {
@@ -110,11 +110,12 @@ int multirotor_pos_control_main(int argc, char *argv[]) {
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("multirotor_pos_control already running\n");
+			warnx("already running");
 			/* this is not an error */
 			exit(0);
 		}
 
+		warnx("start");
 		thread_should_exit = false;
 		deamon_task = task_spawn_cmd("multirotor_pos_control",
 					 SCHED_DEFAULT,
@@ -126,15 +127,16 @@ int multirotor_pos_control_main(int argc, char *argv[]) {
 	}
 
 	if (!strcmp(argv[1], "stop")) {
+		warnx("stop");
 		thread_should_exit = true;
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tmultirotor_pos_control app is running\n");
+			warnx("app is running");
 		} else {
-			printf("\tmultirotor_pos_control app not started\n");
+			warnx("app not started");
 		}
 		exit(0);
 	}
@@ -153,22 +155,13 @@ static float scale_control(float ctl, float end, float dz) {
 	}
 }
 
-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 */
-	warnx("started.");
+	warnx("started");
 	static int mavlink_fd;
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 	mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started");
@@ -203,15 +196,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 	bool reset_sp_alt = true;
 	bool reset_sp_pos = true;
 	hrt_abstime t_prev = 0;
-	float alt_integral = 0.0f;
 	/* 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;
 	float home_alt = 0.0f;
 	hrt_abstime home_alt_t = 0;
 
+	static PID_t xy_pos_pids[2];
+	static PID_t xy_vel_pids[2];
+	static PID_t z_pos_pid;
+	static thrust_pid_t z_vel_pid;
+
 	thread_running = true;
 
 	struct multirotor_position_control_params params;
@@ -219,6 +214,13 @@ 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, params.xy_vel_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.slope_max, 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_CALC_NO_SP, 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);
+
 	int paramcheck_counter = 0;
 
 	while (!thread_should_exit) {
@@ -231,6 +233,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 			orb_check(param_sub, &param_updated);
 			if (param_updated) {
 				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, params.xy_vel_max);
+					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(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
+				thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
 			}
 			paramcheck_counter = 0;
 		}
@@ -269,7 +277,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 			if (reset_sp_alt) {
 				reset_sp_alt = false;
 				local_pos_sp.z = local_pos.z;
-				alt_integral = manual.throttle;
+				z_vel_pid.integral = -manual.throttle;	// thrust PID uses Z downside
 				mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle);
 			}
 
@@ -277,18 +285,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 				reset_sp_pos = false;
 				local_pos_sp.x = local_pos.x;
 				local_pos_sp.y = local_pos.y;
-				pos_x_integral = 0.0f;
-				pos_y_integral = 0.0f;
+				xy_vel_pids[0].integral = 0.0f;
+				xy_vel_pids[1].integral = 0.0f;
 				mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y);
 			}
 
-			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;
+			float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
+			float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
 
-			float pos_sp_speed_x = 0.0f;
-			float pos_sp_speed_y = 0.0f;
-			float pos_sp_speed_z = 0.0f;
+			float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
 
+			/* manual control */
 			if (status.flag_control_manual_enabled) {
 				if (local_pos.home_timestamp != home_alt_t) {
 					if (home_alt_t != 0) {
@@ -299,14 +306,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 					home_alt = local_pos.home_alt;
 				}
 				/* move altitude setpoint with manual controls */
-				float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
-				if (alt_sp_ctl != 0.0f) {
-					pos_sp_speed_z = -alt_sp_ctl * params.alt_rate_max;
-					local_pos_sp.z += pos_sp_speed_z * 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;
+				float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
+				if (z_sp_ctl != 0.0f) {
+					sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
+					local_pos_sp.z += sp_move_rate[2] * dt;
+					if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
+						local_pos_sp.z = local_pos.z + z_sp_offs_max;
+					} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
+						local_pos_sp.z = local_pos.z - z_sp_offs_max;
 					}
 				}
 
@@ -316,76 +323,84 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 					float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
 					if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
 						/* calculate direction and increment of control in NED frame */
-						float pos_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
-						float pos_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.pos_rate_max;
-						pos_sp_speed_x = cosf(pos_sp_ctl_dir) * pos_sp_ctl_speed;
-						pos_sp_speed_y = sinf(pos_sp_ctl_dir) * pos_sp_ctl_speed;
-						local_pos_sp.x += pos_sp_speed_x * dt;
-						local_pos_sp.y += pos_sp_speed_y * dt;
+						float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
+						float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max;
+						sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
+						sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
+						local_pos_sp.x += sp_move_rate[0] * dt;
+						local_pos_sp.y += sp_move_rate[1] * dt;
 						/* 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;
+						float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
 						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;
 						}
 					}
 				}
+			}
 
-				if (params.hard == 0) {
-					pos_sp_speed_x = 0.0f;
-					pos_sp_speed_y = 0.0f;
-					pos_sp_speed_z = 0.0f;
-				}
+			/* run position & altitude controllers, calculate velocity setpoint */
+			float vel_sp[3] = { 0.0f, 0.0f, 0.0f };
+			vel_sp[2] = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt);
+			if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) {
+				/* calculate velocity set point in NED frame */
+				vel_sp[0] = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt);
+				vel_sp[1] = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt);
+			} else {
+				reset_sp_pos = true;
+			}
+			/* calculate direction and norm of thrust in NED frame
+			 * limit 3D speed by ellipsoid:
+			 * (vx/xy_vel_max)^2 + (vy/xy_vel_max)^2 + (vz/z_vel_max)^2 = 1 */
+			float v;
+			float vel_sp_norm = 0.0f;
+			v = vel_sp[0] / params.xy_vel_max;
+			vel_sp_norm += v * v;
+			v = vel_sp[1] / params.xy_vel_max;
+			vel_sp_norm += v * v;
+			v = vel_sp[2] / params.z_vel_max;
+			vel_sp_norm += v * v;
+			vel_sp_norm = sqrtf(vel_sp_norm);
+			if (vel_sp_norm > 1.0f) {
+				vel_sp[0] /= vel_sp_norm;
+				vel_sp[1] /= vel_sp_norm;
+				vel_sp[2] /= vel_sp_norm;
 			}
 
-			/* PID for altitude */
-			/* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */
-			float alt_err = limit_value(local_pos_sp.z - local_pos.z, alt_err_linear_limit);
-			/* P and D components */
-			float thrust_ctl_pd = -(alt_err * params.alt_p + (pos_sp_speed_z - local_pos.vz) * params.alt_d);	// altitude = -z
-			/* 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;
+			/* run velocity controllers, calculate thrust vector */
+			float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
+			thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, vel_sp[2], local_pos.vz, dt);
+			if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) {
+				/* calculate velocity set point in NED frame */
+				thrust_sp[0] = pid_calculate(&xy_vel_pids[0], vel_sp[0], local_pos.vx, 0.0f, dt);
+				thrust_sp[1] = pid_calculate(&xy_vel_pids[1], vel_sp[1], local_pos.vy, 0.0f, dt);
 			}
-			/* add I component */
-			float thrust_ctl = thrust_ctl_pd + alt_integral;
-			if (thrust_ctl < params.thr_min) {
-				thrust_ctl = params.thr_min;
-			} else if (thrust_ctl > params.thr_max) {
-				thrust_ctl = params.thr_max;
+			/* 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]);
+			if (thrust_xy_norm > params.slope_max) {
+				thrust_xy_norm = params.slope_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) {
-				/* 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 + (pos_sp_speed_x - local_pos.vx) * params.pos_d;
-				float pos_y_ctl_pd = - pos_y_err * params.pos_p + (pos_sp_speed_y - local_pos.vy) * params.pos_d;
-				/* 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);
-				/* 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;
-				/* 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;
+				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.thrust = thrust_ctl;
+			/* 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.timestamp = hrt_absolute_time();
 			if (status.flag_control_manual_enabled) {
 				/* publish local position setpoint in manual mode */
@@ -403,8 +418,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 
 	}
 
-	printf("[multirotor_pos_control] exiting\n");
-	mavlink_log_info(mavlink_fd, "[multirotor_pos_control] exiting");
+	warnx("stopped");
+	mavlink_log_info(mavlink_fd, "[multirotor_pos_control] stopped");
 
 	thread_running = false;
 
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 d284de433..f8a982c6c 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -44,31 +44,37 @@
 /* controller parameters */
 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.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, 10.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f);
+PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f);
+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_INT32(MPC_HARD, 0);
 
 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");
-	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->z_p 	=	param_find("MPC_Z_P");
+	h->z_d 	=	param_find("MPC_Z_D");
+	h->z_vel_p 	=	param_find("MPC_Z_VEL_P");
+	h->z_vel_i 	=	param_find("MPC_Z_VEL_I");
+	h->z_vel_d 	=	param_find("MPC_Z_VEL_D");
+	h->z_vel_max 	=	param_find("MPC_Z_VEL_MAX");
+	h->xy_p 	=	param_find("MPC_XY_P");
+	h->xy_d 	=	param_find("MPC_XY_D");
+	h->xy_vel_p 	=	param_find("MPC_XY_VEL_P");
+	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->hard 	=	param_find("MPC_HARD");
 
 	h->rc_scale_pitch    =   param_find("RC_SCALE_PITCH");
 	h->rc_scale_roll    =   param_find("RC_SCALE_ROLL");
@@ -81,16 +87,19 @@ int parameters_update(const struct multirotor_position_control_param_handles *h,
 {
 	param_get(h->thr_min, &(p->thr_min));
 	param_get(h->thr_max, &(p->thr_max));
-	param_get(h->alt_p, &(p->alt_p));
-	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->z_p, &(p->z_p));
+	param_get(h->z_d, &(p->z_d));
+	param_get(h->z_vel_p, &(p->z_vel_p));
+	param_get(h->z_vel_i, &(p->z_vel_i));
+	param_get(h->z_vel_d, &(p->z_vel_d));
+	param_get(h->z_vel_max, &(p->z_vel_max));
+	param_get(h->xy_p, &(p->xy_p));
+	param_get(h->xy_d, &(p->xy_d));
+	param_get(h->xy_vel_p, &(p->xy_vel_p));
+	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->hard, &(p->hard));
 
 	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 13b667ad3..e9b1f5c39 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -44,16 +44,19 @@
 struct multirotor_position_control_params {
 	float thr_min;
 	float thr_max;
-	float alt_p;
-	float alt_i;
-	float alt_d;
-	float alt_rate_max;
-	float pos_p;
-	float pos_i;
-	float pos_d;
-	float pos_rate_max;
+	float z_p;
+	float z_d;
+	float z_vel_p;
+	float z_vel_i;
+	float z_vel_d;
+	float z_vel_max;
+	float xy_p;
+	float xy_d;
+	float xy_vel_p;
+	float xy_vel_i;
+	float xy_vel_d;
+	float xy_vel_max;
 	float slope_max;
-	int hard;
 
 	float rc_scale_pitch;
 	float rc_scale_roll;
@@ -63,16 +66,19 @@ struct multirotor_position_control_params {
 struct multirotor_position_control_param_handles {
 	param_t thr_min;
 	param_t thr_max;
-	param_t alt_p;
-	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 z_p;
+	param_t z_d;
+	param_t z_vel_p;
+	param_t z_vel_i;
+	param_t z_vel_d;
+	param_t z_vel_max;
+	param_t xy_p;
+	param_t xy_d;
+	param_t xy_vel_p;
+	param_t xy_vel_i;
+	param_t xy_vel_d;
+	param_t xy_vel_max;
 	param_t slope_max;
-	param_t hard;
 
 	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
new file mode 100644
index 000000000..89efe1334
--- /dev/null
+++ b/src/modules/multirotor_pos_control/thrust_pid.c
@@ -0,0 +1,179 @@
+/****************************************************************************
+ *
+ *   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>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file thrust_pid.c
+ *
+ * Implementation of generic PID control interface.
+ *
+ * @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"
+#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)
+{
+	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->integral = 0.0f;
+}
+
+__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max)
+{
+	int ret = 0;
+
+	if (isfinite(kp)) {
+		pid->kp = kp;
+
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(ki)) {
+		pid->ki = ki;
+
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(kd)) {
+		pid->kd = kd;
+
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(limit_min)) {
+		pid->limit_min = limit_min;
+
+	}  else {
+		ret = 1;
+	}
+
+	if (isfinite(limit_max)) {
+		pid->limit_max = limit_max;
+
+	}  else {
+		ret = 1;
+	}
+
+	return ret;
+}
+
+__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt)
+{
+	/* 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
+	 */
+
+	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;
+
+	} 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;
+	}
+
+	if (!isfinite(d)) {
+		d = 0.0f;
+	}
+
+	// Calculate the error integral and check for saturation
+	i = pid->integral + (pid->ki * error * dt);
+
+	float output = (error * pid->kp) + i + (d * pid->kd);
+	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);
+
+	} else {
+		if (!isfinite(i)) {
+			i = 0.0f;
+		}
+
+		pid->integral = i;
+	}
+
+	if (isfinite(output)) {
+		if (output > pid->limit_max) {
+			output = pid->limit_max;
+
+		} else if (output < pid->limit_min) {
+			output = pid->limit_min;
+		}
+
+		pid->last_output = output;
+	}
+
+	return pid->last_output;
+}
diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h
new file mode 100644
index 000000000..65ee33c51
--- /dev/null
+++ b/src/modules/multirotor_pos_control/thrust_pid.h
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file thrust_pid.h
+ *
+ * Definition of thrust control PID interface.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef THRUST_PID_H_
+#define THRUST_PID_H_
+
+#include <stdint.h>
+
+__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 kp;
+	float ki;
+	float kd;
+	float sp;
+	float integral;
+	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 float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt);
+
+__END_DECLS
+
+#endif /* THRUST_PID_H_ */