Merge branch 'seatbelt_multirotor' into seatbelt_multirotor_new

WIP, TODO fixedwing

7 files changed, 662 insertions, 350 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 f39d11438..0120fa61c 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,13 +35,14 @@
 /**
  * @file multirotor_pos_control.c
  *
- * Skeleton for multirotor position controller
+ * Multirotor position controller
  */
 
 #include <nuttx/config.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <math.h>
 #include <stdbool.h>
 #include <unistd.h>
 #include <fcntl.h>
@@ -52,15 +53,21 @@
 #include <sys/prctl.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #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 <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/vehicle_global_velocity_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 */
@@ -79,12 +86,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]);
  */
 static void usage(const char *reason);
 
-static void
-usage(const char *reason)
+static float scale_control(float ctl, float end, float dz);
+
+static float norm(float x, float y);
+
+static void usage(const char *reason)
 {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
+
+	fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n");
 	exit(1);
 }
 
@@ -92,9 +103,9 @@ usage(const char *reason)
  * The deamon app only briefly exists to start
  * the background job. The stack size assigned in the
  * Makefile does only apply to this management task.
- * 
+ *
  * The actual stack size should be set in the call
- * to task_spawn_cmd().
+ * to task_spawn().
  */
 int multirotor_pos_control_main(int argc, char *argv[])
 {
@@ -104,32 +115,36 @@ 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,
-					 SCHED_PRIORITY_MAX - 60,
-					 4096,
-					 multirotor_pos_control_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		deamon_task = task_spawn_cmd("multirotor_pos_control",
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_MAX - 60,
+					     4096,
+					     multirotor_pos_control_thread_main,
+					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
 	}
 
 	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);
 	}
 
@@ -137,98 +152,341 @@ int multirotor_pos_control_main(int argc, char *argv[])
 	exit(1);
 }
 
-static int
-multirotor_pos_control_thread_main(int argc, char *argv[])
+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 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] Control started, taking over position control\n");
+	warnx("started");
+	static int mavlink_fd;
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd, "[mpc] started");
 
 	/* structures */
-	struct vehicle_status_s state;
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 	struct vehicle_attitude_s att;
-	//struct vehicle_global_position_setpoint_s global_pos_sp;
-	struct vehicle_local_position_setpoint_s local_pos_sp;
-	struct vehicle_vicon_position_s local_pos;
-	struct manual_control_setpoint_s manual;
+	memset(&att, 0, sizeof(att));
 	struct vehicle_attitude_setpoint_s att_sp;
+	memset(&att_sp, 0, sizeof(att_sp));
+	struct manual_control_setpoint_s manual;
+	memset(&manual, 0, sizeof(manual));
+	struct vehicle_local_position_s local_pos;
+	memset(&local_pos, 0, sizeof(local_pos));
+	struct vehicle_local_position_setpoint_s local_pos_sp;
+	memset(&local_pos_sp, 0, sizeof(local_pos_sp));
+	struct vehicle_global_position_setpoint_s global_pos_sp;
+	memset(&global_pos_sp, 0, sizeof(local_pos_sp));
+	struct vehicle_global_velocity_setpoint_s global_vel_sp;
+	memset(&global_vel_sp, 0, sizeof(global_vel_sp));
 
 	/* subscribe to attitude, motor setpoints and system state */
+	int param_sub = orb_subscribe(ORB_ID(parameter_update));
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
-	//int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
 	int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
+	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
 
-	/* publish attitude setpoint */
+	/* publish setpoint */
+	orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
+	orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 
+	bool reset_sp_alt = true;
+	bool reset_sp_pos = true;
+	hrt_abstime t_prev = 0;
+	/* integrate in NED frame to estimate wind but not attitude offset */
+	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;
+	uint64_t local_home_timestamp = 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;
 
-	int loopcounter = 0;
-
-	struct multirotor_position_control_params p;
-	struct multirotor_position_control_param_handles h;
-	parameters_init(&h);
-	parameters_update(&h, &p);
-
-
-	while (1) {
-		/* get a local copy of the vehicle state */
-		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
-		/* get a local copy of manual setpoint */
-		orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
-		/* get a local copy of attitude */
-		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-		/* get a local copy of local position */
-		orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos);
-		/* get a local copy of local position setpoint */
-		orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
-
-		if (loopcounter == 500) {
-			parameters_update(&h, &p);
-			loopcounter = 0;
+	struct multirotor_position_control_params params;
+	struct multirotor_position_control_param_handles params_h;
+	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(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, 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);
+
+	int paramcheck_counter = 0;
+	bool global_pos_sp_updated = false;
+
+	while (!thread_should_exit) {
+		orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+
+		/* check parameters at 1 Hz*/
+		if (--paramcheck_counter <= 0) {
+			paramcheck_counter = 50;
+			bool param_updated;
+			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, 0.0f);
+					/* use integral_limit_out = tilt_max / 2 */
+					float i_limit;
+					if (params.xy_vel_i == 0.0) {
+						i_limit = params.tilt_max / params.xy_vel_i / 2.0;
+					} else {
+						i_limit = 1.0f;	// not used really
+					}
+					pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_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);
+			}
+		}
+
+		/* only check global position setpoint updates but not read */
+		if (!global_pos_sp_updated) {
+			orb_check(global_pos_sp_sub, &global_pos_sp_updated);
+		}
+
+		hrt_abstime t = hrt_absolute_time();
+		float dt;
+
+		if (t_prev != 0) {
+			dt = (t - t_prev) * 0.000001f;
+
+		} else {
+			dt = 0.0f;
 		}
 
-		// if (state.state_machine == SYSTEM_STATE_AUTO) {
-			
-			// XXX IMPLEMENT POSITION CONTROL HERE
-
-			float dT = 1.0f / 50.0f;
-
-			float x_setpoint = 0.0f;
-
-			// XXX enable switching between Vicon and local position estimate
-			/* local pos is the Vicon position */
-
-			// XXX just an example, lacks rotation around world-body transformation
-			att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p;
-			att_sp.roll_body = 0.0f;
-			att_sp.yaw_body = 0.0f;
-			att_sp.thrust = 0.3f;
-			att_sp.timestamp = hrt_absolute_time();
-
-			/* publish new attitude setpoint */
-			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-		// } else if (state.state_machine == SYSTEM_STATE_STABILIZED) {
-			/* set setpoint to current position */
-			// XXX select pos reset channel on remote
-			/* reset setpoint to current position  (position hold) */
-			// if (1 == 2) {
-			// 	local_pos_sp.x = local_pos.x;
-			// 	local_pos_sp.y = local_pos.y;
-			// 	local_pos_sp.z = local_pos.z;
-			// 	local_pos_sp.yaw = att.yaw;
-			// }
-		// }
+		t_prev = t;
+
+		if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) {
+			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
+			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
+			orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
+
+			if (control_mode.flag_control_manual_enabled) {
+				/* manual mode, reset setpoints if needed */
+				if (reset_sp_alt) {
+					reset_sp_alt = false;
+					local_pos_sp.z = local_pos.z;
+					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);
+				}
+
+				if (control_mode.flag_control_position_enabled && reset_sp_pos) {
+					reset_sp_pos = false;
+					local_pos_sp.x = local_pos.x;
+					local_pos_sp.y = local_pos.y;
+					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);
+				}
+			} else {
+				/* non-manual mode, project global setpoints to local frame */
+				//orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
+				if (local_pos.home_timestamp != local_home_timestamp) {
+					local_home_timestamp = local_pos.home_timestamp;
+					/* init local projection using local position home */
+					double lat_home = local_pos.home_lat * 1e-7;
+					double lon_home = local_pos.home_lon * 1e-7;
+					map_projection_init(lat_home, lon_home);
+					warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home);
+					mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home);
+				}
+
+				if (global_pos_sp_updated) {
+					global_pos_sp_updated = false;
+					orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
+					double sp_lat = global_pos_sp.lat * 1e-7;
+					double sp_lon = global_pos_sp.lon * 1e-7;
+					/* project global setpoint to local setpoint */
+					map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
+
+					if (global_pos_sp.altitude_is_relative) {
+						local_pos_sp.z = -global_pos_sp.altitude;
+
+					} else {
+						local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude;
+					}
+
+					warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
+					mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
+					/* publish local position setpoint as projection of global position setpoint */
+					orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+				}
+			}
+
+			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 sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
+
+			/* manual control - move setpoints */
+			if (control_mode.flag_control_manual_enabled) {
+				if (local_pos.home_timestamp != home_alt_t) {
+					if (home_alt_t != 0) {
+						/* home alt changed, don't follow large ground level changes in manual flight */
+						local_pos_sp.z += local_pos.home_alt - home_alt;
+					}
+
+					home_alt_t = local_pos.home_timestamp;
+					home_alt = local_pos.home_alt;
+				}
+
+				if (control_mode.flag_control_altitude_enabled) {
+					/* move altitude setpoint with manual controls */
+					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;
+						}
+					}
+				}
+
+				if (control_mode.flag_control_position_enabled) {
+					/* move position setpoint with manual controls */
+					float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
+					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 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
+						 * fail safe, should not happen in normal operation */
+						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) / 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;
+						}
+					}
+				}
+
+				/* publish local position setpoint */
+				orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+			}
+
+			/* run position & altitude controllers, calculate velocity setpoint */
+			if (control_mode.flag_control_altitude_enabled) {
+				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];
+			} else {
+				global_vel_sp.vz = 0.0f;
+			}
+
+			if (control_mode.flag_control_position_enabled) {
+				/* 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 - 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;
+				if (xy_vel_sp_norm > 1.0f) {
+					global_vel_sp.vx /= xy_vel_sp_norm;
+					global_vel_sp.vy /= xy_vel_sp_norm;
+				}
+
+			} else {
+				reset_sp_pos = true;
+				global_vel_sp.vx = 0.0f;
+				global_vel_sp.vy = 0.0f;
+			}
+
+			/* publish new velocity setpoint */
+			orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
+			// TODO subcrive to velocity setpoint if altitude/position control disabled
+
+			if (control_mode.flag_control_velocity_enabled) {
+				/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
+				float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
+				if (control_mode.flag_control_altitude_enabled) {
+					thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
+					att_sp.thrust = -thrust_sp[2];
+				}
+				if (control_mode.flag_control_position_enabled) {
+					/* calculate thrust set point in NED frame */
+					thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
+					thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
+
+					/* 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]);
+					/* assuming that vertical component of thrust is g,
+					 * horizontal component = g * tan(alpha) */
+					float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
+
+					if (tilt > params.tilt_max) {
+						tilt = params.tilt_max;
+					}
+					/* convert direction to body frame */
+					thrust_xy_dir -= att.yaw;
+					/* calculate roll and pitch */
+					att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
+					att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
+				}
+
+				att_sp.timestamp = hrt_absolute_time();
+
+				/* publish new attitude setpoint */
+				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+			}
+		} else {
+			reset_sp_alt = true;
+			reset_sp_pos = true;
+		}
 
 		/* run at approximately 50 Hz */
 		usleep(20000);
-		loopcounter++;
 
 	}
 
-	printf("[multirotor pos control] ending now...\n");
+	warnx("stopped");
+	mavlink_log_info(mavlink_fd, "[mpc] 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 6b73dc405..1094fd23b 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -34,29 +34,76 @@
  ****************************************************************************/
 
 /*
- * @file multirotor_position_control_params.c
+ * @file multirotor_pos_control_params.c
  * 
- * Parameters for EKF filter
+ * Parameters for multirotor_pos_control
  */
 
 #include "multirotor_pos_control_params.h"
 
-/* Extended Kalman Filter covariances */
-
 /* controller parameters */
-PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f);
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f);
+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_TILT_MAX, 0.5f);
 
 int parameters_init(struct multirotor_position_control_param_handles *h)
 {
-	/* PID parameters */
-	h->p 	=	param_find("MC_POS_P");
+	h->thr_min 	=	param_find("MPC_THR_MIN");
+	h->thr_max 	=	param_find("MPC_THR_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->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");
+	h->rc_scale_yaw      =   param_find("RC_SCALE_YAW");
 
 	return OK;
 }
 
 int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
 {
-	param_get(h->p, &(p->p));
+	param_get(h->thr_min, &(p->thr_min));
+	param_get(h->thr_max, &(p->thr_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->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));
+	param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
 
 	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 274f6c22a..14a3de2e5 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -42,15 +42,47 @@
 #include <systemlib/param/param.h>
 
 struct multirotor_position_control_params {
-	float p;
-	float i;
-	float d;
+	float thr_min;
+	float thr_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 tilt_max;
+
+	float rc_scale_pitch;
+	float rc_scale_roll;
+	float rc_scale_yaw;
 };
 
 struct multirotor_position_control_param_handles {
-	param_t p;
-	param_t i;
-	param_t d;
+	param_t thr_min;
+	param_t thr_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 tilt_max;
+
+	param_t rc_scale_pitch;
+	param_t rc_scale_roll;
+	param_t rc_scale_yaw;
 };
 
 /**
diff --git a/src/modules/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c
deleted file mode 100644
index 9c53a5bf6..000000000
--- a/src/modules/multirotor_pos_control/position_control.c
+++ /dev/null
@@ -1,235 +0,0 @@
-// /****************************************************************************
-//  *
-//  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
-//  *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
-//  *           @author Laurens Mackay <mackayl@student.ethz.ch>
-//  *           @author Tobias Naegeli <naegelit@student.ethz.ch>
-//  *           @author Martin Rutschmann <rutmarti@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 multirotor_position_control.c
-//  * Implementation of the position control for a multirotor VTOL
-//  */
-
-// #include <stdio.h>
-// #include <stdlib.h>
-// #include <stdio.h>
-// #include <stdint.h>
-// #include <math.h>
-// #include <stdbool.h>
-// #include <float.h>
-// #include <systemlib/pid/pid.h>
-
-// #include "multirotor_position_control.h"
-
-// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
-//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
-//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
-// {
-// 	static PID_t distance_controller;
-
-// 	static int read_ret;
-// 	static global_data_position_t position_estimated;
-
-// 	static uint16_t counter;
-
-// 	static bool initialized;
-// 	static uint16_t pm_counter;
-
-// 	static float lat_next;
-// 	static float lon_next;
-
-// 	static float pitch_current;
-
-// 	static float thrust_total;
-
-
-// 	if (initialized == false) {
-
-// 		pid_init(&distance_controller,
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
-// 			 PID_MODE_DERIVATIV_CALC, 150);//150
-
-// //		pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-// //		pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
-
-// 		thrust_total = 0.0f;
-
-// 		/* Position initialization */
-// 		/* Wait for new position estimate */
-// 		do {
-// 			read_ret = read_lock_position(&position_estimated);
-// 		} while (read_ret != 0);
-
-// 		lat_next = position_estimated.lat;
-// 		lon_next = position_estimated.lon;
-
-// 		/* attitude initialization */
-// 		global_data_lock(&global_data_attitude->access_conf);
-// 		pitch_current = global_data_attitude->pitch;
-// 		global_data_unlock(&global_data_attitude->access_conf);
-
-// 		initialized = true;
-// 	}
-
-// 	/* load new parameters with 10Hz */
-// 	if (counter % 50 == 0) {
-// 		if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
-// 			/* check whether new parameters are available */
-// 			if (global_data_parameter_storage->counter > pm_counter) {
-// 				pid_set_parameters(&distance_controller,
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
-
-// //
-// //				pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-// //				pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
-
-// 				pm_counter = global_data_parameter_storage->counter;
-// 				printf("Position controller changed pid parameters\n");
-// 			}
-// 		}
-
-// 		global_data_unlock(&global_data_parameter_storage->access_conf);
-// 	}
-
-
-// 	/* Wait for new position estimate */
-// 	do {
-// 		read_ret = read_lock_position(&position_estimated);
-// 	} while (read_ret != 0);
-
-// 	/* Get next waypoint */ //TODO: add local copy
-
-// 	if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
-// 		lat_next = global_data_position_setpoint->x;
-// 		lon_next = global_data_position_setpoint->y;
-// 		global_data_unlock(&global_data_position_setpoint->access_conf);
-// 	}
-
-// 	/* Get distance to waypoint */
-// 	float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
-// //	if(counter % 5 == 0)
-// //		printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
-
-// 	/* Get bearing to waypoint (direction on earth surface to next waypoint) */
-// 	float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
-
-// 	if (counter % 5 == 0)
-// 		printf("bearing: %.4f\n", bearing);
-
-// 	/* Calculate speed in direction of bearing (needed for controller) */
-// 	float speed_norm = sqrtf(position_estimated.vx  * position_estimated.vx + position_estimated.vy * position_estimated.vy);
-// //	if(counter % 5 == 0)
-// //		printf("speed_norm: %.4f\n", speed_norm);
-// 	float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
-
-// 	/* Control Thrust in bearing direction  */
-// 	float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
-// 				  CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
-
-// //	if(counter % 5 == 0)
-// //		printf("horizontal thrust: %.4f\n", horizontal_thrust);
-
-// 	/* Get total thrust (from remote for now) */
-// 	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
-// 		thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; 												//TODO: how should we use the RC_CHANNELS_FUNCTION enum?
-// 		global_data_unlock(&global_data_rc_channels->access_conf);
-// 	}
-
-// 	const float max_gas = 500.0f;
-// 	thrust_total *= max_gas / 20000.0f; //TODO: check this
-// 	thrust_total += max_gas / 2.0f;
-
-
-// 	if (horizontal_thrust > thrust_total) {
-// 		horizontal_thrust = thrust_total;
-
-// 	} else if (horizontal_thrust < -thrust_total) {
-// 		horizontal_thrust = -thrust_total;
-// 	}
-
-
-
-// 	//TODO: maybe we want to add a speed controller later...
-
-// 	/* Calclulate thrust in east and north direction */
-// 	float thrust_north = cosf(bearing) * horizontal_thrust;
-// 	float thrust_east = sinf(bearing) * horizontal_thrust;
-
-// 	if (counter % 10 == 0) {
-// 		printf("thrust north: %.4f\n", thrust_north);
-// 		printf("thrust east: %.4f\n", thrust_east);
-// 		fflush(stdout);
-// 	}
-
-// 	/* Get current attitude */
-// 	if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
-// 		pitch_current = global_data_attitude->pitch;
-// 		global_data_unlock(&global_data_attitude->access_conf);
-// 	}
-
-// 	/* Get desired pitch & roll */
-// 	float pitch_desired = 0.0f;
-// 	float roll_desired = 0.0f;
-
-// 	if (thrust_total != 0) {
-// 		float pitch_fraction = -thrust_north / thrust_total;
-// 		float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
-
-// 		if (roll_fraction < -1) {
-// 			roll_fraction = -1;
-
-// 		} else if (roll_fraction > 1) {
-// 			roll_fraction = 1;
-// 		}
-
-// //		if(counter % 5 == 0)
-// //		{
-// //			printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
-// //			fflush(stdout);
-// //		}
-
-// 		pitch_desired = asinf(pitch_fraction);
-// 		roll_desired = asinf(roll_fraction);
-// 	}
-
-// 	att_sp.roll = roll_desired;
-// 	att_sp.pitch = pitch_desired;
-
-// 	counter++;
-// }
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..51dcd7df2
--- /dev/null
+++ b/src/modules/multirotor_pos_control/thrust_pid.c
@@ -0,0 +1,184 @@
+/****************************************************************************
+ *
+ *   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.c
+ *
+ * Implementation of thrust control PID.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#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, 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;
+
+	} 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 */
+	i = pid->integral + (pid->ki * error * dt);
+
+	/* 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) {
+		/* saturated, recalculate output with old integral */
+		output = (error * pid->kp) + pid->integral + (d * pid->kd);
+
+	} else {
+		if (isfinite(i)) {
+			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/position_control.h b/src/modules/multirotor_pos_control/thrust_pid.h
index 2144ebc34..551c032fe 100644
--- a/src/modules/multirotor_pos_control/position_control.h
+++ b/src/modules/multirotor_pos_control/thrust_pid.h
@@ -1,10 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@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
@@ -36,15 +33,43 @@
  ****************************************************************************/
 
 /**
- * @file multirotor_position_control.h
- * Definition of the position control for a multirotor VTOL
+ * @file thrust_pid.h
+ *
+ * Definition of thrust control PID interface.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-// #ifndef POSITION_CONTROL_H_
-// #define POSITION_CONTROL_H_
+#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, float r22);
 
-// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
-//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
-//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
+__END_DECLS
 
-// #endif /* POSITION_CONTROL_H_ */
+#endif /* THRUST_PID_H_ */