initial port of multiplatform version of mc_pos_control

11 files changed, 1676 insertions, 2 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index f76dbbf41..6a368d78f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -69,6 +69,11 @@ add_message_files(
   actuator_armed.msg
   parameter_update.msg
   vehicle_status.msg
+  vehicle_local_position.msg
+  position_setpoint.msg
+  position_setpoint_triplet.msg
+  vehicle_local_position_setpoint.msg
+  vehicle_global_velocity_setpoint.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -180,6 +185,16 @@ target_link_libraries(mc_att_control
   px4
 )
 
+## MC Position Control
+add_executable(mc_pos_control
+	src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
+	src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp)
+add_dependencies(mc_pos_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mc_pos_control
+  ${catkin_LIBRARIES}
+  px4
+)
+
 ## Attitude Estimator dummy
 add_executable(attitude_estimator
 	src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
index 7b29fb3a7..2ecc104df 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -14,7 +14,13 @@ else
 	# try the multiplatform version
 	mc_att_control_m start
 fi
-mc_pos_control start
+
+if mc_pos_control start
+then
+else
+	# try the multiplatform version
+	mc_pos_control_m start
+fi
 
 #
 # Start Land Detector
diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk
index 76edade4b..29eb68096 100644
--- a/makefiles/config_px4fmu-v2_multiplatform.mk
+++ b/makefiles/config_px4fmu-v2_multiplatform.mk
@@ -89,7 +89,8 @@ MODULES		+= modules/position_estimator_inav
 MODULES		+= modules/mc_att_control_multiplatform
 MODULES		+= examples/subscriber
 MODULES		+= examples/publisher
-MODULES		+= modules/mc_pos_control
+# MODULES		+= modules/mc_pos_control
+MODULES		+= modules/mc_pos_control_multiplatform
 MODULES 	+= modules/vtol_att_control
 
 #
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
new file mode 100644
index 000000000..2685a99c3
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
@@ -0,0 +1,947 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * 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.cpp
+ * Multicopter position controller.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "mc_pos_control.h"
+#include "mc_pos_control_params.h"
+
+#define TILT_COS_MAX	0.7f
+#define SIGMA		0.000001f
+#define MIN_DIST	0.01f
+
+MulticopterPositionControl::MulticopterPositionControl() :
+
+	_task_should_exit(false),
+	_control_task(-1),
+	_mavlink_fd(-1),
+
+/* publications */
+	_att_sp_pub(nullptr),
+	_local_pos_sp_pub(nullptr),
+	_global_vel_sp_pub(nullptr),
+
+	_ref_alt(0.0f),
+	_ref_timestamp(0),
+
+	_reset_pos_sp(true),
+	_reset_alt_sp(true),
+	_mode_auto(false),
+	_thrust_int(),
+	_R()
+{
+	memset(&_ref_pos, 0, sizeof(_ref_pos));
+
+	/*
+	 * Do subscriptions
+	 */
+	_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterPositionControl::handle_vehicle_attitude, this, 0);
+	_v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0);
+	_control_mode = _n.subscribe<px4_vehicle_control_mode>(0);
+	_parameter_update = _n.subscribe<px4_parameter_update>(
+			&MulticopterPositionControl::handle_parameter_update, this, 1000);
+	_manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0);
+	_armed = _n.subscribe<px4_actuator_armed>(0);
+	_local_pos_sp = _n.subscribe<px4_vehicle_local_position_setpoint>(0);
+	_global_vel_sp = _n.subscribe<px4_vehicle_global_velocity_setpoint>(0);
+
+
+
+	_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();
+	_pos_sp.zero();
+	_vel.zero();
+	_vel_sp.zero();
+	_vel_prev.zero();
+	_vel_ff.zero();
+	_sp_move_rate.zero();
+
+	_params_handles.thr_min		= PX4_PARAM_INIT(MPC_THR_MIN);
+	_params_handles.thr_max		= PX4_PARAM_INIT(MPC_THR_MAX);
+	_params_handles.z_p		= PX4_PARAM_INIT(MPC_Z_P);
+	_params_handles.z_vel_p		= PX4_PARAM_INIT(MPC_Z_VEL_P);
+	_params_handles.z_vel_i		= PX4_PARAM_INIT(MPC_Z_VEL_I);
+	_params_handles.z_vel_d		= PX4_PARAM_INIT(MPC_Z_VEL_D);
+	_params_handles.z_vel_max	= PX4_PARAM_INIT(MPC_Z_VEL_MAX);
+	_params_handles.z_ff		= PX4_PARAM_INIT(MPC_Z_FF);
+	_params_handles.xy_p		= PX4_PARAM_INIT(MPC_XY_P);
+	_params_handles.xy_vel_p	= PX4_PARAM_INIT(MPC_XY_VEL_P);
+	_params_handles.xy_vel_i	= PX4_PARAM_INIT(MPC_XY_VEL_I);
+	_params_handles.xy_vel_d	= PX4_PARAM_INIT(MPC_XY_VEL_D);
+	_params_handles.xy_vel_max	= PX4_PARAM_INIT(MPC_XY_VEL_MAX);
+	_params_handles.xy_ff		= PX4_PARAM_INIT(MPC_XY_FF);
+	_params_handles.tilt_max_air	= PX4_PARAM_INIT(MPC_TILTMAX_AIR);
+	_params_handles.land_speed	= PX4_PARAM_INIT(MPC_LAND_SPEED);
+	_params_handles.tilt_max_land	= PX4_PARAM_INIT(MPC_TILTMAX_LND);
+
+	/* fetch initial parameter values */
+	parameters_update();
+
+	_R.identity();
+}
+
+MulticopterPositionControl::~MulticopterPositionControl()
+{
+}
+
+int
+MulticopterPositionControl::parameters_update()
+{
+	PX4_PARAM_GET(_params_handles.thr_min, &_params.thr_min);
+	PX4_PARAM_GET(_params_handles.thr_max, &_params.thr_max);
+	PX4_PARAM_GET(_params_handles.tilt_max_air, &_params.tilt_max_air);
+	_params.tilt_max_air = math::radians(_params.tilt_max_air);
+	PX4_PARAM_GET(_params_handles.land_speed, &_params.land_speed);
+	PX4_PARAM_GET(_params_handles.tilt_max_land, &_params.tilt_max_land);
+	_params.tilt_max_land = math::radians(_params.tilt_max_land);
+
+	float v;
+	PX4_PARAM_GET(_params_handles.xy_p, &v);
+	_params.pos_p(0) = v;
+	_params.pos_p(1) = v;
+	PX4_PARAM_GET(_params_handles.z_p, &v);
+	_params.pos_p(2) = v;
+	PX4_PARAM_GET(_params_handles.xy_vel_p, &v);
+	_params.vel_p(0) = v;
+	_params.vel_p(1) = v;
+	PX4_PARAM_GET(_params_handles.z_vel_p, &v);
+	_params.vel_p(2) = v;
+	PX4_PARAM_GET(_params_handles.xy_vel_i, &v);
+	_params.vel_i(0) = v;
+	_params.vel_i(1) = v;
+	PX4_PARAM_GET(_params_handles.z_vel_i, &v);
+	_params.vel_i(2) = v;
+	PX4_PARAM_GET(_params_handles.xy_vel_d, &v);
+	_params.vel_d(0) = v;
+	_params.vel_d(1) = v;
+	PX4_PARAM_GET(_params_handles.z_vel_d, &v);
+	_params.vel_d(2) = v;
+	PX4_PARAM_GET(_params_handles.xy_vel_max, &v);
+	_params.vel_max(0) = v;
+	_params.vel_max(1) = v;
+	PX4_PARAM_GET(_params_handles.z_vel_max, &v);
+	_params.vel_max(2) = v;
+	PX4_PARAM_GET(_params_handles.xy_ff, &v);
+	v = math::constrain(v, 0.0f, 1.0f);
+	_params.vel_ff(0) = v;
+	_params.vel_ff(1) = v;
+	PX4_PARAM_GET(_params_handles.z_ff, &v);
+	v = math::constrain(v, 0.0f, 1.0f);
+	_params.vel_ff(2) = v;
+
+	_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
+
+	return OK;
+}
+
+
+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::update_ref()
+{
+	if (_local_pos->data().ref_timestamp != _ref_timestamp) {
+		double lat_sp, lon_sp;
+		float alt_sp = 0.0f;
+
+		if (_ref_timestamp != 0) {
+			/* calculate current position setpoint in global frame */
+			map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
+			alt_sp = _ref_alt - _pos_sp(2);
+		}
+
+		/* update local projection reference */
+		map_projection_init(&_ref_pos, _local_pos->data().ref_lat, _local_pos->data().ref_lon);
+		_ref_alt = _local_pos->data().ref_alt;
+
+		if (_ref_timestamp != 0) {
+			/* reproject position setpoint to new reference */
+			map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
+			_pos_sp(2) = -(alt_sp - _ref_alt);
+		}
+
+		_ref_timestamp = _local_pos->data().ref_timestamp;
+	}
+}
+
+void
+MulticopterPositionControl::reset_pos_sp()
+{
+	if (_reset_pos_sp) {
+		_reset_pos_sp = false;
+		/* shift position setpoint to make attitude setpoint continuous */
+		_pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0)
+				- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
+		_pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1)
+				- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
+
+		//XXX: port this once a mavlink like interface is available
+		// mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
+		PX4_INFO("[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
+	}
+}
+
+void
+MulticopterPositionControl::reset_alt_sp()
+{
+	if (_reset_alt_sp) {
+		_reset_alt_sp = false;
+		_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
+
+		//XXX: port this once a mavlink like interface is available
+		// mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
+		PX4_INFO("[mpc] reset alt sp: %d", -(int)_pos_sp(2));
+	}
+}
+
+void
+MulticopterPositionControl::limit_pos_sp_offset()
+{
+	math::Vector<3> pos_sp_offs;
+	pos_sp_offs.zero();
+
+	if (_control_mode->data().flag_control_position_enabled) {
+		pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+		pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+	}
+
+	if (_control_mode->data().flag_control_altitude_enabled) {
+		pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+	}
+
+	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.sp_offs_max);
+	}
+}
+
+void
+MulticopterPositionControl::control_manual(float dt)
+{
+	_sp_move_rate.zero();
+
+	if (_control_mode->data().flag_control_altitude_enabled) {
+		/* move altitude setpoint with throttle stick */
+		_sp_move_rate(2) = -scale_control(_manual_control_sp->data().z - 0.5f, 0.5f, alt_ctl_dz);
+	}
+
+	if (_control_mode->data().flag_control_position_enabled) {
+		/* move position setpoint with roll/pitch stick */
+		_sp_move_rate(0) = _manual_control_sp->data().x;
+		_sp_move_rate(1) = _manual_control_sp->data().y;
+	}
+
+	/* limit setpoint move rate */
+	float sp_move_norm = _sp_move_rate.length();
+
+	if (sp_move_norm > 1.0f) {
+		_sp_move_rate /= sp_move_norm;
+	}
+
+	/* _sp_move_rate scaled to 0..1, scale it 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_msg.data().yaw_body);
+	_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);
+
+	if (_control_mode->data().flag_control_altitude_enabled) {
+		/* reset alt setpoint to current altitude if needed */
+		reset_alt_sp();
+	}
+
+	if (_control_mode->data().flag_control_position_enabled) {
+		/* reset position setpoint to current position if needed */
+		reset_pos_sp();
+	}
+
+	/* feed forward setpoint move rate with weight vel_ff */
+	_vel_ff = _sp_move_rate.emult(_params.vel_ff);
+
+	/* 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_offs.zero();
+
+	if (_control_mode->data().flag_control_position_enabled) {
+		pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+		pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+	}
+
+	if (_control_mode->data().flag_control_altitude_enabled) {
+		pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+	}
+
+	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.sp_offs_max);
+	}
+}
+
+void
+MulticopterPositionControl::control_offboard(float dt)
+{
+	if (_pos_sp_triplet->data().current.valid) {
+		if (_control_mode->data().flag_control_position_enabled && _pos_sp_triplet->data().current.position_valid) {
+			/* control position */
+			_pos_sp(0) = _pos_sp_triplet->data().current.x;
+			_pos_sp(1) = _pos_sp_triplet->data().current.y;
+		} else if (_control_mode->data().flag_control_velocity_enabled && _pos_sp_triplet->data().current.velocity_valid) {
+			/* control velocity */
+			/* reset position setpoint to current position if needed */
+			reset_pos_sp();
+
+			/* set position setpoint move rate */
+			_sp_move_rate(0) = _pos_sp_triplet->data().current.vx;
+			_sp_move_rate(1) = _pos_sp_triplet->data().current.vy;
+		}
+
+		if (_pos_sp_triplet->data().current.yaw_valid) {
+			_att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw;
+		} else if (_pos_sp_triplet->data().current.yawspeed_valid) {
+			_att_sp_msg.data().yaw_body = _att_sp_msg.data().yaw_body + _pos_sp_triplet->data().current.yawspeed * dt;
+		}
+
+		if (_control_mode->data().flag_control_altitude_enabled && _pos_sp_triplet->data().current.position_valid) {
+			/* Control altitude */
+			_pos_sp(2) = _pos_sp_triplet->data().current.z;
+		} else if (_control_mode->data().flag_control_climb_rate_enabled && _pos_sp_triplet->data().current.velocity_valid) {
+			/* reset alt setpoint to current altitude if needed */
+			reset_alt_sp();
+
+			/* set altitude setpoint move rate */
+			_sp_move_rate(2) = _pos_sp_triplet->data().current.vz;
+		}
+
+		/* feed forward setpoint move rate with weight vel_ff */
+		_vel_ff = _sp_move_rate.emult(_params.vel_ff);
+
+		/* move position setpoint */
+		_pos_sp += _sp_move_rate * dt;
+
+	} else {
+		reset_pos_sp();
+		reset_alt_sp();
+	}
+}
+
+bool
+MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
+		const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res)
+{
+	/* project center of sphere on line */
+	/* normalized AB */
+	math::Vector<3> ab_norm = line_b - line_a;
+	ab_norm.normalize();
+	math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
+	float cd_len = (sphere_c - d).length();
+
+	/* we have triangle CDX with known CD and CX = R, find DX */
+	if (sphere_r > cd_len) {
+		/* have two roots, select one in A->B direction from D */
+		float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);
+		res = d + ab_norm * dx_len;
+		return true;
+
+	} else {
+		/* have no roots, return D */
+		res = d;
+		return false;
+	}
+}
+
+void
+MulticopterPositionControl::control_auto(float dt)
+{
+	if (!_mode_auto) {
+		_mode_auto = true;
+		/* reset position setpoint on AUTO mode activation */
+		reset_pos_sp();
+		reset_alt_sp();
+	}
+
+	if (_pos_sp_triplet->data().current.valid) {
+		/* in case of interrupted mission don't go to waypoint but stay at current position */
+		_reset_pos_sp = true;
+		_reset_alt_sp = true;
+
+		/* project setpoint to local frame */
+		math::Vector<3> curr_sp;
+		map_projection_project(&_ref_pos,
+				       _pos_sp_triplet->data().current.lat, _pos_sp_triplet->data().current.lon,
+				       &curr_sp.data[0], &curr_sp.data[1]);
+		curr_sp(2) = -(_pos_sp_triplet->data().current.alt - _ref_alt);
+
+		/* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */
+		math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max);	// TODO add mult param here
+
+		/* convert current setpoint to scaled space */
+		math::Vector<3> curr_sp_s = curr_sp.emult(scale);
+
+		/* by default use current setpoint as is */
+		math::Vector<3> pos_sp_s = curr_sp_s;
+
+		if (_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_POSITION && _pos_sp_triplet->data().previous.valid) {
+			/* follow "previous - current" line */
+			math::Vector<3> prev_sp;
+			map_projection_project(&_ref_pos,
+						   _pos_sp_triplet->data().previous.lat, _pos_sp_triplet->data().previous.lon,
+						   &prev_sp.data[0], &prev_sp.data[1]);
+			prev_sp(2) = -(_pos_sp_triplet->data().previous.alt - _ref_alt);
+
+			if ((curr_sp - prev_sp).length() > MIN_DIST) {
+
+				/* find X - cross point of L1 sphere and trajectory */
+				math::Vector<3> pos_s = _pos.emult(scale);
+				math::Vector<3> prev_sp_s = prev_sp.emult(scale);
+				math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
+				math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
+				float curr_pos_s_len = curr_pos_s.length();
+				if (curr_pos_s_len < 1.0f) {
+					/* copter is closer to waypoint than L1 radius */
+					/* check next waypoint and use it to avoid slowing down when passing via waypoint */
+					if (_pos_sp_triplet->data().next.valid) {
+						math::Vector<3> next_sp;
+						map_projection_project(&_ref_pos,
+									   _pos_sp_triplet->data().next.lat, _pos_sp_triplet->data().next.lon,
+									   &next_sp.data[0], &next_sp.data[1]);
+						next_sp(2) = -(_pos_sp_triplet->data().next.alt - _ref_alt);
+
+						if ((next_sp - curr_sp).length() > MIN_DIST) {
+							math::Vector<3> next_sp_s = next_sp.emult(scale);
+
+							/* calculate angle prev - curr - next */
+							math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
+							math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
+
+							/* cos(a) * curr_next, a = angle between current and next trajectory segments */
+							float cos_a_curr_next = prev_curr_s_norm * curr_next_s;
+
+							/* cos(b), b = angle pos - curr_sp - prev_sp */
+							float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;
+
+							if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
+								float curr_next_s_len = curr_next_s.length();
+								/* if curr - next distance is larger than L1 radius, limit it */
+								if (curr_next_s_len > 1.0f) {
+									cos_a_curr_next /= curr_next_s_len;
+								}
+
+								/* feed forward position setpoint offset */
+								math::Vector<3> pos_ff = prev_curr_s_norm *
+										cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
+										(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
+								pos_sp_s += pos_ff;
+							}
+						}
+					}
+
+				} else {
+					bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
+					if (near) {
+						/* L1 sphere crosses trajectory */
+
+					} else {
+						/* copter is too far from trajectory */
+						/* if copter is behind prev waypoint, go directly to prev waypoint */
+						if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) {
+							pos_sp_s = prev_sp_s;
+						}
+
+						/* if copter is in front of curr waypoint, go directly to curr waypoint */
+						if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
+							pos_sp_s = curr_sp_s;
+						}
+
+						pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
+					}
+				}
+			}
+		}
+
+		/* move setpoint not faster than max allowed speed */
+		math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);
+
+		/* difference between current and desired position setpoints, 1 = max speed */
+		math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
+		float d_pos_m_len = d_pos_m.length();
+		if (d_pos_m_len > dt) {
+			pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
+		}
+
+		/* scale result back to normal space */
+		_pos_sp = pos_sp_s.edivide(scale);
+
+		/* update yaw setpoint if needed */
+		if (isfinite(_pos_sp_triplet->data().current.yaw)) {
+			_att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw;
+		}
+
+	} else {
+		/* no waypoint, do nothing, setpoint was already reset */
+	}
+}
+
+void MulticopterPositionControl::handle_parameter_update(const px4_parameter_update &msg)
+{
+	parameters_update();
+}
+
+void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg)
+{
+	static bool reset_int_z = true;
+	static bool reset_int_z_manual = false;
+	static bool reset_int_xy = true;
+	static bool was_armed = false;
+	static uint64_t t_prev = 0;
+
+	uint64_t t = get_time_micros();
+	float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
+	t_prev = t;
+
+	if (_control_mode->data().flag_armed && !was_armed) {
+		/* reset setpoints and integrals on arming */
+		_reset_pos_sp = true;
+		_reset_alt_sp = true;
+		reset_int_z = true;
+		reset_int_xy = true;
+	}
+
+	was_armed = _control_mode->data().flag_armed;
+
+	update_ref();
+
+	if (_control_mode->data().flag_control_altitude_enabled ||
+		_control_mode->data().flag_control_position_enabled ||
+		_control_mode->data().flag_control_climb_rate_enabled ||
+		_control_mode->data().flag_control_velocity_enabled) {
+
+		_pos(0) = _local_pos->data().x;
+		_pos(1) = _local_pos->data().y;
+		_pos(2) = _local_pos->data().z;
+
+		_vel(0) = _local_pos->data().vx;
+		_vel(1) = _local_pos->data().vy;
+		_vel(2) = _local_pos->data().vz;
+
+		_vel_ff.zero();
+		_sp_move_rate.zero();
+
+		/* select control source */
+		if (_control_mode->data().flag_control_manual_enabled) {
+			/* manual control */
+			control_manual(dt);
+			_mode_auto = false;
+
+		} else if (_control_mode->data().flag_control_offboard_enabled) {
+			/* offboard control */
+			control_offboard(dt);
+			_mode_auto = false;
+
+		} else {
+			/* AUTO */
+			control_auto(dt);
+		}
+
+		/* fill local position setpoint */
+		_local_pos_sp_msg.data().timestamp = get_time_micros();
+		_local_pos_sp_msg.data().x = _pos_sp(0);
+		_local_pos_sp_msg.data().y = _pos_sp(1);
+		_local_pos_sp_msg.data().z = _pos_sp(2);
+		_local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body;
+
+		/* publish local position setpoint */
+		if (_local_pos_sp_pub != nullptr) {
+			_local_pos_sp_pub->publish(_local_pos_sp_msg);
+
+		} else {
+			_local_pos_sp_pub = _n.advertise<px4_vehicle_local_position_setpoint>();
+		}
+
+
+		if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) {
+			/* idle state, don't run controller and set zero thrust */
+			_R.identity();
+			memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body));
+			_att_sp_msg.data().R_valid = true;
+
+			_att_sp_msg.data().roll_body = 0.0f;
+			_att_sp_msg.data().pitch_body = 0.0f;
+			_att_sp_msg.data().yaw_body = _att->data().yaw;
+			_att_sp_msg.data().thrust = 0.0f;
+
+			_att_sp_msg.data().timestamp = get_time_micros();
+
+			/* publish attitude setpoint */
+			if (_att_sp_pub != nullptr) {
+				_att_sp_pub->publish(_att_sp_msg);
+
+			} else {
+				_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
+			}
+
+		} else {
+			/* run position & altitude controllers, calculate velocity setpoint */
+			math::Vector<3> pos_err = _pos_sp - _pos;
+
+			_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
+
+			if (!_control_mode->data().flag_control_altitude_enabled) {
+				_reset_alt_sp = true;
+				_vel_sp(2) = 0.0f;
+			}
+
+			if (!_control_mode->data().flag_control_position_enabled) {
+				_reset_pos_sp = true;
+				_vel_sp(0) = 0.0f;
+				_vel_sp(1) = 0.0f;
+			}
+
+			/* use constant descend rate when landing, ignore altitude setpoint */
+			if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) {
+				_vel_sp(2) = _params.land_speed;
+			}
+
+			_global_vel_sp_msg.data().vx = _vel_sp(0);
+			_global_vel_sp_msg.data().vy = _vel_sp(1);
+			_global_vel_sp_msg.data().vz = _vel_sp(2);
+
+			/* publish velocity setpoint */
+			if (_global_vel_sp_pub != nullptr) {
+				_global_vel_sp_pub->publish(_global_vel_sp_msg);
+
+			} else {
+				_global_vel_sp_pub = _n.advertise<px4_vehicle_global_velocity_setpoint>();
+			}
+
+			if (_control_mode->data().flag_control_climb_rate_enabled || _control_mode->data().flag_control_velocity_enabled) {
+				/* reset integrals if needed */
+				if (_control_mode->data().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_control_sp->data().z;
+
+							if (i < _params.thr_min) {
+								i = _params.thr_min;
+
+							} else if (i > _params.thr_max) {
+								i = _params.thr_max;
+							}
+						}
+
+						_thrust_int(2) = -i;
+					}
+
+				} else {
+					reset_int_z = true;
+				}
+
+				if (_control_mode->data().flag_control_velocity_enabled) {
+					if (reset_int_xy) {
+						reset_int_xy = false;
+						_thrust_int(0) = 0.0f;
+						_thrust_int(1) = 0.0f;
+					}
+
+				} else {
+					reset_int_xy = true;
+				}
+
+				/* velocity error */
+				math::Vector<3> vel_err = _vel_sp - _vel;
+
+				/* derivative of velocity error, not includes setpoint acceleration */
+				math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
+				_vel_prev = _vel;
+
+				/* thrust vector in NED frame */
+				math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + _thrust_int;
+
+				if (!_control_mode->data().flag_control_velocity_enabled) {
+					thrust_sp(0) = 0.0f;
+					thrust_sp(1) = 0.0f;
+				}
+
+				if (!_control_mode->data().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->data().flag_control_velocity_enabled && thr_min < 0.0f) {
+					/* don't allow downside thrust direction in manual attitude mode */
+					thr_min = 0.0f;
+				}
+
+				float tilt_max = _params.tilt_max_air;
+
+				/* adjust limits for landing mode */
+				if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid &&
+					_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) {
+					/* limit max tilt and min lift when landing */
+					tilt_max = _params.tilt_max_land;
+
+					if (thr_min < 0.0f) {
+						thr_min = 0.0f;
+					}
+				}
+
+				/* limit min lift */
+				if (-thrust_sp(2) < thr_min) {
+					thrust_sp(2) = -thr_min;
+					saturation_z = true;
+				}
+
+				if (_control_mode->data().flag_control_velocity_enabled) {
+					/* limit max tilt */
+					if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
+						/* absolute horizontal thrust */
+						float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+
+						if (thrust_sp_xy_len > 0.01f) {
+							/* max horizontal thrust for given vertical thrust*/
+							float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
+
+							if (thrust_sp_xy_len > thrust_xy_max) {
+								float k = thrust_xy_max / thrust_sp_xy_len;
+								thrust_sp(0) *= k;
+								thrust_sp(1) *= k;
+								saturation_xy = true;
+							}
+						}
+					}
+
+				} else {
+					/* thrust compensation for altitude only control mode */
+					float att_comp;
+
+					if (PX4_R(_att->data().R, 2, 2) > TILT_COS_MAX) {
+						att_comp = 1.0f / PX4_R(_att->data().R, 2, 2);
+
+					} else if (PX4_R(_att->data().R, 2, 2) > 0.0f) {
+						att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att->data().R, 2, 2) + 1.0f;
+						saturation_z = true;
+
+					} else {
+						att_comp = 1.0f;
+						saturation_z = true;
+					}
+
+					thrust_sp(2) *= att_comp;
+				}
+
+				/* 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 */
+				if (_control_mode->data().flag_control_velocity_enabled && !saturation_xy) {
+					_thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
+					_thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
+				}
+
+				if (_control_mode->data().flag_control_climb_rate_enabled && !saturation_z) {
+					_thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
+
+					/* protection against flipping on ground when landing */
+					if (_thrust_int(2) > 0.0f) {
+						_thrust_int(2) = 0.0f;
+					}
+				}
+
+				/* calculate attitude setpoint from thrust vector */
+				if (_control_mode->data().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_msg.data().yaw_body), cosf(_att_sp_msg.data().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_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body));
+					_att_sp_msg.data().R_valid = true;
+
+					/* calculate euler angles, for logging only, must not be used for control */
+					math::Vector<3> euler = _R.to_euler();
+					_att_sp_msg.data().roll_body = euler(0);
+					_att_sp_msg.data().pitch_body = euler(1);
+					/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
+
+				} else if (!_control_mode->data().flag_control_manual_enabled) {
+					/* autonomous altitude control without position control (failsafe landing),
+						* force level attitude, don't change yaw */
+					_R.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body);
+
+					/* copy rotation matrix to attitude setpoint topic */
+					memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body));
+					_att_sp_msg.data().R_valid = true;
+
+					_att_sp_msg.data().roll_body = 0.0f;
+					_att_sp_msg.data().pitch_body = 0.0f;
+				}
+
+				_att_sp_msg.data().thrust = thrust_abs;
+
+				_att_sp_msg.data().timestamp = get_time_micros();
+
+				/* publish attitude setpoint */
+				if (_att_sp_pub != nullptr) {
+					_att_sp_pub->publish(_att_sp_msg);
+
+				} else {
+					_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
+				}
+
+			} else {
+				reset_int_z = true;
+			}
+		}
+
+	} else {
+		/* position controller disabled, reset setpoints */
+		_reset_alt_sp = true;
+		_reset_pos_sp = true;
+		_mode_auto = false;
+		reset_int_z = true;
+		reset_int_xy = true;
+	}
+
+	/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
+	reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled && !_control_mode->data().flag_control_climb_rate_enabled;
+}
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h
new file mode 100644
index 000000000..05bd1387b
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h
@@ -0,0 +1,219 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 - 2015 PX4 Development Team. All rights reserved.
+ *
+ * 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.h
+ * Multicopter position controller.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#pragma once
+
+#include <px4.h>
+#include <cstdio>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+// #include <poll.h>
+// #include <drivers/drv_hrt.h>
+// #include <arch/board/board.h>
+// #include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+// #include <mavlink/mavlink_log.h>
+
+using namespace px4;
+
+class MulticopterPositionControl
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterPositionControl();
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~MulticopterPositionControl();
+
+	/* Callbacks for topics */
+	void handle_vehicle_attitude(const px4_vehicle_attitude &msg);
+	void handle_parameter_update(const px4_parameter_update &msg);
+
+	void spin() { _n.spin(); }
+
+protected:
+	const float alt_ctl_dz = 0.2f;
+
+	bool		_task_should_exit;		/**< if true, task should exit */
+	int		_control_task;			/**< task handle for task */
+	int		_mavlink_fd;			/**< mavlink fd */
+
+	Publisher<px4_vehicle_attitude_setpoint>	*_att_sp_pub;			/**< attitude setpoint publication */
+	Publisher<px4_vehicle_local_position_setpoint>	*_local_pos_sp_pub;		/**< vehicle local position setpoint publication */
+	Publisher<px4_vehicle_global_velocity_setpoint>		*_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
+
+
+	Subscriber<px4_vehicle_attitude> *_att;			    /**< vehicle attitude */
+	Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp;	    /**< vehicle attitude setpoint */
+	Subscriber<px4_vehicle_control_mode> *_control_mode;	    /**< vehicle control mode */
+	Subscriber<px4_parameter_update> *_parameter_update;	    /**< parameter update */
+	Subscriber<px4_manual_control_setpoint> *_manual_control_sp;   /**< manual control setpoint */
+	Subscriber<px4_actuator_armed> *_armed;			    /**< actuator arming status */
+	Subscriber<px4_vehicle_local_position> *_local_pos;			    /**< local position */
+	Subscriber<px4_position_setpoint_triplet> *_pos_sp_triplet;			    /**< local position */
+	Subscriber<px4_vehicle_local_position_setpoint> *_local_pos_sp;			    /**< local position */
+	Subscriber<px4_vehicle_global_velocity_setpoint> *_global_vel_sp;			    /**< local position */
+
+	px4_vehicle_attitude_setpoint _att_sp_msg;
+	px4_vehicle_local_position_setpoint _local_pos_sp_msg;
+	px4_vehicle_global_velocity_setpoint _global_vel_sp_msg;
+
+	px4::NodeHandle _n;
+
+
+	struct {
+		px4_param_t thr_min;
+		px4_param_t thr_max;
+		px4_param_t z_p;
+		px4_param_t z_vel_p;
+		px4_param_t z_vel_i;
+		px4_param_t z_vel_d;
+		px4_param_t z_vel_max;
+		px4_param_t z_ff;
+		px4_param_t xy_p;
+		px4_param_t xy_vel_p;
+		px4_param_t xy_vel_i;
+		px4_param_t xy_vel_d;
+		px4_param_t xy_vel_max;
+		px4_param_t xy_ff;
+		px4_param_t tilt_max_air;
+		px4_param_t land_speed;
+		px4_param_t tilt_max_land;
+	}		_params_handles;		/**< handles for interesting parameters */
+
+	struct {
+		float thr_min;
+		float thr_max;
+		float tilt_max_air;
+		float land_speed;
+		float tilt_max_land;
+
+		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;
+
+	struct map_projection_reference_s _ref_pos;
+	float _ref_alt;
+	uint64_t _ref_timestamp;
+
+	bool _reset_pos_sp;
+	bool _reset_alt_sp;
+	bool _mode_auto;
+
+	math::Vector<3> _pos;
+	math::Vector<3> _pos_sp;
+	math::Vector<3> _vel;
+	math::Vector<3> _vel_sp;
+	math::Vector<3> _vel_prev;			/**< velocity on previous step */
+	math::Vector<3> _vel_ff;
+	math::Vector<3> _sp_move_rate;
+
+	math::Vector<3> _thrust_int;
+	math::Matrix<3, 3> _R;
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int			parameters_update();
+
+	/**
+	 * Update control outputs
+	 */
+	void		control_update();
+
+	static float	scale_control(float ctl, float end, float dz);
+
+	/**
+	 * Update reference for local position projection
+	 */
+	void		update_ref();
+	/**
+	 * Reset position setpoint to current position
+	 */
+	void		reset_pos_sp();
+
+	/**
+	 * Reset altitude setpoint to current altitude
+	 */
+	void		reset_alt_sp();
+
+	/**
+	 * Check if position setpoint is too far from current position and adjust it if needed.
+	 */
+	void		limit_pos_sp_offset();
+
+	/**
+	 * Set position setpoint using manual control
+	 */
+	void		control_manual(float dt);
+
+	/**
+	 * Set position setpoint using offboard control
+	 */
+	void		control_offboard(float dt);
+
+	bool		cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
+					const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res);
+
+	/**
+	 * Set position setpoint for AUTO
+	 */
+	void		control_auto(float dt);
+
+	/**
+	 * Select between barometric and global (AMSL) altitudes
+	 */
+	void		select_alt(bool global);
+};
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
new file mode 100644
index 000000000..0b5775736
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 - 2015 PX4 Development Team. All rights reserved.
+ *
+ * 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
+ * Multicopter position controller.
+ *
+ * The controller has two loops: P loop for position error and PID loop for velocity error.
+ * Output of velocity controller is thrust vector that splitted to thrust direction
+ * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself).
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "mc_pos_control.h"
+
+bool thread_running = false;     /**< Deamon status flag */
+
+int main(int argc, char **argv)
+{
+	px4::init(argc, argv, "mc_pos_control_m");
+
+	PX4_INFO("starting");
+	MulticopterPositionControl posctl;
+	thread_running = true;
+	posctl.spin();
+
+	PX4_INFO("exiting.");
+	thread_running = false;
+	return 0;
+}
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
new file mode 100644
index 000000000..c741a7f0a
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
@@ -0,0 +1,212 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @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) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_pos_control_params.c
+ * Multicopter position controller parameters.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <px4_defines.h>
+#include "mc_pos_control_params.h"
+#include <systemlib/param/param.h>
+
+/**
+ * Minimum thrust
+ *
+ * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN);
+
+/**
+ * Maximum thrust
+ *
+ * Limit max allowed thrust.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX);
+
+/**
+ * Proportional gain for vertical position error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_P);
+
+/**
+ * Proportional gain for vertical velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P);
+
+/**
+ * Integral gain for vertical velocity error
+ *
+ * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I);
+
+/**
+ * Differential gain for vertical velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D);
+
+/**
+ * Maximum vertical velocity
+ *
+ * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL).
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX);
+
+/**
+ * Vertical velocity feed forward
+ *
+ * Feed forward weight for altitude control in stabilized modes (ALTCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF);
+
+/**
+ * Proportional gain for horizontal position error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_P);
+
+/**
+ * Proportional gain for horizontal velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P);
+
+/**
+ * Integral gain for horizontal velocity error
+ *
+ * Non-zero value allows to resist wind.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I);
+
+/**
+ * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D);
+
+/**
+ * Maximum horizontal velocity
+ *
+ * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL).
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX);
+
+/**
+ * Horizontal velocity feed forward
+ *
+ * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF);
+
+/**
+ * Maximum tilt angle in air
+ *
+ * Limits maximum tilt in AUTO and POSCTRL modes during flight.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR);
+
+/**
+ * Maximum tilt during landing
+ *
+ * Limits maximum tilt angle on landing.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND);
+
+/**
+ * Landing descend rate
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED);
+
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
new file mode 100644
index 000000000..fec3bcb7c
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @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_params.h
+ * Multicopter position controller parameters.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#pragma once
+
+#define PARAM_MPC_THR_MIN_DEFAULT 0.1f
+#define PARAM_MPC_THR_MAX_DEFAULT 1.0f
+#define PARAM_MPC_Z_P_DEFAULT 1.0f
+#define PARAM_MPC_Z_VEL_P_DEFAULT 0.1f
+#define PARAM_MPC_Z_VEL_I_DEFAULT 0.02f
+#define PARAM_MPC_Z_VEL_D_DEFAULT 0.0f
+#define PARAM_MPC_Z_VEL_MAX_DEFAULT 5.0f
+#define PARAM_MPC_Z_FF_DEFAULT 0.5f
+#define PARAM_MPC_XY_P_DEFAULT 1.0f
+#define PARAM_MPC_XY_VEL_P_DEFAULT 0.1f
+#define PARAM_MPC_XY_VEL_I_DEFAULT 0.02f
+#define PARAM_MPC_XY_VEL_D_DEFAULT 0.01f
+#define PARAM_MPC_XY_VEL_MAX_DEFAULT 5.0f
+#define PARAM_MPC_XY_FF_DEFAULT 0.5f
+#define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f
+#define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f
+#define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f
+
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp
new file mode 100644
index 000000000..87996d93b
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 - 2015 PX4 Development Team. All rights reserved.
+ *
+ * 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_m_start_nuttx.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+extern bool thread_running;
+int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+extern int main(int argc, char **argv);
+
+extern "C" __EXPORT int mc_pos_control_m_main(int argc, char *argv[]);
+int mc_pos_control_m_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: mc_pos_control_m {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("mc_pos_control_m",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       3000,
+				       main,
+					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/mc_pos_control_multiplatform/module.mk b/src/modules/mc_pos_control_multiplatform/module.mk
new file mode 100644
index 000000000..2852ebbec
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#
+# 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.
+#
+############################################################################
+
+#
+# Build multicopter position controller
+#
+
+MODULE_COMMAND	= mc_pos_control_m
+
+SRCS		= mc_pos_control_main.cpp \
+		  mc_pos_control_start_nuttx.cpp \
+		  mc_pos_control.cpp \
+		  mc_pos_control_params.c
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index 1bd4509ca..f8561fa3b 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -62,6 +62,10 @@
 #include <px4_actuator_armed.h>
 #include <px4_parameter_update.h>
 #include <px4_vehicle_status.h>
+#include <px4_vehicle_local_position_setpoint.h>
+#include <px4_vehicle_global_velocity_setpoint.h>
+#include <px4_vehicle_local_position.h>
+#include <px4_position_setpoint_triplet.h>
 #endif
 
 #else
@@ -85,6 +89,10 @@
 #include <platforms/nuttx/px4_messages/px4_actuator_armed.h>
 #include <platforms/nuttx/px4_messages/px4_parameter_update.h>
 #include <platforms/nuttx/px4_messages/px4_vehicle_status.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_local_position_setpoint.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_global_velocity_setpoint.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_local_position.h>
+#include <platforms/nuttx/px4_messages/px4_position_setpoint_triplet.h>
 #endif
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>