Merge branch 'seatbelt_multirotor' into seatbelt_multirotor_new

WIP, TODO fixedwing

31 files changed, 1936 insertions, 812 deletions

diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 33ffba6bf..f6096d95c 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -71,6 +71,7 @@ MODULES		+= modules/attitude_estimator_ekf
 MODULES		+= modules/attitude_estimator_so3_comp
 MODULES		+= modules/position_estimator
 MODULES		+= modules/att_pos_estimator_ekf
+MODULES		+= modules/position_estimator_inav
 MODULES		+= examples/flow_position_estimator
 
 #
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
index 68c176459..2aeca3a98 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
@@ -142,7 +142,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	}
 
 	/* Roll (P) */
-	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0, NULL, NULL, NULL);
+	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
 
 
 	/* Pitch (P) */
@@ -152,7 +152,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	/* set pitch plus feedforward roll compensation */
 	rates_sp->pitch = pid_calculate(&pitch_controller,
 					att_sp->pitch_body + pitch_sp_rollcompensation,
-					att->pitch, 0, 0, NULL, NULL, NULL);
+					att->pitch, 0, 0);
 
 	/* Yaw (from coordinated turn constraint or lateral force) */
 	rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
index b96fc3e5a..6c9c137bb 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
@@ -186,88 +186,87 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 
 		/* control */
 
-#warning fix this
-//		if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
-//		    vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
-//			/* attitude control */
-//			fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-//
-//			/* angular rate control */
-//			fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-//
-//			/* pass through throttle */
-//			actuators.control[3] = att_sp.thrust;
-//
-//			/* set flaps to zero */
-//			actuators.control[4] = 0.0f;
-//
-//		} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
-//			if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
-//
-//				/* if the RC signal is lost, try to stay level and go slowly back down to ground */
-//				if (vstatus.rc_signal_lost) {
-//
-//					/* put plane into loiter */
-//					att_sp.roll_body = 0.3f;
-//					att_sp.pitch_body = 0.0f;
-//
-//					/* limit throttle to 60 % of last value if sane */
-//					if (isfinite(manual_sp.throttle) &&
-//					    (manual_sp.throttle >= 0.0f) &&
-//					    (manual_sp.throttle <= 1.0f)) {
-//						att_sp.thrust = 0.6f * manual_sp.throttle;
-//
-//					} else {
-//						att_sp.thrust = 0.0f;
-//					}
-//
-//					att_sp.yaw_body = 0;
-//
-//					// XXX disable yaw control, loiter
-//
-//				} else {
-//
-//					att_sp.roll_body = manual_sp.roll;
-//					att_sp.pitch_body = manual_sp.pitch;
-//					att_sp.yaw_body = 0;
-//					att_sp.thrust = manual_sp.throttle;
-//				}
-//
-//				att_sp.timestamp = hrt_absolute_time();
-//
-//				/* attitude control */
-//				fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-//
-//				/* angular rate control */
-//				fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-//
-//				/* pass through throttle */
-//				actuators.control[3] = att_sp.thrust;
-//
-//				/* pass through flaps */
-//				if (isfinite(manual_sp.flaps)) {
-//					actuators.control[4] = manual_sp.flaps;
-//
-//				} else {
-//					actuators.control[4] = 0.0f;
-//				}
-//
-//			} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-//				/* directly pass through values */
-//				actuators.control[0] = manual_sp.roll;
-//				/* positive pitch means negative actuator -> pull up */
-//				actuators.control[1] = manual_sp.pitch;
-//				actuators.control[2] = manual_sp.yaw;
-//				actuators.control[3] = manual_sp.throttle;
-//
-//				if (isfinite(manual_sp.flaps)) {
-//					actuators.control[4] = manual_sp.flaps;
-//
-//				} else {
-//					actuators.control[4] = 0.0f;
-//				}
-//			}
-//		}
+		if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
+		    vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
+			/* attitude control */
+			fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
+
+			/* angular rate control */
+			fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
+
+			/* pass through throttle */
+			actuators.control[3] = att_sp.thrust;
+
+			/* set flaps to zero */
+			actuators.control[4] = 0.0f;
+
+		} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
+			if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+
+				/* if the RC signal is lost, try to stay level and go slowly back down to ground */
+				if (vstatus.rc_signal_lost) {
+
+					/* put plane into loiter */
+					att_sp.roll_body = 0.3f;
+					att_sp.pitch_body = 0.0f;
+
+					/* limit throttle to 60 % of last value if sane */
+					if (isfinite(manual_sp.throttle) &&
+					    (manual_sp.throttle >= 0.0f) &&
+					    (manual_sp.throttle <= 1.0f)) {
+						att_sp.thrust = 0.6f * manual_sp.throttle;
+
+					} else {
+						att_sp.thrust = 0.0f;
+					}
+
+					att_sp.yaw_body = 0;
+
+					// XXX disable yaw control, loiter
+
+				} else {
+
+					att_sp.roll_body = manual_sp.roll;
+					att_sp.pitch_body = manual_sp.pitch;
+					att_sp.yaw_body = 0;
+					att_sp.thrust = manual_sp.throttle;
+				}
+
+				att_sp.timestamp = hrt_absolute_time();
+
+				/* attitude control */
+				fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
+
+				/* angular rate control */
+				fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
+
+				/* pass through throttle */
+				actuators.control[3] = att_sp.thrust;
+
+				/* pass through flaps */
+				if (isfinite(manual_sp.flaps)) {
+					actuators.control[4] = manual_sp.flaps;
+
+				} else {
+					actuators.control[4] = 0.0f;
+				}
+
+			} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+				/* directly pass through values */
+				actuators.control[0] = manual_sp.roll;
+				/* positive pitch means negative actuator -> pull up */
+				actuators.control[1] = manual_sp.pitch;
+				actuators.control[2] = manual_sp.yaw;
+				actuators.control[3] = manual_sp.throttle;
+
+				if (isfinite(manual_sp.flaps)) {
+					actuators.control[4] = manual_sp.flaps;
+
+				} else {
+					actuators.control[4] = 0.0f;
+				}
+			}
+		}
 
 		/* publish rates */
 		orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
index 66854592a..cdab39edc 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
@@ -196,11 +196,11 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 
 	/* roll rate (PI) */
-	actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT, NULL, NULL, NULL);
+	actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
 	/* pitch rate (PI) */
-	actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT, NULL, NULL, NULL);
+	actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
 	/* yaw rate (PI) */
-	actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT, NULL, NULL, NULL);
+	actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
 
 	counter++;
 
diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index 16fcbd864..d65045d68 100644
--- a/src/modules/fixedwing_backside/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -39,8 +39,6 @@
 
 #include "fixedwing.hpp"
 
-#if 0
-
 namespace control
 {
 
@@ -157,9 +155,8 @@ void BlockMultiModeBacksideAutopilot::update()
 	for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
 		_actuators.control[i] = 0.0f;
 
-#warning this if is incomplete, should be based on flags
 	// only update guidance in auto mode
-	if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
+	if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {	// TODO use vehicle_control_mode here?
 		// update guidance
 		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
 	}
@@ -168,10 +165,9 @@ void BlockMultiModeBacksideAutopilot::update()
 	// once the system switches from manual or auto to stabilized
 	// the setpoint should update to loitering around this position
 
-#warning should be base on flags
 	// handle autopilot modes
 	if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
-		_status.navigation_state == NAVIGATION_STATE_SEATBELT) {
+	    _status.navigation_state == NAVIGATION_STATE_STABILIZE) {	// TODO use vehicle_control_mode here?
 
 		// update guidance
 		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
@@ -223,93 +219,83 @@ void BlockMultiModeBacksideAutopilot::update()
 		// This is not a hack, but a design choice.
 
 		/* do not limit in HIL */
-#warning fix this
-		// if (!_status.flag_hil_enabled) {
-		// 	/* limit to value of manual throttle */
-		// 	_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
-		// 				     _actuators.control[CH_THR] : _manual.throttle;
-		// }
-
-	} else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) {
-
-#warning fix here too
-//		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-//			_actuators.control[CH_AIL] = _manual.roll;
-//			_actuators.control[CH_ELV] = _manual.pitch;
-//			_actuators.control[CH_RDR] = _manual.yaw;
-//			_actuators.control[CH_THR] = _manual.throttle;
-//
-//		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
-
-			// calculate velocity, XXX should be airspeed, but using ground speed for now
-			// for the purpose of control we will limit the velocity feedback between
-			// the min/max velocity
-			float v = _vLimit.update(sqrtf(
-						_pos.vx * _pos.vx +
-						_pos.vy * _pos.vy +
-						_pos.vz * _pos.vz));
-
-			// pitch channel -> rate of climb
-			// TODO, might want to put a gain on this, otherwise commanding
-			// from +1 -> -1 m/s for rate of climb
-			//float dThrottle = _cr2Thr.update(
-			//_crMax.get()*_manual.pitch - _pos.vz);
-
-			// roll channel -> bank angle
-			float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
-			float pCmd = _phi2P.update(phiCmd - _att.roll);
-
-			// throttle channel -> velocity
-			// negative sign because nose over to increase speed
-			float vCmd = _vLimit.update(_manual.throttle *
-					(_vLimit.getMax() - _vLimit.getMin()) +
-					_vLimit.getMin());
-			float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
-			float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
-
-			// yaw rate cmd
-			float rCmd = 0;
-
-			// stabilization
-			_stabilization.update(pCmd, qCmd, rCmd,
-					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
-
-			// output
-			_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
-			_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
-			_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
-
-			// currently using manual throttle
-			// XXX if you enable this watch out, vz might be very noisy
-			//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
-			_actuators.control[CH_THR] = _manual.throttle;
-
-			// XXX limit throttle to manual setting (safety) for now.
-			// If it turns out to be confusing, it can be removed later once
-			// a first binary release can be targeted.
-			// This is not a hack, but a design choice.
-
-			/* do not limit in HIL */
-#warning fix this
-			// if (!_status.flag_hil_enabled) {
-			// 	/* limit to value of manual throttle */
-			// 	_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
-			// 				     _actuators.control[CH_THR] : _manual.throttle;
-			// }
-#warning fix this
-//		}
-
-		// body rates controller, disabled for now
-		else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
-
-			_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
-					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
-
-			_actuators.control[CH_AIL] = _stabilization.getAileron();
-			_actuators.control[CH_ELV] = _stabilization.getElevator();
-			_actuators.control[CH_RDR] = _stabilization.getRudder();
-			_actuators.control[CH_THR] = _manual.throttle;
+		if (_status.hil_state != HIL_STATE_ON) {
+			/* limit to value of manual throttle */
+			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
+						     _actuators.control[CH_THR] : _manual.throttle;
+		}
+
+	} else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {	// TODO use vehicle_control_mode here?
+		_actuators.control[CH_AIL] = _manual.roll;
+		_actuators.control[CH_ELV] = _manual.pitch;
+		_actuators.control[CH_RDR] = _manual.yaw;
+		_actuators.control[CH_THR] = _manual.throttle;
+	} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {	// TODO use vehicle_control_mode here?
+		// calculate velocity, XXX should be airspeed, but using ground speed for now
+		// for the purpose of control we will limit the velocity feedback between
+		// the min/max velocity
+		float v = _vLimit.update(sqrtf(
+					_pos.vx * _pos.vx +
+					_pos.vy * _pos.vy +
+					_pos.vz * _pos.vz));
+
+		// pitch channel -> rate of climb
+		// TODO, might want to put a gain on this, otherwise commanding
+		// from +1 -> -1 m/s for rate of climb
+		//float dThrottle = _cr2Thr.update(
+		//_crMax.get()*_manual.pitch - _pos.vz);
+
+		// roll channel -> bank angle
+		float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
+		float pCmd = _phi2P.update(phiCmd - _att.roll);
+
+		// throttle channel -> velocity
+		// negative sign because nose over to increase speed
+		float vCmd = _vLimit.update(_manual.throttle *
+				(_vLimit.getMax() - _vLimit.getMin()) +
+				_vLimit.getMin());
+		float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
+		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
+
+		// yaw rate cmd
+		float rCmd = 0;
+
+		// stabilization
+		_stabilization.update(pCmd, qCmd, rCmd,
+					  _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+		// output
+		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
+		_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
+		_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
+
+		// currently using manual throttle
+		// XXX if you enable this watch out, vz might be very noisy
+		//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
+		_actuators.control[CH_THR] = _manual.throttle;
+
+		// XXX limit throttle to manual setting (safety) for now.
+		// If it turns out to be confusing, it can be removed later once
+		// a first binary release can be targeted.
+		// This is not a hack, but a design choice.
+
+		/* do not limit in HIL */
+		if (_status.hil_state != HIL_STATE_ON) {
+			/* limit to value of manual throttle */
+			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
+							 _actuators.control[CH_THR] : _manual.throttle;
 		}
+	// body rates controller, disabled for now
+	// TODO
+	} else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {	// TODO use vehicle_control_mode here?
+
+		_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
+					  _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+		_actuators.control[CH_AIL] = _stabilization.getAileron();
+		_actuators.control[CH_ELV] = _stabilization.getElevator();
+		_actuators.control[CH_RDR] = _stabilization.getRudder();
+		_actuators.control[CH_THR] = _manual.throttle;
 	}
 
 	// update all publications
@@ -330,4 +316,3 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
 
 } // namespace control
 
-#endif
diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
index b0de69f55..f4ea05088 100644
--- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
@@ -151,14 +151,12 @@ int control_demo_thread_main(int argc, char *argv[])
 
 	using namespace control;
 
-#warning fix this
-//	fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
+	fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
 
 	thread_running = true;
 
 	while (!thread_should_exit) {
-#warning fix this
-//		autopilot.update();
+		autopilot.update();
 	}
 
 	warnx("exiting.");
diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
index 87a942ffb..73df3fb9e 100644
--- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
+++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
@@ -271,7 +271,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 				pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
 				pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
 				pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
-				pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD_F); //TODO: remove hardcoded value
+				pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value
 			}
 
 			/* only run controller if attitude changed */
@@ -319,7 +319,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 					// XXX what is xtrack_err.past_end?
 					if (distance_res == OK /*&& !xtrack_err.past_end*/) {
 
-						float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f, NULL, NULL, NULL); //p.xtrack_p * xtrack_err.distance
+						float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
 
 						float psi_c = psi_track + delta_psi_c;
 						float psi_e = psi_c - att.yaw;
@@ -336,7 +336,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 						}
 
 						/* calculate roll setpoint, do this artificially around zero */
-						float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f, NULL, NULL, NULL);
+						float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
 						float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
 						float psi_rate_c = delta_psi_rate_c + psi_rate_track;
 
@@ -359,7 +359,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 
 						float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
 
-						attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT, NULL, NULL, NULL);
+						attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
 
 						if (verbose) {
 							printf("psi_rate_c %.4f ", (double)psi_rate_c);
@@ -379,7 +379,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 					if (pos_updated) {
 
 						//TODO: take care of relative vs. ab. altitude
-						attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f, NULL, NULL, NULL);
+						attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
 
 					}
 
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
index 20502c0ea..7014d22c4 100644
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c
@@ -58,13 +58,11 @@
 #include <uORB/uORB.h>
 #include <drivers/drv_gyro.h>
 #include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_control_debug.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/parameter_update.h>
@@ -84,18 +82,12 @@ static bool thread_should_exit;
 static int mc_task;
 static bool motor_test_mode = false;
 
-static orb_advert_t actuator_pub;
-static orb_advert_t control_debug_pub;
-
-
 static int
 mc_thread_main(int argc, char *argv[])
 {
 	/* declare and safely initialize all structs */
 	struct vehicle_control_mode_s control_mode;
 	memset(&control_mode, 0, sizeof(control_mode));
-	struct actuator_armed_s armed;
-	memset(&armed, 0, sizeof(armed));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 	struct vehicle_attitude_setpoint_s att_sp;
@@ -112,16 +104,12 @@ mc_thread_main(int argc, char *argv[])
 	struct actuator_controls_s actuators;
 	memset(&actuators, 0, sizeof(actuators));
 
-	struct vehicle_control_debug_s control_debug;
-	memset(&control_debug, 0, sizeof(control_debug));
-
 	/* subscribe to attitude, motor setpoints and system state */
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	int param_sub = orb_subscribe(ORB_ID(parameter_update));
 	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
 	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 
@@ -142,10 +130,8 @@ mc_thread_main(int argc, char *argv[])
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
 		actuators.control[i] = 0.0f;
 	}
-	
-	control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug);
 
-	actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
+	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 	orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
 	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
@@ -161,20 +147,18 @@ mc_thread_main(int argc, char *argv[])
 	/* store last control mode to detect mode switches */
 	bool flag_control_manual_enabled = false;
 	bool flag_control_attitude_enabled = false;
-	bool flag_armed = false;
-	bool flag_control_position_enabled = false;
-	bool flag_control_velocity_enabled = false;
 
 	/* store if yaw position or yaw speed has been changed */
 	bool control_yaw_position = true;
 
 	/* store if we stopped a yaw movement */
-	bool first_time_after_yaw_speed_control = true;
+	bool reset_yaw_sp = true;
 
 	/* prepare the handle for the failsafe throttle */
 	param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
 	float failsafe_throttle = 0.0f;
 
+
 	while (!thread_should_exit) {
 
 		/* wait for a sensor update, check for exit condition every 500 ms */
@@ -187,6 +171,7 @@ mc_thread_main(int argc, char *argv[])
 		} else if (ret == 0) {
 			/* no return value, ignore */
 		} else {
+
 			/* only update parameters if they changed */
 			if (fds[1].revents & POLLIN) {
 				/* read from param to clear updated flag */
@@ -210,12 +195,6 @@ mc_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
 				}
 
-				orb_check(armed_sub, &updated);
-
-				if (updated) {
-					orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-				}
-
 				/* get a local copy of manual setpoint */
 				orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
 				/* get a local copy of attitude */
@@ -232,6 +211,7 @@ mc_thread_main(int argc, char *argv[])
 				/* get a local copy of the current sensor values */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
 
+
 				/** STEP 1: Define which input is the dominating control input */
 				if (control_mode.flag_control_offboard_enabled) {
 					/* offboard inputs */
@@ -256,50 +236,90 @@ mc_thread_main(int argc, char *argv[])
 					}
 
 
-				} else if (control_mode.flag_control_manual_enabled) {			
+				} else if (control_mode.flag_control_manual_enabled) {
+					/* direct manual input */
 					if (control_mode.flag_control_attitude_enabled) {
+						/* control attitude, update attitude setpoint depending on mode */
 						/* initialize to current yaw if switching to manual or att control */
 						if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled ||
-						    control_mode.flag_control_manual_enabled != flag_control_manual_enabled ||
-						    armed.armed != flag_armed) {
+								control_mode.flag_control_manual_enabled != flag_control_manual_enabled) {
 							att_sp.yaw_body = att.yaw;
 						}
 
 						static bool rc_loss_first_time = true;
 
 						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
+						if (control_mode.failsave_highlevel) {
+							if (!control_mode.flag_control_position_enabled) {
+								/* Don't reset attitude setpoint in position control mode, it's handled by position controller. */
+								att_sp.roll_body = 0.0f;
+								att_sp.pitch_body = 0.0f;
+
+								if (!control_mode.flag_control_altitude_enabled) {
+									/* Don't touch throttle in modes with altitude hold, it's handled by position controller.
+									 *
+									 * Only go to failsafe throttle if last known throttle was
+									 * high enough to create some lift to make hovering state likely.
+									 *
+									 * This is to prevent that someone landing, but not disarming his
+									 * multicopter (throttle = 0) does not make it jump up in the air
+									 * if shutting down his remote.
+									 */
+									if (isfinite(manual.throttle) && manual.throttle > 0.2f) {
+										/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
+										param_get(failsafe_throttle_handle, &failsafe_throttle);
+										att_sp.thrust = failsafe_throttle;
+
+									} else {
+										att_sp.thrust = 0.0f;
+									}
+								}
+							}
 
-						rc_loss_first_time = true;
-
-						att_sp.roll_body = manual.roll;
-						att_sp.pitch_body = manual.pitch;
-
-						/* set attitude if arming */
-						if (!flag_control_attitude_enabled && armed.armed) {
-							att_sp.yaw_body = att.yaw;
-						}
+							/* keep current yaw, do not attempt to go to north orientation,
+							 * since if the pilot regains RC control, he will be lost regarding
+							 * the current orientation.
+							 */
+							if (rc_loss_first_time)
+								att_sp.yaw_body = att.yaw;
 
-						/* only move setpoint if manual input is != 0 */
-						if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) {
-							rates_sp.yaw = manual.yaw;
-							control_yaw_position = false;
-							first_time_after_yaw_speed_control = true;
+							rc_loss_first_time = false;
 
 						} else {
-							if (first_time_after_yaw_speed_control) {
-								att_sp.yaw_body = att.yaw;
-								first_time_after_yaw_speed_control = false;
-							}
+							rc_loss_first_time = true;
 
-							control_yaw_position = true;
-						}
+							/* control yaw in all manual / assisted modes */
+							/* set yaw if arming or switching to attitude stabilized mode */
+							if (!flag_control_attitude_enabled) {
+								reset_yaw_sp = true;
+							}
 
-						att_sp.thrust = manual.throttle;
-						att_sp.timestamp = hrt_absolute_time();
+							/* only move setpoint if manual input is != 0 */
+							if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {	// TODO use landed status instead of throttle
+								rates_sp.yaw = manual.yaw;
+								control_yaw_position = false;
+								reset_yaw_sp = true;
+
+							} else {
+								if (reset_yaw_sp) {
+									att_sp.yaw_body = att.yaw;
+									reset_yaw_sp = false;
+								}
+								control_yaw_position = true;
+							}
 
-						/* STEP 2: publish the controller output */
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+							if (!control_mode.flag_control_position_enabled) {
+								/* don't update attitude setpoint in position control mode */
+								att_sp.roll_body = manual.roll;
+								att_sp.pitch_body = manual.pitch;
+								if (!control_mode.flag_control_position_enabled) {
+									/* don't set throttle in altitude hold modes */
+									att_sp.thrust = manual.throttle;
+								}
+							}
 
+							att_sp.timestamp = hrt_absolute_time();
+						}
 
 						if (motor_test_mode) {
 							printf("testmode");
@@ -308,31 +328,26 @@ mc_thread_main(int argc, char *argv[])
 							att_sp.yaw_body = 0.0f;
 							att_sp.thrust = 0.1f;
 							att_sp.timestamp = hrt_absolute_time();
-							/* STEP 2: publish the result to the vehicle actuators */
-							orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 						}
 
-					} else {
+						/* STEP 2: publish the controller output */
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 
-						//XXX TODO add acro mode here
-
-						/* manual rate inputs, from RC control or joystick */
-//						if (state.flag_control_rates_enabled &&
-//						    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
-//							rates_sp.roll = manual.roll;
-//
-//							rates_sp.pitch = manual.pitch;
-//							rates_sp.yaw = manual.yaw;
-//							rates_sp.thrust = manual.throttle;
-//							rates_sp.timestamp = hrt_absolute_time();
-//						}
+					} else {
+						/* manual rate inputs (ACRO), from RC control or joystick */
+						if (control_mode.flag_control_rates_enabled) {
+							rates_sp.roll = manual.roll;
+							rates_sp.pitch = manual.pitch;
+							rates_sp.yaw = manual.yaw;
+							rates_sp.thrust = manual.throttle;
+							rates_sp.timestamp = hrt_absolute_time();
+						}
 					}
-
 				}
 
 				/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
 				if (control_mode.flag_control_attitude_enabled) {
-					multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, &control_debug_pub, &control_debug);
+					multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
 
 					orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 				}
@@ -340,8 +355,7 @@ mc_thread_main(int argc, char *argv[])
 				/* measure in what intervals the controller runs */
 				perf_count(mc_interval_perf);
 
-				float rates[3];
-				float rates_acc[3];
+				float gyro[3];
 
 				/* get current rate setpoint */
 				bool rates_sp_valid = false;
@@ -352,21 +366,16 @@ mc_thread_main(int argc, char *argv[])
 				}
 
 				/* apply controller */
-				rates[0] = att.rollspeed;
-				rates[1] = att.pitchspeed;
-				rates[2] = att.yawspeed;
+				gyro[0] = att.rollspeed;
+				gyro[1] = att.pitchspeed;
+				gyro[2] = att.yawspeed;
 
-				multirotor_control_rates(&rates_sp, rates, &actuators, &control_debug_pub, &control_debug);
+				multirotor_control_rates(&rates_sp, gyro, &actuators);
 				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
-				orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug);
-
-				/* update control_mode */
+				/* update state */
 				flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled;
 				flag_control_manual_enabled = control_mode.flag_control_manual_enabled;
-				flag_control_position_enabled = control_mode.flag_control_position_enabled;
-				flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled;
-				flag_armed = armed.armed;
 
 				perf_end(mc_loop_perf);
 			} /* end of poll call for attitude updates */
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
index 083311674..8f19c6a4b 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.c
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c
@@ -65,29 +65,50 @@
 PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
 PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
 PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
+//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
+//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
 
 PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f);
 PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
 PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f);
+//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
+//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
+
+//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
+//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
 
 struct mc_att_control_params {
 	float yaw_p;
 	float yaw_i;
 	float yaw_d;
+	//float yaw_awu;
+	//float yaw_lim;
 
 	float att_p;
 	float att_i;
 	float att_d;
+	//float att_awu;
+	//float att_lim;
+
+	//float att_xoff;
+	//float att_yoff;
 };
 
 struct mc_att_control_param_handles {
 	param_t yaw_p;
 	param_t yaw_i;
 	param_t yaw_d;
+	//param_t yaw_awu;
+	//param_t yaw_lim;
 
 	param_t att_p;
 	param_t att_i;
 	param_t att_d;
+	//param_t att_awu;
+	//param_t att_lim;
+
+	//param_t att_xoff;
+	//param_t att_yoff;
 };
 
 /**
@@ -109,10 +130,17 @@ static int parameters_init(struct mc_att_control_param_handles *h)
 	h->yaw_p 	=	param_find("MC_YAWPOS_P");
 	h->yaw_i 	=	param_find("MC_YAWPOS_I");
 	h->yaw_d 	=	param_find("MC_YAWPOS_D");
+	//h->yaw_awu 	=	param_find("MC_YAWPOS_AWU");
+	//h->yaw_lim 	=	param_find("MC_YAWPOS_LIM");
 
 	h->att_p 	= 	param_find("MC_ATT_P");
 	h->att_i 	= 	param_find("MC_ATT_I");
 	h->att_d 	= 	param_find("MC_ATT_D");
+	//h->att_awu 	= 	param_find("MC_ATT_AWU");
+	//h->att_lim 	= 	param_find("MC_ATT_LIM");
+
+	//h->att_xoff 	= 	param_find("MC_ATT_XOFF");
+	//h->att_yoff 	= 	param_find("MC_ATT_YOFF");
 
 	return OK;
 }
@@ -122,17 +150,23 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
 	param_get(h->yaw_p, &(p->yaw_p));
 	param_get(h->yaw_i, &(p->yaw_i));
 	param_get(h->yaw_d, &(p->yaw_d));
+	//param_get(h->yaw_awu, &(p->yaw_awu));
+	//param_get(h->yaw_lim, &(p->yaw_lim));
 
 	param_get(h->att_p, &(p->att_p));
 	param_get(h->att_i, &(p->att_i));
 	param_get(h->att_d, &(p->att_d));
+	//param_get(h->att_awu, &(p->att_awu));
+	//param_get(h->att_lim, &(p->att_lim));
+
+	//param_get(h->att_xoff, &(p->att_xoff));
+	//param_get(h->att_yoff, &(p->att_yoff));
 
 	return OK;
 }
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position,
-				 const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug)
+				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
 {
 	static uint64_t last_run = 0;
 	static uint64_t last_input = 0;
@@ -173,7 +207,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 		/* apply parameters */
 		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
-		pid_set_parameters(&roll_controller,  p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
+		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
 	}
 
 	/* reset integral if on ground */
@@ -187,13 +221,11 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 	/* control pitch (forward) output */
 	rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body ,
-					att->pitch, att->pitchspeed, deltaT, &control_debug->pitch_p, &control_debug->pitch_i, &control_debug->pitch_d);
+					att->pitch, att->pitchspeed, deltaT);
 
 	/* control roll (left/right) output */
 	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
-				       att->roll, att->rollspeed, deltaT, NULL, NULL, NULL);
-	
-	// printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT);
+				       att->roll, att->rollspeed, deltaT);
 
 	if (control_yaw_position) {
 		/* control yaw rate */
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h
index 6dd5b39fd..e78f45c47 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.h
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h
@@ -58,11 +58,8 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_control_debug.h>
-
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position,
-				 const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug);
+				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
 
 #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index ee8c37580..e58d357d5 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -58,9 +58,6 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
-#include <uORB/uORB.h>
-
-
 
 PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */
 PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
@@ -155,8 +152,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 }
 
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators,
-			      const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug)
+			      const float rates[], struct actuator_controls_s *actuators)
 {
 	static uint64_t last_run = 0;
 	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -176,13 +172,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	static struct mc_rate_control_params p;
 	static struct mc_rate_control_param_handles h;
 
-	float pitch_control_last = 0.0f;
-	float roll_control_last = 0.0f;
-
 	static bool initialized = false;
 
-	float diff_filter_factor = 1.0f;
-
 	/* initialize the pid controllers when the function is called for the first time */
 	if (initialized == false) {
 		parameters_init(&h);
@@ -210,13 +201,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	/* control pitch (forward) output */
 	float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
-					    rates[1], 0.0f, deltaT,
-					    &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d);
+					    rates[1], 0.0f, deltaT);
 
 	/* control roll (left/right) output */
 	float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
-					   rates[0], 0.0f, deltaT,
-					   &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d);
+					   rates[0], 0.0f, deltaT);
 
 	/* control yaw rate */ //XXX use library here
 	float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h
index 695ff3e16..362b5ed86 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.h
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.h
@@ -57,10 +57,8 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_control_debug.h>
 
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators,
-			      const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug);
+			      const float rates[], struct actuator_controls_s *actuators);
 
 #endif /* MULTIROTOR_RATE_CONTROL_H_ */
diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk
index d04847745..bc4b48fb4 100644
--- a/src/modules/multirotor_pos_control/module.mk
+++ b/src/modules/multirotor_pos_control/module.mk
@@ -38,4 +38,5 @@
 MODULE_COMMAND	= multirotor_pos_control
 
 SRCS		= multirotor_pos_control.c \
-		  multirotor_pos_control_params.c
+		  multirotor_pos_control_params.c \
+		  thrust_pid.c
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index f39d11438..0120fa61c 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,13 +35,14 @@
 /**
  * @file multirotor_pos_control.c
  *
- * Skeleton for multirotor position controller
+ * Multirotor position controller
  */
 
 #include <nuttx/config.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <math.h>
 #include <stdbool.h>
 #include <unistd.h>
 #include <fcntl.h>
@@ -52,15 +53,21 @@
 #include <sys/prctl.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/pid/pid.h>
+#include <mavlink/mavlink_log.h>
 
 #include "multirotor_pos_control_params.h"
+#include "thrust_pid.h"
 
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
@@ -79,12 +86,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]);
  */
 static void usage(const char *reason);
 
-static void
-usage(const char *reason)
+static float scale_control(float ctl, float end, float dz);
+
+static float norm(float x, float y);
+
+static void usage(const char *reason)
 {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
+
+	fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n");
 	exit(1);
 }
 
@@ -92,9 +103,9 @@ usage(const char *reason)
  * The deamon app only briefly exists to start
  * the background job. The stack size assigned in the
  * Makefile does only apply to this management task.
- * 
+ *
  * The actual stack size should be set in the call
- * to task_spawn_cmd().
+ * to task_spawn().
  */
 int multirotor_pos_control_main(int argc, char *argv[])
 {
@@ -104,32 +115,36 @@ int multirotor_pos_control_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("multirotor pos control already running\n");
+			warnx("already running");
 			/* this is not an error */
 			exit(0);
 		}
 
+		warnx("start");
 		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("multirotor pos control",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 60,
-					 4096,
-					 multirotor_pos_control_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		deamon_task = task_spawn_cmd("multirotor_pos_control",
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_MAX - 60,
+					     4096,
+					     multirotor_pos_control_thread_main,
+					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
+		warnx("stop");
 		thread_should_exit = true;
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tmultirotor pos control app is running\n");
+			warnx("app is running");
+
 		} else {
-			printf("\tmultirotor pos control app not started\n");
+			warnx("app not started");
 		}
+
 		exit(0);
 	}
 
@@ -137,98 +152,341 @@ int multirotor_pos_control_main(int argc, char *argv[])
 	exit(1);
 }
 
-static int
-multirotor_pos_control_thread_main(int argc, char *argv[])
+static float scale_control(float ctl, float end, float dz)
+{
+	if (ctl > dz) {
+		return (ctl - dz) / (end - dz);
+
+	} else if (ctl < -dz) {
+		return (ctl + dz) / (end - dz);
+
+	} else {
+		return 0.0f;
+	}
+}
+
+static float norm(float x, float y)
+{
+	return sqrtf(x * x + y * y);
+}
+
+static int multirotor_pos_control_thread_main(int argc, char *argv[])
 {
 	/* welcome user */
-	printf("[multirotor pos control] Control started, taking over position control\n");
+	warnx("started");
+	static int mavlink_fd;
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd, "[mpc] started");
 
 	/* structures */
-	struct vehicle_status_s state;
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 	struct vehicle_attitude_s att;
-	//struct vehicle_global_position_setpoint_s global_pos_sp;
-	struct vehicle_local_position_setpoint_s local_pos_sp;
-	struct vehicle_vicon_position_s local_pos;
-	struct manual_control_setpoint_s manual;
+	memset(&att, 0, sizeof(att));
 	struct vehicle_attitude_setpoint_s att_sp;
+	memset(&att_sp, 0, sizeof(att_sp));
+	struct manual_control_setpoint_s manual;
+	memset(&manual, 0, sizeof(manual));
+	struct vehicle_local_position_s local_pos;
+	memset(&local_pos, 0, sizeof(local_pos));
+	struct vehicle_local_position_setpoint_s local_pos_sp;
+	memset(&local_pos_sp, 0, sizeof(local_pos_sp));
+	struct vehicle_global_position_setpoint_s global_pos_sp;
+	memset(&global_pos_sp, 0, sizeof(local_pos_sp));
+	struct vehicle_global_velocity_setpoint_s global_vel_sp;
+	memset(&global_vel_sp, 0, sizeof(global_vel_sp));
 
 	/* subscribe to attitude, motor setpoints and system state */
+	int param_sub = orb_subscribe(ORB_ID(parameter_update));
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
-	//int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
 	int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
+	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
 
-	/* publish attitude setpoint */
+	/* publish setpoint */
+	orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
+	orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 
+	bool reset_sp_alt = true;
+	bool reset_sp_pos = true;
+	hrt_abstime t_prev = 0;
+	/* integrate in NED frame to estimate wind but not attitude offset */
+	const float alt_ctl_dz = 0.2f;
+	const float pos_ctl_dz = 0.05f;
+	float home_alt = 0.0f;
+	hrt_abstime home_alt_t = 0;
+	uint64_t local_home_timestamp = 0;
+
+	static PID_t xy_pos_pids[2];
+	static PID_t xy_vel_pids[2];
+	static PID_t z_pos_pid;
+	static thrust_pid_t z_vel_pid;
+
 	thread_running = true;
 
-	int loopcounter = 0;
-
-	struct multirotor_position_control_params p;
-	struct multirotor_position_control_param_handles h;
-	parameters_init(&h);
-	parameters_update(&h, &p);
-
-
-	while (1) {
-		/* get a local copy of the vehicle state */
-		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
-		/* get a local copy of manual setpoint */
-		orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
-		/* get a local copy of attitude */
-		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-		/* get a local copy of local position */
-		orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos);
-		/* get a local copy of local position setpoint */
-		orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
-
-		if (loopcounter == 500) {
-			parameters_update(&h, &p);
-			loopcounter = 0;
+	struct multirotor_position_control_params params;
+	struct multirotor_position_control_param_handles params_h;
+	parameters_init(&params_h);
+	parameters_update(&params_h, &params);
+
+	for (int i = 0; i < 2; i++) {
+		pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
+		pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
+	}
+
+	pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
+	thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
+
+	int paramcheck_counter = 0;
+	bool global_pos_sp_updated = false;
+
+	while (!thread_should_exit) {
+		orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+
+		/* check parameters at 1 Hz*/
+		if (--paramcheck_counter <= 0) {
+			paramcheck_counter = 50;
+			bool param_updated;
+			orb_check(param_sub, &param_updated);
+
+			if (param_updated) {
+				parameters_update(&params_h, &params);
+
+				for (int i = 0; i < 2; i++) {
+					pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
+					/* use integral_limit_out = tilt_max / 2 */
+					float i_limit;
+					if (params.xy_vel_i == 0.0) {
+						i_limit = params.tilt_max / params.xy_vel_i / 2.0;
+					} else {
+						i_limit = 1.0f;	// not used really
+					}
+					pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
+				}
+
+				pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
+				thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
+			}
+		}
+
+		/* only check global position setpoint updates but not read */
+		if (!global_pos_sp_updated) {
+			orb_check(global_pos_sp_sub, &global_pos_sp_updated);
+		}
+
+		hrt_abstime t = hrt_absolute_time();
+		float dt;
+
+		if (t_prev != 0) {
+			dt = (t - t_prev) * 0.000001f;
+
+		} else {
+			dt = 0.0f;
 		}
 
-		// if (state.state_machine == SYSTEM_STATE_AUTO) {
-			
-			// XXX IMPLEMENT POSITION CONTROL HERE
-
-			float dT = 1.0f / 50.0f;
-
-			float x_setpoint = 0.0f;
-
-			// XXX enable switching between Vicon and local position estimate
-			/* local pos is the Vicon position */
-
-			// XXX just an example, lacks rotation around world-body transformation
-			att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p;
-			att_sp.roll_body = 0.0f;
-			att_sp.yaw_body = 0.0f;
-			att_sp.thrust = 0.3f;
-			att_sp.timestamp = hrt_absolute_time();
-
-			/* publish new attitude setpoint */
-			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-		// } else if (state.state_machine == SYSTEM_STATE_STABILIZED) {
-			/* set setpoint to current position */
-			// XXX select pos reset channel on remote
-			/* reset setpoint to current position  (position hold) */
-			// if (1 == 2) {
-			// 	local_pos_sp.x = local_pos.x;
-			// 	local_pos_sp.y = local_pos.y;
-			// 	local_pos_sp.z = local_pos.z;
-			// 	local_pos_sp.yaw = att.yaw;
-			// }
-		// }
+		t_prev = t;
+
+		if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) {
+			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
+			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
+			orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
+
+			if (control_mode.flag_control_manual_enabled) {
+				/* manual mode, reset setpoints if needed */
+				if (reset_sp_alt) {
+					reset_sp_alt = false;
+					local_pos_sp.z = local_pos.z;
+					z_vel_pid.integral = -manual.throttle;	// thrust PID uses Z downside
+					mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle);
+				}
+
+				if (control_mode.flag_control_position_enabled && reset_sp_pos) {
+					reset_sp_pos = false;
+					local_pos_sp.x = local_pos.x;
+					local_pos_sp.y = local_pos.y;
+					xy_vel_pids[0].integral = 0.0f;
+					xy_vel_pids[1].integral = 0.0f;
+					mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y);
+				}
+			} else {
+				/* non-manual mode, project global setpoints to local frame */
+				//orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
+				if (local_pos.home_timestamp != local_home_timestamp) {
+					local_home_timestamp = local_pos.home_timestamp;
+					/* init local projection using local position home */
+					double lat_home = local_pos.home_lat * 1e-7;
+					double lon_home = local_pos.home_lon * 1e-7;
+					map_projection_init(lat_home, lon_home);
+					warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home);
+					mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home);
+				}
+
+				if (global_pos_sp_updated) {
+					global_pos_sp_updated = false;
+					orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
+					double sp_lat = global_pos_sp.lat * 1e-7;
+					double sp_lon = global_pos_sp.lon * 1e-7;
+					/* project global setpoint to local setpoint */
+					map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
+
+					if (global_pos_sp.altitude_is_relative) {
+						local_pos_sp.z = -global_pos_sp.altitude;
+
+					} else {
+						local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude;
+					}
+
+					warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
+					mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
+					/* publish local position setpoint as projection of global position setpoint */
+					orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+				}
+			}
+
+			float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
+			float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
+
+			float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
+
+			/* manual control - move setpoints */
+			if (control_mode.flag_control_manual_enabled) {
+				if (local_pos.home_timestamp != home_alt_t) {
+					if (home_alt_t != 0) {
+						/* home alt changed, don't follow large ground level changes in manual flight */
+						local_pos_sp.z += local_pos.home_alt - home_alt;
+					}
+
+					home_alt_t = local_pos.home_timestamp;
+					home_alt = local_pos.home_alt;
+				}
+
+				if (control_mode.flag_control_altitude_enabled) {
+					/* move altitude setpoint with manual controls */
+					float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
+
+					if (z_sp_ctl != 0.0f) {
+						sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
+						local_pos_sp.z += sp_move_rate[2] * dt;
+
+						if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
+							local_pos_sp.z = local_pos.z + z_sp_offs_max;
+
+						} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
+							local_pos_sp.z = local_pos.z - z_sp_offs_max;
+						}
+					}
+				}
+
+				if (control_mode.flag_control_position_enabled) {
+					/* move position setpoint with manual controls */
+					float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
+					float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
+
+					if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
+						/* calculate direction and increment of control in NED frame */
+						float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
+						float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max;
+						sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
+						sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
+						local_pos_sp.x += sp_move_rate[0] * dt;
+						local_pos_sp.y += sp_move_rate[1] * dt;
+						/* limit maximum setpoint from position offset and preserve direction
+						 * fail safe, should not happen in normal operation */
+						float pos_vec_x = local_pos_sp.x - local_pos.x;
+						float pos_vec_y = local_pos_sp.y - local_pos.y;
+						float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
+
+						if (pos_vec_norm > 1.0f) {
+							local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
+							local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
+						}
+					}
+				}
+
+				/* publish local position setpoint */
+				orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+			}
+
+			/* run position & altitude controllers, calculate velocity setpoint */
+			if (control_mode.flag_control_altitude_enabled) {
+				global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
+			} else {
+				global_vel_sp.vz = 0.0f;
+			}
+
+			if (control_mode.flag_control_position_enabled) {
+				/* calculate velocity set point in NED frame */
+				global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
+				global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];
+
+				/* limit horizontal speed */
+				float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
+				if (xy_vel_sp_norm > 1.0f) {
+					global_vel_sp.vx /= xy_vel_sp_norm;
+					global_vel_sp.vy /= xy_vel_sp_norm;
+				}
+
+			} else {
+				reset_sp_pos = true;
+				global_vel_sp.vx = 0.0f;
+				global_vel_sp.vy = 0.0f;
+			}
+
+			/* publish new velocity setpoint */
+			orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
+			// TODO subcrive to velocity setpoint if altitude/position control disabled
+
+			if (control_mode.flag_control_velocity_enabled) {
+				/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
+				float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
+				if (control_mode.flag_control_altitude_enabled) {
+					thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
+					att_sp.thrust = -thrust_sp[2];
+				}
+				if (control_mode.flag_control_position_enabled) {
+					/* calculate thrust set point in NED frame */
+					thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
+					thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
+
+					/* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
+					/* limit horizontal part of thrust */
+					float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
+					/* assuming that vertical component of thrust is g,
+					 * horizontal component = g * tan(alpha) */
+					float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
+
+					if (tilt > params.tilt_max) {
+						tilt = params.tilt_max;
+					}
+					/* convert direction to body frame */
+					thrust_xy_dir -= att.yaw;
+					/* calculate roll and pitch */
+					att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
+					att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
+				}
+
+				att_sp.timestamp = hrt_absolute_time();
+
+				/* publish new attitude setpoint */
+				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+			}
+		} else {
+			reset_sp_alt = true;
+			reset_sp_pos = true;
+		}
 
 		/* run at approximately 50 Hz */
 		usleep(20000);
-		loopcounter++;
 
 	}
 
-	printf("[multirotor pos control] ending now...\n");
+	warnx("stopped");
+	mavlink_log_info(mavlink_fd, "[mpc] stopped");
 
 	thread_running = false;
 
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
index 6b73dc405..1094fd23b 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -34,29 +34,76 @@
  ****************************************************************************/
 
 /*
- * @file multirotor_position_control_params.c
+ * @file multirotor_pos_control_params.c
  * 
- * Parameters for EKF filter
+ * Parameters for multirotor_pos_control
  */
 
 #include "multirotor_pos_control_params.h"
 
-/* Extended Kalman Filter covariances */
-
 /* controller parameters */
-PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f);
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f);
+PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f);
+PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f);
+PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
 
 int parameters_init(struct multirotor_position_control_param_handles *h)
 {
-	/* PID parameters */
-	h->p 	=	param_find("MC_POS_P");
+	h->thr_min 	=	param_find("MPC_THR_MIN");
+	h->thr_max 	=	param_find("MPC_THR_MAX");
+	h->z_p 	=	param_find("MPC_Z_P");
+	h->z_d 	=	param_find("MPC_Z_D");
+	h->z_vel_p 	=	param_find("MPC_Z_VEL_P");
+	h->z_vel_i 	=	param_find("MPC_Z_VEL_I");
+	h->z_vel_d 	=	param_find("MPC_Z_VEL_D");
+	h->z_vel_max 	=	param_find("MPC_Z_VEL_MAX");
+	h->xy_p 	=	param_find("MPC_XY_P");
+	h->xy_d 	=	param_find("MPC_XY_D");
+	h->xy_vel_p 	=	param_find("MPC_XY_VEL_P");
+	h->xy_vel_i 	=	param_find("MPC_XY_VEL_I");
+	h->xy_vel_d 	=	param_find("MPC_XY_VEL_D");
+	h->xy_vel_max 	=	param_find("MPC_XY_VEL_MAX");
+	h->tilt_max 	=	param_find("MPC_TILT_MAX");
+
+	h->rc_scale_pitch    =   param_find("RC_SCALE_PITCH");
+	h->rc_scale_roll    =   param_find("RC_SCALE_ROLL");
+	h->rc_scale_yaw      =   param_find("RC_SCALE_YAW");
 
 	return OK;
 }
 
 int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
 {
-	param_get(h->p, &(p->p));
+	param_get(h->thr_min, &(p->thr_min));
+	param_get(h->thr_max, &(p->thr_max));
+	param_get(h->z_p, &(p->z_p));
+	param_get(h->z_d, &(p->z_d));
+	param_get(h->z_vel_p, &(p->z_vel_p));
+	param_get(h->z_vel_i, &(p->z_vel_i));
+	param_get(h->z_vel_d, &(p->z_vel_d));
+	param_get(h->z_vel_max, &(p->z_vel_max));
+	param_get(h->xy_p, &(p->xy_p));
+	param_get(h->xy_d, &(p->xy_d));
+	param_get(h->xy_vel_p, &(p->xy_vel_p));
+	param_get(h->xy_vel_i, &(p->xy_vel_i));
+	param_get(h->xy_vel_d, &(p->xy_vel_d));
+	param_get(h->xy_vel_max, &(p->xy_vel_max));
+	param_get(h->tilt_max, &(p->tilt_max));
+
+	param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
+	param_get(h->rc_scale_roll, &(p->rc_scale_roll));
+	param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
 
 	return OK;
 }
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
index 274f6c22a..14a3de2e5 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -42,15 +42,47 @@
 #include <systemlib/param/param.h>
 
 struct multirotor_position_control_params {
-	float p;
-	float i;
-	float d;
+	float thr_min;
+	float thr_max;
+	float z_p;
+	float z_d;
+	float z_vel_p;
+	float z_vel_i;
+	float z_vel_d;
+	float z_vel_max;
+	float xy_p;
+	float xy_d;
+	float xy_vel_p;
+	float xy_vel_i;
+	float xy_vel_d;
+	float xy_vel_max;
+	float tilt_max;
+
+	float rc_scale_pitch;
+	float rc_scale_roll;
+	float rc_scale_yaw;
 };
 
 struct multirotor_position_control_param_handles {
-	param_t p;
-	param_t i;
-	param_t d;
+	param_t thr_min;
+	param_t thr_max;
+	param_t z_p;
+	param_t z_d;
+	param_t z_vel_p;
+	param_t z_vel_i;
+	param_t z_vel_d;
+	param_t z_vel_max;
+	param_t xy_p;
+	param_t xy_d;
+	param_t xy_vel_p;
+	param_t xy_vel_i;
+	param_t xy_vel_d;
+	param_t xy_vel_max;
+	param_t tilt_max;
+
+	param_t rc_scale_pitch;
+	param_t rc_scale_roll;
+	param_t rc_scale_yaw;
 };
 
 /**
diff --git a/src/modules/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c
deleted file mode 100644
index 9c53a5bf6..000000000
--- a/src/modules/multirotor_pos_control/position_control.c
+++ /dev/null
@@ -1,235 +0,0 @@
-// /****************************************************************************
-//  *
-//  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
-//  *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
-//  *           @author Laurens Mackay <mackayl@student.ethz.ch>
-//  *           @author Tobias Naegeli <naegelit@student.ethz.ch>
-//  *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
-//  *
-//  * Redistribution and use in source and binary forms, with or without
-//  * modification, are permitted provided that the following conditions
-//  * are met:
-//  *
-//  * 1. Redistributions of source code must retain the above copyright
-//  *    notice, this list of conditions and the following disclaimer.
-//  * 2. Redistributions in binary form must reproduce the above copyright
-//  *    notice, this list of conditions and the following disclaimer in
-//  *    the documentation and/or other materials provided with the
-//  *    distribution.
-//  * 3. Neither the name PX4 nor the names of its contributors may be
-//  *    used to endorse or promote products derived from this software
-//  *    without specific prior written permission.
-//  *
-//  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-//  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-//  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-//  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-//  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-//  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-//  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-//  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-//  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-//  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-//  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-//  * POSSIBILITY OF SUCH DAMAGE.
-//  *
-//  ****************************************************************************/
-
-// /**
-//  * @file multirotor_position_control.c
-//  * Implementation of the position control for a multirotor VTOL
-//  */
-
-// #include <stdio.h>
-// #include <stdlib.h>
-// #include <stdio.h>
-// #include <stdint.h>
-// #include <math.h>
-// #include <stdbool.h>
-// #include <float.h>
-// #include <systemlib/pid/pid.h>
-
-// #include "multirotor_position_control.h"
-
-// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
-//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
-//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
-// {
-// 	static PID_t distance_controller;
-
-// 	static int read_ret;
-// 	static global_data_position_t position_estimated;
-
-// 	static uint16_t counter;
-
-// 	static bool initialized;
-// 	static uint16_t pm_counter;
-
-// 	static float lat_next;
-// 	static float lon_next;
-
-// 	static float pitch_current;
-
-// 	static float thrust_total;
-
-
-// 	if (initialized == false) {
-
-// 		pid_init(&distance_controller,
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
-// 			 PID_MODE_DERIVATIV_CALC, 150);//150
-
-// //		pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-// //		pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
-
-// 		thrust_total = 0.0f;
-
-// 		/* Position initialization */
-// 		/* Wait for new position estimate */
-// 		do {
-// 			read_ret = read_lock_position(&position_estimated);
-// 		} while (read_ret != 0);
-
-// 		lat_next = position_estimated.lat;
-// 		lon_next = position_estimated.lon;
-
-// 		/* attitude initialization */
-// 		global_data_lock(&global_data_attitude->access_conf);
-// 		pitch_current = global_data_attitude->pitch;
-// 		global_data_unlock(&global_data_attitude->access_conf);
-
-// 		initialized = true;
-// 	}
-
-// 	/* load new parameters with 10Hz */
-// 	if (counter % 50 == 0) {
-// 		if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
-// 			/* check whether new parameters are available */
-// 			if (global_data_parameter_storage->counter > pm_counter) {
-// 				pid_set_parameters(&distance_controller,
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
-
-// //
-// //				pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-// //				pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
-
-// 				pm_counter = global_data_parameter_storage->counter;
-// 				printf("Position controller changed pid parameters\n");
-// 			}
-// 		}
-
-// 		global_data_unlock(&global_data_parameter_storage->access_conf);
-// 	}
-
-
-// 	/* Wait for new position estimate */
-// 	do {
-// 		read_ret = read_lock_position(&position_estimated);
-// 	} while (read_ret != 0);
-
-// 	/* Get next waypoint */ //TODO: add local copy
-
-// 	if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
-// 		lat_next = global_data_position_setpoint->x;
-// 		lon_next = global_data_position_setpoint->y;
-// 		global_data_unlock(&global_data_position_setpoint->access_conf);
-// 	}
-
-// 	/* Get distance to waypoint */
-// 	float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
-// //	if(counter % 5 == 0)
-// //		printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
-
-// 	/* Get bearing to waypoint (direction on earth surface to next waypoint) */
-// 	float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
-
-// 	if (counter % 5 == 0)
-// 		printf("bearing: %.4f\n", bearing);
-
-// 	/* Calculate speed in direction of bearing (needed for controller) */
-// 	float speed_norm = sqrtf(position_estimated.vx  * position_estimated.vx + position_estimated.vy * position_estimated.vy);
-// //	if(counter % 5 == 0)
-// //		printf("speed_norm: %.4f\n", speed_norm);
-// 	float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
-
-// 	/* Control Thrust in bearing direction  */
-// 	float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
-// 				  CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
-
-// //	if(counter % 5 == 0)
-// //		printf("horizontal thrust: %.4f\n", horizontal_thrust);
-
-// 	/* Get total thrust (from remote for now) */
-// 	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
-// 		thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; 												//TODO: how should we use the RC_CHANNELS_FUNCTION enum?
-// 		global_data_unlock(&global_data_rc_channels->access_conf);
-// 	}
-
-// 	const float max_gas = 500.0f;
-// 	thrust_total *= max_gas / 20000.0f; //TODO: check this
-// 	thrust_total += max_gas / 2.0f;
-
-
-// 	if (horizontal_thrust > thrust_total) {
-// 		horizontal_thrust = thrust_total;
-
-// 	} else if (horizontal_thrust < -thrust_total) {
-// 		horizontal_thrust = -thrust_total;
-// 	}
-
-
-
-// 	//TODO: maybe we want to add a speed controller later...
-
-// 	/* Calclulate thrust in east and north direction */
-// 	float thrust_north = cosf(bearing) * horizontal_thrust;
-// 	float thrust_east = sinf(bearing) * horizontal_thrust;
-
-// 	if (counter % 10 == 0) {
-// 		printf("thrust north: %.4f\n", thrust_north);
-// 		printf("thrust east: %.4f\n", thrust_east);
-// 		fflush(stdout);
-// 	}
-
-// 	/* Get current attitude */
-// 	if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
-// 		pitch_current = global_data_attitude->pitch;
-// 		global_data_unlock(&global_data_attitude->access_conf);
-// 	}
-
-// 	/* Get desired pitch & roll */
-// 	float pitch_desired = 0.0f;
-// 	float roll_desired = 0.0f;
-
-// 	if (thrust_total != 0) {
-// 		float pitch_fraction = -thrust_north / thrust_total;
-// 		float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
-
-// 		if (roll_fraction < -1) {
-// 			roll_fraction = -1;
-
-// 		} else if (roll_fraction > 1) {
-// 			roll_fraction = 1;
-// 		}
-
-// //		if(counter % 5 == 0)
-// //		{
-// //			printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
-// //			fflush(stdout);
-// //		}
-
-// 		pitch_desired = asinf(pitch_fraction);
-// 		roll_desired = asinf(roll_fraction);
-// 	}
-
-// 	att_sp.roll = roll_desired;
-// 	att_sp.pitch = pitch_desired;
-
-// 	counter++;
-// }
diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c
new file mode 100644
index 000000000..51dcd7df2
--- /dev/null
+++ b/src/modules/multirotor_pos_control/thrust_pid.c
@@ -0,0 +1,184 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file thrust_pid.c
+ *
+ * Implementation of thrust control PID.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "thrust_pid.h"
+#include <math.h>
+
+__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min)
+{
+	pid->kp = kp;
+	pid->ki = ki;
+	pid->kd = kd;
+	pid->limit_min = limit_min;
+	pid->limit_max = limit_max;
+	pid->mode = mode;
+	pid->dt_min = dt_min;
+	pid->last_output = 0.0f;
+	pid->sp = 0.0f;
+	pid->error_previous = 0.0f;
+	pid->integral = 0.0f;
+}
+
+__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max)
+{
+	int ret = 0;
+
+	if (isfinite(kp)) {
+		pid->kp = kp;
+
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(ki)) {
+		pid->ki = ki;
+
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(kd)) {
+		pid->kd = kd;
+
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(limit_min)) {
+		pid->limit_min = limit_min;
+
+	}  else {
+		ret = 1;
+	}
+
+	if (isfinite(limit_max)) {
+		pid->limit_max = limit_max;
+
+	}  else {
+		ret = 1;
+	}
+
+	return ret;
+}
+
+__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22)
+{
+	/* Alternative integral component calculation
+	 *
+	 * start:
+	 * error = setpoint - current_value
+	 * integral = integral + (Ki * error * dt)
+	 * derivative = (error - previous_error) / dt
+	 * previous_error = error
+	 * output = (Kp * error) + integral + (Kd * derivative)
+	 * wait(dt)
+	 * goto start
+	 */
+
+	if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) {
+		return pid->last_output;
+	}
+
+	float i, d;
+	pid->sp = sp;
+
+	// Calculated current error value
+	float error = pid->sp - val;
+
+	// Calculate or measured current error derivative
+	if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) {
+		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = error;
+
+	} else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) {
+		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = -val;
+
+	} else {
+		d = 0.0f;
+	}
+
+	if (!isfinite(d)) {
+		d = 0.0f;
+	}
+
+	/* calculate the error integral */
+	i = pid->integral + (pid->ki * error * dt);
+
+	/* attitude-thrust compensation
+	 * r22 is (2, 2) componet of rotation matrix for current attitude */
+	float att_comp;
+
+	if (r22 > 0.8f)
+		att_comp = 1.0f / r22;
+	else if (r22 > 0.0f)
+		att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f;
+	else
+		att_comp = 1.0f;
+
+	/* calculate output */
+	float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp;
+
+	/* check for saturation */
+	if (output < pid->limit_min || output > pid->limit_max) {
+		/* saturated, recalculate output with old integral */
+		output = (error * pid->kp) + pid->integral + (d * pid->kd);
+
+	} else {
+		if (isfinite(i)) {
+			pid->integral = i;
+		}
+	}
+
+	if (isfinite(output)) {
+		if (output > pid->limit_max) {
+			output = pid->limit_max;
+
+		} else if (output < pid->limit_min) {
+			output = pid->limit_min;
+		}
+
+		pid->last_output = output;
+	}
+
+	return pid->last_output;
+}
diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h
new file mode 100644
index 000000000..551c032fe
--- /dev/null
+++ b/src/modules/multirotor_pos_control/thrust_pid.h
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file thrust_pid.h
+ *
+ * Definition of thrust control PID interface.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef THRUST_PID_H_
+#define THRUST_PID_H_
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */
+#define THRUST_PID_MODE_DERIVATIV_CALC	0
+/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */
+#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP	1
+
+typedef struct {
+	float kp;
+	float ki;
+	float kd;
+	float sp;
+	float integral;
+	float error_previous;
+	float last_output;
+	float limit_min;
+	float limit_max;
+	float dt_min;
+	uint8_t mode;
+} thrust_pid_t;
+
+__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min);
+__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max);
+__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22);
+
+__END_DECLS
+
+#endif /* THRUST_PID_H_ */
diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c
new file mode 100644
index 000000000..13328edb4
--- /dev/null
+++ b/src/modules/position_estimator_inav/inertial_filter.c
@@ -0,0 +1,31 @@
+/*
+ * inertial_filter.c
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ */
+
+#include "inertial_filter.h"
+
+void inertial_filter_predict(float dt, float x[3])
+{
+	x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
+	x[1] += x[2] * dt;
+}
+
+void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
+{
+	float ewdt = w * dt;
+	if (ewdt > 1.0f)
+		ewdt = 1.0f;	// prevent over-correcting
+	ewdt *= e;
+	x[i] += ewdt;
+
+	if (i == 0) {
+		x[1] += w * ewdt;
+		x[2] += w * w * ewdt / 3.0;
+
+	} else if (i == 1) {
+		x[2] += w * ewdt;
+	}
+}
diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h
new file mode 100644
index 000000000..761c17097
--- /dev/null
+++ b/src/modules/position_estimator_inav/inertial_filter.h
@@ -0,0 +1,13 @@
+/*
+ * inertial_filter.h
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ */
+
+#include <stdbool.h>
+#include <drivers/drv_hrt.h>
+
+void inertial_filter_predict(float dt, float x[3]);
+
+void inertial_filter_correct(float e, float dt, float x[3], int i, float w);
diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk
new file mode 100644
index 000000000..939d76849
--- /dev/null
+++ b/src/modules/position_estimator_inav/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Makefile to build position_estimator_inav
+#
+
+MODULE_COMMAND	 	= position_estimator_inav
+SRCS		 	= position_estimator_inav_main.c \
+			position_estimator_inav_params.c \
+			inertial_filter.c
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
new file mode 100644
index 000000000..d4b2f0e43
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -0,0 +1,576 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 position_estimator_inav_main.c
+ * Model-identification based position estimator for multirotors
+ */
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <float.h>
+#include <string.h>
+#include <nuttx/config.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/optical_flow.h>
+#include <mavlink/mavlink_log.h>
+#include <poll.h>
+#include <systemlib/err.h>
+#include <systemlib/geo/geo.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/conversions.h>
+#include <drivers/drv_hrt.h>
+
+#include "position_estimator_inav_params.h"
+#include "inertial_filter.h"
+
+static bool thread_should_exit = false; /**< Deamon exit flag */
+static bool thread_running = false; /**< Deamon status flag */
+static int position_estimator_inav_task; /**< Handle of deamon task / thread */
+static bool verbose_mode = false;
+
+__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
+
+int position_estimator_inav_thread_main(int argc, char *argv[]);
+
+static void usage(const char *reason);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr,
+		"usage: position_estimator_inav {start|stop|status} [-v]\n\n");
+	exit(1);
+}
+
+/**
+ * The position_estimator_inav_thread 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_create().
+ */
+int position_estimator_inav_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+		if (thread_running) {
+			printf("position_estimator_inav already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		verbose_mode = false;
+		if (argc > 1)
+			if (!strcmp(argv[2], "-v"))
+				verbose_mode = true;
+
+		thread_should_exit = false;
+		position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
+					       SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096,
+					       position_estimator_inav_thread_main,
+					       (argv) ? (const char **) &argv[2] : (const char **) NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tposition_estimator_inav is running\n");
+
+		} else {
+			printf("\tposition_estimator_inav not started\n");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+/****************************************************************************
+ * main
+ ****************************************************************************/
+int position_estimator_inav_thread_main(int argc, char *argv[])
+{
+	warnx("started.");
+	int mavlink_fd;
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd, "[inav] started");
+
+	/* initialize values */
+	float x_est[3] = { 0.0f, 0.0f, 0.0f };
+	float y_est[3] = { 0.0f, 0.0f, 0.0f };
+	float z_est[3] = { 0.0f, 0.0f, 0.0f };
+
+	int baro_init_cnt = 0;
+	int baro_init_num = 200;
+	float baro_alt0 = 0.0f; /* to determine while start up */
+
+	double lat_current = 0.0f; //[°] --> 47.0
+	double lon_current = 0.0f; //[°] --> 8.5
+	double alt_current = 0.0f; //[m] above MSL
+
+	uint32_t accel_counter = 0;
+	uint32_t baro_counter = 0;
+
+	/* declare and safely initialize all structs */
+	struct vehicle_status_s vehicle_status;
+	memset(&vehicle_status, 0, sizeof(vehicle_status));
+	/* make sure that baroINITdone = false */
+	struct sensor_combined_s sensor;
+	memset(&sensor, 0, sizeof(sensor));
+	struct vehicle_gps_position_s gps;
+	memset(&gps, 0, sizeof(gps));
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+	struct vehicle_local_position_s local_pos;
+	memset(&local_pos, 0, sizeof(local_pos));
+	struct optical_flow_s flow;
+	memset(&flow, 0, sizeof(flow));
+	struct vehicle_global_position_s global_pos;
+	memset(&global_pos, 0, sizeof(global_pos));
+
+	/* subscribe */
+	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
+	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+
+	/* advertise */
+	orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
+	orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+
+	struct position_estimator_inav_params params;
+	struct position_estimator_inav_param_handles pos_inav_param_handles;
+	/* initialize parameter handles */
+	parameters_init(&pos_inav_param_handles);
+
+	/* first parameters read at start up */
+	struct parameter_update_s param_update;
+	orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_update); /* read from param topic to clear updated flag */
+	/* first parameters update */
+	parameters_update(&pos_inav_param_handles, &params);
+
+	struct pollfd fds_init[2] = {
+		{ .fd = sensor_combined_sub, .events = POLLIN },
+		{ .fd = vehicle_gps_position_sub, .events = POLLIN }
+	};
+
+	/* wait for initial sensors values: baro, GPS fix, only then can we initialize the projection */
+	bool wait_gps = params.use_gps;
+	bool wait_baro = true;
+	hrt_abstime wait_gps_start = 0;
+	const hrt_abstime wait_gps_delay = 5000000;	// wait for 5s after 3D fix
+
+	thread_running = true;
+
+	while ((wait_gps || wait_baro) && !thread_should_exit) {
+		int ret = poll(fds_init, params.use_gps ? 2 : 1, 1000);
+
+		if (ret < 0) {
+			/* poll error */
+			errx(1, "subscriptions poll error on init.");
+
+		} else if (ret > 0) {
+			if (fds_init[0].revents & POLLIN) {
+				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+
+				if (wait_baro && sensor.baro_counter > baro_counter) {
+					baro_counter = sensor.baro_counter;
+
+					/* mean calculation over several measurements */
+					if (baro_init_cnt < baro_init_num) {
+						baro_alt0 += sensor.baro_alt_meter;
+						baro_init_cnt++;
+
+					} else {
+						wait_baro = false;
+						baro_alt0 /= (float) baro_init_cnt;
+						warnx("init baro: alt = %.3f", baro_alt0);
+						mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0);
+						local_pos.home_alt = baro_alt0;
+						local_pos.home_timestamp = hrt_absolute_time();
+					}
+				}
+			}
+
+			if (params.use_gps && (fds_init[1].revents & POLLIN)) {
+				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
+
+				if (wait_gps && gps.fix_type >= 3) {
+					hrt_abstime t = hrt_absolute_time();
+
+					if (wait_gps_start == 0) {
+						wait_gps_start = t;
+
+					} else if (t - wait_gps_start > wait_gps_delay) {
+						wait_gps = false;
+						/* get GPS position for first initialization */
+						lat_current = gps.lat * 1e-7;
+						lon_current = gps.lon * 1e-7;
+						alt_current = gps.alt * 1e-3;
+
+						local_pos.home_lat = lat_current * 1e7;
+						local_pos.home_lon = lon_current * 1e7;
+						local_pos.home_hdg = 0.0f;
+						local_pos.home_timestamp = t;
+
+						/* initialize coordinates */
+						map_projection_init(lat_current, lon_current);
+						warnx("init GPS: lat = %.10f,  lon = %.10f", lat_current, lon_current);
+						mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current);
+					}
+				}
+			}
+		}
+	}
+
+	hrt_abstime t_prev = 0;
+
+	uint16_t accel_updates = 0;
+	uint16_t baro_updates = 0;
+	uint16_t gps_updates = 0;
+	uint16_t attitude_updates = 0;
+	uint16_t flow_updates = 0;
+
+	hrt_abstime updates_counter_start = hrt_absolute_time();
+	uint32_t updates_counter_len = 1000000;
+
+	hrt_abstime pub_last = hrt_absolute_time();
+	uint32_t pub_interval = 4000;	// limit publish rate to 250 Hz
+
+	/* acceleration in NED frame */
+	float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
+
+	/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
+	float accel_corr[] = { 0.0f, 0.0f, 0.0f };	// N E D
+	float accel_bias[] = { 0.0f, 0.0f, 0.0f };	// body frame
+	float baro_corr = 0.0f;		// D
+	float gps_corr[2][2] = {
+		{ 0.0f, 0.0f },		// N (pos, vel)
+		{ 0.0f, 0.0f },		// E (pos, vel)
+	};
+	float sonar_corr = 0.0f;
+	float sonar_corr_filtered = 0.0f;
+	float flow_corr[] = { 0.0f, 0.0f };	// X, Y
+
+	float sonar_prev = 0.0f;
+	hrt_abstime sonar_time = 0;
+
+	/* main loop */
+	struct pollfd fds[6] = {
+		{ .fd = parameter_update_sub, .events = POLLIN },
+		{ .fd = vehicle_status_sub, .events = POLLIN },
+		{ .fd = vehicle_attitude_sub, .events = POLLIN },
+		{ .fd = sensor_combined_sub, .events = POLLIN },
+		{ .fd = optical_flow_sub, .events = POLLIN },
+		{ .fd = vehicle_gps_position_sub, .events = POLLIN }
+	};
+
+	if (!thread_should_exit) {
+		warnx("main loop started.");
+	}
+
+	while (!thread_should_exit) {
+		int ret = poll(fds, params.use_gps ? 6 : 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate
+		hrt_abstime t = hrt_absolute_time();
+
+		if (ret < 0) {
+			/* poll error */
+			warnx("subscriptions poll error.");
+			thread_should_exit = true;
+			continue;
+
+		} else if (ret > 0) {
+			/* parameter update */
+			if (fds[0].revents & POLLIN) {
+				/* read from param to clear updated flag */
+				struct parameter_update_s update;
+				orb_copy(ORB_ID(parameter_update), parameter_update_sub,
+					 &update);
+				/* update parameters */
+				parameters_update(&pos_inav_param_handles, &params);
+			}
+
+			/* vehicle status */
+			if (fds[1].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_status), vehicle_status_sub,
+					 &vehicle_status);
+			}
+
+			/* vehicle attitude */
+			if (fds[2].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+				attitude_updates++;
+			}
+
+			/* sensor combined */
+			if (fds[3].revents & POLLIN) {
+				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+
+				if (sensor.accelerometer_counter > accel_counter) {
+					if (att.R_valid) {
+						/* correct accel bias, now only for Z */
+						sensor.accelerometer_m_s2[2] -= accel_bias[2];
+						/* transform acceleration vector from body frame to NED frame */
+						for (int i = 0; i < 3; i++) {
+							accel_NED[i] = 0.0f;
+
+							for (int j = 0; j < 3; j++) {
+								accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
+							}
+						}
+
+						accel_corr[0] = accel_NED[0] - x_est[2];
+						accel_corr[1] = accel_NED[1] - y_est[2];
+						accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
+
+					} else {
+						memset(accel_corr, 0, sizeof(accel_corr));
+					}
+
+					accel_counter = sensor.accelerometer_counter;
+					accel_updates++;
+				}
+
+				if (sensor.baro_counter > baro_counter) {
+					baro_corr = - sensor.baro_alt_meter - z_est[0];
+					baro_counter = sensor.baro_counter;
+					baro_updates++;
+				}
+			}
+
+			/* optical flow */
+			if (fds[4].revents & POLLIN) {
+				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
+
+				if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) {
+					if (flow.ground_distance_m != sonar_prev) {
+						sonar_time = t;
+						sonar_prev = flow.ground_distance_m;
+						sonar_corr = -flow.ground_distance_m - z_est[0];
+						sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt;
+
+						if (fabsf(sonar_corr) > params.sonar_err) {
+							// correction is too large: spike or new ground level?
+							if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) {
+								// spike detected, ignore
+								sonar_corr = 0.0f;
+
+							} else {
+								// new ground level
+								baro_alt0 += sonar_corr;
+								warnx("new home: alt = %.3f", baro_alt0);
+								mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
+								local_pos.home_alt = baro_alt0;
+								local_pos.home_timestamp = hrt_absolute_time();
+								z_est[0] += sonar_corr;
+								sonar_corr = 0.0f;
+								sonar_corr_filtered = 0.0f;
+							}
+						}
+					}
+
+				} else {
+					sonar_corr = 0.0f;
+				}
+
+				flow_updates++;
+			}
+
+			if (params.use_gps && (fds[5].revents & POLLIN)) {
+				/* vehicle GPS position */
+				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
+
+				if (gps.fix_type >= 3) {
+					/* project GPS lat lon to plane */
+					float gps_proj[2];
+					map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
+					gps_corr[0][0] = gps_proj[0] - x_est[0];
+					gps_corr[1][0] = gps_proj[1] - y_est[0];
+
+					if (gps.vel_ned_valid) {
+						gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
+						gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
+
+					} else {
+						gps_corr[0][1] = 0.0f;
+						gps_corr[1][1] = 0.0f;
+					}
+
+					gps_updates++;
+
+				} else {
+					memset(gps_corr, 0, sizeof(gps_corr));
+				}
+			}
+		}
+
+		/* end of poll return value check */
+
+		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
+		t_prev = t;
+
+		/* accel bias correction, now only for Z
+		 * not strictly correct, but stable and works */
+		accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt;
+
+		/* inertial filter prediction for altitude */
+		inertial_filter_predict(dt, z_est);
+
+		/* inertial filter correction for altitude */
+		baro_alt0 += sonar_corr * params.w_alt_sonar * dt;
+		inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro);
+		inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar);
+		inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
+
+		/* dont't try to estimate position when no any position source available */
+		bool can_estimate_pos = params.use_gps && gps.fix_type >= 3;
+
+		if (can_estimate_pos) {
+			/* inertial filter prediction for position */
+			inertial_filter_predict(dt, x_est);
+			inertial_filter_predict(dt, y_est);
+
+			/* inertial filter correction for position */
+			inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc);
+			inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc);
+
+			if (params.use_gps && gps.fix_type >= 3) {
+				inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
+				inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
+
+				if (gps.vel_ned_valid) {
+					inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
+					inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
+				}
+			}
+		}
+
+		if (verbose_mode) {
+			/* print updates rate */
+			if (t - updates_counter_start > updates_counter_len) {
+				float updates_dt = (t - updates_counter_start) * 0.000001f;
+				warnx(
+					"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
+					accel_updates / updates_dt,
+					baro_updates / updates_dt,
+					gps_updates / updates_dt,
+					attitude_updates / updates_dt,
+					flow_updates / updates_dt);
+				updates_counter_start = t;
+				accel_updates = 0;
+				baro_updates = 0;
+				gps_updates = 0;
+				attitude_updates = 0;
+				flow_updates = 0;
+			}
+		}
+
+		if (t - pub_last > pub_interval) {
+			pub_last = t;
+			local_pos.timestamp = t;
+			local_pos.valid = can_estimate_pos;
+			local_pos.x = x_est[0];
+			local_pos.vx = x_est[1];
+			local_pos.y = y_est[0];
+			local_pos.vy = y_est[1];
+			local_pos.z = z_est[0];
+			local_pos.vz = z_est[1];
+			local_pos.absolute_alt = local_pos.home_alt - local_pos.z;
+			local_pos.hdg = att.yaw;
+
+			if ((isfinite(local_pos.x)) && (isfinite(local_pos.vx))
+			    && (isfinite(local_pos.y))
+			    && (isfinite(local_pos.vy))
+			    && (isfinite(local_pos.z))
+			    && (isfinite(local_pos.vz))) {
+				orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
+
+				if (params.use_gps) {
+					global_pos.valid = local_pos.valid;
+					double est_lat, est_lon;
+					map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
+					global_pos.lat = (int32_t)(est_lat * 1e7);
+					global_pos.lon = (int32_t)(est_lon * 1e7);
+					global_pos.alt = local_pos.home_alt - local_pos.z;
+					global_pos.relative_alt = -local_pos.z;
+					global_pos.vx = local_pos.vx;
+					global_pos.vy = local_pos.vy;
+					global_pos.vz = local_pos.vz;
+					global_pos.hdg = local_pos.hdg;
+
+					orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
+				}
+			}
+		}
+	}
+
+	warnx("exiting.");
+	mavlink_log_info(mavlink_fd, "[inav] exiting");
+	thread_running = false;
+	return 0;
+}
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
new file mode 100644
index 000000000..70248b3b7
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 position_estimator_inav_params.c
+ *
+ * Parameters for position_estimator_inav
+ */
+
+#include "position_estimator_inav_params.h"
+
+PARAM_DEFINE_INT32(INAV_USE_GPS, 1);
+PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f);
+PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f);
+PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
+
+int parameters_init(struct position_estimator_inav_param_handles *h)
+{
+	h->use_gps = param_find("INAV_USE_GPS");
+	h->w_alt_baro = param_find("INAV_W_ALT_BARO");
+	h->w_alt_acc = param_find("INAV_W_ALT_ACC");
+	h->w_alt_sonar = param_find("INAV_W_ALT_SONAR");
+	h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P");
+	h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V");
+	h->w_pos_acc = param_find("INAV_W_POS_ACC");
+	h->w_pos_flow = param_find("INAV_W_POS_FLOW");
+	h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
+	h->flow_k = param_find("INAV_FLOW_K");
+	h->sonar_filt = param_find("INAV_SONAR_FILT");
+	h->sonar_err = param_find("INAV_SONAR_ERR");
+
+	return OK;
+}
+
+int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
+{
+	param_get(h->use_gps, &(p->use_gps));
+	param_get(h->w_alt_baro, &(p->w_alt_baro));
+	param_get(h->w_alt_acc, &(p->w_alt_acc));
+	param_get(h->w_alt_sonar, &(p->w_alt_sonar));
+	param_get(h->w_pos_gps_p, &(p->w_pos_gps_p));
+	param_get(h->w_pos_gps_v, &(p->w_pos_gps_v));
+	param_get(h->w_pos_acc, &(p->w_pos_acc));
+	param_get(h->w_pos_flow, &(p->w_pos_flow));
+	param_get(h->w_acc_bias, &(p->w_acc_bias));
+	param_get(h->flow_k, &(p->flow_k));
+	param_get(h->sonar_filt, &(p->sonar_filt));
+	param_get(h->sonar_err, &(p->sonar_err));
+
+	return OK;
+}
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
new file mode 100644
index 000000000..1e70a3c48
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 position_estimator_inav_params.h
+ *
+ * Parameters for Position Estimator
+ */
+
+#include <systemlib/param/param.h>
+
+struct position_estimator_inav_params {
+	int use_gps;
+	float w_alt_baro;
+	float w_alt_acc;
+	float w_alt_sonar;
+	float w_pos_gps_p;
+	float w_pos_gps_v;
+	float w_pos_acc;
+	float w_pos_flow;
+	float w_acc_bias;
+	float flow_k;
+	float sonar_filt;
+	float sonar_err;
+};
+
+struct position_estimator_inav_param_handles {
+	param_t use_gps;
+	param_t w_alt_baro;
+	param_t w_alt_acc;
+	param_t w_alt_sonar;
+	param_t w_pos_gps_p;
+	param_t w_pos_gps_v;
+	param_t w_pos_acc;
+	param_t w_pos_flow;
+	param_t w_acc_bias;
+	param_t flow_k;
+	param_t sonar_filt;
+	param_t sonar_err;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct position_estimator_inav_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p);
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index fefa539f9..30dc7df9e 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -60,7 +60,6 @@
 #include <drivers/drv_hrt.h>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -76,7 +75,7 @@
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/vehicle_control_debug.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
@@ -96,7 +95,6 @@
 		log_msgs_written++; \
 	} else { \
 		log_msgs_skipped++; \
-		/*printf("skip\n");*/ \
 	}
 
 #define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \
@@ -104,9 +102,6 @@
 	fds[fdsc_count].events = POLLIN; \
 	fdsc_count++;
 
-
-//#define SDLOG2_DEBUG
-
 static bool main_thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;			/**< Deamon status flag */
 static int deamon_task;						/**< Handle of deamon task / thread */
@@ -194,7 +189,7 @@ static int file_copy(const char *file_old, const char *file_new);
 
 static void handle_command(struct vehicle_command_s *cmd);
 
-static void handle_status(struct actuator_armed_s *armed);
+static void handle_status(struct vehicle_status_s *cmd);
 
 /**
  * Create folder for current logging session. Store folder name in 'log_folder'.
@@ -235,7 +230,7 @@ int sdlog2_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("sdlog2 already running\n");
+			warnx("already running");
 			/* this is not an error */
 			exit(0);
 		}
@@ -252,7 +247,7 @@ int sdlog2_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "stop")) {
 		if (!thread_running) {
-			printf("\tsdlog2 is not started\n");
+			warnx("not started");
 		}
 
 		main_thread_should_exit = true;
@@ -264,7 +259,7 @@ int sdlog2_main(int argc, char *argv[])
 			sdlog2_status();
 
 		} else {
-			printf("\tsdlog2 not started\n");
+			warnx("not started\n");
 		}
 
 		exit(0);
@@ -389,11 +384,6 @@ static void *logwriter_thread(void *arg)
 		/* only get pointer to thread-safe data, do heavy I/O a few lines down */
 		int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part);
 
-#ifdef SDLOG2_DEBUG
-		int rp = logbuf->read_ptr;
-		int wp = logbuf->write_ptr;
-#endif
-
 		/* continue */
 		pthread_mutex_unlock(&logbuffer_mutex);
 
@@ -409,9 +399,6 @@ static void *logwriter_thread(void *arg)
 			n = write(log_file, read_ptr, n);
 
 			should_wait = (n == available) && !is_part;
-#ifdef SDLOG2_DEBUG
-			printf("write %i %i of %i rp=%i wp=%i, is_part=%i, should_wait=%i\n", log_bytes_written, n, available, rp, wp, (int)is_part, (int)should_wait);
-#endif
 
 			if (n < 0) {
 				main_thread_should_exit = true;
@@ -424,14 +411,8 @@ static void *logwriter_thread(void *arg)
 
 		} else {
 			n = 0;
-#ifdef SDLOG2_DEBUG
-			printf("no data available, main_thread_should_exit=%i, logwriter_should_exit=%i\n", (int)main_thread_should_exit, (int)logwriter_should_exit);
-#endif
 			/* exit only with empty buffer */
 			if (main_thread_should_exit || logwriter_should_exit) {
-#ifdef SDLOG2_DEBUG
-				printf("break logwriter thread\n");
-#endif
 				break;
 			}
 			should_wait = true;
@@ -446,10 +427,6 @@ static void *logwriter_thread(void *arg)
 	fsync(log_file);
 	close(log_file);
 
-#ifdef SDLOG2_DEBUG
-	printf("logwriter thread exit\n");
-#endif
-
 	return OK;
 }
 
@@ -606,15 +583,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 		errx(1, "unable to create logging folder, exiting.");
 	}
 
-	const char *converter_in = "/etc/logging/conv.zip";
-	char* converter_out = malloc(150);
-	sprintf(converter_out, "%s/conv.zip", folder_path);
-
-	if (file_copy(converter_in, converter_out)) {
-		errx(1, "unable to copy conversion scripts, exiting.");
-	}
-	free(converter_out);
-
 	/* only print logging path, important to find log file later */
 	warnx("logging to directory: %s", folder_path);
 
@@ -625,21 +593,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 		errx(1, "can't allocate log buffer, exiting.");
 	}
 
-	/* --- IMPORTANT: DEFINE MAX NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
-	/* max number of messages */
-	const ssize_t fdsc = 21;
-
-	/* Sanity check variable and index */
-	ssize_t fdsc_count = 0;
-	/* file descriptors to wait for */
-	struct pollfd fds[fdsc];
-
 	struct vehicle_status_s buf_status;
 	memset(&buf_status, 0, sizeof(buf_status));
 
-	struct actuator_armed_s buf_armed;
-	memset(&buf_armed, 0, sizeof(buf_armed));
-
 	/* warning! using union here to save memory, elements should be used separately! */
 	union {
 		struct vehicle_command_s cmd;
@@ -656,19 +612,18 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_global_position_setpoint_s global_pos_sp;
 		struct vehicle_gps_position_s gps_pos;
 		struct vehicle_vicon_position_s vicon_pos;
-		struct vehicle_control_debug_s control_debug;
 		struct optical_flow_s flow;
 		struct rc_channels_s rc;
 		struct differential_pressure_s diff_pres;
 		struct airspeed_s airspeed;
 		struct esc_status_s esc;
+		struct vehicle_global_velocity_setpoint_s global_vel_sp;
 	} buf;
 	memset(&buf, 0, sizeof(buf));
 
 	struct {
 		int cmd_sub;
 		int status_sub;
-		int armed_sub;
 		int sensor_sub;
 		int att_sub;
 		int att_sp_sub;
@@ -682,11 +637,11 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int global_pos_sp_sub;
 		int gps_pos_sub;
 		int vicon_pos_sub;
-		int control_debug_sub;
 		int flow_sub;
 		int rc_sub;
 		int airspeed_sub;
 		int esc_sub;
+		int global_vel_sp_sub;
 	} subs;
 
 	/* log message buffer: header + body */
@@ -704,7 +659,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GPS_s log_GPS;
 			struct log_ATTC_s log_ATTC;
 			struct log_STAT_s log_STAT;
-			struct log_CTRL_s log_CTRL;
 			struct log_RC_s log_RC;
 			struct log_OUT0_s log_OUT0;
 			struct log_AIRS_s log_AIRS;
@@ -713,6 +667,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GPOS_s log_GPOS;
 			struct log_GPSP_s log_GPSP;
 			struct log_ESC_s log_ESC;
+			struct log_GVSP_s log_GVSP;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -720,18 +675,20 @@ int sdlog2_thread_main(int argc, char *argv[])
 #pragma pack(pop)
 	memset(&log_msg.body, 0, sizeof(log_msg.body));
 
+	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
+	/* number of messages */
+	const ssize_t fdsc = 20;
+	/* Sanity check variable and index */
+	ssize_t fdsc_count = 0;
+	/* file descriptors to wait for */
+	struct pollfd fds[fdsc];
+
 	/* --- VEHICLE COMMAND --- */
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
 	fds[fdsc_count].fd = subs.cmd_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
-	/* --- ACTUATOR ARMED --- */
-	subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-	fds[fdsc_count].fd = subs.armed_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
 	/* --- VEHICLE STATUS --- */
 	subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	fds[fdsc_count].fd = subs.status_sub;
@@ -816,12 +773,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
-	/* --- CONTROL DEBUG --- */
-	subs.control_debug_sub = orb_subscribe(ORB_ID(vehicle_control_debug));
-	fds[fdsc_count].fd = subs.control_debug_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
 	/* --- OPTICAL FLOW --- */
 	subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
 	fds[fdsc_count].fd = subs.flow_sub;
@@ -846,6 +797,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- GLOBAL VELOCITY SETPOINT --- */
+	subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
+	fds[fdsc_count].fd = subs.global_vel_sp_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* WARNING: If you get the error message below,
 	 * then the number of registered messages (fdsc)
 	 * differs from the number of messages in the above list.
@@ -905,33 +862,22 @@ int sdlog2_thread_main(int argc, char *argv[])
 				handled_topics++;
 			}
 
-			/* --- ARMED- LOG MANAGEMENT --- */
+			/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
 			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(actuator_armed), subs.armed_sub, &buf_armed);
+				orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
 
 				if (log_when_armed) {
-					handle_status(&buf_armed);
+					handle_status(&buf_status);
 				}
 
 				handled_topics++;
 			}
 
-			/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
-
-				//if (log_when_armed) {
-				//	handle_status(&buf_armed);
-				//}
-
-				//handled_topics++;
-			}
-
 			if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
 				continue;
 			}
 
-			ifds = 2;	// Begin from fds[2] again
+			ifds = 1;	// Begin from fds[1] again
 
 			pthread_mutex_lock(&logbuffer_mutex);
 
@@ -944,16 +890,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 			if (fds[ifds++].revents & POLLIN) {
 				// Don't orb_copy, it's already done few lines above
 				log_msg.msg_type = LOG_STAT_MSG;
-				// XXX fix this
-				// log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine;
-				// log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode;
-				// log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode;
-				// log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode;
-				log_msg.body.log_STAT.state = 0;
-				log_msg.body.log_STAT.flight_mode = 0;
-				log_msg.body.log_STAT.manual_control_mode = 0;
-				log_msg.body.log_STAT.manual_sas_mode = 0;
-				log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */
+				log_msg.body.log_STAT.main_state = (unsigned char) buf_status.main_state;
+				log_msg.body.log_STAT.navigation_state = (unsigned char) buf_status.navigation_state;
+				log_msg.body.log_STAT.arming_state = (unsigned char) buf_status.arming_state;
 				log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
 				log_msg.body.log_STAT.battery_current = buf_status.battery_current;
 				log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
@@ -1044,9 +983,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
 				log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
 				log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
-				log_msg.body.log_ATT.roll_acc = buf.att.rollacc;
-				log_msg.body.log_ATT.pitch_acc = buf.att.pitchacc;
-				log_msg.body.log_ATT.yaw_acc = buf.att.yawacc;
 				LOGBUFFER_WRITE_AND_COUNT(ATT);
 			}
 
@@ -1162,27 +1098,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 				// TODO not implemented yet
 			}
 
-			/* --- CONTROL DEBUG --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug);
-
-				log_msg.msg_type = LOG_CTRL_MSG;
-				log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p;
-				log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i;
-				log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d;
-				log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p;
-				log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i;
-				log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d;
-				log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p;
-				log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i;
-				log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d;
-				log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p;
-				log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i;
-				log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d;
-
-				LOGBUFFER_WRITE_AND_COUNT(CTRL);
-			}
-
 			/* --- FLOW --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
@@ -1237,14 +1152,18 @@ int sdlog2_thread_main(int argc, char *argv[])
 				}
 			}
 
-#ifdef SDLOG2_DEBUG
-				printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
-#endif
+			/* --- GLOBAL VELOCITY SETPOINT --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp);
+				log_msg.msg_type = LOG_GVSP_MSG;
+				log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx;
+				log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy;
+				log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz;
+				LOGBUFFER_WRITE_AND_COUNT(GVSP);
+			}
+
 			/* signal the other thread new data, but not yet unlock */
 			if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
-#ifdef SDLOG2_DEBUG
-				printf("signal rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
-#endif
 				/* only request write if several packets can be written at once */
 				pthread_cond_signal(&logbuffer_cond);
 			}
@@ -1327,7 +1246,7 @@ int file_copy(const char *file_old, const char *file_new)
 	fclose(source);
 	fclose(target);
 
-	return OK;
+	return ret;
 }
 
 void handle_command(struct vehicle_command_s *cmd)
@@ -1357,10 +1276,12 @@ void handle_command(struct vehicle_command_s *cmd)
 	}
 }
 
-void handle_status(struct actuator_armed_s *armed)
+void handle_status(struct vehicle_status_s *status)
 {
-	if (armed->armed != flag_system_armed) {
-		flag_system_armed = armed->armed;
+	// TODO use flag from actuator_armed here?
+	bool armed = status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR;
+	if (armed != flag_system_armed) {
+		flag_system_armed = armed;
 
 		if (flag_system_armed) {
 			sdlog2_start_log();
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 9be96a62e..38e2596b2 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -63,9 +63,6 @@ struct log_ATT_s {
 	float roll_rate;
 	float pitch_rate;
 	float yaw_rate;
-	float roll_acc;
-	float pitch_acc;
-	float yaw_acc;
 };
 
 /* --- ATSP - ATTITUDE SET POINT --- */
@@ -152,55 +149,36 @@ struct log_ATTC_s {
 /* --- STAT - VEHICLE STATE --- */
 #define LOG_STAT_MSG 10
 struct log_STAT_s {
-	uint8_t state;
-	uint8_t flight_mode;
-	uint8_t manual_control_mode;
-	uint8_t manual_sas_mode;
-	uint8_t armed;
+	uint8_t main_state;
+	uint8_t navigation_state;
+	uint8_t arming_state;
 	float battery_voltage;
 	float battery_current;
 	float battery_remaining;
 	uint8_t battery_warning;
 };
 
-/* --- CTRL - CONTROL DEBUG --- */
-#define LOG_CTRL_MSG 11
-struct log_CTRL_s {
-	float roll_p;
-	float roll_i;
-	float roll_d;
-	float roll_rate_p;
-	float roll_rate_i;
-	float roll_rate_d;
-	float pitch_p;
-	float pitch_i;
-	float pitch_d;
-	float pitch_rate_p;
-	float pitch_rate_i;
-	float pitch_rate_d;
-};
-
 /* --- RC - RC INPUT CHANNELS --- */
-#define LOG_RC_MSG 12
+#define LOG_RC_MSG 11
 struct log_RC_s {
 	float channel[8];
 };
 
 /* --- OUT0 - ACTUATOR_0 OUTPUT --- */
-#define LOG_OUT0_MSG 13
+#define LOG_OUT0_MSG 12
 struct log_OUT0_s {
 	float output[8];
 };
 
 /* --- AIRS - AIRSPEED --- */
-#define LOG_AIRS_MSG 14
+#define LOG_AIRS_MSG 13
 struct log_AIRS_s {
 	float indicated_airspeed;
 	float true_airspeed;
 };
 
 /* --- ARSP - ATTITUDE RATE SET POINT --- */
-#define LOG_ARSP_MSG 15
+#define LOG_ARSP_MSG 14
 struct log_ARSP_s {
 	float roll_rate_sp;
 	float pitch_rate_sp;
@@ -208,7 +186,7 @@ struct log_ARSP_s {
 };
 
 /* --- FLOW - OPTICAL FLOW --- */
-#define LOG_FLOW_MSG 16
+#define LOG_FLOW_MSG 15
 struct log_FLOW_s {
 	int16_t flow_raw_x;
 	int16_t flow_raw_y;
@@ -264,13 +242,21 @@ struct log_ESC_s {
 	uint16_t esc_setpoint_raw;
 };
 
+/* --- GVSP - GLOBAL VELOCITY SETPOINT --- */
+#define LOG_GVSP_MSG 19
+struct log_GVSP_s {
+	float vx;
+	float vy;
+	float vz;
+};
+
 #pragma pack(pop)
 
 /* construct list of all message formats */
 
 static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(TIME, "Q", "StartTime"),
-	LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollR,PitchR,YawR,RollA,PitchA,YawA"),
+	LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
 	LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
@@ -278,8 +264,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
-	LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"),
-	LOG_FORMAT(CTRL, "ffffffffffff", "RP,RI,RD,RRP,RRI,RRD,PP,PI,PD,PRP,PRI,PRD"),
+	LOG_FORMAT(STAT, "BBBfffB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn"),
 	LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
@@ -287,7 +272,8 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
 	LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
-	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
 };
 
 static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 9308100b0..4996a8f66 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -124,7 +124,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
  * @param dt
  * @return
  */
-__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d)
+__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
 {
 	/*  error = setpoint - actual_position
 	 integral = integral + (error*dt)
@@ -196,10 +196,6 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		pid->last_output = output;
 	}
 
-	*ctrl_p = (error * pid->kp);
-	*ctrl_i = (i * pid->ki);
-	*ctrl_d = (d * pid->kd);
-
 	return pid->last_output;
 }
 
diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h
index 5f2650f69..eca228464 100644
--- a/src/modules/systemlib/pid/pid.h
+++ b/src/modules/systemlib/pid/pid.h
@@ -85,7 +85,7 @@ typedef struct {
 __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min);
 __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit);
 //void pid_set(PID_t *pid, float sp);
-__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d);
+__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
 
 __EXPORT void pid_reset_integral(PID_t *pid);
 
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index ed77bb7ef..1ea06d419 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -116,6 +116,9 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp
 #include "topics/vehicle_global_position_set_triplet.h"
 ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s);
 
+#include "topics/vehicle_global_velocity_setpoint.h"
+ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
+
 #include "topics/mission.h"
 ORB_DEFINE(mission, struct mission_s);
 
diff --git a/src/modules/multirotor_pos_control/position_control.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
index 2144ebc34..73961cdfe 100644
--- a/src/modules/multirotor_pos_control/position_control.h
+++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
@@ -1,10 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,15 +33,32 @@
  ****************************************************************************/
 
 /**
- * @file multirotor_position_control.h
- * Definition of the position control for a multirotor VTOL
+ * @file vehicle_global_velocity_setpoint.h
+ * Definition of the global velocity setpoint uORB topic.
  */
 
-// #ifndef POSITION_CONTROL_H_
-// #define POSITION_CONTROL_H_
+#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
+#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
 
-// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
-//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
-//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
+#include "../uORB.h"
 
-// #endif /* POSITION_CONTROL_H_ */
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct vehicle_global_velocity_setpoint_s
+{
+	float vx;		/**< in m/s NED			  		*/
+	float vy;		/**< in m/s NED			  		*/
+	float vz;		/**< in m/s NED			  		*/
+}; /**< Velocity setpoint in NED frame */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_global_velocity_setpoint);
+
+#endif