mc_pos_control: replacement for multirotor_pos_control, rewritten to C++ and new mathlib

9 files changed, 1009 insertions, 1313 deletions

diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index b7840170c..c7323c215 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -262,6 +262,30 @@ public:
 	}
 
 	/**
+	 * element by element multiplication
+	 */
+	const Vector<N> emult(const Vector<N> &v) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = data[i] * v.data[i];
+
+		return res;
+	}
+
+	/**
+	 * element by element division
+	 */
+	const Vector<N> edivide(const Vector<N> &v) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = data[i] / v.data[i];
+
+		return res;
+	}
+
+	/**
 	 * gets the length of this vector squared
 	 */
 	float length_squared() const {
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
new file mode 100644
index 000000000..c06caff1e
--- /dev/null
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -0,0 +1,956 @@
+/****************************************************************************
+ *
+ *   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 mc_pos_control_main.cpp
+ *
+ * Multirotor position controller.
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/pid/pid.h>
+#include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+#include <mavlink/mavlink_log.h>
+
+#define TILT_COS_MAX	0.7f
+#define SIGMA			0.000001f
+
+/**
+ * Multicopter position control app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]);
+
+class MulticopterPositionControl
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterPositionControl();
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~MulticopterPositionControl();
+
+	/**
+	 * Start task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+private:
+
+	bool	_task_should_exit;		/**< if true, task should exit */
+	int		_control_task;			/**< task handle for task */
+
+	int		_att_sub;				/**< vehicle attitude subscription */
+	int		_att_sp_sub;			/**< vehicle attitude setpoint */
+	int		_control_mode_sub;		/**< vehicle control mode subscription */
+	int 	_params_sub;			/**< notification of parameter updates */
+	int 	_manual_sub;			/**< notification of manual control updates */
+	int		_arming_sub;			/**< arming status of outputs */
+	int		_local_pos_sub;			/**< vehicle local position */
+	int		_global_pos_sp_sub;		/**< vehicle global position setpoint */
+
+	orb_advert_t	_local_pos_sp_pub;		/**< local position setpoint publication */
+	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
+	orb_advert_t	_global_vel_sp_pub;		/**< vehicle global velocity setpoint */
+
+	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s	_att_sp;		/**< vehicle attitude setpoint */
+	struct manual_control_setpoint_s	_manual;		/**< r/c channel data */
+	struct vehicle_control_mode_s		_control_mode;	/**< vehicle control mode */
+	struct actuator_armed_s				_arming;		/**< actuator arming status */
+	struct vehicle_local_position_s		_local_pos;		/**< vehicle local position */
+	struct vehicle_local_position_setpoint_s		_local_pos_sp;		/**< vehicle local position */
+	struct vehicle_global_position_setpoint_s	_global_pos_sp;	/**< vehicle global position setpoint */
+	struct vehicle_global_velocity_setpoint_s	_global_vel_sp;	/**< vehicle global velocity setpoint */
+
+	struct {
+		param_t takeoff_alt;
+		param_t takeoff_gap;
+		param_t thr_min;
+		param_t thr_max;
+		param_t z_p;
+		param_t z_vel_p;
+		param_t z_vel_i;
+		param_t z_vel_d;
+		param_t z_vel_max;
+		param_t z_ff;
+		param_t xy_p;
+		param_t xy_vel_p;
+		param_t xy_vel_i;
+		param_t xy_vel_d;
+		param_t xy_vel_max;
+		param_t xy_ff;
+		param_t tilt_max;
+
+		param_t rc_scale_pitch;
+		param_t rc_scale_roll;
+		param_t rc_scale_yaw;
+	}		_params_handles;		/**< handles for interesting parameters */
+
+	struct {
+		float takeoff_alt;
+		float takeoff_gap;
+		float thr_min;
+		float thr_max;
+		float tilt_max;
+
+		float rc_scale_pitch;
+		float rc_scale_roll;
+		float rc_scale_yaw;
+
+		math::Vector<3> pos_p;
+		math::Vector<3> vel_p;
+		math::Vector<3> vel_i;
+		math::Vector<3> vel_d;
+		math::Vector<3> vel_ff;
+		math::Vector<3> vel_max;
+		math::Vector<3> sp_offs_max;
+	}		_params;
+
+	math::Vector<3> _pos;
+	math::Vector<3> _vel;
+	math::Vector<3> _pos_sp;
+	math::Vector<3> _vel_sp;
+	math::Vector<3> _vel_err_prev;			/**< velocity on previous step */
+
+	hrt_abstime _time_prev;
+
+	bool _was_armed;
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int			parameters_update(bool force);
+
+	/**
+	 * Update control outputs
+	 */
+	void		control_update();
+
+	/**
+	 * Check for changes in subscribed topics.
+	 */
+	void		poll_subscriptions();
+
+	static float	scale_control(float ctl, float end, float dz);
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * Main sensor collection task.
+	 */
+	void		task_main() __attribute__((noreturn));
+};
+
+namespace pos_control
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+MulticopterPositionControl	*g_control;
+}
+
+MulticopterPositionControl::MulticopterPositionControl() :
+
+	_task_should_exit(false),
+	_control_task(-1),
+
+/* subscriptions */
+	_att_sub(-1),
+	_att_sp_sub(-1),
+	_control_mode_sub(-1),
+	_params_sub(-1),
+	_manual_sub(-1),
+	_arming_sub(-1),
+	_local_pos_sub(-1),
+	_global_pos_sp_sub(-1),
+
+/* publications */
+	_local_pos_sp_pub(-1),
+	_att_sp_pub(-1),
+	_global_vel_sp_pub(-1),
+
+	_time_prev(0),
+	_was_armed(false)
+{
+	memset(&_att, 0, sizeof(_att));
+	memset(&_att_sp, 0, sizeof(_att_sp));
+	memset(&_manual, 0, sizeof(_manual));
+	memset(&_control_mode, 0, sizeof(_control_mode));
+	memset(&_arming, 0, sizeof(_arming));
+	memset(&_local_pos, 0, sizeof(_local_pos));
+	memset(&_local_pos_sp, 0, sizeof(_local_pos_sp));
+	memset(&_global_pos_sp, 0, sizeof(_global_pos_sp));
+	memset(&_global_vel_sp, 0, sizeof(_global_vel_sp));
+
+	_params.pos_p.zero();
+	_params.vel_p.zero();
+	_params.vel_i.zero();
+	_params.vel_d.zero();
+	_params.vel_max.zero();
+	_params.vel_ff.zero();
+	_params.sp_offs_max.zero();
+
+	_pos.zero();
+	_vel.zero();
+	_pos_sp.zero();
+	_vel_sp.zero();
+	_vel_err_prev.zero();
+
+	_params_handles.takeoff_alt	= param_find("NAV_TAKEOFF_ALT");
+	_params_handles.takeoff_gap	= param_find("NAV_TAKEOFF_GAP");
+	_params_handles.thr_min		= param_find("MPC_THR_MIN");
+	_params_handles.thr_max		= param_find("MPC_THR_MAX");
+	_params_handles.z_p			= param_find("MPC_Z_P");
+	_params_handles.z_vel_p		= param_find("MPC_Z_VEL_P");
+	_params_handles.z_vel_i		= param_find("MPC_Z_VEL_I");
+	_params_handles.z_vel_d		= param_find("MPC_Z_VEL_D");
+	_params_handles.z_vel_max	= param_find("MPC_Z_VEL_MAX");
+	_params_handles.z_ff		= param_find("MPC_Z_FF");
+	_params_handles.xy_p		= param_find("MPC_XY_P");
+	_params_handles.xy_vel_p	= param_find("MPC_XY_VEL_P");
+	_params_handles.xy_vel_i	= param_find("MPC_XY_VEL_I");
+	_params_handles.xy_vel_d	= param_find("MPC_XY_VEL_D");
+	_params_handles.xy_vel_max	= param_find("MPC_XY_VEL_MAX");
+	_params_handles.xy_ff		= param_find("MPC_XY_FF");
+	_params_handles.tilt_max	= param_find("MPC_TILT_MAX");
+	_params_handles.rc_scale_pitch	= param_find("RC_SCALE_PITCH");
+	_params_handles.rc_scale_roll	= param_find("RC_SCALE_ROLL");
+	_params_handles.rc_scale_yaw	= param_find("RC_SCALE_YAW");
+
+	/* fetch initial parameter values */
+	parameters_update(true);
+}
+
+MulticopterPositionControl::~MulticopterPositionControl()
+{
+	if (_control_task != -1) {
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_control_task);
+				break;
+			}
+		} while (_control_task != -1);
+	}
+
+	pos_control::g_control = nullptr;
+}
+
+int
+MulticopterPositionControl::parameters_update(bool force)
+{
+	bool updated;
+	struct parameter_update_s param_upd;
+
+	orb_check(_params_sub, &updated);
+	if (updated)
+		orb_copy(ORB_ID(parameter_update), _params_sub, &param_upd);
+
+	if (updated || force) {
+		param_get(_params_handles.takeoff_alt, &_params.takeoff_alt);
+		param_get(_params_handles.takeoff_gap, &_params.takeoff_gap);
+		param_get(_params_handles.thr_min, &_params.thr_min);
+		param_get(_params_handles.thr_max, &_params.thr_max);
+		param_get(_params_handles.tilt_max, &_params.tilt_max);
+		param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch);
+		param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll);
+		param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw);
+
+		float v;
+		param_get(_params_handles.xy_p, &v);
+		_params.pos_p(0) = v;
+		_params.pos_p(1) = v;
+		param_get(_params_handles.z_p, &v);
+		_params.pos_p(2) = v;
+		param_get(_params_handles.xy_vel_p, &v);
+		_params.vel_p(0) = v;
+		_params.vel_p(1) = v;
+		param_get(_params_handles.z_vel_p, &v);
+		_params.vel_p(2) = v;
+		param_get(_params_handles.xy_vel_i, &v);
+		_params.vel_i(0) = v;
+		_params.vel_i(1) = v;
+		param_get(_params_handles.z_vel_i, &v);
+		_params.vel_i(2) = v;
+		param_get(_params_handles.xy_vel_d, &v);
+		_params.vel_d(0) = v;
+		_params.vel_d(1) = v;
+		param_get(_params_handles.z_vel_d, &v);
+		_params.vel_d(2) = v;
+		param_get(_params_handles.xy_vel_max, &v);
+		_params.vel_max(0) = v;
+		_params.vel_max(1) = v;
+		param_get(_params_handles.z_vel_max, &v);
+		_params.vel_max(2) = v;
+		param_get(_params_handles.xy_ff, &v);
+		_params.vel_ff(0) = v;
+		_params.vel_ff(1) = v;
+		param_get(_params_handles.z_ff, &v);
+		_params.vel_ff(2) = v;
+
+		_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
+	}
+
+	return OK;
+}
+
+void
+MulticopterPositionControl::poll_subscriptions()
+{
+	bool updated;
+
+	orb_check(_att_sub, &updated);
+	if (updated)
+		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+
+	orb_check(_att_sp_sub, &updated);
+	if (updated)
+		orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
+
+	orb_check(_control_mode_sub, &updated);
+	if (updated)
+		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
+
+	orb_check(_manual_sub, &updated);
+	if (updated)
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
+
+	orb_check(_arming_sub, &updated);
+	if (updated)
+		orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
+
+	orb_check(_global_pos_sp_sub, &updated);
+	if (updated)
+		orb_copy(ORB_ID(vehicle_global_position_setpoint), _global_pos_sp_sub, &_global_pos_sp);
+}
+
+float
+MulticopterPositionControl::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;
+	}
+}
+
+void
+MulticopterPositionControl::task_main_trampoline(int argc, char *argv[])
+{
+	pos_control::g_control->task_main();
+}
+
+void
+MulticopterPositionControl::task_main()
+{
+	warnx("started");
+
+	static int mavlink_fd;
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd, "[mpc] started");
+
+	/*
+	 * do subscriptions
+	 */
+	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
+	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	_global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
+
+	parameters_update(true);
+
+	/* initialize values of critical structs until first regular update */
+	_arming.armed = false;
+
+	/* get an initial update for all sensor and status data */
+	poll_subscriptions();
+
+	bool reset_mission_sp = false;
+	bool global_pos_sp_valid = false;
+	bool reset_man_sp_z = true;
+	bool reset_man_sp_xy = true;
+	bool reset_int_z = true;
+	bool reset_int_z_manual = false;
+	bool reset_int_xy = true;
+	bool was_armed = false;
+	bool reset_auto_sp_xy = true;
+	bool reset_auto_sp_z = true;
+	bool reset_takeoff_sp = true;
+
+	hrt_abstime t_prev = 0;
+	const float alt_ctl_dz = 0.2f;
+	const float pos_ctl_dz = 0.05f;
+
+	float ref_alt = 0.0f;
+	hrt_abstime ref_alt_t = 0;
+	hrt_abstime local_ref_timestamp = 0;
+
+	math::Vector<3> sp_move_rate;
+	sp_move_rate.zero();
+	math::Vector<3> thrust_int;
+	thrust_int.zero();
+	math::Matrix<3, 3> R;
+	R.identity();
+
+	/* wakeup source */
+	struct pollfd fds[1];
+
+	/* Setup of loop */
+	fds[0].fd = _local_pos_sub;
+	fds[0].events = POLLIN;
+
+	while (!_task_should_exit) {
+		/* wait for up to 500ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
+
+		/* timed out - periodic check for _task_should_exit */
+		if (pret == 0)
+			continue;
+
+		/* this is undesirable but not much we can do */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			continue;
+		}
+
+		orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
+
+		poll_subscriptions();
+		parameters_update(false);
+
+		hrt_abstime t = hrt_absolute_time();
+		float dt = _time_prev != 0 ? (t - _time_prev) * 0.000001f : 0.0f;
+		_time_prev = t;
+
+		if (_control_mode.flag_armed && !_was_armed) {
+			/* reset setpoints and integrals on arming */
+			reset_man_sp_z = true;
+			reset_man_sp_xy = true;
+			reset_auto_sp_z = true;
+			reset_auto_sp_xy = true;
+			reset_takeoff_sp = true;
+			reset_int_z = true;
+			reset_int_xy = true;
+		}
+		_was_armed = _control_mode.flag_armed;
+
+		if (_control_mode.flag_control_altitude_enabled ||
+				_control_mode.flag_control_position_enabled ||
+				_control_mode.flag_control_climb_rate_enabled ||
+				_control_mode.flag_control_velocity_enabled) {
+
+			_pos(0) = _local_pos.x;
+			_pos(1) = _local_pos.y;
+			_pos(2) = _local_pos.z;
+			_vel(0) = _local_pos.vx;
+			_vel(1) = _local_pos.vy;
+			_vel(2) = _local_pos.vz;
+
+			sp_move_rate.zero();
+
+			if (_control_mode.flag_control_manual_enabled) {
+				/* manual control */
+				/* check for reference point updates and correct setpoint */
+				if (_local_pos.ref_timestamp != ref_alt_t) {
+					if (ref_alt_t != 0) {
+						/* home alt changed, don't follow large ground level changes in manual flight */
+						_pos_sp(2) += _local_pos.ref_alt - ref_alt;
+					}
+
+					ref_alt_t = _local_pos.ref_timestamp;
+					ref_alt = _local_pos.ref_alt;
+					// TODO also correct XY setpoint
+				}
+
+				/* reset setpoints to current position if needed */
+				if (_control_mode.flag_control_altitude_enabled) {
+					if (reset_man_sp_z) {
+						reset_man_sp_z = false;
+						_pos_sp(2) = _pos(2);
+						mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - _pos_sp(2));
+					}
+
+					/* move altitude setpoint with throttle stick */
+					sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
+				}
+
+				if (_control_mode.flag_control_position_enabled) {
+					if (reset_man_sp_xy) {
+						reset_man_sp_xy = false;
+						_pos_sp(0) = _pos(0);
+						_pos_sp(1) = _pos(1);
+						mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
+					}
+
+					/* move position setpoint with roll/pitch stick */
+					sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz);
+					sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz);
+				}
+
+				/* limit setpoint move rate */
+				float sp_move_norm = sp_move_rate.length();
+				if (sp_move_norm > 1.0f) {
+					sp_move_rate /= sp_move_norm;
+				}
+
+				/* scale to max speed and rotate around yaw */
+				math::Matrix<3, 3> R_yaw_sp;
+				R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
+				sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
+
+				/* move position setpoint */
+				_pos_sp += sp_move_rate * dt;
+
+				/* check if position setpoint is too far from actual position */
+				math::Vector<3> pos_sp_offs = (_pos_sp - _pos).edivide(_params.vel_max);
+				float pos_sp_offs_norm = pos_sp_offs.length();
+				if (pos_sp_offs_norm > 1.0f) {
+					pos_sp_offs /= pos_sp_offs_norm;
+					_pos_sp = _pos + pos_sp_offs.emult(_params.vel_max);
+				}
+
+				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
+				_local_pos_sp.yaw = _att_sp.yaw_body;
+
+				/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
+				reset_auto_sp_xy = !_control_mode.flag_control_position_enabled;
+				reset_auto_sp_z = !_control_mode.flag_control_altitude_enabled;
+				reset_takeoff_sp = true;
+
+				/* force reprojection of global setpoint after manual mode */
+				reset_mission_sp = true;
+			} else {
+				// TODO AUTO
+				_pos_sp = _pos;
+			}
+
+			_local_pos_sp.x = _pos_sp(0);
+			_local_pos_sp.y = _pos_sp(1);
+			_local_pos_sp.z = _pos_sp(2);
+
+			/* publish local position setpoint */
+			if (_local_pos_sp_pub > 0) {
+				orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
+
+			} else {
+				_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
+			}
+
+			/* run position & altitude controllers, calculate velocity setpoint */
+			math::Vector<3> pos_err = _pos_sp - _pos;
+			_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
+
+			if (!_control_mode.flag_control_altitude_enabled) {
+				reset_man_sp_z = true;
+				_vel_sp(2) = 0.0f;
+			}
+
+			if (!_control_mode.flag_control_position_enabled) {
+				reset_man_sp_xy = true;
+				_vel_sp(0) = 0.0f;
+				_vel_sp(1) = 0.0f;
+			}
+
+			if (!_control_mode.flag_control_manual_enabled) {
+				/* limit 3D speed only in AUTO mode */
+				float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length();
+
+				if (vel_sp_norm > 1.0f) {
+					_vel_sp /= vel_sp_norm;
+				}
+			}
+
+			_global_vel_sp.vx = _vel_sp(0);
+			_global_vel_sp.vy = _vel_sp(1);
+			_global_vel_sp.vz = _vel_sp(2);
+
+			/* publish velocity setpoint */
+			if (_global_vel_sp_pub > 0) {
+				orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
+
+			} else {
+				_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
+			}
+
+			if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
+				/* reset integrals if needed */
+				if (_control_mode.flag_control_climb_rate_enabled) {
+					if (reset_int_z) {
+						reset_int_z = false;
+						float i = _params.thr_min;
+
+						if (reset_int_z_manual) {
+							i = _manual.throttle;
+
+							if (i < _params.thr_min) {
+								i = _params.thr_min;
+
+							} else if (i > _params.thr_max) {
+								i = _params.thr_max;
+							}
+						}
+
+						thrust_int(2) = -i;
+						mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
+					}
+				} else {
+					reset_int_z = true;
+				}
+
+				if (_control_mode.flag_control_velocity_enabled) {
+					if (reset_int_xy) {
+						reset_int_xy = false;
+						thrust_int(0) = 0.0f;
+						thrust_int(1) = 0.0f;
+						mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral");
+					}
+				} else {
+					reset_int_xy = true;
+				}
+
+				/* calculate desired thrust vector in NED frame */
+				math::Vector<3> vel_err = _vel_sp - _vel;
+				math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + (vel_err - _vel_err_prev).emult(_params.vel_d) / dt + thrust_int;
+
+				_vel_err_prev = vel_err;
+
+				if (!_control_mode.flag_control_velocity_enabled) {
+					thrust_sp(0) = 0.0f;
+					thrust_sp(1) = 0.0f;
+				}
+				if (!_control_mode.flag_control_climb_rate_enabled) {
+					thrust_sp(2) = 0.0f;
+				}
+
+				/* limit thrust vector and check for saturation */
+				bool saturation_xy = false;
+				bool saturation_z = false;
+
+				/* limit min lift */
+				float thr_min = _params.thr_min;
+				if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
+					/* don't allow downside thrust direction in manual attitude mode */
+					thr_min = 0.0f;
+				}
+
+				if (-thrust_sp(2) < thr_min) {
+					thrust_sp(2) = -thr_min;
+					saturation_z = true;
+				}
+
+				/* limit max tilt */
+				float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2));
+
+				if (tilt > _params.tilt_max && _params.thr_min >= 0.0f) {
+					/* crop horizontal component */
+					float k = tanf(_params.tilt_max) / tanf(tilt);
+					thrust_sp(0) *= k;
+					thrust_sp(1) *= k;
+					saturation_xy = true;
+				}
+
+				/* limit max thrust */
+				float thrust_abs = thrust_sp.length();
+
+				if (thrust_abs > _params.thr_max) {
+					if (thrust_sp(2) < 0.0f) {
+						if (-thrust_sp(2) > _params.thr_max) {
+							/* thrust Z component is too large, limit it */
+							thrust_sp(0) = 0.0f;
+							thrust_sp(1) = 0.0f;
+							thrust_sp(2) = -_params.thr_max;
+							saturation_xy = true;
+							saturation_z = true;
+
+						} else {
+							/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
+							float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2));
+							float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+							float k = thrust_xy_max / thrust_xy_abs;
+							thrust_sp(0) *= k;
+							thrust_sp(1) *= k;
+							saturation_xy = true;
+						}
+
+					} else {
+						/* Z component is negative, going down, simply limit thrust vector */
+						float k = _params.thr_max / thrust_abs;
+						thrust_sp *= k;
+						saturation_xy = true;
+						saturation_z = true;
+					}
+
+					thrust_abs = _params.thr_max;
+				}
+
+				/* update integrals */
+				math::Vector<3> m;
+				m(0) = (_control_mode.flag_control_velocity_enabled && !saturation_xy) ? 1.0f : 0.0f;
+				m(1) = m(0);
+				m(2) = (_control_mode.flag_control_climb_rate_enabled && !saturation_z) ? 1.0f : 0.0f;
+
+				thrust_int += vel_err.emult(_params.vel_i) * dt;
+
+				/* calculate attitude and thrust from thrust vector */
+				if (_control_mode.flag_control_velocity_enabled) {
+					/* desired body_z axis = -normalize(thrust_vector) */
+					math::Vector<3> body_x;
+					math::Vector<3> body_y;
+					math::Vector<3> body_z;
+
+					if (thrust_abs > SIGMA) {
+						body_z = -thrust_sp / thrust_abs;
+
+					} else {
+						/* no thrust, set Z axis to safe value */
+						body_z.zero();
+						body_z(2) = 1.0f;
+					}
+
+					/* vector of desired yaw direction in XY plane, rotated by PI/2 */
+					math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
+
+					if (fabsf(body_z(2)) > SIGMA) {
+						/* desired body_x axis, orthogonal to body_z */
+						body_x = y_C % body_z;
+
+						/* keep nose to front while inverted upside down */
+						if (body_z(2) < 0.0f) {
+							body_x = -body_x;
+						}
+						body_x.normalize();
+					} else {
+						/* desired thrust is in XY plane, set X downside to construct correct matrix,
+						 * but yaw component will not be used actually */
+						body_x.zero();
+						body_x(2) = 1.0f;
+					}
+
+					/* desired body_y axis */
+					body_y = body_z % body_x;
+
+					/* fill rotation matrix */
+					for (int i = 0; i < 3; i++) {
+						R(i, 0) = body_x(i);
+						R(i, 1) = body_y(i);
+						R(i, 2) = body_z(i);
+					}
+
+					/* copy rotation matrix to attitude setpoint topic */
+					memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+					_att_sp.R_valid = true;
+
+					/* calculate euler angles, for logging only, must not be used for control */
+					math::Vector<3> euler = R.to_euler();
+					_att_sp.roll_body = euler(0);
+					_att_sp.pitch_body = euler(1);
+					/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
+
+				} else {
+					/* thrust compensation for altitude only control mode */
+					float att_comp;
+
+					if (_att.R[2][2] > TILT_COS_MAX)
+						att_comp = 1.0f / _att.R[2][2];
+					else if (_att.R[2][2] > 0.0f)
+						att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
+					else
+						att_comp = 1.0f;
+
+					thrust_abs *= att_comp;
+				}
+
+				_att_sp.thrust = thrust_abs;
+
+				_att_sp.timestamp = hrt_absolute_time();
+
+				/* publish attitude setpoint */
+				if (_att_sp_pub > 0) {
+					orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
+
+				} else {
+					_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+				}
+
+			} else {
+				reset_int_z = true;
+			}
+		} else {
+			/* position controller disabled, reset setpoints */
+			reset_man_sp_z = true;
+			reset_man_sp_xy = true;
+			reset_int_z = true;
+			reset_int_xy = true;
+			reset_mission_sp = true;
+			reset_auto_sp_xy = true;
+			reset_auto_sp_z = true;
+		}
+
+		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
+		reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled;
+	}
+
+	warnx("stopped");
+	mavlink_log_info(mavlink_fd, "[mpc] stopped");
+
+	_control_task = -1;
+	_exit(0);
+}
+
+int
+MulticopterPositionControl::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("mc_pos_control",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2048,
+				       (main_t)&MulticopterPositionControl::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+int mc_pos_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		errx(1, "usage: mc_pos_control {start|stop|status}");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (pos_control::g_control != nullptr)
+			errx(1, "already running");
+
+		pos_control::g_control = new MulticopterPositionControl;
+
+		if (pos_control::g_control == nullptr)
+			errx(1, "alloc failed");
+
+		if (OK != pos_control::g_control->start()) {
+			delete pos_control::g_control;
+			pos_control::g_control = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (pos_control::g_control == nullptr)
+			errx(1, "not running");
+
+		delete pos_control::g_control;
+		pos_control::g_control = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (pos_control::g_control) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
new file mode 100644
index 000000000..2fe70698f
--- /dev/null
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -0,0 +1,25 @@
+/*
+ * mc_pos_control_params.c
+ *
+ *  Created on: 26.12.2013
+ *      Author: ton
+ */
+
+#include <systemlib/param/param.h>
+
+/* multicopter position controller parameters */
+PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.05f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
+PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.01f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
+PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f);
diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/mc_pos_control/module.mk
index bc4b48fb4..1d98173fa 100644
--- a/src/modules/multirotor_pos_control/module.mk
+++ b/src/modules/mc_pos_control/module.mk
@@ -32,11 +32,10 @@
 ############################################################################
 
 #
-# Build multirotor position control
+# Build multicopter position controller
 #
 
-MODULE_COMMAND	= multirotor_pos_control
+MODULE_COMMAND	= mc_pos_control
 
-SRCS		= multirotor_pos_control.c \
-		  multirotor_pos_control_params.c \
-		  thrust_pid.c
+SRCS		= mc_pos_control_main.cpp \
+			  mc_pos_control_params.c
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
deleted file mode 100644
index 378d3fda7..000000000
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ /dev/null
@@ -1,864 +0,0 @@
-/****************************************************************************
- *
- *   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 multirotor_pos_control.c
- *
- * 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>
-#include <errno.h>
-#include <debug.h>
-#include <termios.h>
-#include <time.h>
-#include <sys/prctl.h>
-#include <drivers/drv_hrt.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.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_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"
-
-#define TILT_COS_MAX	0.7f
-#define SIGMA			0.000001f
-
-static bool thread_should_exit = false;		/**< Deamon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
-static int deamon_task;				/**< Handle of deamon task / thread */
-
-__EXPORT int multirotor_pos_control_main(int argc, char *argv[]);
-
-/**
- * Mainloop of position controller.
- */
-static int multirotor_pos_control_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-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: multirotor_pos_control {start|stop|status}\n\n");
-	exit(1);
-}
-
-/**
- * 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().
- */
-int multirotor_pos_control_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			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);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		warnx("stop");
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("app is running");
-
-		} else {
-			warnx("app not started");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	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 norm(float x, float y)
-{
-	return sqrtf(x * x + y * y);
-}
-
-static float norm3(float x, float y, float z)
-{
-	return sqrtf(x * x + y * y + z * z);
-}
-
-static void cross3(float a[3], float b[3], float res[3])
-{
-	res[0] = a[1] * b[2] - a[2] * b[1];
-	res[1] = a[2] * b[0] - a[0] * b[2];
-	res[2] = a[0] * b[1] - a[1] * b[0];
-}
-
-static float normalize3(float x[3])
-{
-	float n = sqrtf(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]);
-
-	if (n > 0.0f) {
-		x[0] /= n;
-		x[1] /= n;
-		x[2] /= n;
-	}
-
-	return n;
-}
-
-static float rt_atan2f_snf(float u0, float u1)
-{
-	float y;
-	int32_t b_u0;
-	int32_t b_u1;
-
-	if (isnanf(u0) || isnanf(u1)) {
-		y = NAN;
-
-	} else if (isinff(u0) && isinff(u1)) {
-		if (u0 > 0.0f) {
-			b_u0 = 1;
-
-		} else {
-			b_u0 = -1;
-		}
-
-		if (u1 > 0.0f) {
-			b_u1 = 1;
-
-		} else {
-			b_u1 = -1;
-		}
-
-		y = atan2f((float)b_u0, (float)b_u1);
-
-	} else if (u1 == 0.0f) {
-		if (u0 > 0.0f) {
-			y = M_PI_F / 2.0f;
-
-		} else if (u0 < 0.0f) {
-			y = -(M_PI_F / 2.0f);
-
-		} else {
-			y = 0.0F;
-		}
-
-	} else {
-		y = atan2f(u0, u1);
-	}
-
-	return y;
-}
-
-static int multirotor_pos_control_thread_main(int argc, char *argv[])
-{
-	/* welcome user */
-	warnx("started");
-	static int mavlink_fd;
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(mavlink_fd, "[mpc] started");
-
-	/* structures */
-	struct vehicle_control_mode_s control_mode;
-	memset(&control_mode, 0, sizeof(control_mode));
-	struct vehicle_attitude_s att;
-	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(global_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 att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	int manual_sub = orb_subscribe(ORB_ID(manual_control_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 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_mission_sp = false;
-	bool global_pos_sp_valid = false;
-	bool reset_man_sp_z = true;
-	bool reset_man_sp_xy = true;
-	bool reset_int_z = true;
-	bool reset_int_z_manual = false;
-	bool reset_int_xy = true;
-	bool was_armed = false;
-	bool reset_auto_sp_xy = true;
-	bool reset_auto_sp_z = true;
-	bool reset_takeoff_sp = true;
-
-	hrt_abstime t_prev = 0;
-	const float alt_ctl_dz = 0.2f;
-	const float pos_ctl_dz = 0.05f;
-
-	float ref_alt = 0.0f;
-	hrt_abstime ref_alt_t = 0;
-	uint64_t local_ref_timestamp = 0;
-
-	float thrust_int[3] = { 0.0f, 0.0f, 0.0f };
-
-	thread_running = true;
-
-	struct multirotor_position_control_params params;
-	struct multirotor_position_control_param_handles params_h;
-	parameters_init(&params_h);
-	parameters_update(&params_h, &params);
-
-	bool param_updated = true;
-
-	while (!thread_should_exit) {
-
-		if (!param_updated)
-			orb_check(param_sub, &param_updated);
-
-		if (param_updated) {
-			/* clear updated flag */
-			struct parameter_update_s ps;
-			orb_copy(ORB_ID(parameter_update), param_sub, &ps);
-
-			/* update params */
-			parameters_update(&params_h, &params);
-		}
-
-		bool updated;
-
-		orb_check(control_mode_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
-		}
-
-		orb_check(global_pos_sp_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
-			global_pos_sp_valid = true;
-			reset_mission_sp = true;
-		}
-
-		hrt_abstime t = hrt_absolute_time();
-		float dt;
-
-		if (t_prev != 0) {
-			dt = (t - t_prev) * 0.000001f;
-
-		} else {
-			dt = 0.0f;
-		}
-		t_prev = t;
-
-		if (control_mode.flag_armed && !was_armed) {
-			/* reset setpoints and integrals on arming */
-			reset_man_sp_z = true;
-			reset_man_sp_xy = true;
-			reset_auto_sp_z = true;
-			reset_auto_sp_xy = true;
-			reset_takeoff_sp = true;
-			reset_int_z = true;
-			reset_int_xy = true;
-		}
-
-		was_armed = control_mode.flag_armed;
-
-		if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_position_enabled || control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_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);
-
-			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 };
-
-			if (control_mode.flag_control_manual_enabled) {
-				/* manual control */
-				/* check for reference point updates and correct setpoint */
-				if (local_pos.ref_timestamp != ref_alt_t) {
-					if (ref_alt_t != 0) {
-						/* home alt changed, don't follow large ground level changes in manual flight */
-						local_pos_sp.z += local_pos.ref_alt - ref_alt;
-					}
-
-					ref_alt_t = local_pos.ref_timestamp;
-					ref_alt = local_pos.ref_alt;
-					// TODO also correct XY setpoint
-				}
-
-				/* reset setpoints to current position if needed */
-				if (control_mode.flag_control_altitude_enabled) {
-					if (reset_man_sp_z) {
-						reset_man_sp_z = false;
-						local_pos_sp.z = local_pos.z;
-						mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
-					}
-
-					/* move altitude setpoint with throttle stick */
-					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) {
-					if (reset_man_sp_xy) {
-						reset_man_sp_xy = false;
-						local_pos_sp.x = local_pos.x;
-						local_pos_sp.y = local_pos.y;
-						mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
-					}
-
-					/* move position setpoint with roll/pitch stick */
-					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_sp.yaw_body + 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;
-						}
-					}
-				}
-
-				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
-				local_pos_sp.yaw = att_sp.yaw_body;
-
-				/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
-				reset_auto_sp_xy = !control_mode.flag_control_position_enabled;
-				reset_auto_sp_z = !control_mode.flag_control_altitude_enabled;
-				reset_takeoff_sp = true;
-
-				/* force reprojection of global setpoint after manual mode */
-				reset_mission_sp = true;
-
-			} else if (control_mode.flag_control_auto_enabled) {
-				/* AUTO mode, use global setpoint */
-				if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
-					reset_auto_sp_xy = true;
-					reset_auto_sp_z = true;
-
-				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
-					if (reset_takeoff_sp) {
-						reset_takeoff_sp = false;
-						local_pos_sp.x = local_pos.x;
-						local_pos_sp.y = local_pos.y;
-						local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
-						att_sp.yaw_body = att.yaw;
-						mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
-					}
-
-					reset_auto_sp_xy = false;
-					reset_auto_sp_z = true;
-
-				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
-					// TODO
-					reset_auto_sp_xy = true;
-					reset_auto_sp_z = true;
-
-				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
-					/* init local projection using local position ref */
-					if (local_pos.ref_timestamp != local_ref_timestamp) {
-						reset_mission_sp = true;
-						local_ref_timestamp = local_pos.ref_timestamp;
-						double lat_home = local_pos.ref_lat * 1e-7;
-						double lon_home = local_pos.ref_lon * 1e-7;
-						map_projection_init(lat_home, lon_home);
-						mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
-					}
-
-					if (reset_mission_sp) {
-						reset_mission_sp = false;
-						/* update global setpoint projection */
-
-						if (global_pos_sp_valid) {
-							/* global position setpoint valid, use it */
-							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.ref_alt - global_pos_sp.altitude;
-							}
-
-							/* update yaw setpoint only if value is valid */
-							if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI_F) {
-								att_sp.yaw_body = global_pos_sp.yaw;
-							}
-
-							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
-
-						} else {
-							if (reset_auto_sp_xy) {
-								reset_auto_sp_xy = false;
-								/* local position setpoint is invalid,
-								 * use current position as setpoint for loiter */
-								local_pos_sp.x = local_pos.x;
-								local_pos_sp.y = local_pos.y;
-								local_pos_sp.yaw = att.yaw;
-							}
-
-							if (reset_auto_sp_z) {
-								reset_auto_sp_z = false;
-								local_pos_sp.z = local_pos.z;
-							}
-
-							mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
-						}
-					}
-
-					reset_auto_sp_xy = true;
-					reset_auto_sp_z = true;
-				}
-
-				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) {
-					reset_takeoff_sp = true;
-				}
-
-				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
-					reset_mission_sp = true;
-				}
-
-				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
-				local_pos_sp.yaw = att_sp.yaw_body;
-
-				/* reset setpoints after AUTO mode */
-				reset_man_sp_xy = true;
-				reset_man_sp_z = true;
-
-			} else {
-				/* no control (failsafe), loiter or stay on ground */
-				if (local_pos.landed) {
-					/* landed: move setpoint down */
-					/* in air: hold altitude */
-					if (local_pos_sp.z < 5.0f) {
-						/* set altitude setpoint to 5m under ground,
-						 * don't set it too deep to avoid unexpected landing in case of false "landed" signal */
-						local_pos_sp.z = 5.0f;
-						mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
-					}
-
-					reset_man_sp_z = true;
-
-				} else {
-					/* in air: hold altitude */
-					if (reset_man_sp_z) {
-						reset_man_sp_z = false;
-						local_pos_sp.z = local_pos.z;
-						mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
-					}
-
-					reset_auto_sp_z = false;
-				}
-
-				if (control_mode.flag_control_position_enabled) {
-					if (reset_man_sp_xy) {
-						reset_man_sp_xy = false;
-						local_pos_sp.x = local_pos.x;
-						local_pos_sp.y = local_pos.y;
-						local_pos_sp.yaw = att.yaw;
-						att_sp.yaw_body = att.yaw;
-						mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
-					}
-
-					reset_auto_sp_xy = false;
-				}
-			}
-
-			/* 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 = (local_pos_sp.z - local_pos.z) * params.z_p + sp_move_rate[2] * params.z_ff;
-
-			} else {
-				reset_man_sp_z = true;
-				global_vel_sp.vz = 0.0f;
-			}
-
-			if (control_mode.flag_control_position_enabled) {
-				/* calculate velocity set point in NED frame */
-				global_vel_sp.vx = (local_pos_sp.x - local_pos.x) * params.xy_p + sp_move_rate[0] * params.xy_ff;
-				global_vel_sp.vy = (local_pos_sp.y - local_pos.y) * params.xy_p + sp_move_rate[1] * params.xy_ff;
-
-			} else {
-				reset_man_sp_xy = true;
-				global_vel_sp.vx = 0.0f;
-				global_vel_sp.vy = 0.0f;
-			}
-
-			if (!control_mode.flag_control_manual_enabled) {
-				/* limit 3D speed only in AUTO mode */
-				float vel_sp_norm = norm3(global_vel_sp.vx / params.xy_vel_max, global_vel_sp.vy / params.xy_vel_max, global_vel_sp.vz / params.z_vel_max);
-
-				if (vel_sp_norm > 1.0f) {
-					global_vel_sp.vx /= vel_sp_norm;
-					global_vel_sp.vy /= vel_sp_norm;
-					global_vel_sp.vz /= vel_sp_norm;
-				}
-			}
-
-			/* publish new velocity setpoint */
-			orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
-			// TODO subscribe to velocity setpoint if altitude/position control disabled
-
-			if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
-				/* calculate desired thrust vector in NED frame */
-				float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
-
-				if (reset_int_z) {
-					reset_int_z = false;
-					float i = params.thr_min;
-
-					if (reset_int_z_manual) {
-						i = manual.throttle;
-
-						if (i < params.thr_min) {
-							i = params.thr_min;
-
-						} else if (i > params.thr_max) {
-							i = params.thr_max;
-						}
-					}
-
-					thrust_int[2] = -i;
-					mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
-				}
-
-				thrust_sp[2] = (global_vel_sp.vz - local_pos.vz) * params.z_vel_p + thrust_int[2];
-
-				if (control_mode.flag_control_velocity_enabled) {
-					if (reset_int_xy) {
-						reset_int_xy = false;
-						thrust_int[0] = 0.0f;
-						thrust_int[1] = 0.0f;
-						mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral");
-					}
-
-					thrust_sp[0] = (global_vel_sp.vx - local_pos.vx) * params.xy_vel_p + thrust_int[0];
-					thrust_sp[1] = (global_vel_sp.vy - local_pos.vy) * params.xy_vel_p + thrust_int[1];
-
-				} else {
-					reset_int_xy = true;
-				}
-
-				/* limit thrust vector and check for saturation */
-				bool saturation_xy = false;
-				bool saturation_z = false;
-
-				/* limit min lift */
-				float thr_min = params.thr_min;
-				if (!control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
-					/* don't allow downside thrust direction in manual attitude mode */
-					thr_min = 0.0f;
-				}
-
-				if (-thrust_sp[2] < thr_min) {
-					thrust_sp[2] = -thr_min;
-					saturation_z = true;
-				}
-
-				/* limit max tilt */
-				float tilt = atan2f(norm(thrust_sp[0], thrust_sp[1]), -thrust_sp[2]);
-
-				if (tilt > params.tilt_max && params.thr_min > 0.0f) {
-					/* crop horizontal component */
-					float k = tanf(params.tilt_max) / tanf(tilt);
-					thrust_sp[0] *= k;
-					thrust_sp[1] *= k;
-					saturation_xy = true;
-				}
-
-				/* limit max thrust */
-				float thrust_abs = sqrtf(thrust_sp[0] * thrust_sp[0] + thrust_sp[1] * thrust_sp[1] + thrust_sp[2] * thrust_sp[2]);
-
-				if (thrust_abs > params.thr_max) {
-					if (thrust_sp[2] < 0.0f) {
-						if (-thrust_sp[2] > params.thr_max) {
-							/* thrust Z component is too large, limit it */
-							thrust_sp[0] = 0.0f;
-							thrust_sp[1] = 0.0f;
-							thrust_sp[2] = -params.thr_max;
-							saturation_xy = true;
-							saturation_z = true;
-
-						} else {
-							/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
-							float thrust_xy_max = sqrtf(params.thr_max * params.thr_max - thrust_sp[2] * thrust_sp[2]);
-							float thrust_xy_abs = norm(thrust_sp[0], thrust_sp[1]);
-							float k = thrust_xy_max / thrust_xy_abs;
-							thrust_sp[0] *= k;
-							thrust_sp[1] *= k;
-							saturation_xy = true;
-						}
-
-					} else {
-						/* Z component is negative, going down, simply limit thrust vector */
-						float k = params.thr_max / thrust_abs;
-						thrust_sp[0] *= k;
-						thrust_sp[1] *= k;
-						thrust_sp[2] *= k;
-						saturation_xy = true;
-						saturation_z = true;
-					}
-
-					thrust_abs = params.thr_max;
-				}
-
-				/* update integrals */
-				if (control_mode.flag_control_velocity_enabled && !saturation_xy) {
-					thrust_int[0] += (global_vel_sp.vx - local_pos.vx) * params.xy_vel_i * dt;
-					thrust_int[1] += (global_vel_sp.vy - local_pos.vy) * params.xy_vel_i * dt;
-				}
-
-				if (control_mode.flag_control_climb_rate_enabled && !saturation_z) {
-					thrust_int[2] += (global_vel_sp.vz - local_pos.vz) * params.z_vel_i * dt;
-				}
-
-				/* calculate attitude and thrust from thrust vector */
-				if (control_mode.flag_control_velocity_enabled) {
-					/* desired body_z axis = -normalize(thrust_vector) */
-					float body_x[3];
-					float body_y[3];
-					float body_z[3];
-
-					if (thrust_abs > SIGMA) {
-						body_z[0] = -thrust_sp[0] / thrust_abs;
-						body_z[1] = -thrust_sp[1] / thrust_abs;
-						body_z[2] = -thrust_sp[2] / thrust_abs;
-
-					} else {
-						/* no thrust, set Z axis to safe value */
-						body_z[0] = 0.0f;
-						body_z[1] = 0.0f;
-						body_z[2] = 1.0f;
-					}
-
-					/* vector of desired yaw direction in XY plane, rotated by PI/2 */
-					float y_C[3];
-					y_C[0] = -sinf(att_sp.yaw_body);
-					y_C[1] = cosf(att_sp.yaw_body);
-					y_C[2] = 0;
-
-					if (fabsf(body_z[2]) > SIGMA) {
-						/* desired body_x axis = cross(y_C, body_z) */
-						cross3(y_C, body_z, body_x);
-
-						/* keep nose to front while inverted upside down */
-						if (body_z[2] < 0.0f) {
-							body_x[0] = -body_x[0];
-							body_x[1] = -body_x[1];
-							body_x[2] = -body_x[2];
-						}
-						normalize3(body_x);
-					} else {
-						/* desired thrust is in XY plane, set X downside to construct correct matrix,
-						 * but yaw component will not be used actually */
-						body_x[0] = 0.0f;
-						body_x[1] = 0.0f;
-						body_x[2] = 1.0f;
-					}
-
-					/* desired body_y axis = cross(body_z, body_x) */
-					cross3(body_z, body_x, body_y);
-
-					/* fill rotation matrix */
-					for (int i = 0; i < 3; i++) {
-						att_sp.R_body[i][0] = body_x[i];
-						att_sp.R_body[i][1] = body_y[i];
-						att_sp.R_body[i][2] = body_z[i];
-					}
-
-					att_sp.R_valid = true;
-
-					/* calculate roll, pitch from rotation matrix */
-					att_sp.roll_body = rt_atan2f_snf(att_sp.R_body[2][1], att_sp.R_body[2][2]);
-					att_sp.pitch_body = -asinf(att_sp.R_body[2][0]);
-					/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
-					//att_sp.yaw_body = rt_atan2f_snf(att_sp.R_body[1][0], att_sp.R_body[0][0]);
-
-				} else {
-					/* thrust compensation for altitude only control mode */
-					float att_comp;
-
-					if (att.R[2][2] > TILT_COS_MAX)
-						att_comp = 1.0f / att.R[2][2];
-					else if (att.R[2][2] > 0.0f)
-						att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * att.R[2][2] + 1.0f;
-					else
-						att_comp = 1.0f;
-
-					thrust_abs *= att_comp;
-				}
-
-				att_sp.thrust = thrust_abs;
-
-				att_sp.timestamp = hrt_absolute_time();
-
-				/* publish new attitude setpoint */
-				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-
-			} else {
-				reset_int_z = true;
-			}
-
-		} else {
-			/* position controller disabled, reset setpoints */
-			reset_man_sp_z = true;
-			reset_man_sp_xy = true;
-			reset_int_z = true;
-			reset_int_xy = true;
-			reset_mission_sp = true;
-			reset_auto_sp_xy = true;
-			reset_auto_sp_z = true;
-		}
-
-		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
-		reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;
-
-		/* run at approximately 50 Hz */
-		usleep(20000);
-	}
-
-	warnx("stopped");
-	mavlink_log_info(mavlink_fd, "[mpc] stopped");
-
-	thread_running = false;
-
-	fflush(stdout);
-	return 0;
-}
-
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
deleted file mode 100644
index bebcdb3f5..000000000
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ /dev/null
@@ -1,106 +0,0 @@
-/****************************************************************************
- *
- *   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 multirotor_pos_control_params.c
- *
- * Parameters for multirotor_pos_control
- */
-
-#include "multirotor_pos_control_params.h"
-
-/* controller parameters */
-PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f);
-PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f);
-PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
-PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f);
-PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.1f);
-PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
-PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
-PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f);
-PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f);
-PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.05f);
-PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
-PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
-PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
-
-int parameters_init(struct multirotor_position_control_param_handles *h)
-{
-	h->takeoff_alt = param_find("NAV_TAKEOFF_ALT");
-	h->takeoff_gap = param_find("NAV_TAKEOFF_GAP");
-	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_vel_p 	=	param_find("MPC_Z_VEL_P");
-	h->z_vel_i 	=	param_find("MPC_Z_VEL_I");
-	h->z_vel_max 	=	param_find("MPC_Z_VEL_MAX");
-	h->z_ff 	=	param_find("MPC_Z_FF");
-	h->xy_p 	=	param_find("MPC_XY_P");
-	h->xy_vel_p 	=	param_find("MPC_XY_VEL_P");
-	h->xy_vel_i 	=	param_find("MPC_XY_VEL_I");
-	h->xy_vel_max 	=	param_find("MPC_XY_VEL_MAX");
-	h->xy_ff 	=	param_find("MPC_XY_FF");
-	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->takeoff_alt, &(p->takeoff_alt));
-	param_get(h->takeoff_gap, &(p->takeoff_gap));
-	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_vel_p, &(p->z_vel_p));
-	param_get(h->z_vel_i, &(p->z_vel_i));
-	param_get(h->z_vel_max, &(p->z_vel_max));
-	param_get(h->z_ff, &(p->z_ff));
-	param_get(h->xy_p, &(p->xy_p));
-	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_max, &(p->xy_vel_max));
-	param_get(h->xy_ff, &(p->xy_ff));
-	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
deleted file mode 100644
index 37a925dcc..000000000
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ /dev/null
@@ -1,97 +0,0 @@
-/****************************************************************************
- *
- *   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 multirotor_pos_control_params.h
- *
- * Parameters for multirotor_pos_control
- */
-
-#include <systemlib/param/param.h>
-
-struct multirotor_position_control_params {
-	float takeoff_alt;
-	float takeoff_gap;
-	float thr_min;
-	float thr_max;
-	float z_p;
-	float z_vel_p;
-	float z_vel_i;
-	float z_vel_max;
-	float z_ff;
-	float xy_p;
-	float xy_vel_p;
-	float xy_vel_i;
-	float xy_vel_max;
-	float xy_ff;
-	float tilt_max;
-
-	float rc_scale_pitch;
-	float rc_scale_roll;
-	float rc_scale_yaw;
-};
-
-struct multirotor_position_control_param_handles {
-	param_t takeoff_alt;
-	param_t takeoff_gap;
-	param_t thr_min;
-	param_t thr_max;
-	param_t z_p;
-	param_t z_vel_p;
-	param_t z_vel_i;
-	param_t z_vel_max;
-	param_t z_ff;
-	param_t xy_p;
-	param_t xy_vel_p;
-	param_t xy_vel_i;
-	param_t xy_vel_max;
-	param_t xy_ff;
-	param_t tilt_max;
-
-	param_t rc_scale_pitch;
-	param_t rc_scale_roll;
-	param_t rc_scale_yaw;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-int parameters_init(struct multirotor_position_control_param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p);
diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c
deleted file mode 100644
index 39a715322..000000000
--- a/src/modules/multirotor_pos_control/thrust_pid.c
+++ /dev/null
@@ -1,172 +0,0 @@
-/****************************************************************************
- *
- *   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>
-
-#define COS_TILT_MAX	0.7f
-
-__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min)
-{
-	pid->dt_min = dt_min;
-	pid->kp = 0.0f;
-	pid->ki = 0.0f;
-	pid->kd = 0.0f;
-	pid->integral = 0.0f;
-	pid->output_min = 0.0f;
-	pid->output_max = 0.0f;
-	pid->error_previous = 0.0f;
-	pid->last_output = 0.0f;
-}
-
-__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_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(output_min)) {
-		pid->output_min = output_min;
-
-	}  else {
-		ret = 1;
-	}
-
-	if (isfinite(output_max)) {
-		pid->output_max = output_max;
-
-	}  else {
-		ret = 1;
-	}
-
-	return ret;
-}
-
-__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22)
-{
-	if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) {
-		return pid->last_output;
-	}
-
-	float i, d;
-
-	/* error value */
-	float error = sp - val;
-
-	/* error derivative */
-	d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
-	pid->error_previous = -val;
-
-	if (!isfinite(d)) {
-		d = 0.0f;
-	}
-
-	/* calculate the error integral */
-	i = pid->integral + (pid->ki * error * dt);
-
-	/* attitude-thrust compensation
-	 * r22 is (2, 2) component of rotation matrix for current attitude */
-	float att_comp;
-
-	if (r22 > COS_TILT_MAX) {
-		att_comp = 1.0f / r22;
-
-	} else if (r22 > 0.0f) {
-		att_comp = ((1.0f / COS_TILT_MAX - 1.0f) / COS_TILT_MAX) * r22 + 1.0f;
-
-	} else {
-		att_comp = 1.0f;
-	}
-
-	/* calculate PD output */
-	float output = ((error * pid->kp) + (d * pid->kd)) * att_comp;
-
-	/* check for saturation */
-	if (isfinite(i)) {
-		float i_comp = i * att_comp;
-		if ((output + i_comp) >= pid->output_min || (output + i_comp) <= pid->output_max) {
-			/* not saturated, use new integral value */
-			pid->integral = i;
-		}
-	}
-
-	/* add I component to output */
-	output += pid->integral * att_comp;
-
-	/* limit output */
-	if (isfinite(output)) {
-		if (output > pid->output_max) {
-			output = pid->output_max;
-
-		} else if (output < pid->output_min) {
-			output = pid->output_min;
-		}
-
-		pid->last_output = output;
-	}
-
-	return pid->last_output;
-}
-
-__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i)
-{
-	pid->integral = i;
-}
diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h
deleted file mode 100644
index 71c3704d0..000000000
--- a/src/modules/multirotor_pos_control/thrust_pid.h
+++ /dev/null
@@ -1,69 +0,0 @@
-/****************************************************************************
- *
- *   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
-
-typedef struct {
-	float dt_min;
-	float kp;
-	float ki;
-	float kd;
-	float integral;
-	float output_min;
-	float output_max;
-	float error_previous;
-	float last_output;
-} thrust_pid_t;
-
-__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min);
-__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_max);
-__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22);
-__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i);
-
-__END_DECLS
-
-#endif /* THRUST_PID_H_ */