Merged beta into paul_estimator

87 files changed, 9073 insertions, 5178 deletions

diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index ecca04dd7..668bac5d9 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -53,21 +53,6 @@ static const int8_t ret_error = -1; 	// error occurred
 
 KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
-	// ekf matrices
-	F(9, 9),
-	G(9, 6),
-	P(9, 9),
-	P0(9, 9),
-	V(6, 6),
-	// attitude measurement ekf matrices
-	HAtt(4, 9),
-	RAtt(4, 4),
-	// position measurement ekf matrices
-	HPos(6, 9),
-	RPos(6, 6),
-	// attitude representations
-	C_nb(),
-	q(),
 	// subscriptions
 	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
 	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
@@ -112,8 +97,17 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 {
 	using namespace math;
 
+	F.zero();
+	G.zero();
+	V.zero();
+	HAtt.zero();
+	RAtt.zero();
+	HPos.zero();
+	RPos.zero();
+
 	// initial state covariance matrix
-	P0 = Matrix::identity(9) * 0.01f;
+	P0.identity();
+	P0 *= 0.01f;
 	P = P0;
 
 	// initial state
@@ -138,7 +132,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 		_sensors.magnetometer_ga[2]);
 
 	// initialize dcm
-	C_nb = Dcm(q);
+	C_nb = q.to_dcm();
 
 	// HPos is constant
 	HPos(0, 3) = 1.0f;
@@ -228,8 +222,8 @@ void KalmanNav::update()
 		if (correctAtt() == ret_ok) _attitudeInitCounter++;
 
 		if (_attitudeInitCounter > 100) {
-			warnx("initialized EKF attitude\n");
-			warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
+			warnx("initialized EKF attitude");
+			warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f",
 			       double(phi), double(theta), double(psi));
 			_attitudeInitialized = true;
 		}
@@ -259,8 +253,8 @@ void KalmanNav::update()
 		// lat/lon and not have init
 		map_projection_init(lat0, lon0);
 		_positionInitialized = true;
-		warnx("initialized EKF state with GPS\n");
-		warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
+		warnx("initialized EKF state with GPS");
+		warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
 		       double(vN), double(vE), double(vD),
 		       lat, lon, double(alt));
 	}
@@ -320,14 +314,13 @@ void KalmanNav::updatePublications()
 	// global position publication
 	_pos.timestamp = _pubTimeStamp;
 	_pos.time_gps_usec = _gps.timestamp_position;
-	_pos.valid = true;
-	_pos.lat = getLatDegE7();
-	_pos.lon = getLonDegE7();
+	_pos.global_valid = true;
+	_pos.lat = lat * M_RAD_TO_DEG;
+	_pos.lon = lon * M_RAD_TO_DEG;
 	_pos.alt = float(alt);
-	_pos.relative_alt = float(alt); // TODO, make relative
-	_pos.vx = vN;
-	_pos.vy = vE;
-	_pos.vz = vD;
+	_pos.vel_n = vN;
+	_pos.vel_e = vE;
+	_pos.vel_d = vD;
 	_pos.yaw = psi;
 
 	// local position publication
@@ -404,28 +397,28 @@ int KalmanNav::predictState(float dt)
 
 	// attitude prediction
 	if (_attitudeInitialized) {
-		Vector3 w(_sensors.gyro_rad_s);
+		Vector<3> w(_sensors.gyro_rad_s);
 
 		// attitude
 		q = q + q.derivative(w) * dt;
 
 		// renormalize quaternion if needed
-		if (fabsf(q.norm() - 1.0f) > 1e-4f) {
-			q = q.unit();
+		if (fabsf(q.length() - 1.0f) > 1e-4f) {
+			q.normalize();
 		}
 
 		// C_nb update
-		C_nb = Dcm(q);
+		C_nb = q.to_dcm();
 
 		// euler update
-		EulerAngles euler(C_nb);
-		phi = euler.getPhi();
-		theta = euler.getTheta();
-		psi = euler.getPsi();
+		Vector<3> euler = C_nb.to_euler();
+		phi = euler.data[0];
+		theta = euler.data[1];
+		psi = euler.data[2];
 
 		// specific acceleration in nav frame
-		Vector3 accelB(_sensors.accelerometer_m_s2);
-		Vector3 accelN = C_nb * accelB;
+		Vector<3> accelB(_sensors.accelerometer_m_s2);
+		Vector<3> accelN = C_nb * accelB;
 		fN = accelN(0);
 		fE = accelN(1);
 		fD = accelN(2);
@@ -549,10 +542,10 @@ int KalmanNav::predictStateCovariance(float dt)
 	G(5, 4) = C_nb(2, 1);
 	G(5, 5) = C_nb(2, 2);
 
-	// continuous predictioon equations
-	// for discrte time EKF
+	// continuous prediction equations
+	// for discrete time EKF
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
+	P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt;
 
 	return ret_ok;
 }
@@ -577,13 +570,14 @@ int KalmanNav::correctAtt()
 
 	// compensate roll and pitch, but not yaw
 	// XXX take the vectors out of the C_nb matrix to avoid singularities
-	math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose();
+	math::Matrix<3,3> C_rp;
+	C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed();
 
 	// mag measurement
-	Vector3 magBody(_sensors.magnetometer_ga);
+	Vector<3> magBody(_sensors.magnetometer_ga);
 
 	// transform to earth frame
-	Vector3 magNav = C_rp * magBody;
+	Vector<3> magNav = C_rp * magBody;
 
 	// calculate error between estimate and measurement
 	// apply declination correction for true heading as well.
@@ -592,12 +586,12 @@ int KalmanNav::correctAtt()
 	if (yMag < -M_PI_F) yMag += 2*M_PI_F;
 
 	// accel measurement
-	Vector3 zAccel(_sensors.accelerometer_m_s2);
-	float accelMag = zAccel.norm();
-	zAccel = zAccel.unit();
+	Vector<3> zAccel(_sensors.accelerometer_m_s2);
+	float accelMag = zAccel.length();
+	zAccel.normalize();
 
 	// ignore accel correction when accel mag not close to g
-	Matrix RAttAdjust = RAtt;
+	Matrix<4,4> RAttAdjust = RAtt;
 
 	bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
 
@@ -611,14 +605,10 @@ int KalmanNav::correctAtt()
 	}
 
 	// accel predicted measurement
-	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
+	Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized();
 
 	// calculate residual
-	Vector y(4);
-	y(0) = yMag;
-	y(1) = zAccel(0) - zAccelHat(0);
-	y(2) = zAccel(1) - zAccelHat(1);
-	y(3) = zAccel(2) - zAccelHat(2);
+	Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2));
 
 	// HMag
 	HAtt(0, 2) = 1;
@@ -632,17 +622,17 @@ int KalmanNav::correctAtt()
 
 	// compute correction
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
-	Matrix K = P * HAtt.transpose() * S.inverse();
-	Vector xCorrect = K * y;
+	Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance
+	Matrix<9, 4> K = P * HAtt.transposed() * S.inversed();
+	Vector<9> xCorrect = K * y;
 
 	// check correciton is sane
-	for (size_t i = 0; i < xCorrect.getRows(); i++) {
+	for (size_t i = 0; i < xCorrect.get_size(); i++) {
 		float val = xCorrect(i);
 
 		if (isnan(val) || isinf(val)) {
 			// abort correction and return
-			warnx("numerical failure in att correction\n");
+			warnx("numerical failure in att correction");
 			// reset P matrix to P0
 			P = P0;
 			return ret_error;
@@ -669,7 +659,7 @@ int KalmanNav::correctAtt()
 	P = P - K * HAtt * P;
 
 	// fault detection
-	float beta = y.dot(S.inverse() * y);
+	float beta = y * (S.inversed() * y);
 
 	if (beta > _faultAtt.get()) {
 		warnx("fault in attitude: beta = %8.4f", (double)beta);
@@ -678,7 +668,7 @@ int KalmanNav::correctAtt()
 
 	// update quaternions from euler
 	// angle correction
-	q = Quaternion(EulerAngles(phi, theta, psi));
+	q.from_euler(phi, theta, psi);
 
 	return ret_ok;
 }
@@ -688,7 +678,7 @@ int KalmanNav::correctPos()
 	using namespace math;
 
 	// residual
-	Vector y(6);
+	Vector<6> y;
 	y(0) = _gps.vel_n_m_s - vN;
 	y(1) = _gps.vel_e_m_s - vE;
 	y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
@@ -698,17 +688,17 @@ int KalmanNav::correctPos()
 
 	// compute correction
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
-	Matrix K = P * HPos.transpose() * S.inverse();
-	Vector xCorrect = K * y;
+	Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance
+	Matrix<9,6> K = P * HPos.transposed() * S.inversed();
+	Vector<9> xCorrect = K * y;
 
 	// check correction is sane
-	for (size_t i = 0; i < xCorrect.getRows(); i++) {
+	for (size_t i = 0; i < xCorrect.get_size(); i++) {
 		float val = xCorrect(i);
 
 		if (!isfinite(val)) {
 			// abort correction and return
-			warnx("numerical failure in gps correction\n");
+			warnx("numerical failure in gps correction");
 			// fallback to GPS
 			vN = _gps.vel_n_m_s;
 			vE = _gps.vel_e_m_s;
@@ -735,7 +725,7 @@ int KalmanNav::correctPos()
 	P = P - K * HPos * P;
 
 	// fault detetcion
-	float beta = y.dot(S.inverse() * y);
+	float beta = y * (S.inversed() * y);
 
 	static int counter = 0;
 	if (beta > _faultPos.get() && (counter % 10 == 0)) {
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index a69bde1a6..46ee4b6c8 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -125,17 +125,17 @@ public:
 	virtual void updateParams();
 protected:
 	// kalman filter
-	math::Matrix F;             /**< Jacobian(f,x), where dx/dt = f(x,u) */
-	math::Matrix G;             /**< noise shaping matrix for gyro/accel */
-	math::Matrix P;             /**< state covariance matrix */
-	math::Matrix P0;            /**< initial state covariance matrix */
-	math::Matrix V;             /**< gyro/ accel noise matrix */
-	math::Matrix HAtt;          /**< attitude measurement matrix */
-	math::Matrix RAtt;          /**< attitude measurement noise matrix */
-	math::Matrix HPos;          /**< position measurement jacobian matrix */
-	math::Matrix RPos;          /**< position measurement noise matrix */
+	math::Matrix<9,9> F;             /**< Jacobian(f,x), where dx/dt = f(x,u) */
+	math::Matrix<9,6> G;             /**< noise shaping matrix for gyro/accel */
+	math::Matrix<9,9> P;             /**< state covariance matrix */
+	math::Matrix<9,9> P0;            /**< initial state covariance matrix */
+	math::Matrix<6,6> V;             /**< gyro/ accel noise matrix */
+	math::Matrix<4,9> HAtt;          /**< attitude measurement matrix */
+	math::Matrix<4,4> RAtt;          /**< attitude measurement noise matrix */
+	math::Matrix<6,9> HPos;          /**< position measurement jacobian matrix */
+	math::Matrix<6,6> RPos;          /**< position measurement noise matrix */
 	// attitude
-	math::Dcm C_nb;             /**< direction cosine matrix from body to nav frame */
+	math::Matrix<3,3> C_nb;             /**< direction cosine matrix from body to nav frame */
 	math::Quaternion q;         /**< quaternion from body to nav frame */
 	// subscriptions
 	control::UOrbSubscription<sensor_combined_s> _sensors;          /**< sensors sub. */
diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
index 372b2d162..3d20d4d2d 100644
--- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp
+++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
@@ -107,7 +107,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 30,
-					 4096,
+					 8192,
 					 kalman_demo_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 1741fab8b..3881bdf55 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -58,9 +58,13 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
+#include <lib/mathlib/mathlib.h>
+
 #include <systemlib/systemlib.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -214,6 +218,10 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 	struct sensor_combined_s raw;
 	memset(&raw, 0, sizeof(raw));
+	struct vehicle_gps_position_s gps;
+	memset(&gps, 0, sizeof(gps));
+	struct vehicle_global_position_s global_pos;
+	memset(&global_pos, 0, sizeof(global_pos));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 	struct vehicle_control_mode_s control_mode;
@@ -221,12 +229,32 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 	uint64_t last_data = 0;
 	uint64_t last_measurement = 0;
+	uint64_t last_vel_t = 0;
+
+	/* current velocity */
+	math::Vector<3> vel;
+	vel.zero();
+	/* previous velocity */
+	math::Vector<3> vel_prev;
+	vel_prev.zero();
+	/* actual acceleration (by GPS velocity) in body frame */
+	math::Vector<3> acc;
+	acc.zero();
+	/* rotation matrix */
+	math::Matrix<3, 3> R;
+	R.identity();
 
 	/* subscribe to raw data */
 	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
 	/* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
 	orb_set_interval(sub_raw, 3);
 
+	/* subscribe to GPS */
+	int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position));
+
+	/* subscribe to GPS */
+	int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position));
+
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
 
@@ -264,6 +292,10 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
 	unsigned offset_count = 0;
 
+	/* rotation matrix for magnetic declination */
+	math::Matrix<3, 3> R_decl;
+	R_decl.identity();
+
 	/* register the perf counter */
 	perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
 
@@ -298,6 +330,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 				/* update parameters */
 				parameters_update(&ekf_param_handles, &ekf_params);
+
+				/* update mag declination rotation matrix */
+				R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
 			}
 
 			/* only run filter if sensor values changed */
@@ -306,6 +341,18 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 				/* get latest measurements */
 				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
 
+				bool gps_updated;
+				orb_check(sub_gps, &gps_updated);
+				if (gps_updated) {
+					orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);
+				}
+
+				bool global_pos_updated;
+				orb_check(sub_global_pos, &global_pos_updated);
+				if (global_pos_updated) {
+					orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
+				}
+
 				if (!initialized) {
 					// XXX disabling init for now
 					initialized = true;
@@ -349,9 +396,50 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
 					}
 
-					z_k[3] = raw.accelerometer_m_s2[0];
-					z_k[4] = raw.accelerometer_m_s2[1];
-					z_k[5] = raw.accelerometer_m_s2[2];
+					hrt_abstime vel_t = 0;
+					bool vel_valid = false;
+					if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) {
+						vel_valid = true;
+						if (gps_updated) {
+							vel_t = gps.timestamp_velocity;
+							vel(0) = gps.vel_n_m_s;
+							vel(1) = gps.vel_e_m_s;
+							vel(2) = gps.vel_d_m_s;
+						}
+
+					} else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
+						vel_valid = true;
+						if (global_pos_updated) {
+							vel_t = global_pos.timestamp;
+							vel(0) = global_pos.vel_n;
+							vel(1) = global_pos.vel_e;
+							vel(2) = global_pos.vel_d;
+						}
+					}
+
+					if (vel_valid) {
+						/* velocity is valid */
+						if (vel_t != 0) {
+							/* velocity updated */
+							if (last_vel_t != 0 && vel_t != last_vel_t) {
+								float vel_dt = (vel_t - last_vel_t) / 1000000.0f;
+								/* calculate acceleration in body frame */
+								acc = R.transposed() * ((vel - vel_prev) / vel_dt);
+							}
+							last_vel_t = vel_t;
+							vel_prev = vel;
+						}
+
+					} else {
+						/* velocity is valid, reset acceleration */
+						acc.zero();
+						vel_prev.zero();
+						last_vel_t = 0;
+					}
+
+					z_k[3] = raw.accelerometer_m_s2[0] - acc(0);
+					z_k[4] = raw.accelerometer_m_s2[1] - acc(1);
+					z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
 
 					/* update magnetometer measurements */
 					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
@@ -421,7 +509,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						continue;
 					}
 
-					if (last_data > 0 && raw.timestamp - last_data > 12000)
+					if (last_data > 0 && raw.timestamp - last_data > 30000)
 						printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
 
 					last_data = raw.timestamp;
@@ -429,10 +517,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					/* send out */
 					att.timestamp = raw.timestamp;
 
-					// XXX Apply the same transformation to the rotation matrix
-					att.roll = euler[0] - ekf_params.roll_off;
-					att.pitch = euler[1] - ekf_params.pitch_off;
-					att.yaw = euler[2] - ekf_params.yaw_off;
+					att.roll = euler[0];
+					att.pitch = euler[1];
+					att.yaw = euler[2] + ekf_params.mag_decl;
 
 					att.rollspeed = x_aposteriori[0];
 					att.pitchspeed = x_aposteriori[1];
@@ -441,12 +528,20 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					att.pitchacc = x_aposteriori[4];
 					att.yawacc = x_aposteriori[5];
 
-					//att.yawspeed =z_k[2] ;
+					att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
+					att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
+					att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);
+
 					/* copy offsets */
 					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
 
+					/* magnetic declination */
+
+					math::Matrix<3, 3> R_body = (&Rot_matrix[0]);
+					R = R_decl * R_body;
+
 					/* copy rotation matrix */
-					memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
+					memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
 					att.R_valid = true;
 
 					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index 3cfddf28e..4154e3db4 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -65,6 +65,11 @@ PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
 PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f);
 PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
 
+/* magnetic declination, in degrees */
+PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
+
+PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
+
 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {
 	/* PID parameters */
@@ -83,6 +88,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->pitch_off =	param_find("ATT_PITCH_OFF3");
 	h->yaw_off   =	param_find("ATT_YAW_OFF3");
 
+	h->mag_decl   =	param_find("ATT_MAG_DECL");
+
+	h->acc_comp   =	param_find("ATT_ACC_COMP");
+
 	return OK;
 }
 
@@ -103,5 +112,9 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->pitch_off, &(p->pitch_off));
 	param_get(h->yaw_off, &(p->yaw_off));
 
+	param_get(h->mag_decl, &(p->mag_decl));
+
+	param_get(h->acc_comp, &(p->acc_comp));
+
 	return OK;
 }
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
index 09817d58e..74a141609 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
@@ -47,12 +47,16 @@ struct attitude_estimator_ekf_params {
 	float roll_off;
 	float pitch_off;
 	float yaw_off;
+	float mag_decl;
+	int acc_comp;
 };
 
 struct attitude_estimator_ekf_param_handles {
 	param_t r0, r1, r2, r3;
 	param_t q0, q1, q2, q3, q4;
 	param_t roll_off, pitch_off, yaw_off;
+	param_t mag_decl;
+	param_t acc_comp;
 };
 
 /**
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 5eeca5a1a..36b75dd58 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -194,15 +194,13 @@ int do_accel_calibration(int mavlink_fd)
 		int32_t board_rotation_int;
 		param_get(board_rotation_h, &(board_rotation_int));
 		enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
-		math::Matrix board_rotation(3, 3);
+		math::Matrix<3,3> board_rotation;
 		get_rot_matrix(board_rotation_id, &board_rotation);
-		math::Matrix board_rotation_t = board_rotation.transpose();
-		math::Vector3 accel_offs_vec;
-		accel_offs_vec.set(&accel_offs[0]);
-		math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec;
-		math::Matrix accel_T_mat(3, 3);
-		accel_T_mat.set(&accel_T[0][0]);
-		math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
+		math::Matrix<3,3> board_rotation_t = board_rotation.transposed();
+		math::Vector<3> accel_offs_vec(&accel_offs[0]);
+		math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
+		math::Matrix<3,3> accel_T_mat(&accel_T[0][0]);
+		math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
 
 		accel_scale.x_offset = accel_offs_rotated(0);
 		accel_scale.x_scale = accel_T_rotated(0, 0);
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 54219a34a..8129dddb3 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -69,7 +69,6 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_armed.h>
@@ -153,16 +152,12 @@ static uint64_t last_print_mode_reject_time = 0;
 static bool on_usb_power = false;
 
 static float takeoff_alt = 5.0f;
+static int parachute_enabled = 0;
 
 static struct vehicle_status_s status;
-
-/* armed topic */
 static struct actuator_armed_s armed;
-
 static struct safety_s safety;
-
-/* flags for control apps */
-struct vehicle_control_mode_s control_mode;
+static struct vehicle_control_mode_s control_mode;
 
 /* tasks waiting for low prio thread */
 typedef enum {
@@ -199,7 +194,7 @@ void usage(const char *reason);
 /**
  * React to commands that are sent e.g. from the mavlink module.
  */
-void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
+bool handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
 
 /**
  * Mainloop of commander.
@@ -210,9 +205,11 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua
 
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
-void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
+void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
+
+transition_result_t set_main_state_rc(struct vehicle_status_s *status);
 
-transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
+void set_control_mode();
 
 void print_reject_mode(struct vehicle_status_s *current_status, const char *msg);
 
@@ -364,16 +361,16 @@ void print_status()
 	warnx("arming: %s", armed_str);
 }
 
-static orb_advert_t control_mode_pub;
 static orb_advert_t status_pub;
 
 int arm()
 {
-	int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
+	int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
 
 	if (arming_res == TRANSITION_CHANGED) {
 		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
 		return 0;
+
 	} else {
 		return 1;
 	}
@@ -381,20 +378,22 @@ int arm()
 
 int disarm()
 {
-	int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+	int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 
 	if (arming_res == TRANSITION_CHANGED) {
 		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
 		return 0;
+
 	} else {
 		return 1;
 	}
 }
 
-void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
+bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
 {
 	/* result of the command */
-	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+	enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+	bool ret = false;
 
 	/* only handle high-priority commands here */
 
@@ -407,12 +406,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 			/* set HIL state */
 			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
-			int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
+			int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
 
 			/* if HIL got enabled, reset battery status state */
-			if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
+			if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
 				/* reset the arming mode to disarmed */
-				arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
+				arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
 
 				if (arming_res != TRANSITION_DENIED) {
 					mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
@@ -422,18 +421,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				}
 			}
 
+			if (hil_ret == OK)
+				ret = true;
+
 			// TODO remove debug code
 			//mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode);
 			/* set arming state */
 			arming_res = TRANSITION_NOT_CHANGED;
 
 			if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
-				if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) {
+				if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) {
 					print_reject_arm("NOT ARMING: Press safety switch first.");
 					arming_res = TRANSITION_DENIED;
 
 				} else {
-					arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
+					arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
 				}
 
 				if (arming_res == TRANSITION_CHANGED) {
@@ -443,7 +445,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			} else {
 				if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
 					arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-					arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed);
+					arming_res = arming_state_transition(status, safety, new_arming_state, armed);
 
 					if (arming_res == TRANSITION_CHANGED) {
 						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
@@ -454,6 +456,9 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				}
 			}
 
+			if (arming_res == TRANSITION_CHANGED)
+				ret = true;
+
 			/* set main state */
 			transition_result_t main_res = TRANSITION_DENIED;
 
@@ -494,6 +499,9 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				}
 			}
 
+			if (main_res == TRANSITION_CHANGED)
+				ret = true;
+
 			if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
 
@@ -504,29 +512,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			break;
 		}
 
-	case VEHICLE_CMD_NAV_TAKEOFF: {
-			if (armed->armed) {
-				transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
-
-				if (nav_res == TRANSITION_CHANGED) {
-					mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
-				}
-
-				if (nav_res != TRANSITION_DENIED) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-				}
-
-			} else {
-				/* reject TAKEOFF not armed */
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-			}
-
-			break;
-		}
-
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
 			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
@@ -536,12 +521,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 					arming_res = TRANSITION_DENIED;
 
 				} else {
-					arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
+					arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
 				}
 
 				if (arming_res == TRANSITION_CHANGED) {
 					mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd");
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
+					ret = true;
 
 				} else {
 					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
@@ -551,29 +537,64 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 		break;
 
-	default:
-		break;
-	}
+	case VEHICLE_CMD_OVERRIDE_GOTO: {
+			// TODO listen vehicle_command topic directly from navigator (?)
+			unsigned int mav_goto = cmd->param1;
 
-	/* supported command handling stop */
-	if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
-		tune_positive();
+			if (mav_goto == 0) {	// MAV_GOTO_DO_HOLD
+				status->set_nav_state = NAV_STATE_LOITER;
+				status->set_nav_state_timestamp = hrt_absolute_time();
+				mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				ret = true;
 
-	} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
-		/* we do not care in the high prio loop about commands we don't know */
-	} else {
-		tune_negative();
+			} else if (mav_goto == 1) {	// MAV_GOTO_DO_CONTINUE
+				status->set_nav_state = NAV_STATE_MISSION;
+				status->set_nav_state_timestamp = hrt_absolute_time();
+				mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				ret = true;
 
-		if (result == VEHICLE_CMD_RESULT_DENIED) {
-			mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command);
+			} else {
+				mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7);
+			}
+		}
+		break;
 
-		} else if (result == VEHICLE_CMD_RESULT_FAILED) {
-			mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command);
+		/* Flight termination */
+	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
 
-		} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
-			mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
+			//XXX: to enable the parachute, a param needs to be set
+			//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
+			if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
+				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				ret = true;
+
+			} else {
+				/* reject parachute depoyment not armed */
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
 
 		}
+		break;
+
+	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+	case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
+	case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
+	case VEHICLE_CMD_PREFLIGHT_STORAGE:
+		/* ignore commands that handled in low prio loop */
+		break;
+
+	default:
+		/* warn about unsupported commands */
+		answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
+		break;
+	}
+
+	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+		/* already warned about unsupported commands in "default" case */
+		answer_command(*cmd, result);
 	}
 
 	/* send any requested ACKs */
@@ -588,7 +609,6 @@ int commander_thread_main(int argc, char *argv[])
 {
 	/* not yet initialized */
 	commander_initialized = false;
-	bool home_position_set = false;
 
 	bool battery_tune_played = false;
 	bool arm_tune_played = false;
@@ -598,10 +618,32 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_system_id = param_find("MAV_SYS_ID");
 	param_t _param_component_id = param_find("MAV_COMP_ID");
 	param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
+	param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
 
 	/* welcome user */
 	warnx("starting");
 
+	char *main_states_str[MAIN_STATE_MAX];
+	main_states_str[0] = "MANUAL";
+	main_states_str[1] = "SEATBELT";
+	main_states_str[2] = "EASY";
+	main_states_str[3] = "AUTO";
+
+	char *arming_states_str[ARMING_STATE_MAX];
+	arming_states_str[0] = "INIT";
+	arming_states_str[1] = "STANDBY";
+	arming_states_str[2] = "ARMED";
+	arming_states_str[3] = "ARMED_ERROR";
+	arming_states_str[4] = "STANDBY_ERROR";
+	arming_states_str[5] = "REBOOT";
+	arming_states_str[6] = "IN_AIR_RESTORE";
+
+	char *failsafe_states_str[FAILSAFE_STATE_MAX];
+	failsafe_states_str[0] = "NORMAL";
+	failsafe_states_str[1] = "RTL";
+	failsafe_states_str[2] = "LAND";
+	failsafe_states_str[3] = "TERMINATION";
+
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
 
@@ -616,25 +658,17 @@ int commander_thread_main(int argc, char *argv[])
 
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	/* Main state machine */
-	/* make sure we are in preflight state */
+	/* vehicle status topic */
 	memset(&status, 0, sizeof(status));
 	status.condition_landed = true;	// initialize to safe value
 	// We want to accept RC inputs as default
 	status.rc_input_blocked = false;
-
-	/* armed topic */
-	orb_advert_t armed_pub;
-	/* Initialize armed with all false */
-	memset(&armed, 0, sizeof(armed));
-
-	/* Initialize all flags to false */
-	memset(&control_mode, 0, sizeof(control_mode));
-
 	status.main_state = MAIN_STATE_MANUAL;
-	status.navigation_state = NAVIGATION_STATE_DIRECT;
+	status.set_nav_state = NAV_STATE_NONE;
+	status.set_nav_state_timestamp = 0;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
+	status.failsafe_state = FAILSAFE_STATE_NORMAL;
 
 	/* neither manual nor offboard control commands have been received */
 	status.offboard_control_signal_found_once = false;
@@ -644,9 +678,6 @@ int commander_thread_main(int argc, char *argv[])
 	status.rc_signal_lost = true;
 	status.offboard_control_signal_lost = true;
 
-	/* allow manual override initially */
-	control_mode.flag_external_manual_override_ok = true;
-
 	/* set battery warning flag */
 	status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
 	status.condition_battery_voltage_valid = false;
@@ -654,21 +685,22 @@ int commander_thread_main(int argc, char *argv[])
 	// XXX for now just set sensors as initialized
 	status.condition_system_sensors_initialized = true;
 
-	// XXX just disable offboard control for now
-	control_mode.flag_control_offboard_enabled = false;
+	status.counter++;
+	status.timestamp = hrt_absolute_time();
 
-	/* advertise to ORB */
+	/* publish initial state */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
-	/* publish current state machine */
 
-	/* publish initial state */
-	status.counter++;
-	status.timestamp = hrt_absolute_time();
-	orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+	/* armed topic */
+	orb_advert_t armed_pub;
+	/* Initialize armed with all false */
+	memset(&armed, 0, sizeof(armed));
 
-	armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+	/* vehicle control mode topic */
+	memset(&control_mode, 0, sizeof(control_mode));
+	orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
 
-	control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
+	armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
 
 	/* home position */
 	orb_advert_t home_pub = -1;
@@ -819,11 +851,9 @@ int commander_thread_main(int argc, char *argv[])
 				    status.system_type == VEHICLE_TYPE_QUADROTOR ||
 				    status.system_type == VEHICLE_TYPE_HEXAROTOR ||
 				    status.system_type == VEHICLE_TYPE_OCTOROTOR) {
-					control_mode.flag_external_manual_override_ok = false;
 					status.is_rotary_wing = true;
 
 				} else {
-					control_mode.flag_external_manual_override_ok = true;
 					status.is_rotary_wing = false;
 				}
 
@@ -834,10 +864,10 @@ int commander_thread_main(int argc, char *argv[])
 
 				/* re-check RC calibration */
 				rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
-
-				/* navigation parameters */
-				param_get(_param_takeoff_alt, &takeoff_alt);
 			}
+			/* navigation parameters */
+			param_get(_param_takeoff_alt, &takeoff_alt);
+			param_get(_param_enable_parachute, &parachute_enabled);
 		}
 
 		orb_check(sp_man_sub, &updated);
@@ -875,7 +905,7 @@ int commander_thread_main(int argc, char *argv[])
 			// XXX this would be the right approach to do it, but do we *WANT* this?
 			// /* disarm if safety is now on and still armed */
 			// if (safety.safety_switch_available && !safety.safety_off) {
-			// 	(void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+			// 	(void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 			// }
 		}
 
@@ -888,7 +918,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* update condition_global_position_valid */
-		check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed);
+		check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed);
 
 		/* update local position estimate */
 		orb_check(local_position_sub, &updated);
@@ -902,10 +932,12 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
 
+		static bool published_condition_landed_fw = false;
 		if (status.is_rotary_wing && status.condition_local_altitude_valid) {
 			if (status.condition_landed != local_position.landed) {
 				status.condition_landed = local_position.landed;
 				status_changed = true;
+				published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes
 
 				if (status.condition_landed) {
 					mavlink_log_critical(mavlink_fd, "#audio: LANDED");
@@ -914,6 +946,12 @@ int commander_thread_main(int argc, char *argv[])
 					mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
 				}
 			}
+		} else {
+			if (!published_condition_landed_fw) {
+				status.condition_landed = false; // Fixedwing does not have a landing detector currently
+				published_condition_landed_fw = true;
+				status_changed = true;
+			}
 		}
 
 		/* update battery status */
@@ -921,6 +959,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
 			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
@@ -995,10 +1034,10 @@ int commander_thread_main(int argc, char *argv[])
 			battery_tune_played = false;
 
 			if (armed.armed) {
-				arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
+				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
 
 			} else {
-				arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
 			}
 
 			status_changed = true;
@@ -1009,7 +1048,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
 			// XXX check for sensors
-			arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 
 		} else {
 			// XXX: Add emergency stuff if sensors are lost
@@ -1043,25 +1082,18 @@ int commander_thread_main(int argc, char *argv[])
 			 * position to the current position.
 			 */
 
-			if (!home_position_set && gps_position.fix_type >= 3 &&
-			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&	// XXX note that vdop is 0 for mtk
-			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) {
-				/* copy position data to uORB home message, store it locally as well */
-				// TODO use global position estimate
-				home.lat = gps_position.lat;
-				home.lon = gps_position.lon;
-				home.alt = gps_position.alt;
-
-				home.eph_m = gps_position.eph_m;
-				home.epv_m = gps_position.epv_m;
+			if (!status.condition_home_position_valid && gps_position.fix_type >= 3 &&
+			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&
+			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
+			    && global_position.global_valid) {
 
-				home.s_variance_m_s = gps_position.s_variance_m_s;
-				home.p_variance_m = gps_position.p_variance_m;
+				/* copy position data to uORB home message, store it locally as well */
+				home.lat = global_position.lat;
+				home.lon = global_position.lon;
+				home.alt = global_position.alt;
 
-				double home_lat_d = home.lat * 1e-7;
-				double home_lon_d = home.lon * 1e-7;
-				warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d);
-				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d);
+				warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt);
+				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt);
 
 				/* announce new home position */
 				if (home_pub > 0) {
@@ -1072,120 +1104,162 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 				/* mark home position as set */
-				home_position_set = true;
+				status.condition_home_position_valid = true;
 				tune_positive();
 			}
 		}
 
-		/* ignore RC signals if in offboard control mode */
-		if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0 && !status.rc_input_blocked) {
-			/* start RC input check */
-			if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
-				/* handle the case where RC signal was regained */
-				if (!status.rc_signal_found_once) {
-					status.rc_signal_found_once = true;
-					mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
-					status_changed = true;
+		/* start RC input check */
+		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
+			/* handle the case where RC signal was regained */
+			if (!status.rc_signal_found_once) {
+				status.rc_signal_found_once = true;
+				mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
+				status_changed = true;
 
-				} else {
-					if (status.rc_signal_lost) {
-						mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
-						status_changed = true;
-					}
+			} else {
+				if (status.rc_signal_lost) {
+					mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
+					status_changed = true;
 				}
+			}
 
-				status.rc_signal_lost = false;
+			status.rc_signal_lost = false;
 
-				transition_result_t res;	// store all transitions results here
+			transition_result_t res;	// store all transitions results here
 
-				/* arm/disarm by RC */
-				res = TRANSITION_NOT_CHANGED;
+			/* arm/disarm by RC */
+			res = TRANSITION_NOT_CHANGED;
 
-				/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
-				 * do it only for rotary wings */
-				if (status.is_rotary_wing &&
-				    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
-				    (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY ||
-				     (status.condition_landed && (
-					      status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
-					      status.navigation_state == NAVIGATION_STATE_VECTOR
-				      ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
-					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
-						arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-						res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed);
-						stick_off_counter = 0;
+			/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
+			 * do it only for rotary wings */
+			if (status.is_rotary_wing &&
+			    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
+			    (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
+			    sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
 
-					} else {
-						stick_off_counter++;
-					}
+				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
+					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+					res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+					stick_off_counter = 0;
 
 				} else {
-					stick_off_counter = 0;
+					stick_off_counter++;
 				}
 
-				/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
-				if (status.arming_state == ARMING_STATE_STANDBY &&
-				    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
-					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						if (safety.safety_switch_available && !safety.safety_off) {
-							print_reject_arm("NOT ARMING: Press safety switch first.");
-
-						} else if (status.main_state != MAIN_STATE_MANUAL) {
-							print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
+			} else {
+				stick_off_counter = 0;
+			}
 
-						} else {
-							res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
-						}
+			/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
+			if (status.arming_state == ARMING_STATE_STANDBY &&
+			    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+					if (safety.safety_switch_available && !safety.safety_off) {
+						print_reject_arm("NOT ARMING: Press safety switch first.");
 
-						stick_on_counter = 0;
+					} else if (status.main_state != MAIN_STATE_MANUAL) {
+						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 
 					} else {
-						stick_on_counter++;
+						res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
 					}
 
-				} else {
 					stick_on_counter = 0;
+
+				} else {
+					stick_on_counter++;
 				}
 
-				if (res == TRANSITION_CHANGED) {
-					if (status.arming_state == ARMING_STATE_ARMED) {
-						mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
+			} else {
+				stick_on_counter = 0;
+			}
 
-					} else {
-						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
-					}
+			if (res == TRANSITION_CHANGED) {
+				if (status.arming_state == ARMING_STATE_ARMED) {
+					mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
 
-				} else if (res == TRANSITION_DENIED) {
-					warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-					mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+				} else {
+					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
 				}
 
-				/* fill current_status according to mode switches */
-				check_mode_switches(&sp_man, &status);
+			} else if (res == TRANSITION_DENIED) {
+				/* DENIED here indicates bug in the commander */
+				mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
+			}
 
-				/* evaluate the main state machine */
-				res = check_main_state_machine(&status);
+			if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+				/* recover from failsafe */
+				transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+			}
 
-				if (res == TRANSITION_CHANGED) {
-					//mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
-					tune_positive();
+			/* fill status according to mode switches */
+			check_mode_switches(&sp_man, &status);
+
+			/* evaluate the main state machine according to mode switches */
+			res = set_main_state_rc(&status);
+
+			if (res == TRANSITION_CHANGED) {
+				tune_positive();
+
+			} else if (res == TRANSITION_DENIED) {
+				/* DENIED here indicates bug in the commander */
+				mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
+			}
+
+		} else {
+			if (!status.rc_signal_lost) {
+				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
+				status.rc_signal_lost = true;
+				status_changed = true;
+			}
+
+			if (armed.armed) {
+				if (status.main_state == MAIN_STATE_AUTO) {
+					/* check if AUTO mode still allowed */
+					transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
 
-				} else if (res == TRANSITION_DENIED) {
-					/* DENIED here indicates bug in the commander */
-					warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-					mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+					if (res == TRANSITION_DENIED) {
+						/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
+						res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+
+						if (res == TRANSITION_DENIED) {
+							/* LAND not allowed, set TERMINATION state */
+							transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+						}
+					}
+
+				} else {
+					/* failsafe for manual modes */
+					transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
+
+					if (res == TRANSITION_DENIED) {
+						/* RTL not allowed (no global position estimate), try LAND */
+						res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+
+						if (res == TRANSITION_DENIED) {
+							/* LAND not allowed, set TERMINATION state */
+							res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+						}
+					}
 				}
 
 			} else {
-				if (!status.rc_signal_lost) {
-					mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
-					status.rc_signal_lost = true;
-					status_changed = true;
+				if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+					/* reset failsafe when disarmed */
+					transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
 				}
 			}
 		}
 
+		// TODO remove this hack
+		/* flight termination in manual mode if assisted switch is on easy position */
+		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
+			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
+				tune_positive();
+			}
+		}
 
 		/* handle commands last, as the system needs to be updated to handle them */
 		orb_check(cmd_sub, &updated);
@@ -1195,40 +1269,42 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
 
 			/* handle it */
-			handle_command(&status, &safety, &control_mode, &cmd, &armed);
-		}
-
-		/* evaluate the navigation state machine */
-		transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
-
-		if (res == TRANSITION_DENIED) {
-			/* DENIED here indicates bug in the commander */
-			warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
-			mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
+			if (handle_command(&status, &safety, &cmd, &armed))
+				status_changed = true;
 		}
 
 		/* check which state machines for changes, clear "changed" flag */
 		bool arming_state_changed = check_arming_state_changed();
 		bool main_state_changed = check_main_state_changed();
-		bool navigation_state_changed = check_navigation_state_changed();
+		bool failsafe_state_changed = check_failsafe_state_changed();
 
 		hrt_abstime t1 = hrt_absolute_time();
 
-		if (navigation_state_changed || arming_state_changed) {
-			control_mode.flag_armed = armed.armed;	// copy armed state to vehicle_control_mode topic
+		/* print new state */
+		if (arming_state_changed) {
+			status_changed = true;
+			mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]);
+		}
+
+		if (main_state_changed) {
+			status_changed = true;
+			mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
 		}
 
-		if (arming_state_changed || main_state_changed || navigation_state_changed) {
-			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
+		if (failsafe_state_changed) {
 			status_changed = true;
+			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]);
 		}
 
 		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
 		if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
-			status.timestamp = t1;
-			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+			set_control_mode();
 			control_mode.timestamp = t1;
 			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+
+			status.timestamp = t1;
+			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+
 			armed.timestamp = t1;
 			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		}
@@ -1401,108 +1477,108 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 }
 
 void
-check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status)
+check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status)
 {
 	/* main mode switch */
 	if (!isfinite(sp_man->mode_switch)) {
 		/* default to manual if signal is invalid */
-		current_status->mode_switch = MODE_SWITCH_MANUAL;
+		status->mode_switch = MODE_SWITCH_MANUAL;
 
 	} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
-		current_status->mode_switch = MODE_SWITCH_AUTO;
+		status->mode_switch = MODE_SWITCH_AUTO;
 
 	} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
-		current_status->mode_switch = MODE_SWITCH_MANUAL;
+		status->mode_switch = MODE_SWITCH_MANUAL;
 
 	} else {
-		current_status->mode_switch = MODE_SWITCH_ASSISTED;
+		status->mode_switch = MODE_SWITCH_ASSISTED;
 	}
 
-	/* land switch */
+	/* return switch */
 	if (!isfinite(sp_man->return_switch)) {
-		current_status->return_switch = RETURN_SWITCH_NONE;
+		status->return_switch = RETURN_SWITCH_NONE;
 
 	} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
-		current_status->return_switch = RETURN_SWITCH_RETURN;
+		status->return_switch = RETURN_SWITCH_RETURN;
 
 	} else {
-		current_status->return_switch = RETURN_SWITCH_NONE;
+		status->return_switch = RETURN_SWITCH_NORMAL;
 	}
 
 	/* assisted switch */
 	if (!isfinite(sp_man->assisted_switch)) {
-		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+		status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
 
 	} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
-		current_status->assisted_switch = ASSISTED_SWITCH_EASY;
+		status->assisted_switch = ASSISTED_SWITCH_EASY;
 
 	} else {
-		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+		status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
 	}
 
 	/* mission switch  */
 	if (!isfinite(sp_man->mission_switch)) {
-		current_status->mission_switch = MISSION_SWITCH_MISSION;
+		status->mission_switch = MISSION_SWITCH_NONE;
 
 	} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
-		current_status->mission_switch = MISSION_SWITCH_NONE;
+		status->mission_switch = MISSION_SWITCH_LOITER;
 
 	} else {
-		current_status->mission_switch = MISSION_SWITCH_MISSION;
+		status->mission_switch = MISSION_SWITCH_MISSION;
 	}
 }
 
 transition_result_t
-check_main_state_machine(struct vehicle_status_s *current_status)
+set_main_state_rc(struct vehicle_status_s *status)
 {
-	/* evaluate the main state machine */
+	/* set main state according to RC switches */
 	transition_result_t res = TRANSITION_DENIED;
 
-	switch (current_status->mode_switch) {
+	switch (status->mode_switch) {
 	case MODE_SWITCH_MANUAL:
-		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+		res = main_state_transition(status, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
 	case MODE_SWITCH_ASSISTED:
-		if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) {
-			res = main_state_transition(current_status, MAIN_STATE_EASY);
+		if (status->assisted_switch == ASSISTED_SWITCH_EASY) {
+			res = main_state_transition(status, MAIN_STATE_EASY);
 
 			if (res != TRANSITION_DENIED)
 				break;	// changed successfully or already in this state
 
 			// else fallback to SEATBELT
-			print_reject_mode(current_status, "EASY");
+			print_reject_mode(status, "EASY");
 		}
 
-		res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+		res = main_state_transition(status, MAIN_STATE_SEATBELT);
 
 		if (res != TRANSITION_DENIED)
 			break;	// changed successfully or already in this mode
 
-		if (current_status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
-			print_reject_mode(current_status, "SEATBELT");
+		if (status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
+			print_reject_mode(status, "SEATBELT");
 
 		// else fallback to MANUAL
-		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+		res = main_state_transition(status, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
 	case MODE_SWITCH_AUTO:
-		res = main_state_transition(current_status, MAIN_STATE_AUTO);
+		res = main_state_transition(status, MAIN_STATE_AUTO);
 
 		if (res != TRANSITION_DENIED)
 			break;	// changed successfully or already in this state
 
 		// else fallback to SEATBELT (EASY likely will not work too)
-		print_reject_mode(current_status, "AUTO");
-		res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+		print_reject_mode(status, "AUTO");
+		res = main_state_transition(status, MAIN_STATE_SEATBELT);
 
 		if (res != TRANSITION_DENIED)
 			break;	// changed successfully or already in this state
 
 		// else fallback to MANUAL
-		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+		res = main_state_transition(status, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
@@ -1514,7 +1590,104 @@ check_main_state_machine(struct vehicle_status_s *current_status)
 }
 
 void
-print_reject_mode(struct vehicle_status_s *current_status, const char *msg)
+
+set_control_mode()
+{
+	/* set vehicle_control_mode according to main state and failsafe state */
+	control_mode.flag_armed = armed.armed;
+	control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
+	control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
+
+	control_mode.flag_control_termination_enabled = false;
+
+	/* set this flag when navigator should act */
+	bool navigator_enabled = false;
+
+	switch (status.failsafe_state) {
+	case FAILSAFE_STATE_NORMAL:
+		switch (status.main_state) {
+		case MAIN_STATE_MANUAL:
+			control_mode.flag_control_manual_enabled = true;
+			control_mode.flag_control_auto_enabled = false;
+			control_mode.flag_control_rates_enabled = status.is_rotary_wing;
+			control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+
+		case MAIN_STATE_SEATBELT:
+			control_mode.flag_control_manual_enabled = true;
+			control_mode.flag_control_auto_enabled = false;
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			control_mode.flag_control_altitude_enabled = true;
+			control_mode.flag_control_climb_rate_enabled = true;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+
+		case MAIN_STATE_EASY:
+			control_mode.flag_control_manual_enabled = true;
+			control_mode.flag_control_auto_enabled = false;
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			control_mode.flag_control_altitude_enabled = true;
+			control_mode.flag_control_climb_rate_enabled = true;
+			control_mode.flag_control_position_enabled = true;
+			control_mode.flag_control_velocity_enabled = true;
+			break;
+
+		case MAIN_STATE_AUTO:
+			navigator_enabled = true;
+
+		default:
+			break;
+		}
+
+		break;
+
+	case FAILSAFE_STATE_RTL:
+		navigator_enabled = true;
+		break;
+
+	case FAILSAFE_STATE_LAND:
+		navigator_enabled = true;
+		break;
+
+	case FAILSAFE_STATE_TERMINATION:
+		/* disable all controllers on termination */
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = false;
+		control_mode.flag_control_attitude_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_termination_enabled = true;
+		break;
+
+	default:
+		break;
+	}
+
+	/* navigator has control, set control mode flags according to nav state*/
+	if (navigator_enabled) {
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = true;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_position_enabled = true;
+		control_mode.flag_control_velocity_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+	}
+}
+
+void
+print_reject_mode(struct vehicle_status_s *status, const char *msg)
 {
 	hrt_abstime t = hrt_absolute_time();
 
@@ -1526,7 +1699,7 @@ print_reject_mode(struct vehicle_status_s *current_status, const char *msg)
 
 		// only buzz if armed, because else we're driving people nuts indoors
 		// they really need to look at the leds as well.
-		if (current_status->arming_state == ARMING_STATE_ARMED) {
+		if (status->arming_state == ARMING_STATE_ARMED) {
 			tune_negative();
 		} else {
 
@@ -1550,133 +1723,6 @@ print_reject_arm(const char *msg)
 	}
 }
 
-transition_result_t
-check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
-{
-	transition_result_t res = TRANSITION_DENIED;
-
-	if (status->main_state == MAIN_STATE_AUTO) {
-		if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
-			// TODO AUTO_LAND handling
-			if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
-				/* don't switch to other states until takeoff not completed */
-				// XXX: only respect the condition_landed when the local position is actually valid
-				if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
-					return TRANSITION_NOT_CHANGED;
-				}
-			}
-
-			if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF &&
-			    status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
-			    status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
-			    status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
-				/* possibly on ground, switch to TAKEOFF if needed */
-				if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
-					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
-					return res;
-				}
-			}
-
-			/* switch to AUTO mode */
-			if (status->rc_signal_found_once && !status->rc_signal_lost) {
-				/* act depending on switches when manual control enabled */
-				if (status->return_switch == RETURN_SWITCH_RETURN) {
-					/* RTL */
-					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
-
-				} else {
-					if (status->mission_switch == MISSION_SWITCH_MISSION) {
-						/* MISSION */
-						res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
-
-					} else {
-						/* LOITER */
-						res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
-					}
-				}
-
-			} else {
-				/* switch to MISSION when no RC control and first time in some AUTO mode  */
-				if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
-				    status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
-				    status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
-				    status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
-					res = TRANSITION_NOT_CHANGED;
-
-				} else {
-					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
-				}
-			}
-
-		} else {
-			/* disarmed, always switch to AUTO_READY */
-			res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
-		}
-
-	} else {
-		/* manual control modes */
-		if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) {
-			/* switch to failsafe mode */
-			bool manual_control_old = control_mode->flag_control_manual_enabled;
-
-			if (!status->condition_landed && status->condition_local_position_valid) {
-				/* in air: try to hold position if possible */
-				res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
-
-			} else {
-				/* landed: don't try to hold position but land (if taking off) */
-				res = TRANSITION_DENIED;
-			}
-
-			if (res == TRANSITION_DENIED) {
-				res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
-			}
-
-			control_mode->flag_control_manual_enabled = false;
-
-			if (res == TRANSITION_NOT_CHANGED && manual_control_old) {
-				/* mark navigation state as changed to force immediate flag publishing */
-				set_navigation_state_changed();
-				res = TRANSITION_CHANGED;
-			}
-
-			if (res == TRANSITION_CHANGED) {
-				if (control_mode->flag_control_position_enabled) {
-					mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD");
-
-				} else {
-					if (status->condition_landed) {
-						mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)");
-
-					} else {
-						mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD");
-					}
-				}
-			}
-
-		} else {
-			switch (status->main_state) {
-			case MAIN_STATE_MANUAL:
-				res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
-				break;
-
-			case MAIN_STATE_SEATBELT:
-				res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
-				break;
-
-			case MAIN_STATE_EASY:
-				res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
-				break;
-
-			default:
-				break;
-			}
-		}
-	}
-
-	return res;
-}
-
 void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
 {
 	switch (result) {
@@ -1746,7 +1792,8 @@ void *commander_low_prio_loop(void *arg)
 		/* ignore commands the high-prio loop handles */
 		if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
 		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
-		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF)
+		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
+		    cmd.command == VEHICLE_CMD_DO_SET_SERVO)
 			continue;
 
 		/* only handle low-priority commands here */
@@ -1784,7 +1831,7 @@ void *commander_low_prio_loop(void *arg)
 				/* try to go to INIT/PREFLIGHT arming state */
 
 				// XXX disable interrupts in arming_state_transition
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) {
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
 				}
@@ -1844,7 +1891,7 @@ void *commander_low_prio_loop(void *arg)
 				else
 					tune_negative();
 
-				arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 
 				break;
 			}
@@ -1900,7 +1947,7 @@ void *commander_low_prio_loop(void *arg)
 		break;
 
 		default:
-			answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
+			/* don't answer on unsupported commands, it will be done in main loop */
 			break;
 		}
 
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index e10b7f18d..80ca68f21 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -45,12 +45,42 @@
 #include <nuttx/config.h>
 #include <systemlib/param/param.h>
 
-PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f);
-PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f);
 PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
+
+/**
+ * Empty cell voltage.
+ *
+ * Defines the voltage where a single cell of the battery is considered empty.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
+
+/**
+ * Full cell voltage.
+ *
+ * Defines the voltage where a single cell of the battery is considered full.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
+
+/**
+ * Number of cells.
+ *
+ * Defines the number of cells the attached battery consists of.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
+
+/**
+ * Battery capacity.
+ *
+ * Defines the capacity of the attached battery.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 7ae61d9ef..31955d3e5 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -48,7 +48,6 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
@@ -67,12 +66,11 @@ static const int ERROR = -1;
 
 static bool arming_state_changed = true;
 static bool main_state_changed = true;
-static bool navigation_state_changed = true;
+static bool failsafe_state_changed = true;
 
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
-	const struct vehicle_control_mode_s *control_mode,
-	arming_state_t new_arming_state, struct actuator_armed_s *armed)
+			arming_state_t new_arming_state, struct actuator_armed_s *armed)
 {
 	/*
 	 * Perform an atomic state update
@@ -88,8 +86,9 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 	} else {
 
 		/* enforce lockdown in HIL */
-		if (control_mode->flag_system_hil_enabled) {
+		if (status->hil_state == HIL_STATE_ON) {
 			armed->lockdown = true;
+
 		} else {
 			armed->lockdown = false;
 		}
@@ -111,7 +110,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 			/* allow coming from INIT and disarming from ARMED */
 			if (status->arming_state == ARMING_STATE_INIT
 			    || status->arming_state == ARMING_STATE_ARMED
-			    || control_mode->flag_system_hil_enabled) {
+			    || status->hil_state == HIL_STATE_ON) {
 
 				/* sensors need to be initialized for STANDBY state */
 				if (status->condition_system_sensors_initialized) {
@@ -128,7 +127,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 			/* allow arming from STANDBY and IN-AIR-RESTORE */
 			if ((status->arming_state == ARMING_STATE_STANDBY
 			     || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
-			    && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */
+			    && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */
 				ret = TRANSITION_CHANGED;
 				armed->armed = true;
 				armed->ready_to_arm = true;
@@ -224,54 +223,54 @@ check_arming_state_changed()
 }
 
 transition_result_t
-main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state)
+main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
-	/* only check transition if the new state is actually different from the current one */
-	if (new_main_state == current_state->main_state) {
-		ret = TRANSITION_NOT_CHANGED;
+	/* transition may be denied even if requested the same state because conditions may be changed */
+	switch (new_main_state) {
+	case MAIN_STATE_MANUAL:
+		ret = TRANSITION_CHANGED;
+		break;
 
-	} else {
+	case MAIN_STATE_SEATBELT:
 
-		switch (new_main_state) {
-		case MAIN_STATE_MANUAL:
+		/* need at minimum altitude estimate */
+		if (!status->is_rotary_wing ||
+		    (status->condition_local_altitude_valid ||
+		     status->condition_global_position_valid)) {
 			ret = TRANSITION_CHANGED;
-			break;
-
-		case MAIN_STATE_SEATBELT:
-
-			/* need at minimum altitude estimate */
-			if (current_state->condition_local_altitude_valid ||
-				current_state->condition_global_position_valid) {
-				ret = TRANSITION_CHANGED;
-			}
-
-			break;
+		}
 
-		case MAIN_STATE_EASY:
+		break;
 
-			/* need at minimum local position estimate */
-			if (current_state->condition_local_position_valid ||
-				current_state->condition_global_position_valid) {
-				ret = TRANSITION_CHANGED;
-			}
+	case MAIN_STATE_EASY:
 
-			break;
+		/* need at minimum local position estimate */
+		if (status->condition_local_position_valid ||
+		    status->condition_global_position_valid) {
+			ret = TRANSITION_CHANGED;
+		}
 
-		case MAIN_STATE_AUTO:
+		break;
 
-			/* need global position estimate */
-			if (current_state->condition_global_position_valid) {
-				ret = TRANSITION_CHANGED;
-			}
+	case MAIN_STATE_AUTO:
 
-			break;
+		/* need global position estimate */
+		if (status->condition_global_position_valid) {
+			ret = TRANSITION_CHANGED;
 		}
 
-		if (ret == TRANSITION_CHANGED) {
-			current_state->main_state = new_main_state;
+		break;
+	}
+
+	if (ret == TRANSITION_CHANGED) {
+		if (status->main_state != new_main_state) {
+			status->main_state = new_main_state;
 			main_state_changed = true;
+
+		} else {
+			ret = TRANSITION_NOT_CHANGED;
 		}
 	}
 
@@ -290,162 +289,11 @@ check_main_state_changed()
 	}
 }
 
-transition_result_t
-navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
-{
-	transition_result_t ret = TRANSITION_DENIED;
-
-	/* only check transition if the new state is actually different from the current one */
-	if (new_navigation_state == status->navigation_state) {
-		ret = TRANSITION_NOT_CHANGED;
-
-	} else {
-
-		switch (new_navigation_state) {
-		case NAVIGATION_STATE_DIRECT:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = false;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = false;
-			control_mode->flag_control_climb_rate_enabled = false;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_STABILIZE:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = false;
-			control_mode->flag_control_climb_rate_enabled = false;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_ALTHOLD:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_VECTOR:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_AUTO_READY:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = false;
-			control_mode->flag_control_attitude_enabled = false;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = false;
-			control_mode->flag_control_climb_rate_enabled = false;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
-
-		case NAVIGATION_STATE_AUTO_TAKEOFF:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
-
-		case NAVIGATION_STATE_AUTO_LOITER:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_AUTO_MISSION:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
-
-		case NAVIGATION_STATE_AUTO_RTL:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
-
-		case NAVIGATION_STATE_AUTO_LAND:
-
-			/* deny transitions from landed state */
-			if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
-				ret = TRANSITION_CHANGED;
-				control_mode->flag_control_rates_enabled = true;
-				control_mode->flag_control_attitude_enabled = true;
-				control_mode->flag_control_velocity_enabled = true;
-				control_mode->flag_control_position_enabled = true;
-				control_mode->flag_control_altitude_enabled = true;
-				control_mode->flag_control_climb_rate_enabled = true;
-				control_mode->flag_control_manual_enabled = false;
-				control_mode->flag_control_auto_enabled = true;
-			}
-
-			break;
-
-		default:
-			break;
-		}
-
-		if (ret == TRANSITION_CHANGED) {
-			status->navigation_state = new_navigation_state;
-			control_mode->auto_state = status->navigation_state;
-			navigation_state_changed = true;
-		}
-	}
-
-	return ret;
-}
-
 bool
-check_navigation_state_changed()
+check_failsafe_state_changed()
 {
-	if (navigation_state_changed) {
-		navigation_state_changed = false;
+	if (failsafe_state_changed) {
+		failsafe_state_changed = false;
 		return true;
 
 	} else {
@@ -453,16 +301,10 @@ check_navigation_state_changed()
 	}
 }
 
-void
-set_navigation_state_changed()
-{
-	navigation_state_changed = true;
-}
-
 /**
 * Transition from one hil state to another
 */
-int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd)
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
 	bool valid_transition = false;
 	int ret = ERROR;
@@ -491,7 +333,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 			    || current_status->arming_state == ARMING_STATE_STANDBY
 			    || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
 
-				current_control_mode->flag_system_hil_enabled = true;
 				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
 				valid_transition = true;
 
@@ -537,9 +378,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 		current_status->timestamp = hrt_absolute_time();
 		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
 
-		current_control_mode->timestamp = hrt_absolute_time();
-		orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
-
 		// XXX also set lockdown here
 
 		ret = OK;
@@ -552,6 +390,73 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 }
 
 
+/**
+* Transition from one failsafe state to another
+*/
+transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state)
+{
+	transition_result_t ret = TRANSITION_DENIED;
+
+	/* transition may be denied even if requested the same state because conditions may be changed */
+	if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) {
+		/* transitions from TERMINATION to other states not allowed */
+		if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) {
+			ret = TRANSITION_NOT_CHANGED;
+		}
+
+	} else {
+		switch (new_failsafe_state) {
+		case FAILSAFE_STATE_NORMAL:
+			/* always allowed (except from TERMINATION state) */
+			ret = TRANSITION_CHANGED;
+			break;
+
+		case FAILSAFE_STATE_RTL:
+
+			/* global position and home position required for RTL */
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->set_nav_state = NAV_STATE_RTL;
+				status->set_nav_state_timestamp = hrt_absolute_time();
+				ret = TRANSITION_CHANGED;
+			}
+
+			break;
+
+		case FAILSAFE_STATE_LAND:
+
+			/* at least relative altitude estimate required for landing */
+			if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
+				status->set_nav_state = NAV_STATE_LAND;
+				status->set_nav_state_timestamp = hrt_absolute_time();
+				ret = TRANSITION_CHANGED;
+			}
+
+			break;
+
+		case FAILSAFE_STATE_TERMINATION:
+			/* always allowed */
+			ret = TRANSITION_CHANGED;
+			break;
+
+		default:
+			break;
+		}
+
+		if (ret == TRANSITION_CHANGED) {
+			if (status->failsafe_state != new_failsafe_state) {
+				status->failsafe_state = new_failsafe_state;
+				failsafe_state_changed = true;
+
+			} else {
+				ret = TRANSITION_NOT_CHANGED;
+			}
+		}
+	}
+
+	return ret;
+}
+
+
 
 // /*
 //  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 0bfdf36a8..f04879ff9 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -48,7 +48,6 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/safety.h>
-#include <uORB/topics/vehicle_control_mode.h>
 
 typedef enum {
 	TRANSITION_DENIED = -1,
@@ -58,7 +57,7 @@ typedef enum {
 } transition_result_t;
 
 transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed);
+		arming_state_t new_arming_state, struct actuator_armed_s *armed);
 
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
@@ -68,12 +67,14 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
 
 bool check_main_state_changed();
 
-transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
+transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state);
 
 bool check_navigation_state_changed();
 
+bool check_failsafe_state_changed();
+
 void set_navigation_state_changed();
 
-int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
 
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp
index 448a42a99..e8fecef0d 100644
--- a/src/modules/controllib/uorb/blocks.cpp
+++ b/src/modules/controllib/uorb/blocks.cpp
@@ -54,26 +54,26 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {};
 
 void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
 				   vehicle_attitude_s &att,
-				   vehicle_global_position_setpoint_s &posCmd,
-				   vehicle_global_position_setpoint_s &lastPosCmd)
+				   position_setpoint_s &missionCmd,
+				   position_setpoint_s &lastMissionCmd)
 {
 
 	// heading to waypoint
 	float psiTrack = get_bearing_to_next_waypoint(
 				 (double)pos.lat / (double)1e7d,
 				 (double)pos.lon / (double)1e7d,
-				 (double)posCmd.lat / (double)1e7d,
-				 (double)posCmd.lon / (double)1e7d);
+				 missionCmd.lat,
+				 missionCmd.lon);
 
 	// cross track
 	struct crosstrack_error_s xtrackError;
 	get_distance_to_line(&xtrackError,
 			     (double)pos.lat / (double)1e7d,
 			     (double)pos.lon / (double)1e7d,
-			     (double)lastPosCmd.lat / (double)1e7d,
-			     (double)lastPosCmd.lon / (double)1e7d,
-			     (double)posCmd.lat / (double)1e7d,
-			     (double)posCmd.lon / (double)1e7d);
+			     lastMissionCmd.lat,
+			     lastMissionCmd.lon,
+			     missionCmd.lat,
+			     missionCmd.lon);
 
 	_psiCmd = _wrap_2pi(psiTrack -
 			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
@@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c
 	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
 	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
 	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
-	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
+	_missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20),
 	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
 	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
 	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
index 46dc1bec2..7c80c4b2b 100644
--- a/src/modules/controllib/uorb/blocks.hpp
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -43,7 +43,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
@@ -82,8 +82,8 @@ public:
 	virtual ~BlockWaypointGuidance();
 	void update(vehicle_global_position_s &pos,
 		    vehicle_attitude_s &att,
-		    vehicle_global_position_setpoint_s &posCmd,
-		    vehicle_global_position_setpoint_s &lastPosCmd);
+		    position_setpoint_s &missionCmd,
+		    position_setpoint_s &lastMissionCmd);
 	float getPsiCmd() { return _psiCmd; }
 };
 
@@ -98,7 +98,7 @@ protected:
 	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
 	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
 	UOrbSubscription<vehicle_global_position_s> _pos;
-	UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
+	UOrbSubscription<position_setpoint_triplet_s> _missionCmd;
 	UOrbSubscription<manual_control_setpoint_s> _manual;
 	UOrbSubscription<vehicle_status_s> _status;
 	UOrbSubscription<parameter_update_s> _param_update;
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
new file mode 100644
index 000000000..fa88dfaff
--- /dev/null
+++ b/src/modules/dataman/dataman.c
@@ -0,0 +1,779 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	Lorenz Meier
+ *              Jean Cyr
+ *
+ * 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 dataman.c
+ * DATAMANAGER driver.
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <sys/ioctl.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <queue.h>
+
+#include "dataman.h"
+
+/**
+ * data manager app start / stop handling function
+ *
+ * @ingroup apps
+ */
+
+__EXPORT int dataman_main(int argc, char *argv[]);
+__EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen);
+__EXPORT ssize_t dm_write(dm_item_t  item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen);
+__EXPORT int dm_clear(dm_item_t item);
+__EXPORT int dm_restart(dm_reset_reason restart_type);
+
+/* Types of function calls supported by the worker task */
+typedef enum {
+	dm_write_func = 0,
+	dm_read_func,
+	dm_clear_func,
+	dm_restart_func,
+	dm_number_of_funcs
+} dm_function_t;
+
+/* Work task work item */
+typedef struct {
+	sq_entry_t link;	/**< list linkage */
+	sem_t wait_sem;
+	dm_function_t func;
+	ssize_t result;
+	union {
+		struct {
+			dm_item_t item;
+			unsigned char index;
+			dm_persitence_t persistence;
+			const void *buf;
+			size_t count;
+		} write_params;
+		struct {
+			dm_item_t item;
+			unsigned char index;
+			void *buf;
+			size_t count;
+		} read_params;
+		struct {
+			dm_item_t item;
+		} clear_params;
+		struct {
+			dm_reset_reason reason;
+		} restart_params;
+	};
+} work_q_item_t;
+
+/* Usage statistics */
+static unsigned g_func_counts[dm_number_of_funcs];
+
+/* table of maximum number of instances for each item type */
+static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
+	DM_KEY_SAFE_POINTS_MAX,
+	DM_KEY_FENCE_POINTS_MAX,
+	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
+	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
+	DM_KEY_WAYPOINTS_ONBOARD_MAX
+};
+
+/* Table of offset for index 0 of each item type */
+static unsigned int g_key_offsets[DM_KEY_NUM_KEYS];
+
+/* The data manager store file handle and file name */
+static int g_fd = -1, g_task_fd = -1;
+static const char *k_data_manager_device_path = "/fs/microsd/dataman";
+
+/* The data manager work queues */
+
+typedef struct {
+	sq_queue_t q;		/* Nuttx queue */
+	sem_t mutex;		/* Mutual exclusion on work queue adds and deletes */
+	unsigned size;		/* Current size of queue */
+	unsigned max_size;	/* Maximum queue size reached */
+} work_q_t;
+
+static work_q_t g_free_q;	/* queue of free work items. So that we don't always need to call malloc and free*/
+static work_q_t g_work_q;	/* pending work items. To be consumed by worker thread */
+
+sem_t g_work_queued_sema;	/* To notify worker thread a work item has been queued */
+sem_t g_init_sema;
+
+static bool g_task_should_exit;		/**< if true, dataman task should exit */
+
+#define DM_SECTOR_HDR_SIZE 4	/* data manager per item header overhead */
+static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */
+
+static void init_q(work_q_t *q)
+{
+	sq_init(&(q->q));				/* Initialize the NuttX queue structure */
+	sem_init(&(q->mutex), 1, 1);	/* Queue is initially unlocked */
+	q->size = q->max_size = 0;		/* Queue is initially empty */
+}
+
+static inline void
+destroy_q(work_q_t *q)
+{
+	sem_destroy(&(q->mutex));		/* Destroy the queue lock */
+}
+
+static inline void
+lock_queue(work_q_t *q)
+{
+	sem_wait(&(q->mutex));	/* Acquire the queue lock */
+}
+
+static inline void
+unlock_queue(work_q_t *q)
+{
+	sem_post(&(q->mutex));	/* Release the queue lock */
+}
+
+static work_q_item_t *
+create_work_item(void)
+{
+	work_q_item_t *item;
+
+	/* Try to reuse item from free item queue */
+	lock_queue(&g_free_q);
+	if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q))))
+		g_free_q.size--;
+	unlock_queue(&g_free_q);
+
+	/* If we there weren't any free items then obtain memory for a new one */
+	if (item == NULL)
+		item = (work_q_item_t *)malloc(sizeof(work_q_item_t));
+
+	/* If we got one then lock the item*/
+	if (item)
+		sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */
+
+	/* return the item pointer, or NULL if all failed */
+	return item;
+}
+
+/* Work queue management functions */
+
+static inline void
+destroy_work_item(work_q_item_t *item)
+{
+	sem_destroy(&item->wait_sem); /* Destroy the item lock */
+	/* Return the item to the free item queue for later reuse */
+	lock_queue(&g_free_q);
+	sq_addfirst(&item->link, &(g_free_q.q));
+
+	/* Update the queue size and potentially the maximum queue size */
+	if (++g_free_q.size > g_free_q.max_size)
+		g_free_q.max_size = g_free_q.size;
+
+	unlock_queue(&g_free_q);
+}
+
+static inline work_q_item_t *
+dequeue_work_item(void)
+{
+	work_q_item_t *work;
+
+	/* retrieve the 1st item on the work queue */
+	lock_queue(&g_work_q);
+
+	if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q)))
+		g_work_q.size--;
+
+	unlock_queue(&g_work_q);
+	return work;
+}
+
+static int
+enqueue_work_item_and_wait_for_result(work_q_item_t *item)
+{
+	/* put the work item at the end of the work queue */
+	lock_queue(&g_work_q);
+	sq_addlast(&item->link, &(g_work_q.q));
+
+	/* Adjust the queue size and potentially the maximum queue size */
+	if (++g_work_q.size > g_work_q.max_size)
+		g_work_q.max_size = g_work_q.size;
+
+	unlock_queue(&g_work_q);
+
+	/* tell the work thread that work is available */
+	sem_post(&g_work_queued_sema);
+
+	/* wait for the result */
+	sem_wait(&item->wait_sem);
+
+	int result = item->result;
+
+	destroy_work_item(item);
+
+	return result;
+}
+
+/* Calculate the offset in file of specific item */
+static int
+calculate_offset(dm_item_t item, unsigned char index)
+{
+
+	/* Make sure the item type is valid */
+	if (item >= DM_KEY_NUM_KEYS)
+		return -1;
+
+	/* Make sure the index for this item type is valid */
+	if (index >= g_per_item_max_index[item])
+		return -1;
+
+	/* Calculate and return the item index based on type and index */
+	return g_key_offsets[item] + (index * k_sector_size);
+}
+
+/* Each data item is stored as follows
+ *
+ * byte 0: Length of user data item
+ * byte 1: Persistence of this data item
+ * byte 2: Unused (for future use)
+ * byte 3: Unused (for future use)
+ * byte DM_SECTOR_HDR_SIZE... : data item value
+ *
+ * The total size must not exceed k_sector_size
+ */
+
+/* write to the data manager file */
+static ssize_t
+_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
+{
+	unsigned char buffer[k_sector_size];
+	size_t len;
+	int offset;
+
+	/* Get the offset for this item */
+	offset = calculate_offset(item, index);
+
+	/* If item type or index out of range, return error */
+	if (offset < 0) 
+		return -1;
+
+	/* Make sure caller has not given us more data than we can handle */
+	if (count > DM_MAX_DATA_SIZE) 
+		return -1;
+
+	/* Write out the data, prefixed with length and persistence level */
+	buffer[0] = count;
+	buffer[1] = persistence;
+	buffer[2] = 0;
+	buffer[3] = 0;
+	memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count);
+	count += DM_SECTOR_HDR_SIZE;
+
+	len = -1;
+
+	/* Seek to the right spot in the data manager file and write the data item */
+	if (lseek(g_task_fd, offset, SEEK_SET) == offset)
+		if ((len = write(g_task_fd, buffer, count)) == count)
+			fsync(g_task_fd); /* Make sure data is written to physical media */
+
+	/* Make sure the write succeeded */
+	if (len != count)
+		return -1;
+
+	/* All is well... return the number of user data written */
+	return count - DM_SECTOR_HDR_SIZE;
+}
+
+/* Retrieve from the data manager file */
+static ssize_t
+_read(dm_item_t item, unsigned char index, void *buf, size_t count)
+{
+	unsigned char buffer[k_sector_size];
+	int len, offset;
+
+	/* Get the offset for this item */
+	offset = calculate_offset(item, index);
+
+	/* If item type or index out of range, return error */
+	if (offset < 0)
+		return -1;
+
+	/* Make sure the caller hasn't asked for more data than we can handle */
+	if (count > DM_MAX_DATA_SIZE)
+		return -1;
+
+	/* Read the prefix and data */
+	len = -1;
+	if (lseek(g_task_fd, offset, SEEK_SET) == offset)
+		len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE);
+
+	/* Check for read error */
+	if (len < 0)
+		return -1;
+
+	/* A zero length entry is a empty entry */
+	if (len == 0)
+		buffer[0] = 0;
+
+	/* See if we got data */
+	if (buffer[0] > 0) {
+		/* We got more than requested!!! */
+		if (buffer[0] > count)
+			return -1;
+
+		/* Looks good, copy it to the caller's buffer */
+		memcpy(buf, buffer + DM_SECTOR_HDR_SIZE, buffer[0]);
+	}
+
+	/* Return the number of bytes of caller data read */
+	return buffer[0];
+}
+
+static int
+_clear(dm_item_t item)
+{
+	int i, result = 0;
+
+	/* Get the offset of 1st item of this type */
+	int offset = calculate_offset(item, 0);
+
+	/* Check for item type out of range */
+	if (offset < 0)
+		return -1;
+
+	/* Clear all items of this type */
+	for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) {
+		char buf[1];
+
+		if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+			result = -1;
+			break;
+		}
+
+		/* Avoid SD flash wear by only doing writes where necessary */
+		if (read(g_task_fd, buf, 1) < 1)
+			break;
+
+		/* If item has length greater than 0 it needs to be overwritten */
+		if (buf[0]) {
+			if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+				result = -1;
+				break;
+			}
+
+			buf[0] = 0;
+
+			if (write(g_task_fd, buf, 1) != 1) {
+				result = -1;
+				break;
+			}
+		}
+
+		offset += k_sector_size;
+	}
+
+	/* Make sure data is actually written to physical media */
+	fsync(g_task_fd);
+	return result;
+}
+
+/* Tell the data manager about the type of the last reset */
+static int
+_restart(dm_reset_reason reason)
+{
+	unsigned char buffer[2];
+	int offset, result = 0;
+
+	/* We need to scan the entire file and invalidate and data that should not persist after the last reset */
+
+	/* Loop through all of the data segments and delete those that are not persistent */
+	offset = 0;
+
+	while (1) {
+		size_t len;
+
+		/* Get data segment at current offset */
+		if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+			result = -1;
+			break;
+		}
+
+		len = read(g_task_fd, buffer, sizeof(buffer));
+
+		if (len == 0)
+			break;
+
+		/* check if segment contains data */
+		if (buffer[0]) {
+			int clear_entry = 0;
+
+			/* Whether data gets deleted depends on reset type and data segment's persistence setting */
+			if (reason == DM_INIT_REASON_POWER_ON) {
+				if (buffer[1] != DM_PERSIST_POWER_ON_RESET) {
+					clear_entry = 1;
+				}
+
+			} else {
+				if ((buffer[1] != DM_PERSIST_POWER_ON_RESET) && (buffer[1] != DM_PERSIST_IN_FLIGHT_RESET)) {
+					clear_entry = 1;
+				}
+			}
+
+			/* Set segment to unused if data does not persist */
+			if (clear_entry) {
+				if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+					result = -1;
+					break;
+				}
+
+				buffer[0] = 0;
+
+				len = write(g_task_fd, buffer, 1);
+
+				if (len != 1) {
+					result = -1;
+					break;
+				}
+			}
+		}
+
+		offset += k_sector_size;
+	}
+
+	fsync(g_task_fd);
+
+	/* tell the caller how it went */
+	return result;
+}
+
+/* write to the data manager file */
+__EXPORT ssize_t
+dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
+{
+	work_q_item_t *work;
+
+	/* Make sure data manager has been started and is not shutting down */
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* get a work item and queue up a write request */
+	if ((work = create_work_item()) == NULL) 
+		return -1;
+
+	work->func = dm_write_func;
+	work->write_params.item = item;
+	work->write_params.index = index;
+	work->write_params.persistence = persistence;
+	work->write_params.buf = buf;
+	work->write_params.count = count;
+
+	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
+	return (ssize_t)enqueue_work_item_and_wait_for_result(work);
+}
+
+/* Retrieve from the data manager file */
+__EXPORT ssize_t
+dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
+{
+	work_q_item_t *work;
+
+	/* Make sure data manager has been started and is not shutting down */
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* get a work item and queue up a read request */
+	if ((work = create_work_item()) == NULL)
+		return -1;
+
+	work->func = dm_read_func;
+	work->read_params.item = item;
+	work->read_params.index = index;
+	work->read_params.buf = buf;
+	work->read_params.count = count;
+
+	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
+	return (ssize_t)enqueue_work_item_and_wait_for_result(work);
+}
+
+__EXPORT int
+dm_clear(dm_item_t item)
+{
+	work_q_item_t *work;
+
+	/* Make sure data manager has been started and is not shutting down */
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* get a work item and queue up a clear request */
+	if ((work = create_work_item()) == NULL)
+		return -1;
+
+	work->func = dm_clear_func;
+	work->clear_params.item = item;
+
+	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
+	return enqueue_work_item_and_wait_for_result(work);
+}
+
+/* Tell the data manager about the type of the last reset */
+__EXPORT int
+dm_restart(dm_reset_reason reason)
+{
+	work_q_item_t *work;
+
+	/* Make sure data manager has been started and is not shutting down */
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* get a work item and queue up a restart request */
+	if ((work = create_work_item()) == NULL)
+		return -1;
+
+	work->func = dm_restart_func;
+	work->restart_params.reason = reason;
+
+	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
+	return enqueue_work_item_and_wait_for_result(work);
+}
+
+static int
+task_main(int argc, char *argv[])
+{
+	work_q_item_t *work;
+
+	/* inform about start */
+	warnx("Initializing..");
+
+	/* Initialize global variables */
+	g_key_offsets[0] = 0;
+
+	for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++)
+		g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * k_sector_size);
+
+	unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * k_sector_size);
+
+	for (unsigned i = 0; i < dm_number_of_funcs; i++)
+		g_func_counts[i] = 0;
+
+	g_task_should_exit = false;
+
+	init_q(&g_work_q);
+	init_q(&g_free_q);
+
+	sem_init(&g_work_queued_sema, 1, 0);
+
+	/* Open or create the data manager file */
+	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
+	if (g_task_fd < 0) {
+		warnx("Could not open data manager file %s", k_data_manager_device_path);
+		sem_post(&g_init_sema); /* Don't want to hang startup */
+		return -1;
+	}
+	if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
+		close(g_task_fd);
+		warnx("Could not seek data manager file %s", k_data_manager_device_path);
+		sem_post(&g_init_sema); /* Don't want to hang startup */
+		return -1;
+	}
+	fsync(g_task_fd);
+
+	/* We use two file descriptors, one for the caller context and one for the worker thread */
+	/* They are actually the same but we need to some way to reject caller request while the */
+	/* worker thread is shutting down but still processing requests */
+	g_fd = g_task_fd;
+
+	warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset);
+
+	/* Tell startup that the worker thread has completed its initialization */
+	sem_post(&g_init_sema);
+
+	/* Start the endless loop, waiting for then processing work requests */
+	while (true) {
+
+		/* do we need to exit ??? */
+		if ((g_task_should_exit) && (g_fd >= 0)) {
+			/* Close the file handle to stop further queuing */
+			g_fd = -1;
+		}
+
+		if (!g_task_should_exit) {
+			/* wait for work */
+			sem_wait(&g_work_queued_sema);
+		}
+
+		/* Empty the work queue */
+		while ((work = dequeue_work_item())) {
+
+			/* handle each work item with the appropriate handler */
+			switch (work->func) {
+			case dm_write_func:
+				g_func_counts[dm_write_func]++;
+				work->result =
+					_write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf, work->write_params.count);
+				break;
+
+			case dm_read_func:
+				g_func_counts[dm_read_func]++;
+				work->result =
+					_read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count);
+				break;
+
+			case dm_clear_func:
+				g_func_counts[dm_clear_func]++;
+				work->result = _clear(work->clear_params.item);
+				break;
+
+			case dm_restart_func:
+				g_func_counts[dm_restart_func]++;
+				work->result = _restart(work->restart_params.reason);
+				break;
+
+			default: /* should never happen */
+				work->result = -1;
+				break;
+			}
+
+			/* Inform the caller that work is done */
+			sem_post(&work->wait_sem);
+		}
+
+		/* time to go???? */
+		if ((g_task_should_exit) && (g_fd < 0))
+			break;
+	}
+
+	close(g_task_fd);
+	g_task_fd = -1;
+
+	/* The work queue is now empty, empty the free queue */
+	for (;;) {
+		if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
+			break;
+
+		free(work);
+	}
+
+	destroy_q(&g_work_q);
+	destroy_q(&g_free_q);
+	sem_destroy(&g_work_queued_sema);
+
+	return 0;
+}
+
+static int
+start(void)
+{
+	int task;
+
+	sem_init(&g_init_sema, 1, 0);
+
+	/* start the worker thread */
+	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) {
+		warn("task start failed");
+		return -1;
+	}
+
+	/* wait for the thread to actuall initialize */
+	sem_wait(&g_init_sema);
+	sem_destroy(&g_init_sema);
+
+	return 0;
+}
+
+static void
+status(void)
+{
+	/* display usage statistics */
+	warnx("Writes   %d", g_func_counts[dm_write_func]);
+	warnx("Reads    %d", g_func_counts[dm_read_func]);
+	warnx("Clears   %d", g_func_counts[dm_clear_func]);
+	warnx("Restarts %d", g_func_counts[dm_restart_func]);
+	warnx("Max Q lengths work %d, free %d", g_work_q.max_size, g_free_q.max_size);
+}
+
+static void
+stop(void)
+{
+	/* Tell the worker task to shut down */
+	g_task_should_exit = true;
+	sem_post(&g_work_queued_sema);
+}
+
+static void
+usage(void)
+{
+	errx(1, "usage: dataman {start|stop|status|poweronrestart|inflightrestart}");
+}
+
+int
+dataman_main(int argc, char *argv[])
+{
+	if (argc < 2)
+		usage();
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (g_fd >= 0)
+			errx(1, "already running");
+
+		start();
+
+		if (g_fd < 0)
+			errx(1, "start failed");
+
+		exit(0);
+	}
+
+	/* Worker thread should be running for all other commands */
+	if (g_fd < 0)
+		errx(1, "not running");
+
+	if (!strcmp(argv[1], "stop"))
+		stop();
+	else if (!strcmp(argv[1], "status"))
+		status();
+    else if (!strcmp(argv[1], "poweronrestart"))
+        dm_restart(DM_INIT_REASON_POWER_ON);
+    else if (!strcmp(argv[1], "inflightrestart"))
+        dm_restart(DM_INIT_REASON_IN_FLIGHT);
+	else
+		usage();
+
+	exit(1);
+}
+
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
new file mode 100644
index 000000000..a70638ccc
--- /dev/null
+++ b/src/modules/dataman/dataman.h
@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file dataman.h
+ *
+ * DATAMANAGER driver.
+ */
+#ifndef _DATAMANAGER_H
+#define _DATAMANAGER_H
+
+#include <uORB/topics/mission.h>
+#include <uORB/topics/fence.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+	/* Types of items that the data manager can store */
+	typedef enum {
+		DM_KEY_SAFE_POINTS = 0,		/* Safe points coordinates, safe point 0 is home point */
+		DM_KEY_FENCE_POINTS,		/* Fence vertex coordinates */
+		DM_KEY_WAYPOINTS_OFFBOARD_0,	/* Mission way point coordinates sent over mavlink */
+		DM_KEY_WAYPOINTS_OFFBOARD_1,	/* (alernate between 0 and 1) */
+		DM_KEY_WAYPOINTS_ONBOARD,	/* Mission way point coordinates generated onboard */
+		DM_KEY_NUM_KEYS			/* Total number of item types defined */
+	} dm_item_t;
+
+	/* The maximum number of instances for each item type */
+	enum {
+		DM_KEY_SAFE_POINTS_MAX = 8,
+		DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
+		DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
+		DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
+		DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED
+	};
+
+	/* Data persistence levels */
+	typedef enum {
+		DM_PERSIST_POWER_ON_RESET = 0,	/* Data survives all resets */
+		DM_PERSIST_IN_FLIGHT_RESET,     /* Data survives in-flight resets only */
+		DM_PERSIST_VOLATILE             /* Data does not survive resets */
+	} dm_persitence_t;
+
+	/* The reason for the last reset */
+	typedef enum {
+		DM_INIT_REASON_POWER_ON = 0,	/* Data survives resets */
+		DM_INIT_REASON_IN_FLIGHT	/* Data survives in-flight resets only */
+	} dm_reset_reason;
+
+	/* Maximum size in bytes of a single item instance */
+	#define DM_MAX_DATA_SIZE 126
+
+	/* Retrieve from the data manager store */
+	__EXPORT ssize_t
+	dm_read(
+		dm_item_t item,			/* The item type to retrieve */
+		unsigned char index,		/* The index of the item */
+		void *buffer,			/* Pointer to caller data buffer */
+		size_t buflen			/* Length in bytes of data to retrieve */
+	);
+
+	/* write to the data manager store */
+	__EXPORT ssize_t
+	dm_write(
+		dm_item_t  item,		/* The item type to store */
+		unsigned char index,		/* The index of the item */
+		dm_persitence_t persistence,	/* The persistence level of this item */
+		const void *buffer,		/* Pointer to caller data buffer */
+		size_t buflen			/* Length in bytes of data to retrieve */
+	);
+
+	/* Retrieve from the data manager store */
+	__EXPORT int
+	dm_clear(
+		dm_item_t item			/* The item type to clear */
+		);
+
+	/* Tell the data manager about the type of the last reset */
+	__EXPORT int
+	dm_restart(
+		dm_reset_reason restart_type	/* The last reset type */
+	);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/modules/multirotor_att_control/module.mk b/src/modules/dataman/module.mk
index 7569e1c7e..dce7a6235 100755..100644
--- a/src/modules/multirotor_att_control/module.mk
+++ b/src/modules/dataman/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   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
@@ -32,11 +32,11 @@
 ############################################################################
 
 #
-# Build multirotor attitude controller
+# Main Navigation Controller
 #
 
-MODULE_COMMAND	= multirotor_att_control
+MODULE_COMMAND	= dataman
 
-SRCS		= multirotor_att_control_main.c \
-		  multirotor_attitude_control.c \
-		  multirotor_rate_control.c
+SRCS		= dataman.c
+
+INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index 6dc19df41..f7c0b6148 100644
--- a/src/modules/fixedwing_backside/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -117,7 +117,7 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
 	_vCmd(this, "V_CMD"),
 	_crMax(this, "CR_MAX"),
 	_attPoll(),
-	_lastPosCmd(),
+	_lastMissionCmd(),
 	_timeStamp(0)
 {
 	_attPoll.fd = _att.getHandle();
@@ -141,8 +141,8 @@ void BlockMultiModeBacksideAutopilot::update()
 	setDt(dt);
 
 	// store old position command before update if new command sent
-	if (_posCmd.updated()) {
-		_lastPosCmd = _posCmd.getData();
+	if (_missionCmd.updated()) {
+		_lastMissionCmd = _missionCmd.getData();
 	}
 
 	// check for new updates
@@ -159,7 +159,7 @@ void BlockMultiModeBacksideAutopilot::update()
 	if (_status.main_state == MAIN_STATE_AUTO) {
 		// TODO use vehicle_control_mode here?
 		// update guidance
-		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
+		_guide.update(_pos, _att, _missionCmd.current, _lastMissionCmd.current);
 	}
 
 	// XXX handle STABILIZED (loiter on spot) as well
@@ -174,15 +174,15 @@ void BlockMultiModeBacksideAutopilot::update()
 		// of control we will limit the velocity feedback between
 		// the min/max velocity
 		float v = _vLimit.update(sqrtf(
-					_pos.vx * _pos.vx +
+					_pos.vel_n * _pos.vel_n +
 					_pos.vy * _pos.vy +
-					_pos.vz * _pos.vz));
+					_pos.vel_d * _pos.vel_d));
 
 		// limit velocity command between min/max velocity
 		float vCmd = _vLimit.update(_vCmd.get());
 
 		// altitude hold
-		float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt);
+		float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt);
 
 		// heading hold
 		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
@@ -236,9 +236,9 @@ void BlockMultiModeBacksideAutopilot::update()
 		// 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.vel_n * _pos.vel_n +
 					_pos.vy * _pos.vy +
-					_pos.vz * _pos.vz));
+					_pos.vel_d * _pos.vel_d));
 
 		// pitch channel -> rate of climb
 		// TODO, might want to put a gain on this, otherwise commanding
diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp
index 567efeb35..e1c85c261 100644
--- a/src/modules/fixedwing_backside/fixedwing.hpp
+++ b/src/modules/fixedwing_backside/fixedwing.hpp
@@ -264,7 +264,7 @@ private:
 	BlockParamFloat _crMax;
 
 	struct pollfd _attPoll;
-	vehicle_global_position_set_triplet_s _lastPosCmd;
+	position_setpoint_triplet_s _lastMissionCmd;
 	enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
 	uint64_t _timeStamp;
 public:
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 73df3fb9e..888dd0942 100644
--- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
+++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
@@ -299,7 +299,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
 					start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
 					global_sp_updated_set_once = true;
-					psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
+					psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon,
 							(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
 
 					printf("next wp direction: %0.4f\n", (double)psi_track);
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 60c902ce5..17b1028f9 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -37,6 +37,7 @@
  * Implementation of a generic attitude controller based on classic orthogonal PIDs.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  *
  */
 
@@ -62,6 +63,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_global_position.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/pid/pid.h>
@@ -106,26 +108,30 @@ private:
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
 	int		_control_task;			/**< task handle for sensor task */
 
-	int		_att_sub;			/**< vehicle attitude subscription */
-	int		_accel_sub;			/**< accelerometer subscription */
+	int		_att_sub;				/**< vehicle attitude subscription */
+	int		_accel_sub;				/**< accelerometer subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
-	int		_vcontrol_mode_sub;			/**< vehicle status subscription */
-	int 		_params_sub;			/**< notification of parameter updates */
-	int 		_manual_sub;			/**< notification of manual control updates */
+	int		_vcontrol_mode_sub;		/**< vehicle status subscription */
+	int 	_params_sub;			/**< notification of parameter updates */
+	int 	_manual_sub;			/**< notification of manual control updates */
+	int		_global_pos_sub;		/**< global position subscription */
 
 	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint point */
 	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
+	orb_advert_t	_actuators_1_pub;		/**< actuator control group 1 setpoint (Airframe) */
 
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct accel_report				_accel;			/**< body frame accelerations */
+	struct vehicle_attitude_s				_att;			/**< vehicle attitude */
+	struct accel_report						_accel;			/**< body frame accelerations */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct airspeed_s				_airspeed;		/**< airspeed */
-	struct vehicle_control_mode_s			_vcontrol_mode;		/**< vehicle control mode */
+	struct airspeed_s						_airspeed;		/**< airspeed */
+	struct vehicle_control_mode_s			_vcontrol_mode;	/**< vehicle control mode */
 	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
+	struct actuator_controls_s			_actuators_airframe;		/**< actuator control inputs */
+	struct vehicle_global_position_s		_global_pos;	/**< global position */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -137,6 +143,7 @@ private:
 		float p_p;
 		float p_d;
 		float p_i;
+		float p_ff;
 		float p_rmax_pos;
 		float p_rmax_neg;
 		float p_integrator_max;
@@ -144,13 +151,17 @@ private:
 		float r_p;
 		float r_d;
 		float r_i;
+		float r_ff;
 		float r_integrator_max;
 		float r_rmax;
 		float y_p;
 		float y_i;
 		float y_d;
+		float y_ff;
 		float y_roll_feedforward;
 		float y_integrator_max;
+		float y_coordinated_min_speed;
+		float y_rmax;
 
 		float airspeed_min;
 		float airspeed_trim;
@@ -163,6 +174,7 @@ private:
 		param_t p_p;
 		param_t p_d;
 		param_t p_i;
+		param_t p_ff;
 		param_t p_rmax_pos;
 		param_t p_rmax_neg;
 		param_t p_integrator_max;
@@ -170,13 +182,17 @@ private:
 		param_t r_p;
 		param_t r_d;
 		param_t r_i;
+		param_t r_ff;
 		param_t r_integrator_max;
 		param_t r_rmax;
 		param_t y_p;
 		param_t y_i;
 		param_t y_d;
+		param_t y_ff;
 		param_t y_roll_feedforward;
 		param_t y_integrator_max;
+		param_t y_coordinated_min_speed;
+		param_t y_rmax;
 
 		param_t airspeed_min;
 		param_t airspeed_trim;
@@ -227,6 +243,11 @@ private:
 	void		vehicle_setpoint_poll();
 
 	/**
+	 * Check for global position updates.
+	 */
+	void		global_pos_poll();
+
+	/**
 	 * Shim for calling task_main from task_create.
 	 */
 	static void	task_main_trampoline(int argc, char *argv[]);
@@ -261,11 +282,13 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
 	_manual_sub(-1),
+	_global_pos_sub(-1),
 
 /* publications */
 	_rate_sp_pub(-1),
 	_actuators_0_pub(-1),
 	_attitude_sp_pub(-1),
+	_actuators_1_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@@ -273,31 +296,45 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_setpoint_valid(false),
 	_airspeed_valid(false)
 {
+	/* safely initialize structs */
+	_att = {0};
+	_accel = {0};
+	_att_sp = {0};
+	_manual = {0};
+	_airspeed = {0};
+	_vcontrol_mode = {0};
+	_actuators = {0};
+	_actuators_airframe = {0};
+	_global_pos = {0};
+
+
 	_parameter_handles.tconst = param_find("FW_ATT_TC");
-	_parameter_handles.p_p = param_find("FW_P_P");
-	_parameter_handles.p_d = param_find("FW_P_D");
-	_parameter_handles.p_i = param_find("FW_P_I");
+	_parameter_handles.p_p = param_find("FW_PR_P");
+	_parameter_handles.p_i = param_find("FW_PR_I");
+	_parameter_handles.p_ff = param_find("FW_PR_FF");
 	_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
 	_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
-	_parameter_handles.p_integrator_max = param_find("FW_P_IMAX");
+	_parameter_handles.p_integrator_max = param_find("FW_PR_IMAX");
 	_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
 
-	_parameter_handles.r_p = param_find("FW_R_P");
-	_parameter_handles.r_d = param_find("FW_R_D");
-	_parameter_handles.r_i = param_find("FW_R_I");
-	_parameter_handles.r_integrator_max = param_find("FW_R_IMAX");
+	_parameter_handles.r_p = param_find("FW_RR_P");
+	_parameter_handles.r_i = param_find("FW_RR_I");
+	_parameter_handles.r_ff = param_find("FW_RR_FF");
+	_parameter_handles.r_integrator_max = param_find("FW_RR_IMAX");
 	_parameter_handles.r_rmax = param_find("FW_R_RMAX");
 
-	_parameter_handles.y_p = param_find("FW_Y_P");
-	_parameter_handles.y_i = param_find("FW_Y_I");
-	_parameter_handles.y_d = param_find("FW_Y_D");
-	_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
-	_parameter_handles.y_integrator_max = param_find("FW_Y_IMAX");
+	_parameter_handles.y_p = param_find("FW_YR_P");
+	_parameter_handles.y_i = param_find("FW_YR_I");
+	_parameter_handles.y_ff = param_find("FW_YR_FF");
+	_parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
+	_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
 
 	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
 	_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
 
+	_parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -333,24 +370,26 @@ FixedwingAttitudeControl::parameters_update()
 
 	param_get(_parameter_handles.tconst, &(_parameters.tconst));
 	param_get(_parameter_handles.p_p, &(_parameters.p_p));
-	param_get(_parameter_handles.p_d, &(_parameters.p_d));
 	param_get(_parameter_handles.p_i, &(_parameters.p_i));
+	param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
 	param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
 	param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
 	param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
 	param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward));
 
 	param_get(_parameter_handles.r_p, &(_parameters.r_p));
-	param_get(_parameter_handles.r_d, &(_parameters.r_d));
 	param_get(_parameter_handles.r_i, &(_parameters.r_i));
+	param_get(_parameter_handles.r_ff, &(_parameters.r_ff));
+
 	param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
 	param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));
 
 	param_get(_parameter_handles.y_p, &(_parameters.y_p));
 	param_get(_parameter_handles.y_i, &(_parameters.y_i));
-	param_get(_parameter_handles.y_d, &(_parameters.y_d));
-	param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward));
+	param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
 	param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
+	param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
+	param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
 
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
@@ -358,28 +397,29 @@ FixedwingAttitudeControl::parameters_update()
 
 	/* pitch control parameters */
 	_pitch_ctrl.set_time_constant(_parameters.tconst);
-	_pitch_ctrl.set_k_p(math::radians(_parameters.p_p));
-	_pitch_ctrl.set_k_i(math::radians(_parameters.p_i));
-	_pitch_ctrl.set_k_d(math::radians(_parameters.p_d));
-	_pitch_ctrl.set_integrator_max(math::radians(_parameters.p_integrator_max));
+	_pitch_ctrl.set_k_p(_parameters.p_p);
+	_pitch_ctrl.set_k_i(_parameters.p_i);
+	_pitch_ctrl.set_k_ff(_parameters.p_ff);
+	_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
 	_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
 	_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
-	_pitch_ctrl.set_roll_ff(math::radians(_parameters.p_roll_feedforward));
+	_pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward);
 
 	/* roll control parameters */
 	_roll_ctrl.set_time_constant(_parameters.tconst);
-	_roll_ctrl.set_k_p(math::radians(_parameters.r_p));
-	_roll_ctrl.set_k_i(math::radians(_parameters.r_i));
-	_roll_ctrl.set_k_d(math::radians(_parameters.r_d));
-	_roll_ctrl.set_integrator_max(math::radians(_parameters.r_integrator_max));
+	_roll_ctrl.set_k_p(_parameters.r_p);
+	_roll_ctrl.set_k_i(_parameters.r_i);
+	_roll_ctrl.set_k_ff(_parameters.r_ff);
+	_roll_ctrl.set_integrator_max(_parameters.r_integrator_max);
 	_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
 
 	/* yaw control parameters */
-	_yaw_ctrl.set_k_side(math::radians(_parameters.y_p));
-	_yaw_ctrl.set_k_i(math::radians(_parameters.y_i));
-	_yaw_ctrl.set_k_d(math::radians(_parameters.y_d));
-	_yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward));
-	_yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max));
+	_yaw_ctrl.set_k_p(_parameters.y_p);
+	_yaw_ctrl.set_k_i(_parameters.y_i);
+	_yaw_ctrl.set_k_ff(_parameters.y_ff);
+	_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
+	_yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed);
+	_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
 
 	return OK;
 }
@@ -421,6 +461,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll()
 
 	if (airspeed_updated) {
 		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+//		warnx("airspeed poll: ind: %.4f,  true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s);
 		return true;
 	}
 
@@ -453,6 +494,18 @@ FixedwingAttitudeControl::vehicle_setpoint_poll()
 }
 
 void
+FixedwingAttitudeControl::global_pos_poll()
+{
+	/* check if there is a new global position */
+	bool global_pos_updated;
+	orb_check(_global_pos_sub, &global_pos_updated);
+
+	if (global_pos_updated) {
+		orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
+	}
+}
+
+void
 FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
 {
 	att_control::g_control->task_main();
@@ -476,6 +529,7 @@ FixedwingAttitudeControl::task_main()
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vcontrol_mode_sub, 200);
@@ -552,6 +606,8 @@ FixedwingAttitudeControl::task_main()
 
 			vehicle_manual_poll();
 
+			global_pos_poll();
+
 			/* lock integrator until control is started */
 			bool lock_integrator;
 
@@ -562,22 +618,28 @@ FixedwingAttitudeControl::task_main()
 				lock_integrator = true;
 			}
 
+			/* Simple handling of failsafe: deploy parachute if failsafe is on */
+			if (_vcontrol_mode.flag_control_termination_enabled) {
+				_actuators_airframe.control[1] = 1.0f;
+//				warnx("_actuators_airframe.control[1] = 1.0f;");
+			} else {
+				_actuators_airframe.control[1] = 0.0f;
+//				warnx("_actuators_airframe.control[1] = -1.0f;");
+			}
+
 			/* decide if in stabilized or full manual control */
 
 			if (_vcontrol_mode.flag_control_attitude_enabled) {
 
-				/* scale from radians to normalized -1 .. 1 range */
-				const float actuator_scaling = 1.0f / (M_PI_F / 4.0f);
-
 				/* scale around tuning airspeed */
 
 				float airspeed;
 
 				/* if airspeed is smaller than min, the sensor is not giving good readings */
 				if (!_airspeed_valid ||
-				    (_airspeed.indicated_airspeed_m_s < _parameters.airspeed_min) ||
+				    (_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
 				    !isfinite(_airspeed.indicated_airspeed_m_s)) {
-					airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
+					airspeed = _parameters.airspeed_trim;
 
 				} else {
 					airspeed = _airspeed.indicated_airspeed_m_s;
@@ -586,7 +648,8 @@ FixedwingAttitudeControl::task_main()
 				float airspeed_scaling = _parameters.airspeed_trim / airspeed;
 				//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
 
-				float roll_sp, pitch_sp;
+				float roll_sp = 0.0f;
+				float pitch_sp = 0.0f;
 				float throttle_sp = 0.0f;
 
 				if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
@@ -636,46 +699,86 @@ FixedwingAttitudeControl::task_main()
 					}
 				}
 
-				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
+				/* Prepare speed_body_u and speed_body_w */
+				float speed_body_u = 0.0f;
+				float speed_body_v = 0.0f;
+				float speed_body_w = 0.0f;
+				if(_att.R_valid) 	{
+					speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d;
+					speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
+					speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
+				} else	{
+					warnx("Did not get a valid R\n");
+				}
 
-					float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
-									    airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-					_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
+				/* Run attitude controllers */
+				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
+					_roll_ctrl.control_attitude(roll_sp, _att.roll);
+					_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
+					_yaw_ctrl.control_attitude(_att.roll, _att.pitch,
+							speed_body_u, speed_body_v, speed_body_w,
+							_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
+
+					/* Run attitude RATE controllers which need the desired attitudes from above */
+					float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
+							_att.rollspeed, _att.yawspeed,
+							_yaw_ctrl.get_desired_rate(),
+							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f;
+					if (!isfinite(roll_u)) {
+						warnx("roll_u %.4f", roll_u);
+					}
 
-					float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
-									      lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-					_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
+					float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
+							_att.pitchspeed, _att.yawspeed,
+							_yaw_ctrl.get_desired_rate(),
+							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f;
+					if (!isfinite(pitch_u)) {
+						warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
+								pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body);
+					}
 
-					float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
-									  _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-					_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
+					float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
+							_att.pitchspeed, _att.yawspeed,
+							_pitch_ctrl.get_desired_rate(),
+							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f;
+					if (!isfinite(yaw_u)) {
+						warnx("yaw_u %.4f", yaw_u);
+					}
 
 					/* throttle passed through */
 					_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+					if (!isfinite(throttle_sp)) {
+						warnx("throttle_sp %.4f", throttle_sp);
+					}
+				} else {
+					warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp);
+				}
 
-					// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
-					// 			airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
-					// 			_actuators.control[2], _actuators.control[3]);
+				// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
+				// 			airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
+				// 			_actuators.control[2], _actuators.control[3]);
 
-					/*
-					 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
-					 * only once available
-					 */
-					vehicle_rates_setpoint_s rates_sp;
-					rates_sp.roll = _roll_ctrl.get_desired_rate();
-					rates_sp.pitch = _pitch_ctrl.get_desired_rate();
-					rates_sp.yaw = 0.0f; // XXX not yet implemented
+				/*
+				 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
+				 * only once available
+				 */
+				vehicle_rates_setpoint_s rates_sp;
+				rates_sp.roll = _roll_ctrl.get_desired_rate();
+				rates_sp.pitch = _pitch_ctrl.get_desired_rate();
+				rates_sp.yaw = _yaw_ctrl.get_desired_rate();
 
-					rates_sp.timestamp = hrt_absolute_time();
+				rates_sp.timestamp = hrt_absolute_time();
 
-					if (_rate_sp_pub > 0) {
-						/* publish the attitude setpoint */
-						orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
+				if (_rate_sp_pub > 0) {
+					/* publish the attitude setpoint */
+					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
 
-					} else {
-						/* advertise and publish */
-						_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
-					}
+				} else {
+					/* advertise and publish */
+					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
 				}
 
 			} else {
@@ -693,6 +796,7 @@ FixedwingAttitudeControl::task_main()
 
 			/* lazily publish the setpoint only once available */
 			_actuators.timestamp = hrt_absolute_time();
+			_actuators_airframe.timestamp = hrt_absolute_time();
 
 			if (_actuators_0_pub > 0) {
 				/* publish the attitude setpoint */
@@ -703,6 +807,19 @@ FixedwingAttitudeControl::task_main()
 				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
 			}
 
+			if (_actuators_1_pub > 0) {
+				/* publish the attitude setpoint */
+				orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe);
+//				warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
+//						(double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2],
+//						(double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5],
+//						(double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]);
+
+			} else {
+				/* advertise and publish */
+				_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
+			}
+
 		}
 
 		perf_end(_loop_perf);
diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index be76524da..1c615094c 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+f *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -38,36 +38,32 @@
  * Parameters defined by the fixed-wing attitude control task
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
 
 #include <systemlib/param/param.h>
 
+
 /*
  * Controller parameters, accessible via MAVLink
  *
  */
-
 // @DisplayName		Attitude Time Constant
-// @Description		This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
+// @Description		This defines the latency between a step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
 // @Range		0.4 to 1.0 seconds, in tens of seconds
 PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
 
-// @DisplayName		Proportional gain.
-// @Description		This defines how much the elevator input will be commanded dependend on the current pitch error.
+// @DisplayName		Pitch rate proportional gain.
+// @Description		This defines how much the elevator input will be commanded depending on the current body angular rate error.
 // @Range		10 to 200, 1 increments
-PARAM_DEFINE_FLOAT(FW_P_P, 40.0f);
-
-// @DisplayName		Damping gain.
-// @Description		This gain damps the airframe pitch rate. In particular relevant for flying wings.
-// @Range		0.0 to 10.0, 0.1 increments
-PARAM_DEFINE_FLOAT(FW_P_D, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f);
 
-// @DisplayName		Integrator gain.
+// @DisplayName		Pitch rate integrator gain.
 // @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
 // @Range		0 to 50.0
-PARAM_DEFINE_FLOAT(FW_P_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f);
 
 // @DisplayName		Maximum positive / up pitch rate.
 // @Description		This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
@@ -79,58 +75,104 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
 // @Range		0 to 90.0 degrees per seconds, in 1 increments
 PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
 
-// @DisplayName		Pitch Integrator Anti-Windup
-// @Description		This limits the range in degrees the integrator can wind up to.
-// @Range		0.0 to 45.0
-// @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f);
+// @DisplayName		Pitch rate integrator limit
+// @Description		The portion of the integrator part in the control surface deflection is limited to this value
+// @Range		0.0 to 1
+// @Increment		0.1
+PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
 
-// @DisplayName		Roll feedforward gain.
+// @DisplayName		Roll to Pitch feedforward gain.
 // @Description		This compensates during turns and ensures the nose stays level.
 // @Range		0.5 2.0
 // @Increment		0.05
 // @User		User
-PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f);
+PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...)
 
-// @DisplayName		Proportional Gain.
-// @Description		This gain controls the roll angle to roll actuator output.
+// @DisplayName		Roll rate proportional Gain.
+// @Description		This defines how much the aileron input will be commanded depending on the current body angular rate error.
 // @Range		10.0 200.0
 // @Increment		10.0
 // @User		User
-PARAM_DEFINE_FLOAT(FW_R_P, 40.0f);
+PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
 
-// @DisplayName		Damping Gain
-// @Description		Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence.
-// @Range		0.0 10.0
-// @Increment		1.0
-// @User		User
-PARAM_DEFINE_FLOAT(FW_R_D, 0.0f);
-
-// @DisplayName		Integrator Gain
-// @Description		This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors.
+// @DisplayName		Roll rate integrator Gain
+// @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
 // @Range		0.0 100.0
 // @Increment		5.0
 // @User		User
-PARAM_DEFINE_FLOAT(FW_R_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f);
 
 // @DisplayName		Roll Integrator Anti-Windup
-// @Description		This limits the range in degrees the integrator can wind up to.
-// @Range		0.0 to 45.0
-// @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f);
+// @Description		The portion of the integrator part in the control surface deflection is limited to this value.
+// @Range		0.0 to 1.0
+// @Increment		0.1
+PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
 
 // @DisplayName		Maximum Roll Rate
 // @Description		This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
 // @Range		0 to 90.0 degrees per seconds
 // @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_R_RMAX, 60);
+PARAM_DEFINE_FLOAT(FW_R_RMAX, 0);
+
+// @DisplayName		Yaw rate proportional gain.
+// @Description		This defines how much the rudder input will be commanded depending on the current body angular rate error.
+// @Range		10 to 200, 1 increments
+PARAM_DEFINE_FLOAT(FW_YR_P, 0.05);
+
+// @DisplayName		Yaw rate integrator gain.
+// @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
+// @Range		0 to 50.0
+PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f);
+
+// @DisplayName		Yaw rate integrator limit
+// @Description		The portion of the integrator part in the control surface deflection is limited to this value
+// @Range		0.0 to 1
+// @Increment		0.1
+PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
+
+// @DisplayName		Maximum Yaw Rate
+// @Description		This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
+// @Range		0 to 90.0 degrees per seconds
+// @Increment		1.0
+PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0);
+
+// @DisplayName		Roll rate feed forward
+// @Description		Direct feed forward from rate setpoint to control surface output
+// @Range		0 to 10
+// @Increment		0.1
+PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f);
+
+// @DisplayName		Pitch rate feed forward
+// @Description		Direct feed forward from rate setpoint to control surface output
+// @Range		0 to 10
+// @Increment		0.1
+PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f);
+
+// @DisplayName		Yaw rate feed forward
+// @Description		Direct feed forward from rate setpoint to control surface output
+// @Range		0 to 10
+// @Increment		0.1
+PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
+
+// @DisplayName		Minimal speed for yaw coordination
+// @Description		For airspeeds above this value the yaw rate is calculated for a coordinated turn. Set to a very high value to disable.
+// @Range		0 to 90.0 degrees per seconds
+// @Increment		1.0
+PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f);
+
+/* Airspeed parameters: the following parameters about airspeed are used by the attitude and the positon controller */
+
+// @DisplayName		Minimum Airspeed
+// @Description		If the airspeed falls below this value the TECS controller will try to increase airspeed more aggressively
+// @Range		0.0 to 30
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f);
 
+// @DisplayName		Trim Airspeed
+// @Description		The TECS controller tries to fly at this airspeed
+// @Range		0.0 to 30
+PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
 
-PARAM_DEFINE_FLOAT(FW_Y_P, 0);
-PARAM_DEFINE_FLOAT(FW_Y_I, 0);
-PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f);
-PARAM_DEFINE_FLOAT(FW_Y_D, 0);
-PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 1);
-PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f);
-PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f);
-PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f);
+// @DisplayName		Maximum Airspeed
+// @Description		If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively
+// @Range		0.0 to 30
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
index b32b3686f..294e1fc51 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -758,8 +758,8 @@ FixedwingEstimator::task_main()
 				// 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z)
 
 				math::Quaternion q(states[0], states[1], states[2], states[3]);
-				math::Dcm R(q);
-				math::EulerAngles euler(R);
+				math::Matrix<3, 3> R = q.to_dcm();
+				math::Vector<3> euler = R.to_euler();
 
 				for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
 					_att.R[i][j] = R(i, j);
@@ -773,9 +773,9 @@ FixedwingEstimator::task_main()
 				_att.R_valid = true;
 
 				_att.timestamp = last_sensor_timestamp;
-				_att.roll = euler.getPhi();
-				_att.pitch = euler.getTheta();
-				_att.yaw = euler.getPsi();
+				_att.roll = euler(0);
+				_att.pitch = euler(1);
+				_att.yaw = euler(2);
 
 				_att.rollspeed = angRate.x - states[10];
 				_att.pitchspeed = angRate.y - states[11];
@@ -867,10 +867,11 @@ FixedwingEstimator::start()
 void print_status()
 {
 	math::Quaternion q(states[0], states[1], states[2], states[3]);
-	math::EulerAngles euler(q);
+	math::Matrix<3, 3> R = q.to_dcm();
+	math::Vector<3> euler = R.to_euler();
 
 	printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n",
-		(double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi()));
+		(double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));
 
 	// State vector:
 	// 0-3: quaternions (q0, q1, q2, q3)
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index a9648b207..45fdaa355 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -49,6 +49,7 @@
  * More details and acknowledgements in the referenced library headers.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
@@ -67,7 +68,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
@@ -75,6 +76,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
@@ -83,9 +85,12 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
-
+#include <mavlink/mavlink_log.h>
+#include <launchdetection/LaunchDetector.h>
 #include <ecl/l1/ecl_l1_pos_controller.h>
 #include <external_lgpl/tecs/tecs.h>
+#include "landingslope.h"
+
 
 /**
  * L1 control app start / stop handling function
@@ -115,19 +120,20 @@ public:
 	int		start();
 
 private:
+	int		_mavlink_fd;
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
 	int		_control_task;			/**< task handle for sensor task */
 
 	int		_global_pos_sub;
-	int		_global_set_triplet_sub;
+	int		_pos_sp_triplet_sub;
 	int		_att_sub;			/**< vehicle attitude subscription */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
 	int		_control_mode_sub;			/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;		/**< notification of manual control updates */
-	int		_accel_sub;			/**< body frame accelerations */
+	int		_sensor_combined_sub;			/**< for body frame accelerations */
 
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
 	orb_advert_t	_nav_capabilities_pub;		/**< navigation capabilities publication */
@@ -139,8 +145,8 @@ private:
 	struct airspeed_s				_airspeed;		/**< airspeed */
 	struct vehicle_control_mode_s				_control_mode;		/**< vehicle status */
 	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
-	struct vehicle_global_position_set_triplet_s	_global_triplet;	/**< triplet of global setpoints */
-	struct accel_report				_accel;			/**< body frame accelerations */
+	struct position_setpoint_triplet_s			_pos_sp_triplet;		/**< triplet of mission items */
+	struct sensor_combined_s				_sensor_combined;			/**< for body frame accelerations */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -160,17 +166,33 @@ private:
 
 	/* land states */
 	/* not in non-abort mode for landing yet */
-	bool land_noreturn;
+	bool land_noreturn_horizontal;
+	bool land_noreturn_vertical;
+	bool land_stayonground;
+	bool land_motor_lim;
+	bool land_onslope;
+
+	/* takeoff/launch states */
+	bool launch_detected;
+	bool usePreTakeoffThrust;
+
+	/* Landingslope object */
+	Landingslope landingslope;
+
+	float flare_curve_alt_last;
 	/* heading hold */
 	float target_bearing;
 
+	/* Launch detection */
+	LaunchDetector launchDetector;
+
 	/* throttle and airspeed states */
 	float _airspeed_error;				///< airspeed error to setpoint in m/s
 	bool _airspeed_valid;				///< flag if a valid airspeed estimate exists
 	uint64_t _airspeed_last_valid;			///< last time airspeed was valid. Used to detect sensor failures
 	float _groundspeed_undershoot;			///< ground speed error to min. speed in m/s
 	bool _global_pos_valid;				///< global position is valid
-	math::Dcm _R_nb;				///< current attitude
+	math::Matrix<3, 3> _R_nb;				///< current attitude
 
 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
@@ -206,6 +228,17 @@ private:
 		float throttle_land_max;
 
 		float loiter_hold_radius;
+
+		float heightrate_p;
+		float speedrate_p;
+
+		float land_slope_angle;
+		float land_slope_length;
+		float land_H1_virt;
+		float land_flare_alt_relative;
+		float land_thrust_lim_alt_relative;
+		float land_heading_hold_horizontal_distance;
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -240,6 +273,17 @@ private:
 		param_t throttle_land_max;
 
 		param_t loiter_hold_radius;
+
+		param_t heightrate_p;
+		param_t speedrate_p;
+
+		param_t land_slope_angle;
+		param_t land_slope_length;
+		param_t land_H1_virt;
+		param_t land_flare_alt_relative;
+		param_t land_thrust_lim_alt_relative;
+		param_t land_heading_hold_horizontal_distance;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -272,7 +316,7 @@ private:
 	/**
 	 * Check for accel updates.
 	 */
-	void		vehicle_accel_poll();
+	void		vehicle_sensor_combined_poll();
 
 	/**
 	 * Check for set triplet updates.
@@ -280,13 +324,18 @@ private:
 	void		vehicle_setpoint_poll();
 
 	/**
+	 * Publish navigation capabilities
+	 */
+	void navigation_capabilities_publish();
+
+	/**
 	 * Control position.
 	 */
-	bool		control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed,
-					 const struct vehicle_global_position_set_triplet_s &global_triplet);
+	bool		control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed,
+					 const struct position_setpoint_triplet_s &_pos_sp_triplet);
 
 	float calculate_target_airspeed(float airspeed_demand);
-	void calculate_gndspeed_undershoot();
+	void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet);
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -318,7 +367,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 /* subscriptions */
 	_global_pos_sub(-1),
-	_global_set_triplet_sub(-1),
+	_pos_sp_triplet_sub(-1),
 	_att_sub(-1),
 	_airspeed_sub(-1),
 	_control_mode_sub(-1),
@@ -338,8 +387,31 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_airspeed_valid(false),
 	_groundspeed_undershoot(0.0f),
 	_global_pos_valid(false),
-	land_noreturn(false)
+	land_noreturn_horizontal(false),
+	land_noreturn_vertical(false),
+	land_stayonground(false),
+	land_motor_lim(false),
+	land_onslope(false),
+	flare_curve_alt_last(0.0f),
+	_mavlink_fd(-1),
+	launchDetector(),
+	launch_detected(false),
+	usePreTakeoffThrust(false)
 {
+	/* safely initialize structs */
+	vehicle_attitude_s			_att = {0};
+	vehicle_attitude_setpoint_s		_att_sp = {0};
+	navigation_capabilities_s		_nav_capabilities = {0};
+	manual_control_setpoint_s		_manual = {0};
+	airspeed_s				_airspeed = {0};
+	vehicle_control_mode_s			_control_mode = {0};
+	vehicle_global_position_s		_global_pos = {0};
+	position_setpoint_triplet_s			_pos_sp_triplet = {0};
+	sensor_combined_s				_sensor_combined = {0};
+
+
+
+
 	_nav_capabilities.turn_distance = 0.0f;
 
 	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
@@ -358,6 +430,13 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
 	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
 
+	_parameter_handles.land_slope_angle = param_find("FW_LND_ANG");
+	_parameter_handles.land_slope_length = param_find("FW_LND_SLLR");
+	_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
+	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
+	_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
+	_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
+
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.min_sink_rate = 			param_find("FW_T_SINK_MIN");
 	_parameter_handles.max_sink_rate =			param_find("FW_T_SINK_MAX");
@@ -370,6 +449,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.roll_throttle_compensation = 	param_find("FW_T_RLL2THR");
 	_parameter_handles.speed_weight = 			param_find("FW_T_SPDWEIGHT");
 	_parameter_handles.pitch_damping = 			param_find("FW_T_PTCH_DAMP");
+	_parameter_handles.heightrate_p =			param_find("FW_T_HRATE_P");
+	_parameter_handles.speedrate_p =			param_find("FW_T_SRATE_P");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -435,6 +516,16 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
 	param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
 
+	param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
+	param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
+
+	param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
+	param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length));
+	param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
+	param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
+	param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
+	param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
+
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
 	_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
@@ -447,12 +538,14 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
 	_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
 	_tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega);
-	_tecs.set_roll_throttle_compensation(math::radians(_parameters.roll_throttle_compensation));
+	_tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation);
 	_tecs.set_speed_weight(_parameters.speed_weight);
 	_tecs.set_pitch_damping(_parameters.pitch_damping);
 	_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
 	_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
 	_tecs.set_max_climb_rate(_parameters.max_climb_rate);
+	_tecs.set_heightrate_p(_parameters.heightrate_p);
+	_tecs.set_speedrate_p(_parameters.speedrate_p);
 
 	/* sanity check parameters */
 	if (_parameters.airspeed_max < _parameters.airspeed_min ||
@@ -464,6 +557,18 @@ FixedwingPositionControl::parameters_update()
 		return 1;
 	}
 
+	/* Update the landing slope */
+	landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt);
+
+	/* Update and publish the navigation capabilities */
+	_nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
+	_nav_capabilities.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement();
+	_nav_capabilities.landing_flare_length = landingslope.flare_length();
+	navigation_capabilities_publish();
+
+	/* Update Launch Detector Parameters */
+	launchDetector.updateParams();
+
 	return OK;
 }
 
@@ -533,14 +638,14 @@ FixedwingPositionControl::vehicle_attitude_poll()
 }
 
 void
-FixedwingPositionControl::vehicle_accel_poll()
+FixedwingPositionControl::vehicle_sensor_combined_poll()
 {
 	/* check if there is a new position */
-	bool accel_updated;
-	orb_check(_accel_sub, &accel_updated);
+	bool sensors_updated;
+	orb_check(_sensor_combined_sub, &sensors_updated);
 
-	if (accel_updated) {
-		orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+	if (sensors_updated) {
+		orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
 	}
 }
 
@@ -548,11 +653,11 @@ void
 FixedwingPositionControl::vehicle_setpoint_poll()
 {
 	/* check if there is a new setpoint */
-	bool global_sp_updated;
-	orb_check(_global_set_triplet_sub, &global_sp_updated);
+	bool pos_sp_triplet_updated;
+	orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated);
 
-	if (global_sp_updated) {
-		orb_copy(ORB_ID(vehicle_global_position_set_triplet), _global_set_triplet_sub, &_global_triplet);
+	if (pos_sp_triplet_updated) {
+		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
 		_setpoint_valid = true;
 	}
 }
@@ -595,17 +700,29 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
 }
 
 void
-FixedwingPositionControl::calculate_gndspeed_undershoot()
+FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 
 	if (_global_pos_valid) {
-		/* get ground speed vector */
-		math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy);
 
-		/* rotate with current attitude */
-		math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
+		/* rotate ground speed vector with current attitude */
+		math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
 		yaw_vector.normalize();
-		float ground_speed_body = yaw_vector * ground_speed_vector;
+		float ground_speed_body = yaw_vector * ground_speed;
+
+		/* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/
+		float distance = 0.0f;
+		float delta_altitude = 0.0f;
+		if (pos_sp_triplet.previous.valid) {
+			distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
+			delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt;
+		} else {
+			distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
+			delta_altitude = pos_sp_triplet.current.alt -  _global_pos.alt;
+		}
+
+		float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance));
+
 
 		/*
 		 * Ground speed undershoot is the amount of ground velocity not reached
@@ -616,20 +733,29 @@ FixedwingPositionControl::calculate_gndspeed_undershoot()
 		 * not exceeded) travels towards a waypoint (and is not pushed more and more away
 		 * by wind). Not countering this would lead to a fly-away.
 		 */
-		_groundspeed_undershoot = math::max(_parameters.airspeed_min - ground_speed_body, 0.0f);
+		_groundspeed_undershoot = math::max(ground_speed_desired - ground_speed_body, 0.0f);
 
 	} else {
 		_groundspeed_undershoot = 0;
 	}
 }
 
+void FixedwingPositionControl::navigation_capabilities_publish()
+{
+	if (_nav_capabilities_pub > 0) {
+		orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
+	} else {
+		_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
+	}
+}
+
 bool
-FixedwingPositionControl::control_position(const math::Vector2f &current_position, const math::Vector2f &ground_speed,
-		const struct vehicle_global_position_set_triplet_s &global_triplet)
+FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed,
+		const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 	bool setpoint = true;
 
-	calculate_gndspeed_undershoot();
+	calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet);
 
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
@@ -637,11 +763,11 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 	float baro_altitude = _global_pos.alt;
 
 	/* filter speed and altitude for controller */
-	math::Vector3 accel_body(_accel.x, _accel.y, _accel.z);
-	math::Vector3 accel_earth = _R_nb.transpose() * accel_body;
+	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
+	math::Vector<3> accel_earth = _R_nb * accel_body;
 
 	_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
-	float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
+	float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
 
 	/* no throttle limit as default */
 	float throttle_max = 1.0f;
@@ -658,252 +784,277 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 		/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
 		_tecs.set_speed_weight(_parameters.speed_weight);
 
-		/* execute navigation once we have a setpoint */
-		if (_setpoint_valid) {
+		/* current waypoint (the one currently heading for) */
+		math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
-			/* current waypoint (the one currently heading for) */
-			math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
+		/* current waypoint (the one currently heading for) */
+		math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
-			/* previous waypoint */
-			math::Vector2f prev_wp;
 
-			if (global_triplet.previous_valid) {
-				prev_wp.setX(global_triplet.previous.lat / 1e7f);
-				prev_wp.setY(global_triplet.previous.lon / 1e7f);
+		/* previous waypoint */
+		math::Vector<2> prev_wp;
 
-			} else {
-				/*
-				 * No valid previous waypoint, go for the current wp.
-				 * This is automatically handled by the L1 library.
-				 */
-				prev_wp.setX(global_triplet.current.lat / 1e7f);
-				prev_wp.setY(global_triplet.current.lon / 1e7f);
+		if (pos_sp_triplet.previous.valid) {
+			prev_wp(0) = (float)pos_sp_triplet.previous.lat;
+			prev_wp(1) = (float)pos_sp_triplet.previous.lon;
 
-			}
+		} else {
+			/*
+			 * No valid previous waypoint, go for the current wp.
+			 * This is automatically handled by the L1 library.
+			 */
+			prev_wp(0) = (float)pos_sp_triplet.current.lat;
+			prev_wp(1) = (float)pos_sp_triplet.current.lon;
 
-			// XXX add RTL switch
-			if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) {
+		}
 
-				math::Vector2f rtl_pos(_launch_lat, _launch_lon);
+		if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) {
+			/* waypoint is a plain navigation waypoint */
+			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
+			_att_sp.roll_body = _l1_control.nav_roll();
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-				_l1_control.navigate_waypoints(rtl_pos, rtl_pos, current_position, ground_speed);
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+			_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
+						    _airspeed.indicated_airspeed_m_s, eas2tas,
+						    false, math::radians(_parameters.pitch_limit_min),
+						    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+						    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 
-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _launch_alt, calculate_target_airspeed(_parameters.airspeed_trim),
-							    _airspeed.indicated_airspeed_m_s, eas2tas,
-							    false, math::radians(_parameters.pitch_limit_min),
-							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
 
-				// XXX handle case when having arrived at home (loiter)
+			/* waypoint is a loiter waypoint */
+			_l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
+						  pos_sp_triplet.current.loiter_direction, ground_speed);
+			_att_sp.roll_body = _l1_control.nav_roll();
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-			} else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
-				/* waypoint is a plain navigation waypoint */
-				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+			_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
+						    _airspeed.indicated_airspeed_m_s, eas2tas,
+						    false, math::radians(_parameters.pitch_limit_min),
+						    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+						    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 
-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
-							    _airspeed.indicated_airspeed_m_s, eas2tas,
-							    false, math::radians(_parameters.pitch_limit_min),
-							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
 
-			} else if (global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-				   global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-				   global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+			float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
 
-				/* waypoint is a loiter waypoint */
-				_l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius,
-							  global_triplet.current.loiter_direction, ground_speed);
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+			/* Horizontal landing control */
+			/* switch to heading hold for the last meters, continue heading hold after */
+			float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
+			//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
+			if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
 
-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
-							    _airspeed.indicated_airspeed_m_s, eas2tas,
-							    false, math::radians(_parameters.pitch_limit_min),
-							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+				/* heading hold, along the line connecting this and the last waypoint */
+
+				if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
+					if (pos_sp_triplet.previous.valid) {
+						target_bearing = bearing_lastwp_currwp;
+					} else {
+						target_bearing = _att.yaw;
+					}
+					mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
+				}
 
-			} else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) {
+//					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
 
-				/* switch to heading hold for the last meters, continue heading hold after */
+				_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
 
-				float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY());
-				//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
-				if (wp_distance < 15.0f || land_noreturn) {
+				/* limit roll motion to prevent wings from touching the ground first */
+				_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
 
-					/* heading hold, along the line connecting this and the last waypoint */
-					
+				land_noreturn_horizontal = true;
 
-					// if (global_triplet.previous_valid) {
-					// 	target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
-					// } else {
+			} else {
 
-					if (!land_noreturn)
-						target_bearing = _att.yaw;
-					//}
+				/* normal navigation */
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
+			}
 
-					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
+			_att_sp.roll_body = _l1_control.nav_roll();
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-					_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
 
-					if (altitude_error > -5.0f)
-						land_noreturn = true;
+			/* Vertical landing control */
+			//xxx: using the tecs altitude controller for slope control for now
 
-				} else {
+//				/* do not go down too early */
+//				if (wp_distance > 50.0f) {
+//					altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt;
+//				}
+			/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
+			// XXX this could make a great param
 
-					/* normal navigation */
-					_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
-				}
+			float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1)
+			float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
+			float airspeed_land = 1.3f * _parameters.airspeed_min;
+			float airspeed_approach = 1.3f * _parameters.airspeed_min;
 
-				/* do not go down too early */
-				if (wp_distance > 50.0f) {
-					altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
-				}
+			float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
+			float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);
+			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
+			float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
 
+			if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
 
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+				/* land with minimal speed */
 
-				/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
-				// XXX this could make a great param
+//					/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
+//					_tecs.set_speed_weight(2.0f);
 
-				float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
-				float land_pitch_min = math::radians(5.0f);
-				float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
-				float airspeed_land = _parameters.airspeed_min;
-				float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f;
+				/* kill the throttle if param requests it */
+				throttle_max = _parameters.throttle_max;
 
-				if (altitude_error > -4.0f) {
+				 if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
+					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
+					if (!land_motor_lim) {
+						land_motor_lim  = true;
+						mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle");
+					}
 
-					/* land with minimal speed */
+				 }
 
-					/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
-					_tecs.set_speed_weight(2.0f);
+				float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    false, flare_angle_rad,
-								    0.0f, _parameters.throttle_max, throttle_land,
-								    math::radians(-10.0f), math::radians(15.0f));
+				/* avoid climbout */
+				if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
+				{
+					flare_curve_alt = pos_sp_triplet.current.alt;
+					land_stayonground = true;
+				}
 
-					/* kill the throttle if param requests it */
-					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
+				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land),
+												    _airspeed.indicated_airspeed_m_s, eas2tas,
+												    false, flare_pitch_angle_rad,
+												    0.0f, throttle_max, throttle_land,
+												    flare_pitch_angle_rad,  math::radians(15.0f));
 
-					/* limit roll motion to prevent wings from touching the ground first */
-					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
+				if (!land_noreturn_vertical) {
+					mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
+					land_noreturn_vertical = true;
+				}
+				//warnx("Landing:  flare, _global_pos.alt  %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
 
-				} else if (wp_distance < 60.0f && altitude_error > -20.0f) {
+				flare_curve_alt_last = flare_curve_alt;
 
-					/* minimize speed to approach speed */
+			} else if (wp_distance < L_wp_distance) {
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    false, flare_angle_rad,
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+				/* minimize speed to approach speed, stay on landing slope */
+				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach),
+							    _airspeed.indicated_airspeed_m_s, eas2tas,
+							    false, flare_pitch_angle_rad,
+							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+				//warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length);
+
+				if (!land_onslope) {
+
+					mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
+					land_onslope = true;
+				}
 
+			} else {
+
+				 /* intersect glide slope:
+				 * if current position is higher or within 10m of slope follow the glide slope
+				 * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
+				 * */
+				float altitude_desired = _global_pos.alt;
+				if (_global_pos.alt > landing_slope_alt_desired - 10.0f) {
+					/* stay on slope */
+					altitude_desired = landing_slope_alt_desired;
+					//warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement);
 				} else {
+					/* continue horizontally */
+					altitude_desired =  math::max(_global_pos.alt, L_altitude);
+					//warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance);
+				}
 
-					/* normal cruise speed */
+				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach),
+							    _airspeed.indicated_airspeed_m_s, eas2tas,
+							    false, math::radians(_parameters.pitch_limit_min),
+							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+			}
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    false, math::radians(_parameters.pitch_limit_min),
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
+
+			/* Perform launch detection */
+//			warnx("Launch detection running");
+			if(!launch_detected) { //do not do further checks once a launch was detected
+				if (launchDetector.launchDetectionEnabled()) {
+					static hrt_abstime last_sent = 0;
+					if(hrt_absolute_time() - last_sent > 4e6) {
+//						warnx("Launch detection running");
+						mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
+						last_sent = hrt_absolute_time();
+					}
+					launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
+					if (launchDetector.getLaunchDetected()) {
+						launch_detected = true;
+						mavlink_log_info(_mavlink_fd, "#audio: Takeoff");
+					}
+				} else	{
+					/* no takeoff detection --> fly */
+					launch_detected = true;
 				}
+			}
 
-			} else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
+			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
+			_att_sp.roll_body = _l1_control.nav_roll();
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+			if (launch_detected) {
+				usePreTakeoffThrust = false;
 
 				/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
-				if (altitude_error > 10.0f) {
+				if (altitude_error > 15.0f) {
 
 					/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)),
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
+									_airspeed.indicated_airspeed_m_s, eas2tas,
+									true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)),
+									_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+									math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 
 					/* limit roll motion to ensure enough lift */
 					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
 
 				} else {
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    false, math::radians(_parameters.pitch_limit_min),
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
+									_airspeed.indicated_airspeed_m_s, eas2tas,
+									false, math::radians(_parameters.pitch_limit_min),
+									_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+									math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 				}
-			}
-
-			// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
-			//       (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
-			// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(),
-			//       (double)next_wp.getX(), (double)next_wp.getY(), (global_triplet.previous_valid) ? "valid" : "invalid");
-
-			// XXX at this point we always want no loiter hold if a
-			// mission is active
-			_loiter_hold = false;
-
-		} else if (_control_mode.flag_armed) {
-
-			/* hold position, but only if armed, climb 20m in case this is engaged on ground level */
-
-			// XXX rework with smarter state machine
-
-			if (!_loiter_hold) {
-				_loiter_hold_lat = _global_pos.lat / 1e7f;
-				_loiter_hold_lon = _global_pos.lon / 1e7f;
-				_loiter_hold_alt = _global_pos.alt + 25.0f;
-				_loiter_hold = true;
-			}
-
-			altitude_error = _loiter_hold_alt - _global_pos.alt;
-
-			math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon);
-
-			/* loiter around current position */
-			_l1_control.navigate_loiter(loiter_hold_pos, current_position, _parameters.loiter_hold_radius,
-						  1, ground_speed);
-			_att_sp.roll_body = _l1_control.nav_roll();
-			_att_sp.yaw_body = _l1_control.nav_bearing();
-
-			/* climb with full throttle if the altitude error is bigger than 5 meters */
-			bool climb_out = (altitude_error > 3);
-
-			float min_pitch;
-
-			if (climb_out) {
-				min_pitch = math::radians(20.0f);
 
 			} else {
-				min_pitch = math::radians(_parameters.pitch_limit_min);
+				usePreTakeoffThrust = true;
 			}
+		}
 
-			_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _loiter_hold_alt, calculate_target_airspeed(_parameters.airspeed_trim),
-						    _airspeed.indicated_airspeed_m_s, eas2tas,
-						    climb_out, min_pitch,
-						    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-						    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+		// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
+		//       (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
+		// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1),
+		//       (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid");
 
-			if (climb_out) {
-				/* limit roll motion to ensure enough lift */
-				_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
-			}
-		}
+		// XXX at this point we always want no loiter hold if a
+		// mission is active
+		_loiter_hold = false;
 
 		/* reset land state */
-		if (global_triplet.current.nav_cmd != NAV_CMD_LAND) {
-			land_noreturn = false;
+		if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
+			land_noreturn_horizontal = false;
+			land_noreturn_vertical = false;
+			land_stayonground = false;
+			land_motor_lim = false;
+			land_onslope = false;
+		}
+
+		/* reset takeoff/launch state */
+		if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
+			launch_detected = false;
+			usePreTakeoffThrust = false;
 		}
 
 		if (was_circle_mode && !_l1_control.circle_mode()) {
@@ -1000,8 +1151,14 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 		setpoint = false;
 	}
 
+	if (usePreTakeoffThrust) {
+		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
+	}
+	else {
+		_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
+	}
 	_att_sp.pitch_body = _tecs.get_pitch_demand();
-	_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
+
 
 	return setpoint;
 }
@@ -1018,9 +1175,9 @@ FixedwingPositionControl::task_main()
 	 * do subscriptions
 	 */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	_global_set_triplet_sub = orb_subscribe(ORB_ID(vehicle_global_position_set_triplet));
+	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -1080,6 +1237,11 @@ FixedwingPositionControl::task_main()
 		/* only run controller if position changed */
 		if (fds[1].revents & POLLIN) {
 
+			/* XXX Hack to get mavlink output going */
+			if (_mavlink_fd < 0) {
+				/* try to open the mavlink log device every once in a while */
+				_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+			}
 
 			static uint64_t last_run = 0;
 			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -1097,18 +1259,18 @@ FixedwingPositionControl::task_main()
 
 			vehicle_attitude_poll();
 			vehicle_setpoint_poll();
-			vehicle_accel_poll();
+			vehicle_sensor_combined_poll();
 			vehicle_airspeed_poll();
 			// vehicle_baro_poll();
 
-			math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
-			math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
+			math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
+			math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
 
 			/*
 			 * Attempt to control position, on success (= sensors present and not in manual mode),
 			 * publish setpoint.
 			 */
-			if (control_position(current_position, ground_speed, _global_triplet)) {
+			if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
 				_att_sp.timestamp = hrt_absolute_time();
 
 				/* lazily publish the setpoint only once available */
@@ -1121,7 +1283,8 @@ FixedwingPositionControl::task_main()
 					_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
 				}
 
-				float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy);
+				/* XXX check if radius makes sense here */
+				float turn_distance = _l1_control.switch_distance(100.0f);
 
 				/* lazily publish navigation capabilities */
 				if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
@@ -1129,11 +1292,8 @@ FixedwingPositionControl::task_main()
 					/* set new turn distance */
 					_nav_capabilities.turn_distance = turn_distance;
 
-					if (_nav_capabilities_pub > 0) {
-						orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
-					} else {
-						_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
-					}
+					navigation_capabilities_publish();
+
 				}
 
 			}
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 31a9cdefa..62a340e90 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -40,12 +40,10 @@
  */
 
 #include <nuttx/config.h>
-
 #include <systemlib/param/param.h>
 
 /*
  * Controller parameters, accessible via MAVLink
- *
  */
 
 /**
@@ -119,48 +117,268 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
  */
 PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
 
-
+/**
+ * Controller roll limit
+ *
+ * The maximum roll the controller will output.
+ *
+ * @unit degrees
+ * @min 0.0
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
 
+/**
+ * Throttle limit max
+ *
+ * This is the maximum throttle % that can be used by the controller. 
+ * For overpowered aircraft, this should be reduced to a value that 
+ * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX.
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
 
+/**
+ * Throttle limit min
+ *
+ * This is the minimum throttle % that can be used by the controller. 
+ * For electric aircraft this will normally be set to zero, but can be set 
+ * to a small non-zero value if a folding prop is fitted to prevent the 
+ * prop from folding and unfolding repeatedly in-flight or to provide 
+ * some aerodynamic drag from a turning prop to improve the descent rate.
+ *
+ * For aircraft with internal combustion engine this parameter should be set
+ * for desired idle rpm.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
 
-
-PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
-
+/**
+ * Throttle limit value before flare
+ *
+ * This throttle value will be set as throttle limit at FW_LND_TLALT, 
+ * before arcraft will flare.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
 
+/**
+ * Maximum climb rate
+ *
+ * This is the best climb rate that the aircraft can achieve with 
+ * the throttle set to THR_MAX and the airspeed set to the 
+ * default value. For electric aircraft make sure this number can be 
+ * achieved towards the end of flight when the battery voltage has reduced. 
+ * The setting of this parameter can be checked by commanding a positive 
+ * altitude change of 100m in loiter, RTL or guided mode. If the throttle 
+ * required to climb is close to THR_MAX and the aircraft is maintaining 
+ * airspeed, then this parameter is set correctly. If the airspeed starts 
+ * to reduce, then the parameter is set to high, and if the throttle 
+ * demand required to climb and maintain speed is noticeably less than 
+ * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or 
+ * FW_THR_MAX reduced.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
 
-
+/**
+ * Minimum descent rate
+ *
+ * This is the sink rate of the aircraft with the throttle 
+ * set to THR_MIN and flown at the same airspeed as used 
+ * to measure FW_T_CLMB_MAX.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
 
+/**
+ * Maximum descent rate
+ *
+ * This sets the maximum descent rate that the controller will use. 
+ * If this value is too large, the aircraft can over-speed on descent. 
+ * This should be set to a value that can be achieved without 
+ * exceeding the lower pitch angle limit and without over-speeding 
+ * the aircraft.
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
 
+/**
+ * TECS time constant
+ *
+ * This is the time constant of the TECS control algorithm (in seconds). 
+ * Smaller values make it faster to respond, larger values make it slower
+ * to respond.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
 
-
+/**
+ * Throttle damping factor
+ *
+ * This is the damping gain for the throttle demand loop. 
+ * Increase to add damping to correct for oscillations in speed and height.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
 
-
+/**
+ * Integrator gain
+ *
+ * This is the integrator gain on the control loop. 
+ * Increasing this gain increases the speed at which speed 
+ * and height offsets are trimmed out, but reduces damping and 
+ * increases overshoot.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
 
-
+/**
+ * Maximum vertical acceleration
+ *
+ * This is the maximum vertical acceleration (in metres/second^2) 
+ * either up or down that the controller will use to correct speed 
+ * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) 
+ * allows for reasonably aggressive pitch changes if required to recover 
+ * from under-speed conditions.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
 
-
+/**
+ * Complementary filter "omega" parameter for height
+ *
+ * This is the cross-over frequency (in radians/second) of the complementary 
+ * filter used to fuse vertical acceleration and barometric height to obtain 
+ * an estimate of height rate and height. Increasing this frequency weights 
+ * the solution more towards use of the barometer, whilst reducing it weights 
+ * the solution more towards use of the accelerometer data.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
 
-
+/**
+ * Complementary filter "omega" parameter for speed
+ *
+ * This is the cross-over frequency (in radians/second) of the complementary 
+ * filter used to fuse longitudinal acceleration and airspeed to obtain an 
+ * improved airspeed estimate. Increasing this frequency weights the solution
+ * more towards use of the arispeed sensor, whilst reducing it weights the 
+ * solution more towards use of the accelerometer data.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
 
-
+/**
+ * Roll -> Throttle feedforward
+ *
+ * Increasing this gain turn increases the amount of throttle that will 
+ * be used to compensate for the additional drag created by turning. 
+ * Ideally this should be set to  approximately 10 x the extra sink rate 
+ * in m/s created by a 45 degree bank turn. Increase this gain if 
+ * the aircraft initially loses energy in turns and reduce if the 
+ * aircraft initially gains energy in turns. Efficient high aspect-ratio 
+ * aircraft (eg powered sailplanes) can use a lower value, whereas 
+ * inefficient low aspect-ratio models (eg delta wings) can use a higher value.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
 
-
+/**
+ * Speed <--> Altitude priority
+ *
+ * This parameter adjusts the amount of weighting that the pitch control 
+ * applies to speed vs height errors. Setting it to 0.0 will cause the 
+ * pitch control to control height and ignore speed errors. This will 
+ * normally improve height accuracy but give larger airspeed errors. 
+ * Setting it to 2.0 will cause the pitch control loop to control speed 
+ * and ignore height errors. This will normally reduce airspeed errors, 
+ * but give larger height errors. The default value of 1.0 allows the pitch 
+ * control to simultaneously control height and speed. 
+ * Note to Glider Pilots - set this parameter to 2.0 (The glider will 
+ * adjust its pitch angle to maintain airspeed, ignoring changes in height).
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
 
-
+/**
+ * Pitch damping factor
+ *
+ * This is the damping gain for the pitch demand loop. Increase to add 
+ * damping to correct for oscillations in height. The default value of 0.0 
+ * will work well provided the pitch to servo controller has been tuned 
+ * properly.
+ *
+ * @group L1 Control
+ */
 PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
 
+/**
+ * Height rate P factor
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
 
-PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
+/**
+ * Speed rate P factor
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
+
+/**
+ * Landing slope angle
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
+
+/**
+ * Landing slope length
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
+
+/**
+ *
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
+
+/**
+ * Landing flare altitude (relative)
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
+
+/**
+ * Landing throttle limit altitude (relative)
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
+
+/**
+ * Landing heading hold horizontal distance
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp
new file mode 100644
index 000000000..e5f7023ae
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/landingslope.cpp
@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Thomas Gubler <thomasgubler@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: landingslope.cpp
+ *
+ */
+
+#include "landingslope.h"
+
+#include <nuttx/config.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <math.h>
+#include <unistd.h>
+#include <mathlib/mathlib.h>
+
+void Landingslope::update(float landing_slope_angle_rad,
+		float flare_relative_alt,
+		float motor_lim_relative_alt,
+		float H1_virt)
+{
+
+	_landing_slope_angle_rad = landing_slope_angle_rad;
+	_flare_relative_alt = flare_relative_alt;
+	_motor_lim_relative_alt = motor_lim_relative_alt;
+	_H1_virt = H1_virt;
+
+	calculateSlopeValues();
+}
+
+void Landingslope::calculateSlopeValues()
+{
+	_H0 =  _flare_relative_alt + _H1_virt;
+	_d1 = _flare_relative_alt/tanf(_landing_slope_angle_rad);
+	_flare_constant = (_H0 * _d1)/_flare_relative_alt;
+	_flare_length = - logf(_H1_virt/_H0) * _flare_constant;
+	_horizontal_slope_displacement = (_flare_length - _d1);
+}
+
+float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude)
+{
+	return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
+}
+
+float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
+{
+	/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
+	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
+		return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude);
+	else
+		return wp_altitude;
+}
+
+float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
+{
+	/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
+	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
+		return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
+	else
+		return wp_landing_altitude;
+}
+
diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h
new file mode 100644
index 000000000..76d65a55f
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/landingslope.h
@@ -0,0 +1,120 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Thomas Gubler <thomasgubler@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: landingslope.h
+ *
+ */
+
+#ifndef LANDINGSLOPE_H_
+#define LANDINGSLOPE_H_
+
+#include <math.h>
+#include <systemlib/err.h>
+
+class Landingslope
+{
+private:
+	/* see Documentation/fw_landing.png for a plot of the landing slope */
+	float _landing_slope_angle_rad;			/**< phi in the plot */
+	float _flare_relative_alt;				/**< h_flare,rel in the plot */
+	float _motor_lim_relative_alt;
+	float _H1_virt;							/**< H1 in the plot */
+	float _H0;								/**< h_flare,rel + H1 in the plot */
+	float _d1;								/**< d1 in the plot */
+	float _flare_constant;
+	float _flare_length;					/**< d1 + delta d in the plot */
+	float _horizontal_slope_displacement;  /**< delta d in the plot */
+
+	void calculateSlopeValues();
+
+public:
+	Landingslope() {}
+	~Landingslope() {}
+
+	/**
+	 *
+	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 */
+	float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude);
+
+	/**
+	 *
+	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 * Performs check if aircraft is in front of waypoint to avoid climbout
+	 */
+	float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude);
+
+	/**
+	 *
+	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 */
+	__EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
+	{
+		return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative
+	}
+
+	/**
+	 *
+	 * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude
+	 */
+	__EXPORT static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
+	{
+		return (slope_altitude - wp_landing_altitude)/tanf(landing_slope_angle_rad) + horizontal_slope_displacement;
+
+	}
+
+
+	float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
+
+	void update(float landing_slope_angle_rad,
+			float flare_relative_alt,
+			float motor_lim_relative_alt,
+			float H1_virt);
+
+
+	inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;}
+	inline float flare_relative_alt() {return _flare_relative_alt;}
+	inline float motor_lim_relative_alt() {return _motor_lim_relative_alt;}
+	inline float H1_virt() {return _H1_virt;}
+	inline float H0() {return _H0;}
+	inline float d1() {return _d1;}
+	inline float flare_constant() {return _flare_constant;}
+	inline float flare_length() {return _flare_length;}
+	inline float horizontal_slope_displacement() {return _horizontal_slope_displacement;}
+
+};
+
+
+#endif /* LANDINGSLOPE_H_ */
diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk
index b00b9aa5a..cf419ec7e 100644
--- a/src/modules/fw_pos_control_l1/module.mk
+++ b/src/modules/fw_pos_control_l1/module.mk
@@ -38,4 +38,5 @@
 MODULE_COMMAND	= fw_pos_control_l1
 
 SRCS		= fw_pos_control_l1_main.cpp \
-		  fw_pos_control_l1_params.c
+		  fw_pos_control_l1_params.c \
+		  landingslope.cpp
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 20853379d..ade4469c5 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -68,15 +68,28 @@
 
 #include "waypoints.h"
 #include "orb_topics.h"
-#include "missionlib.h"
 #include "mavlink_hil.h"
 #include "util.h"
 #include "waypoints.h"
 #include "mavlink_parameters.h"
 
+#include <uORB/topics/mission_result.h>
+
 /* define MAVLink specific parameters */
+/**
+ * MAVLink system ID
+ * @group MAVLink
+ */
 PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
+/**
+ * MAVLink component ID
+ * @group MAVLink
+ */
 PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
+/**
+ * MAVLink type
+ * @group MAVLink
+ */
 PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
 
 __EXPORT int mavlink_main(int argc, char *argv[]);
@@ -198,8 +211,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 	}
 
 	/* arming state */
-	if (v_status.arming_state == ARMING_STATE_ARMED
-			|| v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+	if (armed.armed) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
@@ -207,29 +219,35 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
 	union px4_custom_mode custom_mode;
 	custom_mode.data = 0;
-	if (v_status.main_state == MAIN_STATE_MANUAL) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
-	} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
-	} else if (v_status.main_state == MAIN_STATE_EASY) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
-	} else if (v_status.main_state == MAIN_STATE_AUTO) {
+	if (pos_sp_triplet.nav_state == NAV_STATE_NONE) {
+		/* use main state when navigator is not active */
+		if (v_status.main_state == MAIN_STATE_MANUAL) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+		} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
+		} else if (v_status.main_state == MAIN_STATE_EASY) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
+		} else if (v_status.main_state == MAIN_STATE_AUTO) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+		}
+	} else {
+		/* use navigation state when navigator is active */
 		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-		if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
+		if (pos_sp_triplet.nav_state == NAV_STATE_READY) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
-		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
+		} else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
-		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
+		} else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
-		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) {
+		} else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
-		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+		} else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
 		}
 	}
@@ -644,6 +662,10 @@ int mavlink_thread_main(int argc, char *argv[])
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
 	}
 
+	int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
+	struct mission_result_s mission_result;
+	memset(&mission_result, 0, sizeof(mission_result));
+
 	thread_running = true;
 
 	/* arm counter to go off immediately */
@@ -690,25 +712,36 @@ int mavlink_thread_main(int argc, char *argv[])
 
 		lowspeed_counter++;
 
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
+		bool updated;
+		orb_check(mission_result_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
+
+			if (mission_result.mission_reached) {
+				mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index);
+			}
+		}
+
+		mavlink_waypoint_eventloop(hrt_absolute_time());
 
 		/* sleep quarter the time */
 		usleep(25000);
 
 		/* check if waypoint has been reached against the last positions */
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
+		mavlink_waypoint_eventloop(hrt_absolute_time());
 
 		/* sleep quarter the time */
 		usleep(25000);
 
 		/* send parameters at 20 Hz (if queued for sending) */
 		mavlink_pm_queued_send();
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
+		mavlink_waypoint_eventloop(hrt_absolute_time());
 
 		/* sleep quarter the time */
 		usleep(25000);
 
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
+		mavlink_waypoint_eventloop(hrt_absolute_time());
 
 		if (baudrate > 57600) {
 			mavlink_pm_queued_send();
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 37e5b8c61..73a7e132b 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -79,7 +79,6 @@ __BEGIN_DECLS
 #include "mavlink_bridge_header.h"
 #include "waypoints.h"
 #include "orb_topics.h"
-#include "missionlib.h"
 #include "mavlink_hil.h"
 #include "mavlink_parameters.h"
 #include "util.h"
@@ -352,7 +351,7 @@ handle_message(mavlink_message_t *msg)
 		tstatus.rxerrors = rstatus.rxerrors;
 		tstatus.fixed = rstatus.fixed;
 
-		if (telemetry_status_pub == 0) {
+		if (telemetry_status_pub <= 0) {
 			telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
 
 		} else {
@@ -578,6 +577,7 @@ handle_message(mavlink_message_t *msg)
 			hil_gps.alt = gps.alt;
 			hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
 			hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
+			hil_gps.timestamp_variance = gps.time_usec;
 			hil_gps.s_variance_m_s = 5.0f;
 			hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
 			hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
@@ -589,6 +589,7 @@ handle_message(mavlink_message_t *msg)
 			if (heading_rad > M_PI_F)
 				heading_rad -= 2.0f * M_PI_F;
 
+			hil_gps.timestamp_velocity = gps.time_usec;
 			hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
 			hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m
 			hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m
@@ -632,13 +633,13 @@ handle_message(mavlink_message_t *msg)
 				orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
 				// global position packet
 				hil_global_pos.timestamp = timestamp;
-				hil_global_pos.valid = true;
+				hil_global_pos.global_valid = true;
 				hil_global_pos.lat = hil_state.lat;
 				hil_global_pos.lon = hil_state.lon;
 				hil_global_pos.alt = hil_state.alt / 1000.0f;
-				hil_global_pos.vx = hil_state.vx / 100.0f;
-				hil_global_pos.vy = hil_state.vy / 100.0f;
-				hil_global_pos.vz = hil_state.vz / 100.0f;
+				hil_global_pos.vel_n = hil_state.vx / 100.0f;
+				hil_global_pos.vel_e = hil_state.vy / 100.0f;
+				hil_global_pos.vel_d = hil_state.vz / 100.0f;
 
 			} else {
 				pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
@@ -682,8 +683,8 @@ handle_message(mavlink_message_t *msg)
 
 			/* Calculate Rotation Matrix */
 			math::Quaternion q(hil_state.attitude_quaternion);
-			math::Dcm C_nb(q);
-			math::EulerAngles euler(C_nb);
+			math::Matrix<3,3> C_nb = q.to_dcm();
+			math::Vector<3> euler = C_nb.to_euler();
 
 			/* set rotation matrix */
 			for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
@@ -698,9 +699,9 @@ handle_message(mavlink_message_t *msg)
 			hil_attitude.q[3] = q(3);
 			hil_attitude.q_valid = true;
 
-			hil_attitude.roll = euler.getPhi();
-			hil_attitude.pitch = euler.getTheta();
-			hil_attitude.yaw = euler.getPsi();
+			hil_attitude.roll = euler(0);
+			hil_attitude.pitch = euler(1);
+			hil_attitude.yaw = euler(2);
 			hil_attitude.rollspeed = hil_state.rollspeed;
 			hil_attitude.pitchspeed = hil_state.pitchspeed;
 			hil_attitude.yawspeed = hil_state.yawspeed;
@@ -843,7 +844,7 @@ receive_thread(void *arg)
 					handle_message(&msg);
 
 					/* handle packet with waypoint component */
-					mavlink_wpm_message_handler(&msg, &global_pos, &local_pos);
+					mavlink_wpm_message_handler(&msg);
 
 					/* handle packet with parameter component */
 					mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
deleted file mode 100644
index 124b3b2ae..000000000
--- a/src/modules/mavlink/missionlib.c
+++ /dev/null
@@ -1,390 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.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 missionlib.h
- * MAVLink missionlib components
- */
-
-// XXX trim includes
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <math.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <mqueue.h>
-#include <string.h>
-#include "mavlink_bridge_header.h"
-#include <drivers/drv_hrt.h>
-#include <time.h>
-#include <float.h>
-#include <unistd.h>
-#include <nuttx/sched.h>
-#include <sys/prctl.h>
-#include <termios.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <poll.h>
-
-#include <systemlib/err.h>
-#include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <mavlink/mavlink_log.h>
-
-#include "geo/geo.h"
-#include "waypoints.h"
-#include "orb_topics.h"
-#include "missionlib.h"
-#include "mavlink_hil.h"
-#include "util.h"
-#include "waypoints.h"
-#include "mavlink_parameters.h"
-
-static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
-static uint64_t loiter_start_time;
-
-static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
-	struct vehicle_global_position_setpoint_s *sp);
-
-int
-mavlink_missionlib_send_message(mavlink_message_t *msg)
-{
-	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
-
-	mavlink_send_uart_bytes(chan, missionlib_msg_buf, len);
-	return 0;
-}
-
-int
-mavlink_missionlib_send_gcs_string(const char *string)
-{
-	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
-	mavlink_statustext_t statustext;
-	int i = 0;
-
-	while (i < len - 1) {
-		statustext.text[i] = string[i];
-
-		if (string[i] == '\0')
-			break;
-
-		i++;
-	}
-
-	if (i > 1) {
-		/* Enforce null termination */
-		statustext.text[i] = '\0';
-		mavlink_message_t msg;
-
-		mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
-		return mavlink_missionlib_send_message(&msg);
-
-	} else {
-		return 1;
-	}
-}
-
-/**
- * Get system time since boot in microseconds
- *
- * @return the system time since boot in microseconds
- */
-uint64_t mavlink_missionlib_get_system_timestamp()
-{
-	return hrt_absolute_time();
-}
-
-/**
- * Set special vehicle setpoint fields based on current mission item.
- *
- * @return true if the mission item could be interpreted
- * successfully, it return false on failure.
- */
-bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
-	struct vehicle_global_position_setpoint_s *sp)
-{
-	switch (command) {
-		case MAV_CMD_NAV_LOITER_UNLIM:
-			sp->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
-			break;
-		case MAV_CMD_NAV_LOITER_TIME:
-			sp->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
-			loiter_start_time = hrt_absolute_time();
-			break;
-		// case MAV_CMD_NAV_LOITER_TURNS:
-		// 	sp->nav_cmd = NAV_CMD_LOITER_TURN_COUNT;
-		// 	break;
-		case MAV_CMD_NAV_WAYPOINT:
-			sp->nav_cmd = NAV_CMD_WAYPOINT;
-			break;
-		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-			sp->nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
-			break;
-		case MAV_CMD_NAV_LAND:
-			sp->nav_cmd = NAV_CMD_LAND;
-			break;
-		case MAV_CMD_NAV_TAKEOFF:
-			sp->nav_cmd = NAV_CMD_TAKEOFF;
-			break;
-		default:
-			/* abort */
-			return false;
-	}
-
-	sp->loiter_radius = param3;
-	sp->loiter_direction = (param3 >= 0) ? 1 : -1;
-
-	sp->param1 = param1;
-	sp->param2 = param2;
-	sp->param3 = param3;
-	sp->param4 = param4;
-
-
-	/* define the turn distance */
-	float orbit = 15.0f;
-
-	if (command == (int)MAV_CMD_NAV_WAYPOINT) {
-
-		orbit = param2;
-
-	} else if (command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-			command == (int)MAV_CMD_NAV_LOITER_TIME ||
-			command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
-
-		orbit = param3;
-	} else {
-
-		// XXX set default orbit via param
-		// 15 initialized above
-	}
-
-	sp->turn_distance_xy = orbit;
-	sp->turn_distance_z = orbit;
-}
-
-/**
- * This callback is executed each time a waypoint changes.
- *
- * It publishes the vehicle_global_position_setpoint_s or the
- * vehicle_local_position_setpoint_s topic, depending on the type of waypoint
- */
-void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
-		float param2, float param3, float param4, float param5_lat_x,
-		float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command)
-{
-	static orb_advert_t global_position_setpoint_pub = -1;
-	static orb_advert_t global_position_set_triplet_pub = -1;
-	static orb_advert_t local_position_setpoint_pub = -1;
-	static unsigned last_waypoint_index = -1;
-	char buf[50] = {0};
-
-	// XXX include check if WP is supported, jump to next if not
-
-	/* Update controller setpoints */
-	if (frame == (int)MAV_FRAME_GLOBAL) {
-		/* global, absolute waypoint */
-		struct vehicle_global_position_setpoint_s sp;
-		sp.lat = param5_lat_x * 1e7f;
-		sp.lon = param6_lon_y * 1e7f;
-		sp.altitude = param7_alt_z;
-		sp.altitude_is_relative = false;
-		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
-		set_special_fields(param1, param2, param3, param4, command, &sp);
-
-		/* Initialize setpoint publication if necessary */
-		if (global_position_setpoint_pub < 0) {
-			global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
-		}
-
-
-		/* fill triplet: previous, current, next waypoint */
-		struct vehicle_global_position_set_triplet_s triplet;
-
-		/* current waypoint is same as sp */
-		memcpy(&(triplet.current), &sp, sizeof(sp));
-
-		/*
-		 * Check if previous WP (in mission, not in execution order) 
-		 * is available and identify correct index
-		 */
-		int last_setpoint_index = -1;
-		bool last_setpoint_valid = false;
-
-		if (index > 0) {
-			last_setpoint_index = index - 1;
-		}
-
-		while (last_setpoint_index >= 0) {
-
-			if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL &&
-				(wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
-				wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
-				last_setpoint_valid = true;
-				break;
-			}
-
-			last_setpoint_index--;
-		}
-
-		/*
-		 * Check if next WP (in mission, not in execution order) 
-		 * is available and identify correct index
-		 */
-		int next_setpoint_index = -1;
-		bool next_setpoint_valid = false;
-
-		/* next waypoint */
-		if (wpm->size > 1) {
-			next_setpoint_index = index + 1;
-		}
-
-		while (next_setpoint_index < wpm->size) {
-
-			if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
-					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
-				next_setpoint_valid = true;
-				break;
-			}
-
-			next_setpoint_index++;
-		}
-
-		/* populate last and next */
-
-		triplet.previous_valid = false;
-		triplet.next_valid = false;
-
-		if (last_setpoint_valid) {
-			triplet.previous_valid = true;
-			struct vehicle_global_position_setpoint_s sp;
-			sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f;
-			sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
-			sp.altitude = wpm->waypoints[last_setpoint_index].z;
-			sp.altitude_is_relative = false;
-			sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
-			set_special_fields(wpm->waypoints[last_setpoint_index].param1,
-				wpm->waypoints[last_setpoint_index].param2,
-				wpm->waypoints[last_setpoint_index].param3,
-				wpm->waypoints[last_setpoint_index].param4,
-				wpm->waypoints[last_setpoint_index].command, &sp);
-			memcpy(&(triplet.previous), &sp, sizeof(sp));
-		}
-
-		if (next_setpoint_valid) {
-			triplet.next_valid = true;
-			struct vehicle_global_position_setpoint_s sp;
-			sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f;
-			sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
-			sp.altitude = wpm->waypoints[next_setpoint_index].z;
-			sp.altitude_is_relative = false;
-			sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
-			set_special_fields(wpm->waypoints[next_setpoint_index].param1,
-				wpm->waypoints[next_setpoint_index].param2,
-				wpm->waypoints[next_setpoint_index].param3,
-				wpm->waypoints[next_setpoint_index].param4,
-				wpm->waypoints[next_setpoint_index].command, &sp);
-			memcpy(&(triplet.next), &sp, sizeof(sp));
-		}
-
-		/* Initialize triplet publication if necessary */
-		if (global_position_set_triplet_pub < 0) {
-			global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
-		}
-
-		sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
-
-	} else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-		/* global, relative alt (in relation to HOME) waypoint */
-		struct vehicle_global_position_setpoint_s sp;
-		sp.lat = param5_lat_x * 1e7f;
-		sp.lon = param6_lon_y * 1e7f;
-		sp.altitude = param7_alt_z;
-		sp.altitude_is_relative = true;
-		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
-		set_special_fields(param1, param2, param3, param4, command, &sp);
-
-		/* Initialize publication if necessary */
-		if (global_position_setpoint_pub < 0) {
-			global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
-		}
-
-
-
-		sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
-
-	} else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) {
-		/* local, absolute waypoint */
-		struct vehicle_local_position_setpoint_s sp;
-		sp.x = param5_lat_x;
-		sp.y = param6_lon_y;
-		sp.z = param7_alt_z;
-		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
-
-		/* Initialize publication if necessary */
-		if (local_position_setpoint_pub < 0) {
-			local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp);
-		}
-
-		sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
-	} else {
-		warnx("non-navigation WP, ignoring");
-		mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring.");
-		return;
-	}
-
-	/* only set this for known waypoint types (non-navigation types would have returned earlier) */
-	last_waypoint_index = index;
-
-	mavlink_missionlib_send_gcs_string(buf);
-	printf("%s\n", buf);
-	//printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);
-}
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index 5d3d6a73c..89a097c24 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -37,7 +37,6 @@
 
 MODULE_COMMAND	 = mavlink
 SRCS		 += mavlink.c \
-			   missionlib.c \
 			   mavlink_parameters.c \
 			   mavlink_receiver.cpp \
 			   orb_listener.c \
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index d3b735cfe..a70ee7d81 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -60,7 +60,6 @@
 
 #include "waypoints.h"
 #include "orb_topics.h"
-#include "missionlib.h"
 #include "mavlink_hil.h"
 #include "util.h"
 
@@ -68,8 +67,10 @@ extern bool gcs_link;
 
 struct vehicle_global_position_s global_pos;
 struct vehicle_local_position_s local_pos;
+struct home_position_s home;
 struct navigation_capabilities_s nav_cap;
 struct vehicle_status_s v_status;
+struct position_setpoint_triplet_s pos_sp_triplet;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
 struct actuator_armed_s armed;
@@ -136,7 +137,7 @@ static const struct listener listeners[] = {
 	{l_input_rc,			&mavlink_subs.input_rc_sub,	0},
 	{l_global_position,		&mavlink_subs.global_pos_sub,	0},
 	{l_local_position,		&mavlink_subs.local_pos_sub,	0},
-	{l_global_position_setpoint,	&mavlink_subs.spg_sub,		0},
+	{l_global_position_setpoint,	&mavlink_subs.triplet_sub,	0},
 	{l_local_position_setpoint,	&mavlink_subs.spl_sub,		0},
 	{l_attitude_setpoint,		&mavlink_subs.spa_sub,		0},
 	{l_actuator_outputs,		&mavlink_subs.act_0_sub,	0},
@@ -245,10 +246,10 @@ l_vehicle_attitude(const struct listener *l)
 		hrt_abstime t = hrt_absolute_time();
 		if (t >= last_sent_vfr + 100000) {
 			last_sent_vfr = t;
-			float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
+			float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
 			uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
 			float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
-			mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
+			mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d);
 		}
 		
 		/* send quaternion values if it exists */
@@ -312,6 +313,7 @@ l_vehicle_status(const struct listener *l)
 	/* immediately communicate state changes back to user */
 	orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
 	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
+	orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.position_setpoint_triplet_sub, &pos_sp_triplet);
 
 	/* enable or disable HIL */
 	if (v_status.hil_state == HIL_STATE_ON)
@@ -378,13 +380,13 @@ l_global_position(const struct listener *l)
 
 	mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
 						 global_pos.timestamp / 1000,
-					     global_pos.lat,
-					     global_pos.lon,
+					     global_pos.lat * 1e7,
+					     global_pos.lon * 1e7,
 					     global_pos.alt * 1000.0f,
-					     global_pos.relative_alt * 1000.0f,
-					     global_pos.vx * 100.0f,
-					     global_pos.vy * 100.0f,
-					     global_pos.vz * 100.0f,
+					     (global_pos.alt - home.alt) * 1000.0f,
+					     global_pos.vel_n * 100.0f,
+					     global_pos.vel_e * 100.0f,
+					     global_pos.vel_d * 100.0f,
 					     _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
 }
 
@@ -408,23 +410,19 @@ l_local_position(const struct listener *l)
 void
 l_global_position_setpoint(const struct listener *l)
 {
-	struct vehicle_global_position_setpoint_s global_sp;
+	struct position_setpoint_triplet_s triplet;
+	orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet);
 
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp);
-
-	uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
-
-	if (global_sp.altitude_is_relative)
-		coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+	if (!triplet.current.valid)
+		return;
 
 	if (gcs_link)
 		mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
-				coordinate_frame,
-				global_sp.lat,
-				global_sp.lon,
-				global_sp.altitude * 1000.0f,
-				global_sp.yaw * M_RAD_TO_DEG_F * 100.0f);
+				MAV_FRAME_GLOBAL,
+				(int32_t)(triplet.current.lat * 1e7d),
+				(int32_t)(triplet.current.lon * 1e7d),
+				(int32_t)(triplet.current.alt * 1e3f),
+				(int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f));
 }
 
 void
@@ -659,11 +657,9 @@ l_optical_flow(const struct listener *l)
 void
 l_home(const struct listener *l)
 {
-	struct home_position_s home;
-
 	orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
 
-	mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt);
+	mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f);
 }
 
 void
@@ -760,6 +756,10 @@ uorb_receive_start(void)
 	status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	orb_set_interval(status_sub, 300);		/* max 3.33 Hz updates */
 
+	/* --- POSITION SETPOINT TRIPLET --- */
+	mavlink_subs.position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+	orb_set_interval(mavlink_subs.position_setpoint_triplet_sub, 0);		/* not polled, don't limit */
+
 	/* --- RC CHANNELS VALUE --- */
 	rc_sub = orb_subscribe(ORB_ID(rc_channels));
 	orb_set_interval(rc_sub, 100);			/* 10Hz updates */
@@ -777,9 +777,9 @@ uorb_receive_start(void)
 	orb_set_interval(mavlink_subs.local_pos_sub, 1000);	/* 1Hz active updates */
 
 	/* --- GLOBAL SETPOINT VALUE --- */
-	mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
-	orb_set_interval(mavlink_subs.spg_sub, 2000);	/* 0.5 Hz updates */
-
+	mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+	orb_set_interval(mavlink_subs.triplet_sub, 2000);	/* 0.5 Hz updates */
+	
 	/* --- LOCAL SETPOINT VALUE --- */
 	mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
 	orb_set_interval(mavlink_subs.spl_sub, 2000);	/* 0.5 Hz updates */
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index 91773843b..89f647bdc 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -45,14 +45,14 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -86,7 +86,7 @@ struct mavlink_subscriptions {
 	int local_pos_sub;
 	int spa_sub;
 	int spl_sub;
-	int spg_sub;
+	int triplet_sub;
 	int debug_key_value;
 	int input_rc_sub;
 	int optical_flow;
@@ -94,6 +94,7 @@ struct mavlink_subscriptions {
 	int home_sub;
 	int airspeed_sub;
 	int navigation_capabilities_sub;
+	int position_setpoint_triplet_sub;
 };
 
 extern struct mavlink_subscriptions mavlink_subs;
@@ -110,6 +111,9 @@ extern struct navigation_capabilities_s nav_cap;
 /** Vehicle status */
 extern struct vehicle_status_s v_status;
 
+/** Position setpoint triplet */
+extern struct position_setpoint_triplet_s pos_sp_triplet;
+
 /** RC channels */
 extern struct rc_channels_s rc;
 
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 7e4a2688f..168666d4e 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
  *   Author: @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
  *           @author Lorenz Meier <lm@inf.ethz.ch>
  *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
@@ -44,29 +44,155 @@
 #include <sys/prctl.h>
 #include <unistd.h>
 #include <stdio.h>
-
 #include "mavlink_bridge_header.h"
-#include "missionlib.h"
 #include "waypoints.h"
 #include "util.h"
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <geo/geo.h>
+#include <dataman/dataman.h>
+#include <drivers/drv_hrt.h>
+#include <systemlib/err.h>
 
-#ifndef FM_PI
-#define FM_PI 3.1415926535897932384626433832795f
-#endif
+bool verbose = true;
 
-bool debug = false;
-bool verbose = false;
+orb_advert_t mission_pub = -1;
+struct mission_s mission;
 
+uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
 
-#define MAVLINK_WPM_NO_PRINTF
+void
+mavlink_missionlib_send_message(mavlink_message_t *msg)
+{
+	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
 
-uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
+	mavlink_send_uart_bytes(chan, missionlib_msg_buf, len);
+}
 
-void mavlink_wpm_init(mavlink_wpm_storage *state)
+
+
+int
+mavlink_missionlib_send_gcs_string(const char *string)
+{
+	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
+	mavlink_statustext_t statustext;
+	int i = 0;
+
+	while (i < len - 1) {
+		statustext.text[i] = string[i];
+
+		if (string[i] == '\0')
+			break;
+
+		i++;
+	}
+
+	if (i > 1) {
+		/* Enforce null termination */
+		statustext.text[i] = '\0';
+		mavlink_message_t msg;
+
+		mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
+		mavlink_missionlib_send_message(&msg);
+		return OK;
+
+	} else {
+		return 1;
+	}
+}
+
+void publish_mission()
+{
+	/* Initialize mission publication if necessary */
+	if (mission_pub < 0) {
+		mission_pub = orb_advertise(ORB_ID(mission), &mission);
+
+	} else {
+		orb_publish(ORB_ID(mission), mission_pub, &mission);
+	}
+}
+
+int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
+{
+	/* only support global waypoints for now */
+	switch (mavlink_mission_item->frame) {
+		case MAV_FRAME_GLOBAL:
+			mission_item->lat = (double)mavlink_mission_item->x;
+			mission_item->lon = (double)mavlink_mission_item->y;
+			mission_item->altitude = mavlink_mission_item->z;
+			mission_item->altitude_is_relative = false;
+			break;
+
+		case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+			mission_item->lat = (double)mavlink_mission_item->x;
+			mission_item->lon = (double)mavlink_mission_item->y;
+			mission_item->altitude = mavlink_mission_item->z;
+			mission_item->altitude_is_relative = true;
+			break;
+
+		case MAV_FRAME_LOCAL_NED:
+		case MAV_FRAME_LOCAL_ENU:
+			return MAV_MISSION_UNSUPPORTED_FRAME;
+		case MAV_FRAME_MISSION:
+		default:
+			return MAV_MISSION_ERROR;
+	}
+
+	switch (mavlink_mission_item->command) {
+		case MAV_CMD_NAV_TAKEOFF:
+			mission_item->pitch_min = mavlink_mission_item->param2;
+			break;
+		default:
+			mission_item->acceptance_radius = mavlink_mission_item->param2;
+			break;
+	}
+
+	mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F);
+	mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
+	mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
+	mission_item->nav_cmd = mavlink_mission_item->command;
+
+	mission_item->time_inside = mavlink_mission_item->param1;
+	mission_item->autocontinue = mavlink_mission_item->autocontinue;
+	// mission_item->index = mavlink_mission_item->seq;
+	mission_item->origin = ORIGIN_MAVLINK;
+
+	return OK;
+}
+
+int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
 {
-	// Set all waypoints to zero
+	if (mission_item->altitude_is_relative) {
+		mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
+	} else {
+		mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+	}
+	
+	switch (mission_item->nav_cmd) {
+		case NAV_CMD_TAKEOFF:
+			mavlink_mission_item->param2 = mission_item->pitch_min;
+			break;
+		default:
+			mavlink_mission_item->param2 = mission_item->acceptance_radius;
+			break;
+	}
+
+	mavlink_mission_item->x = (float)mission_item->lat;
+	mavlink_mission_item->y = (float)mission_item->lon;
+	mavlink_mission_item->z = mission_item->altitude;
+
+	mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F;
+	mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction;
+	mavlink_mission_item->command = mission_item->nav_cmd;
+	mavlink_mission_item->param1 = mission_item->time_inside;
+	mavlink_mission_item->autocontinue = mission_item->autocontinue;
+	// mavlink_mission_item->seq = mission_item->index;
+
+	return OK;
+}
 
-	// Set count to zero
+void mavlink_wpm_init(mavlink_wpm_storage *state)
+{
 	state->size = 0;
 	state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
 	state->current_state = MAVLINK_WPM_STATE_IDLE;
@@ -75,14 +201,7 @@ void mavlink_wpm_init(mavlink_wpm_storage *state)
 	state->timestamp_lastaction = 0;
 	state->timestamp_last_send_setpoint = 0;
 	state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
-	state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT;
-	state->idle = false;      				///< indicates if the system is following the waypoints or is waiting
-	state->current_active_wp_id = -1;		///< id of current waypoint
-	state->yaw_reached = false;						///< boolean for yaw attitude reached
-	state->pos_reached = false;						///< boolean for position reached
-	state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
-	state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
-
+	state->current_dataman_id = 0;
 }
 
 /*
@@ -93,25 +212,14 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
 	mavlink_message_t msg;
 	mavlink_mission_ack_t wpa;
 
-	wpa.target_system = wpm->current_partner_sysid;
-	wpa.target_component = wpm->current_partner_compid;
+	wpa.target_system = sysid;
+	wpa.target_component = compid;
 	wpa.type = type;
 
 	mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa);
 	mavlink_missionlib_send_message(&msg);
 
-	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
-
-	if (MAVLINK_WPM_TEXT_FEEDBACK) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-		mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
-#else
-
-		if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
-
-#endif
-		mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
-	}
+	if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system);
 }
 
 /*
@@ -126,45 +234,19 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
 void mavlink_wpm_send_waypoint_current(uint16_t seq)
 {
 	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
-
 		mavlink_message_t msg;
 		mavlink_mission_current_t wpc;
 
-		wpc.seq = cur->seq;
+		wpc.seq = seq;
 
 		mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
 		mavlink_missionlib_send_message(&msg);
 
-		// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
-
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Set current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq);
+		if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq);
 
 	} else {
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds\n");
-	}
-}
-
-/*
- *  @brief Directs the MAV to fly to a position
- *
- *  Sends a message to the controller, advising it to fly to the coordinates
- *  of the waypoint with a given orientation
- *
- *  @param seq The waypoint sequence number the MAV should fly to.
- */
-void mavlink_wpm_send_setpoint(uint16_t seq)
-{
-	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
-		mavlink_missionlib_current_waypoint_changed(cur->seq, cur->param1,
-				cur->param2, cur->param3, cur->param4, cur->x,
-				cur->y, cur->z, cur->frame, cur->command);
-
-		wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp();
-
-	} else {
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n");
+		mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
+		if (verbose) warnx("ERROR: index out of bounds");
 	}
 }
 
@@ -173,34 +255,47 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou
 	mavlink_message_t msg;
 	mavlink_mission_count_t wpc;
 
-	wpc.target_system = wpm->current_partner_sysid;
-	wpc.target_component = wpm->current_partner_compid;
-	wpc.count = count;
+	wpc.target_system = sysid;
+	wpc.target_component = compid;
+	wpc.count = mission.count;
 
 	mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
 	mavlink_missionlib_send_message(&msg);
 
-	if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system);
-
-	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
+	if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system);
 }
 
 void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
 {
-	if (seq < wpm->size) {
-		mavlink_message_t msg;
-		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
-		wp->target_system = wpm->current_partner_sysid;
-		wp->target_component = wpm->current_partner_compid;
-		mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp);
-		mavlink_missionlib_send_message(&msg);
 
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
+	struct mission_item_s mission_item;
+	ssize_t len = sizeof(struct mission_item_s);
+	
+	dm_item_t dm_current;
+
+	if (wpm->current_dataman_id == 0) {
+		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+	} else {
+		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+	}
+
+	if (dm_read(dm_current, seq, &mission_item, len) == len) {
 
-		// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
+		/* create mission_item_s from mavlink_mission_item_t */
+		mavlink_mission_item_t wp;
+		map_mission_item_to_mavlink_mission_item(&mission_item, &wp);
 
+		mavlink_message_t msg;
+		wp.target_system = sysid;
+		wp.target_component = compid;
+		wp.seq = seq;
+		mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp);
+		mavlink_missionlib_send_message(&msg);
+
+		if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system);
 	} else {
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n");
+		mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
+		if (verbose) warnx("ERROR: could not read WP%u", seq);
 	}
 }
 
@@ -209,18 +304,17 @@ void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t s
 	if (seq < wpm->max_size) {
 		mavlink_message_t msg;
 		mavlink_mission_request_t wpr;
-		wpr.target_system = wpm->current_partner_sysid;
-		wpr.target_component = wpm->current_partner_compid;
+		wpr.target_system = sysid;
+		wpr.target_component = compid;
 		wpr.seq = seq;
 		mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr);
 		mavlink_missionlib_send_message(&msg);
 
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system);
-
-		// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
+		if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system);
 
 	} else {
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n");
+		mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
+		if (verbose) warnx("ERROR: Waypoint index exceeds list capacity");
 	}
 }
 
@@ -241,293 +335,33 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
 	mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached);
 	mavlink_missionlib_send_message(&msg);
 
-	if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq);
-
-	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
-}
-
-/*
- * Calculate distance in global frame.
- *
- * The distance calculation is based on the WGS84 geoid (GPS)
- */
-float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z)
-{
-
-	if (seq < wpm->size) {
-		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
-
-		double current_x_rad = wp->x / 180.0 * M_PI;
-		double current_y_rad = wp->y / 180.0 * M_PI;
-		double x_rad = lat / 180.0 * M_PI;
-		double y_rad = lon / 180.0 * M_PI;
-
-		double d_lat = x_rad - current_x_rad;
-		double d_lon = y_rad - current_y_rad;
-
-		double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad);
-		double c = 2 * atan2(sqrt(a), sqrt(1 - a));
-
-		const double radius_earth = 6371000.0;
-
-		float dxy = radius_earth * c;
-		float dz = alt - wp->z;
-
-		*dist_xy = fabsf(dxy);
-		*dist_z = fabsf(dz);
-
-		return sqrtf(dxy * dxy + dz * dz);
-
-	} else {
-		return -1.0f;
-	}
-
-}
-
-/*
- * Calculate distance in local frame (NED)
- */
-float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z)
-{
-	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
-
-		float dx = (cur->x - x);
-		float dy = (cur->y - y);
-		float dz = (cur->z - z);
-
-		*dist_xy = sqrtf(dx * dx + dy * dy);
-		*dist_z = fabsf(dz);
-
-		return sqrtf(dx * dx + dy * dy + dz * dz);
-
-	} else {
-		return -1.0f;
-	}
-}
-
-void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance)
-{
-	static uint16_t counter;
-
-	if ((!global_pos->valid && !local_pos->xy_valid) ||
-		/* no waypoint */
-		wpm->size == 0) {
-		/* nothing to check here, return */
-		return;
-	}
-
-	if (wpm->current_active_wp_id < wpm->size) {
-
-		float orbit;
-		if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) {
-
-			orbit = wpm->waypoints[wpm->current_active_wp_id].param2;
-
-		} else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
-
-			orbit = wpm->waypoints[wpm->current_active_wp_id].param3;
-		} else {
-
-			// XXX set default orbit via param
-			orbit = 15.0f;
-		}
-
-		/* keep vertical orbit */
-		float vertical_switch_distance = orbit;
-
-		/* Take the larger turn distance - orbit or turn_distance */
-		if (orbit < turn_distance)
-			orbit = turn_distance;
-
-		int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
-		float dist = -1.0f;
-
-		float dist_xy = -1.0f;
-		float dist_z = -1.0f;
-
-		if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z);
-
-		} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z);
-
-		} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
-			dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z);
-
-		} else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
-			/* Check if conditions of mission item are satisfied */
-			// XXX TODO
-		}
-
-		if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
-			wpm->pos_reached = true;
-		}
-
-		// check if required yaw reached
-		float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
-		float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
-		if (fabsf(yaw_err) < 0.05f) {
-			wpm->yaw_reached = true;
-		}
-	}
-
-	//check if the current waypoint was reached
-	if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) {
-		if (wpm->current_active_wp_id < wpm->size) {
-			mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-
-			if (wpm->timestamp_firstinside_orbit == 0) {
-				// Announce that last waypoint was reached
-				mavlink_wpm_send_waypoint_reached(cur_wp->seq);
-				wpm->timestamp_firstinside_orbit = now;
-			}
-
-			// check if the MAV was long enough inside the waypoint orbit
-			//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
-
-			bool time_elapsed = false;
-
-			if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
-				time_elapsed = true;
-			} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
-				time_elapsed = true;
-			}
-
-			if (time_elapsed) {
-
-				/* safeguard against invalid missions with last wp autocontinue on */
-				if (wpm->current_active_wp_id == wpm->size - 1) {
-					/* stop handling missions here */
-					cur_wp->autocontinue = false;
-				}
-
-				if (cur_wp->autocontinue) {
-
-					cur_wp->current = 0;
-
-					float navigation_lat = -1.0f;
-					float navigation_lon = -1.0f;
-					float navigation_alt = -1.0f;
-					int navigation_frame = -1;
-
-					/* initialize to current position in case we don't find a suitable navigation waypoint */
-					if (global_pos->valid) {
-						navigation_lat = global_pos->lat/1e7;
-						navigation_lon = global_pos->lon/1e7;
-						navigation_alt = global_pos->alt;
-						navigation_frame = MAV_FRAME_GLOBAL;
-					} else if (local_pos->xy_valid && local_pos->z_valid) {
-						navigation_lat = local_pos->x;
-						navigation_lon = local_pos->y;
-						navigation_alt = local_pos->z;
-						navigation_frame = MAV_FRAME_LOCAL_NED;
-					}
-
-					/* guard against missions without final land waypoint */
-					/* only accept supported navigation waypoints, skip unknown ones */
-					do {
-
-						/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
-						if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) {
-
-							/* this is a navigation waypoint */
-							navigation_frame = cur_wp->frame;
-							navigation_lat = cur_wp->x;
-							navigation_lon = cur_wp->y;
-							navigation_alt = cur_wp->z;
-						}
-
-						if (wpm->current_active_wp_id == wpm->size - 1) {
-
-							/* if we're not landing at the last nav waypoint, we're falling back to loiter */
-							if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) {
-								/* the last waypoint was reached, if auto continue is
-								 * activated AND it is NOT a land waypoint, keep the system loitering there.
-								 */
-								cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
-								cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
-								cur_wp->frame = navigation_frame;
-								cur_wp->x = navigation_lat;
-								cur_wp->y = navigation_lon;
-								cur_wp->z = navigation_alt;
-							}
-
-							/* we risk an endless loop for missions without navigation waypoints, abort. */
-							break;
-
-						} else {
-							if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
-								wpm->current_active_wp_id++;
-						}
-			
-					} while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM));
-
-					// Fly to next waypoint
-					wpm->timestamp_firstinside_orbit = 0;
-					mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-					mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-					wpm->waypoints[wpm->current_active_wp_id].current = true;
-					wpm->pos_reached = false;
-					wpm->yaw_reached = false;
-					printf("Set new waypoint (%u)\n", wpm->current_active_wp_id);
-				}
-			}
-		}
-
-	} else {
-		wpm->timestamp_lastoutside_orbit = now;
-	}
-
-	counter++;
+	if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq);
 }
 
 
-int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap)
+void mavlink_waypoint_eventloop(uint64_t now)
 {
 	/* check for timed-out operations */
 	if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
 
-#ifdef MAVLINK_WPM_NO_PRINTF
-		mavlink_missionlib_send_gcs_string("Operation timeout switching -> IDLE");
-#else
+		mavlink_missionlib_send_gcs_string("Operation timeout");
 
-		if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_state);
+		if (verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state);
 
-#endif
 		wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-		wpm->current_count = 0;
 		wpm->current_partner_sysid = 0;
 		wpm->current_partner_compid = 0;
-		wpm->current_wp_id = -1;
-
-		if (wpm->size == 0) {
-			wpm->current_active_wp_id = -1;
-		}
 	}
-
-	check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance);
-
-	return OK;
 }
 
 
-void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos)
+void mavlink_wpm_message_handler(const mavlink_message_t *msg)
 {
-	uint64_t now = mavlink_missionlib_get_system_timestamp();
+	uint64_t now = hrt_absolute_time();
 
 	switch (msg->msgid) {
 
-	case MAVLINK_MSG_ID_MISSION_ACK: {
+		case MAVLINK_MSG_ID_MISSION_ACK: {
 			mavlink_mission_ack_t wpa;
 			mavlink_msg_mission_ack_decode(msg, &wpa);
 
@@ -537,8 +371,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 				if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
 					if (wpm->current_wp_id == wpm->size - 1) {
 
-						mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
-
 						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 						wpm->current_wp_id = 0;
 					}
@@ -546,12 +378,13 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 			} else {
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
+				if (verbose) warnx("REJ. WP CMD: curr partner id mismatch");
 			}
 
 			break;
 		}
 
-	case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
+		case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
 			mavlink_mission_set_current_t wpc;
 			mavlink_msg_mission_set_current_decode(msg, &wpc);
 
@@ -560,44 +393,32 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
 					if (wpc.seq < wpm->size) {
-						// if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n");
-						wpm->current_active_wp_id = wpc.seq;
-						uint32_t i;
-
-						for (i = 0; i < wpm->size; i++) {
-							if (i == wpm->current_active_wp_id) {
-								wpm->waypoints[i].current = true;
 
-							} else {
-								wpm->waypoints[i].current = false;
-							}
-						}
-
-						mavlink_missionlib_send_gcs_string("NEW WP SET");
-
-						wpm->yaw_reached = false;
-						wpm->pos_reached = false;
-						mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-						mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-						wpm->timestamp_firstinside_orbit = 0;
+						mission.current_index = wpc.seq;
+						publish_mission();
+						
+						mavlink_wpm_send_waypoint_current(wpc.seq);
 
 					} else {
 						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
+						if (verbose) warnx("IGN WP CURR CMD: Not in list");
 					}
 
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
+					if (verbose) warnx("IGN WP CURR CMD: Busy");
 
 				}
 
 			} else {
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
+				if (verbose) warnx("REJ. WP CMD: target id mismatch");
 			}
 
 			break;
 		}
 
-	case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
+		case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
 			mavlink_mission_request_list_t wprl;
 			mavlink_msg_mission_request_list_decode(msg, &wprl);
 
@@ -606,437 +427,304 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
 					if (wpm->size > 0) {
-						//if (verbose && wpm->current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
-//                        if (verbose && wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
+						
 						wpm->current_state = MAVLINK_WPM_STATE_SENDLIST;
 						wpm->current_wp_id = 0;
 						wpm->current_partner_sysid = msg->sysid;
 						wpm->current_partner_compid = msg->compid;
 
 					} else {
-						// if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
+						if (verbose) warnx("No waypoints send");
 					}
 
 					wpm->current_count = wpm->size;
 					mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count);
 
 				} else {
-					// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm->current_state);
+					mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
+					if (verbose) warnx("IGN REQUEST LIST: Busy");
 				}
 			} else {
-				// if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n");
+				mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch");
+				if (verbose) warnx("REJ. REQUEST LIST: target id mismatch");
 			}
 
 			break;
 		}
 
-	case MAVLINK_MSG_ID_MISSION_REQUEST: {
+		case MAVLINK_MSG_ID_MISSION_REQUEST: {
 			mavlink_mission_request_t wpr;
 			mavlink_msg_mission_request_decode(msg, &wpr);
 
 			if (msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
 				wpm->timestamp_lastaction = now;
 
-				//ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
-				if ((wpm->current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm->current_wp_id || wpr.seq == wpm->current_wp_id + 1) && wpr.seq < wpm->size)) {
-					if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND");
-#else
+				if (wpr.seq >= wpm->size) {
 
-						if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
+					if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq);
+					break;
+				}
 
-#endif
-					}
+				/* 
+				 * Ensure that we are in the correct state and that the first request has id 0 
+				 * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
+				 */
+				if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
 
-					if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id + 1) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ");
-#else
+					if (wpr.seq == 0) {
+						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
+						wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
+					} else {
+						mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
+						if (verbose) warnx("REJ. WP CMD: First id != 0");
+						break;
+					}
 
-						if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+				} else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
 
-#endif
-					}
+					if (wpr.seq == wpm->current_wp_id) {
 
-					if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ");
-#else
+						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
 
-						if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+					} else if (wpr.seq == wpm->current_wp_id + 1) {
 
-#endif
+						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
+					
+					} else {
+						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
+						if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1);
+						break;
 					}
 
-					wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
-					wpm->current_wp_id = wpr.seq;
-					mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpr.seq);
-
 				} else {
-					// if (verbose)
-					{
-						if (!(wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-							mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-#else
-
-							if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm->current_state);
-
-#endif
-							break;
 
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
-							if (wpr.seq != 0) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-								mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
-#else
-
-								if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
-
-#endif
-							}
-
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
-							if (wpr.seq != wpm->current_wp_id && wpr.seq != wpm->current_wp_id + 1) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-								mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
-#else
-
-								if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1);
-
-#endif
-
-							} else if (wpr.seq >= wpm->size) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-								mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
-#else
-
-								if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
+					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
+					if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state);
+					break;
+				}
 
-#endif
-							}
+				wpm->current_wp_id = wpr.seq;
+				wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
 
-						} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-							mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?");
-#else
+				if (wpr.seq < wpm->size) {
 
-							if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n");
+					mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid,wpm->current_wp_id);
 
-#endif
-						}
-					}
+				} else {
+					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
+					if (verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq);
 				}
 
+
 			} else {
 				//we we're target but already communicating with someone else
 				if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-#else
-
-					if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm->current_partner_sysid);
 
-#endif
+					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
+					if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid);
 
 				} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-					mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-#else
 
-					if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
-
-#endif
+					mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
+					if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
 				}
-
 			}
-
 			break;
 		}
 
-	case MAVLINK_MSG_ID_MISSION_COUNT: {
+		case MAVLINK_MSG_ID_MISSION_COUNT: {
 			mavlink_mission_count_t wpc;
 			mavlink_msg_mission_count_decode(msg, &wpc);
 
 			if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
 				wpm->timestamp_lastaction = now;
 
-				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id == 0)) {
-//                	printf("wpc count in: %d\n",wpc.count);
-//                	printf("Comp id: %d\n",msg->compid);
-//                	printf("Current partner sysid: %d\n",wpm->current_partner_sysid);
-
-					if (wpc.count > 0) {
-						if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-							mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST");
-#else
+				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
 
-							if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid);
+					if (wpc.count > NUM_MISSIONS_SUPPORTED) {
+						if (verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
+						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE);
+						break;
+					}
 
-#endif
-						}
+					if (wpc.count == 0) {
+						mavlink_missionlib_send_gcs_string("COUNT 0");
+						if (verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE");
+						break;
+					}
+					
+					if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid);
 
-						if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-							mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
-#else
+					wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
+					wpm->current_wp_id = 0;
+					wpm->current_partner_sysid = msg->sysid;
+					wpm->current_partner_compid = msg->compid;
+					wpm->current_count = wpc.count;
 
-							if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
+					mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
 
-#endif
-						}
+				} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
 
-						wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
-						wpm->current_wp_id = 0;
-						wpm->current_partner_sysid = msg->sysid;
-						wpm->current_partner_compid = msg->compid;
-						wpm->current_count = wpc.count;
+					if (wpm->current_wp_id == 0) {
+						mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
+						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid);
+					} else {
+						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
+						if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id);
+					}
+				} else {
+						mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
+						if (verbose) warnx("IGN MISSION_COUNT CMD: Busy");
+				}
+			} else {
 
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY");
-#else
+				mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch");
+				if (verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
+			}
+		}
+		break;
 
-						if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n");
+		case MAVLINK_MSG_ID_MISSION_ITEM: {
+			mavlink_mission_item_t wp;
+			mavlink_msg_mission_item_decode(msg, &wp);
 
-#endif
-						wpm->rcv_size = 0;
-						//while(waypoints_receive_buffer->size() > 0)
-//                        {
-//                            delete waypoints_receive_buffer->back();
-//                            waypoints_receive_buffer->pop_back();
-//                        }
+			if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) {
 
-						mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
+				wpm->timestamp_lastaction = now;
 
-					} else if (wpc.count == 0) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("COUNT 0");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n");
-
-#endif
-						wpm->rcv_size = 0;
-						//while(waypoints_receive_buffer->size() > 0)
-//                        {
-//                            delete waypoints->back();
-//                            waypoints->pop_back();
-//                        }
-						wpm->current_active_wp_id = -1;
-						wpm->yaw_reached = false;
-						wpm->pos_reached = false;
-						break;
+				/*
+				 * ensure that we are in the correct state and that the first waypoint has id 0
+				 * and the following waypoints have the correct ids
+				 */
 
-					} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("IGN WP CMD");
-#else
+				if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
 
-						if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
+				 	if (wp.seq != 0) {
+				 		mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0");
+				 		warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq);
+				 		break;
+				 	}
+				} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
 
-#endif
+					if (wp.seq >= wpm->current_count) {
+						mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds");
+						warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq);
+						break;
 					}
 
-				} else {
-					if (!(wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_GETLIST)) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm->current_state);
-
-#endif
-
-					} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id != 0) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm->current_wp_id);
+					if (wp.seq != wpm->current_wp_id) {
+						warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, wpm->current_wp_id);
+						mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
+						break;
+					}
+				}
 
-#endif
+				wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
 
-					} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?");
-#else
+				struct mission_item_s mission_item;
 
-						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n");
+				int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item);
 
-#endif
-					}
+				if (ret != OK) {
+					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret);
+					wpm->current_state = MAVLINK_WPM_STATE_IDLE;
+					break;
 				}
 
-			} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-#else
+				ssize_t len = sizeof(struct mission_item_s);
 
-				if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
+				dm_item_t dm_next;
 
-#endif
-			}
+				if (wpm->current_dataman_id == 0) {
+					dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1;
+					mission.dataman_id = 1;
+				} else {
+					dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0;
+					mission.dataman_id = 0;
+				}
 
-		}
-		break;
+				if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
+					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
+					wpm->current_state = MAVLINK_WPM_STATE_IDLE;
+					break;
+				}
 
-	case MAVLINK_MSG_ID_MISSION_ITEM: {
-			mavlink_mission_item_t wp;
-			mavlink_msg_mission_item_decode(msg, &wp);
+				if (wp.current) {
+					mission.current_index = wp.seq;
+				}
 
-			mavlink_missionlib_send_gcs_string("GOT WP");
-//			printf("sysid=%d, current_partner_sysid=%d\n", msg->sysid, wpm->current_partner_sysid);
-//			printf("compid=%d, current_partner_compid=%d\n", msg->compid, wpm->current_partner_compid);
+				wpm->current_wp_id = wp.seq + 1;
 
-//            if((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/))
-			if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) {
+				if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
+					
+					if (verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count);
 
-				wpm->timestamp_lastaction = now;
+					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
 
-//                printf("wpm->current_state=%u, wp.seq = %d, wpm->current_wp_id=%d\n", wpm->current_state, wp.seq, wpm->current_wp_id);
-
-//                wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO
-
-				//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
-				if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && wp.seq < wpm->current_count)) {
-					//mavlink_missionlib_send_gcs_string("DEBUG 2");
-
-//                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid);
-//                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid);
-//                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid);
-//
-					wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
-					mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]);
-					memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
-//                    printf("WP seq: %d\n",wp.seq);
-					wpm->current_wp_id = wp.seq + 1;
-
-					// if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
-//					printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
-
-//					printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count);
-					if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-						mavlink_missionlib_send_gcs_string("GOT ALL WPS");
-						// if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_count);
-
-						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0);
-
-						if (wpm->current_active_wp_id > wpm->rcv_size - 1) {
-							wpm->current_active_wp_id = wpm->rcv_size - 1;
-						}
-
-						// switch the waypoints list
-						// FIXME CHECK!!!
-						uint32_t i;
-
-						for (i = 0; i < wpm->current_count; ++i) {
-							wpm->waypoints[i] = wpm->rcv_waypoints[i];
-						}
-
-						wpm->size = wpm->current_count;
-
-						//get the new current waypoint
-
-						for (i = 0; i < wpm->size; i++) {
-							if (wpm->waypoints[i].current == 1) {
-								wpm->current_active_wp_id = i;
-								//// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id);
-								wpm->yaw_reached = false;
-								wpm->pos_reached = false;
-								mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-								mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-								wpm->timestamp_firstinside_orbit = 0;
-								break;
-							}
-						}
-
-						if (i == wpm->size) {
-							wpm->current_active_wp_id = -1;
-							wpm->yaw_reached = false;
-							wpm->pos_reached = false;
-							wpm->timestamp_firstinside_orbit = 0;
-						}
+					mission.count = wpm->current_count;
+					
+					publish_mission();
 
-						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
+					wpm->current_dataman_id = mission.dataman_id;
+					wpm->size = wpm->current_count;
 
-					} else {
-						mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
-					}
+					wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 
 				} else {
-					if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-						//we're done receiving waypoints, answer with ack.
-						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0);
-						printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
-					}
-
-					// if (verbose)
-					{
-						if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) {
-//							 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state);
-							break;
-
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
-							if (!(wp.seq == 0)) {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq);
-							} else {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
-							}
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-							if (!(wp.seq == wpm->current_wp_id)) {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id);
-								mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
-
-							} else if (!(wp.seq < wpm->current_count)) {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq);
-							} else {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
-							}
-						} else {
-//							 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
-						}
-					}
+					mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
 				}
+
 			} else {
-				//we we're target but already communicating with someone else
-				if ((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
-					// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm->current_partner_sysid);
-				} else if (wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/) {
-					// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
-				}
+				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
+				if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
 			}
 
 			break;
 		}
 
-	case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
+		case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
 			mavlink_mission_clear_all_t wpca;
 			mavlink_msg_mission_clear_all_decode(msg, &wpca);
 
-			if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-				wpm->timestamp_lastaction = now;
+			if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
+
+				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
+					wpm->timestamp_lastaction = now;
+
+					wpm->size = 0;
+
+					/* prepare mission topic */
+					mission.dataman_id = -1;
+					mission.count = 0;
+					mission.current_index = -1;
+					publish_mission();
+
+					if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) {
+						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
+					} else {
+						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
+					}
+
+					
+				} else {
+					mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy");
+					if (verbose) warnx("IGN WP CLEAR CMD: Busy");
+				}
 
-				// if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
-				// Delete all waypoints
-				wpm->size = 0;
-				wpm->current_active_wp_id = -1;
-				wpm->yaw_reached = false;
-				wpm->pos_reached = false;
 
 			} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
-				// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state);
+
+				mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch");
+				if (verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
 			}
 
 			break;
 		}
 
 	default: {
-			// if (debug) // printf("Waypoint: received message of unknown type");
+			/* other messages might should get caught by mavlink and others */
 			break;
 		}
 	}
-
-	// check_waypoints_reached(now, global_pos, local_pos);
 }
diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
index d7d6b31dc..f8b58c7d9 100644
--- a/src/modules/mavlink/waypoints.h
+++ b/src/modules/mavlink/waypoints.h
@@ -46,18 +46,10 @@
  or in the same folder as this source file */
 
 #include <v1.0/mavlink_types.h>
-
-// #ifndef MAVLINK_SEND_UART_BYTES
-// #define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
-// #endif
-//extern mavlink_system_t mavlink_system;
 #include "mavlink_bridge_header.h"
 #include <stdbool.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/mission.h>
 
-// FIXME XXX - TO BE MOVED TO XML
 enum MAVLINK_WPM_STATES {
 	MAVLINK_WPM_STATE_IDLE = 0,
 	MAVLINK_WPM_STATE_SENDLIST,
@@ -78,55 +70,43 @@ enum MAVLINK_WPM_CODES {
 };
 
 
-/* WAYPOINT MANAGER - MISSION LIB */
-
-#define MAVLINK_WPM_MAX_WP_COUNT 15
-#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE				  ///< Enable double buffer and in-flight updates
-#ifndef MAVLINK_WPM_TEXT_FEEDBACK
-#define MAVLINK_WPM_TEXT_FEEDBACK 0						  ///< Report back status information as text
-#endif
+#define MAVLINK_WPM_MAX_WP_COUNT 255
 #define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000         ///< Protocol communication timeout in useconds
 #define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000           ///< When to send a new setpoint
 #define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000
 
 
 struct mavlink_wpm_storage {
-	mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT];      ///< Currently active waypoints
-#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
-	mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT];  ///< Receive buffer for next waypoints
-#endif
 	uint16_t size;
 	uint16_t max_size;
-	uint16_t rcv_size;
 	enum MAVLINK_WPM_STATES current_state;
 	int16_t current_wp_id;							///< Waypoint in current transmission
-	int16_t current_active_wp_id;					///< Waypoint the system is currently heading towards
 	uint16_t current_count;
 	uint8_t current_partner_sysid;
 	uint8_t current_partner_compid;
 	uint64_t timestamp_lastaction;
 	uint64_t timestamp_last_send_setpoint;
-	uint64_t timestamp_firstinside_orbit;
-	uint64_t timestamp_lastoutside_orbit;
 	uint32_t timeout;
-	uint32_t delay_setpoint;
-	float accept_range_yaw;
-	float accept_range_distance;
-	bool yaw_reached;
-	bool pos_reached;
-	bool idle;
+	int current_dataman_id;
 };
 
 typedef struct mavlink_wpm_storage mavlink_wpm_storage;
 
+int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
+int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
+
+
 void mavlink_wpm_init(mavlink_wpm_storage *state);
-int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position,
-			       struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap);
-void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos ,
-				 struct vehicle_local_position_s *local_pos);
+void mavlink_waypoint_eventloop(uint64_t now);
+void mavlink_wpm_message_handler(const mavlink_message_t *msg);
 
 extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		float param2, float param3, float param4, float param5_lat_x,
 		float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
 
+static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
+
+void mavlink_missionlib_send_message(mavlink_message_t *msg);
+int mavlink_missionlib_send_gcs_string(const char *string);
+
 #endif /* WAYPOINTS_H_ */
diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c
index 0edb53a3e..ab9ce45f3 100644
--- a/src/modules/mavlink_onboard/mavlink.c
+++ b/src/modules/mavlink_onboard/mavlink.c
@@ -441,7 +441,8 @@ int mavlink_thread_main(int argc, char *argv[])
 			get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode);
 
 			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
+			// TODO fix navigation state, use control_mode topic
+			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, 0, mavlink_state);
 
 			/* send status (values already copied in the section above) */
 			mavlink_msg_sys_status_send(chan,
diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h
index 1b49c9ce4..bbc9f6e66 100644
--- a/src/modules/mavlink_onboard/orb_topics.h
+++ b/src/modules/mavlink_onboard/orb_topics.h
@@ -50,7 +50,7 @@
 #include <uORB/topics/vehicle_command.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/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/optical_flow.h>
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
new file mode 100644
index 000000000..db5e2e9bb
--- /dev/null
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -0,0 +1,869 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Author: @author Tobias Naegeli <naegelit@student.ethz.ch>
+ *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control_main.c
+ * Multicopter attitude controller.
+ *
+ * The controller has two loops: P loop for angular error and PD loop for angular rate error.
+ * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
+ * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
+ * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
+ * These two approaches fused seamlessly with weight depending on angular error.
+ * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/pid/pid.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+
+/**
+ * Multicopter attitude control app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
+
+#define MIN_TAKEOFF_THROTTLE	0.3f
+#define YAW_DEADZONE	0.05f
+#define RATES_I_LIMIT	0.5f
+
+class MulticopterAttitudeControl
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterAttitudeControl();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~MulticopterAttitudeControl();
+
+	/**
+	 * Start the sensors task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+private:
+
+	bool	_task_should_exit;		/**< if true, sensor task should exit */
+	int		_control_task;			/**< task handle for sensor task */
+
+	int		_v_att_sub;				/**< vehicle attitude subscription */
+	int		_v_att_sp_sub;			/**< vehicle attitude setpoint subscription */
+	int		_v_rates_sp_sub;		/**< vehicle rates setpoint subscription */
+	int		_v_control_mode_sub;	/**< vehicle control mode subscription */
+	int		_params_sub;			/**< parameter updates subscription */
+	int		_manual_control_sp_sub;	/**< manual control setpoint subscription */
+	int		_armed_sub;				/**< arming status subscription */
+
+	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
+	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
+	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
+
+	struct vehicle_attitude_s			_v_att;				/**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s	_v_att_sp;			/**< vehicle attitude setpoint */
+	struct vehicle_rates_setpoint_s		_v_rates_sp;		/**< vehicle rates setpoint */
+	struct manual_control_setpoint_s	_manual_control_sp;	/**< manual control setpoint */
+	struct vehicle_control_mode_s		_v_control_mode;	/**< vehicle control mode */
+	struct actuator_controls_s			_actuators;			/**< actuator controls */
+	struct actuator_armed_s				_armed;				/**< actuator arming status */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	math::Matrix<3, 3>	_R_sp;			/**< attitude setpoint rotation matrix */
+	math::Matrix<3, 3>	_R;				/**< rotation matrix for current state */
+	math::Vector<3>		_rates_prev;	/**< angular rates on previous step */
+	math::Vector<3>		_rates_sp;		/**< angular rates setpoint */
+	math::Vector<3>		_rates_int;		/**< angular rates integral error */
+	float				_thrust_sp;		/**< thrust setpoint */
+	math::Vector<3>		_att_control;	/**< attitude control vector */
+
+	math::Matrix<3, 3>	I;				/**< identity matrix */
+
+	bool	_reset_yaw_sp;			/**< reset yaw setpoint flag */
+
+	struct {
+		param_t roll_p;
+		param_t roll_rate_p;
+		param_t roll_rate_i;
+		param_t roll_rate_d;
+		param_t pitch_p;
+		param_t pitch_rate_p;
+		param_t pitch_rate_i;
+		param_t pitch_rate_d;
+		param_t yaw_p;
+		param_t yaw_rate_p;
+		param_t yaw_rate_i;
+		param_t yaw_rate_d;
+		param_t yaw_ff;
+
+		param_t rc_scale_yaw;
+	}		_params_handles;		/**< handles for interesting parameters */
+
+	struct {
+		math::Vector<3> att_p;					/**< P gain for angular error */
+		math::Vector<3> rate_p;				/**< P gain for angular rate error */
+		math::Vector<3> rate_i;				/**< I gain for angular rate error */
+		math::Vector<3> rate_d;				/**< D gain for angular rate error */
+		float yaw_ff;						/**< yaw control feed-forward */
+
+		float rc_scale_yaw;
+	}		_params;
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int			parameters_update();
+
+	/**
+	 * Check for parameter update and handle it.
+	 */
+	void		parameter_update_poll();
+
+	/**
+	 * Check for changes in vehicle control mode.
+	 */
+	void		vehicle_control_mode_poll();
+
+	/**
+	 * Check for changes in manual inputs.
+	 */
+	void		vehicle_manual_poll();
+
+	/**
+	 * Check for attitude setpoint updates.
+	 */
+	void		vehicle_attitude_setpoint_poll();
+
+	/**
+	 * Check for rates setpoint updates.
+	 */
+	void		vehicle_rates_setpoint_poll();
+
+	/**
+	 * Check for arming status updates.
+	 */
+	void		arming_status_poll();
+
+	/**
+	 * Attitude controller.
+	 */
+	void		control_attitude(float dt);
+
+	/**
+	 * Attitude rates controller.
+	 */
+	void		control_attitude_rates(float dt);
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * Main sensor collection task.
+	 */
+	void		task_main() __attribute__((noreturn));
+};
+
+namespace mc_att_control
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+MulticopterAttitudeControl	*g_control;
+}
+
+MulticopterAttitudeControl::MulticopterAttitudeControl() :
+
+	_task_should_exit(false),
+	_control_task(-1),
+
+/* subscriptions */
+	_v_att_sub(-1),
+	_v_att_sp_sub(-1),
+	_v_control_mode_sub(-1),
+	_params_sub(-1),
+	_manual_control_sp_sub(-1),
+	_armed_sub(-1),
+
+/* publications */
+	_att_sp_pub(-1),
+	_v_rates_sp_pub(-1),
+	_actuators_0_pub(-1),
+
+/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control"))
+
+{
+	memset(&_v_att, 0, sizeof(_v_att));
+	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
+	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
+	memset(&_armed, 0, sizeof(_armed));
+
+	_params.att_p.zero();
+	_params.rate_p.zero();
+	_params.rate_i.zero();
+	_params.rate_d.zero();
+
+	_R_sp.identity();
+	_R.identity();
+	_rates_prev.zero();
+	_rates_sp.zero();
+	_rates_int.zero();
+	_thrust_sp = 0.0f;
+	_att_control.zero();
+
+	I.identity();
+
+	_params_handles.roll_p			= 	param_find("MC_ROLL_P");
+	_params_handles.roll_rate_p		= 	param_find("MC_ROLLRATE_P");
+	_params_handles.roll_rate_i		= 	param_find("MC_ROLLRATE_I");
+	_params_handles.roll_rate_d		= 	param_find("MC_ROLLRATE_D");
+	_params_handles.pitch_p			= 	param_find("MC_PITCH_P");
+	_params_handles.pitch_rate_p	= 	param_find("MC_PITCHRATE_P");
+	_params_handles.pitch_rate_i	= 	param_find("MC_PITCHRATE_I");
+	_params_handles.pitch_rate_d	= 	param_find("MC_PITCHRATE_D");
+	_params_handles.yaw_p			=	param_find("MC_YAW_P");
+	_params_handles.yaw_rate_p		= 	param_find("MC_YAWRATE_P");
+	_params_handles.yaw_rate_i		= 	param_find("MC_YAWRATE_I");
+	_params_handles.yaw_rate_d		= 	param_find("MC_YAWRATE_D");
+	_params_handles.yaw_ff			= 	param_find("MC_YAW_FF");
+
+	_params_handles.rc_scale_yaw	= 	param_find("RC_SCALE_YAW");
+
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+MulticopterAttitudeControl::~MulticopterAttitudeControl()
+{
+	if (_control_task != -1) {
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_control_task);
+				break;
+			}
+		} while (_control_task != -1);
+	}
+
+	mc_att_control::g_control = nullptr;
+}
+
+int
+MulticopterAttitudeControl::parameters_update()
+{
+	float v;
+
+	/* roll */
+	param_get(_params_handles.roll_p, &v);
+	_params.att_p(0) = v;
+	param_get(_params_handles.roll_rate_p, &v);
+	_params.rate_p(0) = v;
+	param_get(_params_handles.roll_rate_i, &v);
+	_params.rate_i(0) = v;
+	param_get(_params_handles.roll_rate_d, &v);
+	_params.rate_d(0) = v;
+
+	/* pitch */
+	param_get(_params_handles.pitch_p, &v);
+	_params.att_p(1) = v;
+	param_get(_params_handles.pitch_rate_p, &v);
+	_params.rate_p(1) = v;
+	param_get(_params_handles.pitch_rate_i, &v);
+	_params.rate_i(1) = v;
+	param_get(_params_handles.pitch_rate_d, &v);
+	_params.rate_d(1) = v;
+
+	/* yaw */
+	param_get(_params_handles.yaw_p, &v);
+	_params.att_p(2) = v;
+	param_get(_params_handles.yaw_rate_p, &v);
+	_params.rate_p(2) = v;
+	param_get(_params_handles.yaw_rate_i, &v);
+	_params.rate_i(2) = v;
+	param_get(_params_handles.yaw_rate_d, &v);
+	_params.rate_d(2) = v;
+
+	param_get(_params_handles.yaw_ff, &_params.yaw_ff);
+
+	param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw);
+
+	return OK;
+}
+
+void
+MulticopterAttitudeControl::parameter_update_poll()
+{
+	bool updated;
+
+	/* Check HIL state if vehicle status has changed */
+	orb_check(_params_sub, &updated);
+
+	if (updated) {
+		struct parameter_update_s param_update;
+		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
+		parameters_update();
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_control_mode_poll()
+{
+	bool updated;
+
+	/* Check HIL state if vehicle status has changed */
+	orb_check(_v_control_mode_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_manual_poll()
+{
+	bool updated;
+
+	/* get pilots inputs */
+	orb_check(_manual_control_sp_sub, &updated);
+
+	if (updated) {
+
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_v_att_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_rates_setpoint_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_v_rates_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
+	}
+}
+
+void
+MulticopterAttitudeControl::arming_status_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_armed_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+	}
+}
+
+/*
+ * Attitude controller.
+ * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
+ * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes)
+ */
+void
+MulticopterAttitudeControl::control_attitude(float dt)
+{
+	float yaw_sp_move_rate = 0.0f;
+	bool publish_att_sp = false;
+
+	if (_v_control_mode.flag_control_manual_enabled) {
+		/* manual input, set or modify attitude setpoint */
+
+		if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) {
+			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
+			vehicle_attitude_setpoint_poll();
+		}
+
+		if (!_v_control_mode.flag_control_climb_rate_enabled) {
+			/* pass throttle directly if not in altitude stabilized mode */
+			_v_att_sp.thrust = _manual_control_sp.throttle;
+			publish_att_sp = true;
+		}
+
+		if (!_armed.armed) {
+			/* reset yaw setpoint when disarmed */
+			_reset_yaw_sp = true;
+		}
+
+		/* move yaw setpoint in all modes */
+		if (_v_att_sp.thrust < 0.1f) {
+			// TODO
+			//if (_status.condition_landed) {
+			/* reset yaw setpoint if on ground */
+			//	reset_yaw_sp = true;
+			//}
+		} else {
+			float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw;
+
+			if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
+				/* move yaw setpoint */
+				yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw;
+
+				if (_manual_control_sp.yaw > 0.0f) {
+					yaw_sp_move_rate -= YAW_DEADZONE;
+
+				} else {
+					yaw_sp_move_rate += YAW_DEADZONE;
+				}
+
+				yaw_sp_move_rate *= _params.rc_scale_yaw;
+				_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
+				_v_att_sp.R_valid = false;
+				publish_att_sp = true;
+			}
+		}
+
+		/* reset yaw setpint to current position if needed */
+		if (_reset_yaw_sp) {
+			_reset_yaw_sp = false;
+			_v_att_sp.yaw_body = _v_att.yaw;
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+		if (!_v_control_mode.flag_control_velocity_enabled) {
+			/* update attitude setpoint if not in position control mode */
+			_v_att_sp.roll_body = _manual_control_sp.roll;
+			_v_att_sp.pitch_body = _manual_control_sp.pitch;
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+	} else {
+		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
+		vehicle_attitude_setpoint_poll();
+
+		/* reset yaw setpoint after non-manual control mode */
+		_reset_yaw_sp = true;
+	}
+
+	_thrust_sp = _v_att_sp.thrust;
+
+	/* construct attitude setpoint rotation matrix */
+	if (_v_att_sp.R_valid) {
+		/* rotation matrix in _att_sp is valid, use it */
+		_R_sp.set(&_v_att_sp.R_body[0][0]);
+
+	} else {
+		/* rotation matrix in _att_sp is not valid, use euler angles instead */
+		_R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
+
+		/* copy rotation matrix back to setpoint struct */
+		memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body));
+		_v_att_sp.R_valid = true;
+	}
+
+	/* publish the attitude setpoint if needed */
+	if (publish_att_sp) {
+		_v_att_sp.timestamp = hrt_absolute_time();
+
+		if (_att_sp_pub > 0) {
+			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp);
+
+		} else {
+			_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
+		}
+	}
+
+	/* rotation matrix for current state */
+	_R.set(_v_att.R);
+
+	/* all input data is ready, run controller itself */
+
+	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
+	math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2));
+	math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2));
+
+	/* axis and sin(angle) of desired rotation */
+	math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z);
+
+	/* calculate angle error */
+	float e_R_z_sin = e_R.length();
+	float e_R_z_cos = R_z * R_sp_z;
+
+	/* calculate weight for yaw control */
+	float yaw_w = _R_sp(2, 2) * _R_sp(2, 2);
+
+	/* calculate rotation matrix after roll/pitch only rotation */
+	math::Matrix<3, 3> R_rp;
+
+	if (e_R_z_sin > 0.0f) {
+		/* get axis-angle representation */
+		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
+		math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
+
+		e_R = e_R_z_axis * e_R_z_angle;
+
+		/* cross product matrix for e_R_axis */
+		math::Matrix<3, 3> e_R_cp;
+		e_R_cp.zero();
+		e_R_cp(0, 1) = -e_R_z_axis(2);
+		e_R_cp(0, 2) = e_R_z_axis(1);
+		e_R_cp(1, 0) = e_R_z_axis(2);
+		e_R_cp(1, 2) = -e_R_z_axis(0);
+		e_R_cp(2, 0) = -e_R_z_axis(1);
+		e_R_cp(2, 1) = e_R_z_axis(0);
+
+		/* rotation matrix for roll/pitch only rotation */
+		R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
+
+	} else {
+		/* zero roll/pitch rotation */
+		R_rp = _R;
+	}
+
+	/* R_rp and _R_sp has the same Z axis, calculate yaw error */
+	math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0));
+	math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
+	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
+
+	if (e_R_z_cos < 0.0f) {
+		/* for large thrust vector rotations use another rotation method:
+		 * calculate angle and axis for R -> R_sp rotation directly */
+		math::Quaternion q;
+		q.from_dcm(_R.transposed() * _R_sp);
+		math::Vector<3> e_R_d = q.imag();
+		e_R_d.normalize();
+		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
+
+		/* use fusion of Z axis based rotation and direct rotation */
+		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
+		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
+	}
+
+	/* calculate angular rates setpoint */
+	_rates_sp = _params.att_p.emult(e_R);
+
+	/* feed forward yaw setpoint rate */
+	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
+}
+
+/*
+ * Attitude rates controller.
+ * Input: '_rates_sp' vector, '_thrust_sp'
+ * Output: '_att_control' vector
+ */
+void
+MulticopterAttitudeControl::control_attitude_rates(float dt)
+{
+	/* reset integral if disarmed */
+	if (!_armed.armed) {
+		_rates_int.zero();
+	}
+
+	/* current body angular rates */
+	math::Vector<3> rates;
+	rates(0) = _v_att.rollspeed;
+	rates(1) = _v_att.pitchspeed;
+	rates(2) = _v_att.yawspeed;
+
+	/* angular rates error */
+	math::Vector<3> rates_err = _rates_sp - rates;
+	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
+	_rates_prev = rates;
+
+	/* update integral only if not saturated on low limit */
+	if (_thrust_sp > 0.1f) {
+		for (int i = 0; i < 3; i++) {
+			if (fabsf(_att_control(i)) < _thrust_sp) {
+				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
+
+				if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
+				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
+					_rates_int(i) = rate_i;
+				}
+			}
+		}
+	}
+}
+
+void
+MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
+{
+	mc_att_control::g_control->task_main();
+}
+
+void
+MulticopterAttitudeControl::task_main()
+{
+	warnx("started");
+	fflush(stdout);
+
+	/*
+	 * do subscriptions
+	 */
+	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+	_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+	/* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */
+	orb_set_interval(_v_att_sub, 5);
+
+	/* initialize parameters cache */
+	parameters_update();
+
+	/* wakeup source: vehicle attitude */
+	struct pollfd fds[1];
+
+	fds[0].fd = _v_att_sub;
+	fds[0].events = POLLIN;
+
+	while (!_task_should_exit) {
+
+		/* wait for up to 100ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+		/* timed out - periodic check for _task_should_exit */
+		if (pret == 0)
+			continue;
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			/* sleep a bit before next try */
+			usleep(100000);
+			continue;
+		}
+
+		perf_begin(_loop_perf);
+
+		/* run controller on attitude changes */
+		if (fds[0].revents & POLLIN) {
+			static uint64_t last_run = 0;
+			float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
+			last_run = hrt_absolute_time();
+
+			/* guard against too small (< 2ms) and too large (> 20ms) dt's */
+			if (dt < 0.002f) {
+				dt = 0.002f;
+
+			} else if (dt > 0.02f) {
+				dt = 0.02f;
+			}
+
+			/* copy attitude topic */
+			orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
+
+			/* check for updates in other topics */
+			parameter_update_poll();
+			vehicle_control_mode_poll();
+			arming_status_poll();
+			vehicle_manual_poll();
+
+			if (_v_control_mode.flag_control_attitude_enabled) {
+				control_attitude(dt);
+
+				/* publish attitude rates setpoint */
+				_v_rates_sp.roll = _rates_sp(0);
+				_v_rates_sp.pitch = _rates_sp(1);
+				_v_rates_sp.yaw = _rates_sp(2);
+				_v_rates_sp.thrust = _thrust_sp;
+				_v_rates_sp.timestamp = hrt_absolute_time();
+
+				if (_v_rates_sp_pub > 0) {
+					orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+
+				} else {
+					_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+				}
+
+			} else {
+				/* attitude controller disabled */
+				// TODO poll 'attitude_rates_setpoint' topic
+				_rates_sp.zero();
+				_thrust_sp = 0.0f;
+			}
+
+			if (_v_control_mode.flag_control_rates_enabled) {
+				control_attitude_rates(dt);
+
+				/* publish actuator controls */
+				_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+				_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+				_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+				_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+				_actuators.timestamp = hrt_absolute_time();
+
+				if (_actuators_0_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
+
+				} else {
+					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+				}
+			}
+		}
+
+		perf_end(_loop_perf);
+	}
+
+	warnx("exit");
+
+	_control_task = -1;
+	_exit(0);
+}
+
+int
+MulticopterAttitudeControl::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("mc_att_control",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2048,
+				       (main_t)&MulticopterAttitudeControl::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+int mc_att_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		errx(1, "usage: mc_att_control {start|stop|status}");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (mc_att_control::g_control != nullptr)
+			errx(1, "already running");
+
+		mc_att_control::g_control = new MulticopterAttitudeControl;
+
+		if (mc_att_control::g_control == nullptr)
+			errx(1, "alloc failed");
+
+		if (OK != mc_att_control::g_control->start()) {
+			delete mc_att_control::g_control;
+			mc_att_control::g_control = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (mc_att_control::g_control == nullptr)
+			errx(1, "not running");
+
+		delete mc_att_control::g_control;
+		mc_att_control::g_control = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (mc_att_control::g_control) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/mc_att_control/mc_att_control_params.c
index ca7794c59..27a45b6bb 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.h
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -1,12 +1,9 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Author: @author Tobias Naegeli <naegelit@student.ethz.ch>
+ *   	     @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @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
@@ -37,28 +34,23 @@
  *
  ****************************************************************************/
 
-/*
- * @file multirotor_attitude_control.h
- *
- * Definition of rate controller for multirotors.
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
+/**
+ * @file mc_att_control_params.c
+ * Parameters for multicopter attitude controller.
  */
 
-#ifndef MULTIROTOR_RATE_CONTROL_H_
-#define MULTIROTOR_RATE_CONTROL_H_
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
-
-void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators, bool reset_integral);
+#include <systemlib/param/param.h>
 
-#endif /* MULTIROTOR_RATE_CONTROL_H_ */
+PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f);
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f);
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f);
+PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f);
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f);
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f);
+PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
+PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/mc_att_control/module.mk
index bc4b48fb4..64b876f69 100644
--- a/src/modules/multirotor_pos_control/module.mk
+++ b/src/modules/mc_att_control/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -32,11 +32,10 @@
 ############################################################################
 
 #
-# Build multirotor position control
+# Multirotor attitude controller (vector based, no Euler singularities)
 #
 
-MODULE_COMMAND	= multirotor_pos_control
+MODULE_COMMAND	= mc_att_control
 
-SRCS		= multirotor_pos_control.c \
-		  multirotor_pos_control_params.c \
-		  thrust_pid.c
+SRCS		= mc_att_control_main.cpp \
+			  mc_att_control_params.c
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
new file mode 100644
index 000000000..25d34c872
--- /dev/null
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -0,0 +1,1110 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_pos_control_main.cpp
+ * Multicopter position controller.
+ *
+ * The controller has two loops: P loop for position error and PID loop for velocity error.
+ * Output of velocity controller is thrust vector that splitted to thrust direction
+ * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself).
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/pid/pid.h>
+#include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+#include <mavlink/mavlink_log.h>
+
+#define TILT_COS_MAX	0.7f
+#define SIGMA			0.000001f
+
+/**
+ * Multicopter position control app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]);
+
+class MulticopterPositionControl
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterPositionControl();
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~MulticopterPositionControl();
+
+	/**
+	 * Start task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+private:
+
+	bool	_task_should_exit;		/**< if true, task should exit */
+	int		_control_task;			/**< task handle for task */
+	int		_mavlink_fd;			/**< mavlink fd */
+
+	int		_att_sub;				/**< vehicle attitude subscription */
+	int		_att_sp_sub;			/**< vehicle attitude setpoint */
+	int		_control_mode_sub;		/**< vehicle control mode subscription */
+	int 	_params_sub;			/**< notification of parameter updates */
+	int 	_manual_sub;			/**< notification of manual control updates */
+	int		_arming_sub;			/**< arming status of outputs */
+	int		_global_pos_sub;			/**< vehicle local position */
+	int		_pos_sp_triplet_sub;		/**< position setpoint triplet */
+
+	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
+	orb_advert_t	_pos_sp_triplet_pub;	/**< position setpoint triplet publication */
+	orb_advert_t	_global_vel_sp_pub;		/**< vehicle global velocity setpoint */
+
+	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s	_att_sp;		/**< vehicle attitude setpoint */
+	struct manual_control_setpoint_s	_manual;		/**< r/c channel data */
+	struct vehicle_control_mode_s		_control_mode;	/**< vehicle control mode */
+	struct actuator_armed_s				_arming;		/**< actuator arming status */
+	struct vehicle_global_position_s		_global_pos;	/**< vehicle global position */
+	struct position_setpoint_triplet_s		_pos_sp_triplet;	/**< vehicle global position setpoint triplet */
+	struct vehicle_global_velocity_setpoint_s	_global_vel_sp;	/**< vehicle global velocity setpoint */
+
+	struct {
+		param_t thr_min;
+		param_t thr_max;
+		param_t z_p;
+		param_t z_vel_p;
+		param_t z_vel_i;
+		param_t z_vel_d;
+		param_t z_vel_max;
+		param_t z_ff;
+		param_t xy_p;
+		param_t xy_vel_p;
+		param_t xy_vel_i;
+		param_t xy_vel_d;
+		param_t xy_vel_max;
+		param_t xy_ff;
+		param_t tilt_max;
+		param_t land_speed;
+		param_t land_tilt_max;
+
+		param_t rc_scale_pitch;
+		param_t rc_scale_roll;
+	}		_params_handles;		/**< handles for interesting parameters */
+
+	struct {
+		float thr_min;
+		float thr_max;
+		float tilt_max;
+		float land_speed;
+		float land_tilt_max;
+
+		float rc_scale_pitch;
+		float rc_scale_roll;
+
+		math::Vector<3> pos_p;
+		math::Vector<3> vel_p;
+		math::Vector<3> vel_i;
+		math::Vector<3> vel_d;
+		math::Vector<3> vel_ff;
+		math::Vector<3> vel_max;
+		math::Vector<3> sp_offs_max;
+	}		_params;
+
+	double _lat_sp;
+	double _lon_sp;
+	float _alt_sp;
+
+	bool _reset_lat_lon_sp;
+	bool _reset_alt_sp;
+	bool _use_global_alt;			/**< switch between global (AMSL) and barometric altitudes */
+
+	math::Vector<3> _vel;
+	math::Vector<3> _vel_sp;
+	math::Vector<3> _vel_prev;			/**< velocity on previous step */
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int			parameters_update(bool force);
+
+	/**
+	 * Update control outputs
+	 */
+	void		control_update();
+
+	/**
+	 * Check for changes in subscribed topics.
+	 */
+	void		poll_subscriptions();
+
+	static float	scale_control(float ctl, float end, float dz);
+
+	/**
+	 * Reset lat/lon to current position
+	 */
+	void		reset_lat_lon_sp();
+
+	/**
+	 * Reset altitude setpoint to current altitude
+	 */
+	void		reset_alt_sp();
+
+	/**
+	 * Select between barometric and global (AMSL) altitudes
+	 */
+	void		select_alt(bool global);
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * Main sensor collection task.
+	 */
+	void		task_main() __attribute__((noreturn));
+};
+
+namespace pos_control
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+MulticopterPositionControl	*g_control;
+}
+
+MulticopterPositionControl::MulticopterPositionControl() :
+
+	_task_should_exit(false),
+	_control_task(-1),
+	_mavlink_fd(-1),
+
+/* subscriptions */
+	_att_sub(-1),
+	_att_sp_sub(-1),
+	_control_mode_sub(-1),
+	_params_sub(-1),
+	_manual_sub(-1),
+	_arming_sub(-1),
+	_global_pos_sub(-1),
+	_pos_sp_triplet_sub(-1),
+
+/* publications */
+	_att_sp_pub(-1),
+	_pos_sp_triplet_pub(-1),
+	_global_vel_sp_pub(-1),
+
+	_lat_sp(0.0),
+	_lon_sp(0.0),
+	_alt_sp(0.0f),
+
+	_reset_lat_lon_sp(true),
+	_reset_alt_sp(true),
+	_use_global_alt(false)
+{
+	memset(&_att, 0, sizeof(_att));
+	memset(&_att_sp, 0, sizeof(_att_sp));
+	memset(&_manual, 0, sizeof(_manual));
+	memset(&_control_mode, 0, sizeof(_control_mode));
+	memset(&_arming, 0, sizeof(_arming));
+	memset(&_global_pos, 0, sizeof(_global_pos));
+	memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet));
+	memset(&_global_vel_sp, 0, sizeof(_global_vel_sp));
+
+	_params.pos_p.zero();
+	_params.vel_p.zero();
+	_params.vel_i.zero();
+	_params.vel_d.zero();
+	_params.vel_max.zero();
+	_params.vel_ff.zero();
+	_params.sp_offs_max.zero();
+
+	_vel.zero();
+	_vel_sp.zero();
+	_vel_prev.zero();
+
+	_params_handles.thr_min		= param_find("MPC_THR_MIN");
+	_params_handles.thr_max		= param_find("MPC_THR_MAX");
+	_params_handles.z_p			= param_find("MPC_Z_P");
+	_params_handles.z_vel_p		= param_find("MPC_Z_VEL_P");
+	_params_handles.z_vel_i		= param_find("MPC_Z_VEL_I");
+	_params_handles.z_vel_d		= param_find("MPC_Z_VEL_D");
+	_params_handles.z_vel_max	= param_find("MPC_Z_VEL_MAX");
+	_params_handles.z_ff		= param_find("MPC_Z_FF");
+	_params_handles.xy_p		= param_find("MPC_XY_P");
+	_params_handles.xy_vel_p	= param_find("MPC_XY_VEL_P");
+	_params_handles.xy_vel_i	= param_find("MPC_XY_VEL_I");
+	_params_handles.xy_vel_d	= param_find("MPC_XY_VEL_D");
+	_params_handles.xy_vel_max	= param_find("MPC_XY_VEL_MAX");
+	_params_handles.xy_ff		= param_find("MPC_XY_FF");
+	_params_handles.tilt_max	= param_find("MPC_TILT_MAX");
+	_params_handles.land_speed	= param_find("MPC_LAND_SPEED");
+	_params_handles.land_tilt_max	= param_find("MPC_LAND_TILT");
+	_params_handles.rc_scale_pitch	= param_find("RC_SCALE_PITCH");
+	_params_handles.rc_scale_roll	= param_find("RC_SCALE_ROLL");
+
+	/* fetch initial parameter values */
+	parameters_update(true);
+}
+
+MulticopterPositionControl::~MulticopterPositionControl()
+{
+	if (_control_task != -1) {
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_control_task);
+				break;
+			}
+		} while (_control_task != -1);
+	}
+
+	pos_control::g_control = nullptr;
+}
+
+int
+MulticopterPositionControl::parameters_update(bool force)
+{
+	bool updated;
+	struct parameter_update_s param_upd;
+
+	orb_check(_params_sub, &updated);
+
+	if (updated)
+		orb_copy(ORB_ID(parameter_update), _params_sub, &param_upd);
+
+	if (updated || force) {
+		param_get(_params_handles.thr_min, &_params.thr_min);
+		param_get(_params_handles.thr_max, &_params.thr_max);
+		param_get(_params_handles.tilt_max, &_params.tilt_max);
+		param_get(_params_handles.land_speed, &_params.land_speed);
+		param_get(_params_handles.land_tilt_max, &_params.land_tilt_max);
+		param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch);
+		param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll);
+
+		float v;
+		param_get(_params_handles.xy_p, &v);
+		_params.pos_p(0) = v;
+		_params.pos_p(1) = v;
+		param_get(_params_handles.z_p, &v);
+		_params.pos_p(2) = v;
+		param_get(_params_handles.xy_vel_p, &v);
+		_params.vel_p(0) = v;
+		_params.vel_p(1) = v;
+		param_get(_params_handles.z_vel_p, &v);
+		_params.vel_p(2) = v;
+		param_get(_params_handles.xy_vel_i, &v);
+		_params.vel_i(0) = v;
+		_params.vel_i(1) = v;
+		param_get(_params_handles.z_vel_i, &v);
+		_params.vel_i(2) = v;
+		param_get(_params_handles.xy_vel_d, &v);
+		_params.vel_d(0) = v;
+		_params.vel_d(1) = v;
+		param_get(_params_handles.z_vel_d, &v);
+		_params.vel_d(2) = v;
+		param_get(_params_handles.xy_vel_max, &v);
+		_params.vel_max(0) = v;
+		_params.vel_max(1) = v;
+		param_get(_params_handles.z_vel_max, &v);
+		_params.vel_max(2) = v;
+		param_get(_params_handles.xy_ff, &v);
+		_params.vel_ff(0) = v;
+		_params.vel_ff(1) = v;
+		param_get(_params_handles.z_ff, &v);
+		_params.vel_ff(2) = v;
+
+		_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
+	}
+
+	return OK;
+}
+
+void
+MulticopterPositionControl::poll_subscriptions()
+{
+	bool updated;
+
+	orb_check(_att_sub, &updated);
+
+	if (updated)
+		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+
+	orb_check(_att_sp_sub, &updated);
+
+	if (updated)
+		orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
+
+	orb_check(_control_mode_sub, &updated);
+
+	if (updated)
+		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
+
+	orb_check(_manual_sub, &updated);
+
+	if (updated)
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
+
+	orb_check(_arming_sub, &updated);
+
+	if (updated)
+		orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
+
+	orb_check(_global_pos_sub, &updated);
+
+	if (updated)
+		orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
+}
+
+float
+MulticopterPositionControl::scale_control(float ctl, float end, float dz)
+{
+	if (ctl > dz) {
+		return (ctl - dz) / (end - dz);
+
+	} else if (ctl < -dz) {
+		return (ctl + dz) / (end - dz);
+
+	} else {
+		return 0.0f;
+	}
+}
+
+void
+MulticopterPositionControl::task_main_trampoline(int argc, char *argv[])
+{
+	pos_control::g_control->task_main();
+}
+
+void
+MulticopterPositionControl::reset_lat_lon_sp()
+{
+	if (_reset_lat_lon_sp) {
+		_reset_lat_lon_sp = false;
+		_lat_sp = _global_pos.lat;
+		_lon_sp = _global_pos.lon;
+		mavlink_log_info(_mavlink_fd, "[mpc] reset lat/lon sp: %.7f, %.7f", _lat_sp, _lon_sp);
+	}
+}
+
+void
+MulticopterPositionControl::reset_alt_sp()
+{
+	if (_reset_alt_sp) {
+		_reset_alt_sp = false;
+		_alt_sp = _use_global_alt ? _global_pos.alt : _global_pos.baro_alt;
+		mavlink_log_info(_mavlink_fd, "[mpc] reset alt (%s) sp: %.2f", _use_global_alt ? "AMSL" : "baro", (double)_alt_sp);
+	}
+}
+
+void
+MulticopterPositionControl::select_alt(bool global)
+{
+	if (global != _use_global_alt) {
+		_use_global_alt = global;
+
+		if (global) {
+			/* switch from barometric to global altitude */
+			_alt_sp += _global_pos.alt - _global_pos.baro_alt;
+
+		} else {
+			/* switch from global to barometric altitude */
+			_alt_sp += _global_pos.baro_alt - _global_pos.alt;
+		}
+	}
+}
+
+void
+MulticopterPositionControl::task_main()
+{
+	warnx("started");
+
+	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(_mavlink_fd, "[mpc] started");
+
+	/*
+	 * do subscriptions
+	 */
+	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
+	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+
+	parameters_update(true);
+
+	/* initialize values of critical structs until first regular update */
+	_arming.armed = false;
+
+	/* get an initial update for all sensor and status data */
+	poll_subscriptions();
+
+	bool reset_int_z = true;
+	bool reset_int_z_manual = false;
+	bool reset_int_xy = true;
+	bool was_armed = false;
+
+	hrt_abstime t_prev = 0;
+
+	const float alt_ctl_dz = 0.2f;
+	const float pos_ctl_dz = 0.05f;
+
+	math::Vector<3> sp_move_rate;
+	sp_move_rate.zero();
+	math::Vector<3> thrust_int;
+	thrust_int.zero();
+	math::Matrix<3, 3> R;
+	R.identity();
+
+	/* wakeup source */
+	struct pollfd fds[1];
+
+	/* Setup of loop */
+	fds[0].fd = _global_pos_sub;
+	fds[0].events = POLLIN;
+
+	while (!_task_should_exit) {
+		/* wait for up to 500ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
+
+		/* timed out - periodic check for _task_should_exit */
+		if (pret == 0)
+			continue;
+
+		/* this is undesirable but not much we can do */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			continue;
+		}
+
+		poll_subscriptions();
+		parameters_update(false);
+
+		hrt_abstime t = hrt_absolute_time();
+		float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
+		t_prev = t;
+
+		if (_control_mode.flag_armed && !was_armed) {
+			/* reset setpoints and integrals on arming */
+			_reset_lat_lon_sp = true;
+			_reset_alt_sp = true;
+			reset_int_z = true;
+			reset_int_xy = true;
+		}
+
+		was_armed = _control_mode.flag_armed;
+
+		if (_control_mode.flag_control_altitude_enabled ||
+		    _control_mode.flag_control_position_enabled ||
+		    _control_mode.flag_control_climb_rate_enabled ||
+		    _control_mode.flag_control_velocity_enabled) {
+
+			_vel(0) = _global_pos.vel_n;
+			_vel(1) = _global_pos.vel_e;
+			_vel(2) = _global_pos.vel_d;
+
+			sp_move_rate.zero();
+
+			float alt = _global_pos.alt;
+
+			/* select control source */
+			if (_control_mode.flag_control_manual_enabled) {
+				/* select altitude source and update setpoint */
+				select_alt(_global_pos.global_valid);
+
+				if (!_use_global_alt) {
+					alt = _global_pos.baro_alt;
+				}
+
+				/* manual control */
+				if (_control_mode.flag_control_altitude_enabled) {
+					/* reset alt setpoint to current altitude if needed */
+					reset_alt_sp();
+
+					/* move altitude setpoint with throttle stick */
+					sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
+				}
+
+				if (_control_mode.flag_control_position_enabled) {
+					/* reset lat/lon setpoint to current position if needed */
+					reset_lat_lon_sp();
+
+					/* move position setpoint with roll/pitch stick */
+					sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz);
+					sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz);
+				}
+
+				/* limit setpoint move rate */
+				float sp_move_norm = sp_move_rate.length();
+
+				if (sp_move_norm > 1.0f) {
+					sp_move_rate /= sp_move_norm;
+				}
+
+				/* scale to max speed and rotate around yaw */
+				math::Matrix<3, 3> R_yaw_sp;
+				R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
+				sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
+
+				/* move position setpoint */
+				add_vector_to_global_position(_lat_sp, _lon_sp, sp_move_rate(0) * dt, sp_move_rate(1) * dt, &_lat_sp, &_lon_sp);
+				_alt_sp -= sp_move_rate(2) * dt;
+
+				/* check if position setpoint is too far from actual position */
+				math::Vector<3> pos_sp_offs;
+				pos_sp_offs.zero();
+
+				if (_control_mode.flag_control_position_enabled) {
+					get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_sp_offs.data[0], &pos_sp_offs.data[1]);
+					pos_sp_offs(0) /= _params.sp_offs_max(0);
+					pos_sp_offs(1) /= _params.sp_offs_max(1);
+				}
+
+				if (_control_mode.flag_control_altitude_enabled) {
+					pos_sp_offs(2) = -(_alt_sp - alt) / _params.sp_offs_max(2);
+				}
+
+				float pos_sp_offs_norm = pos_sp_offs.length();
+
+				if (pos_sp_offs_norm > 1.0f) {
+					pos_sp_offs /= pos_sp_offs_norm;
+					add_vector_to_global_position(_global_pos.lat, _global_pos.lon, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp);
+					_alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2);
+				}
+
+				/* fill position setpoint triplet */
+				_pos_sp_triplet.previous.valid = true;
+				_pos_sp_triplet.current.valid = true;
+				_pos_sp_triplet.next.valid = true;
+
+				_pos_sp_triplet.nav_state = NAV_STATE_NONE;
+				_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
+				_pos_sp_triplet.current.lat = _lat_sp;
+				_pos_sp_triplet.current.lon = _lon_sp;
+				_pos_sp_triplet.current.alt = _alt_sp;
+				_pos_sp_triplet.current.yaw = _att_sp.yaw_body;
+				_pos_sp_triplet.current.loiter_radius = 0.0f;
+				_pos_sp_triplet.current.loiter_direction = 1.0f;
+				_pos_sp_triplet.current.pitch_min = 0.0f;
+
+				/* publish position setpoint triplet */
+				if (_pos_sp_triplet_pub > 0) {
+					orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
+
+				} else {
+					_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
+				}
+
+			} else {
+				/* always use AMSL altitude for AUTO */
+				select_alt(true);
+
+				/* AUTO */
+				bool updated;
+				orb_check(_pos_sp_triplet_sub, &updated);
+
+				if (updated)
+					orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+
+				if (_pos_sp_triplet.current.valid) {
+					/* in case of interrupted mission don't go to waypoint but stay at current position */
+					_reset_lat_lon_sp = true;
+					_reset_alt_sp = true;
+
+					/* update position setpoint */
+					_lat_sp = _pos_sp_triplet.current.lat;
+					_lon_sp = _pos_sp_triplet.current.lon;
+					_alt_sp = _pos_sp_triplet.current.alt;
+
+					/* update yaw setpoint if needed */
+					if (isfinite(_pos_sp_triplet.current.yaw)) {
+						_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
+					}
+
+				} else {
+					/* no waypoint, loiter, reset position setpoint if needed */
+					reset_lat_lon_sp();
+					reset_alt_sp();
+				}
+			}
+
+			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
+				/* idle state, don't run controller and set zero thrust */
+				R.identity();
+				memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+				_att_sp.R_valid = true;
+
+				_att_sp.roll_body = 0.0f;
+				_att_sp.pitch_body = 0.0f;
+				_att_sp.yaw_body = _att.yaw;
+				_att_sp.thrust = 0.0f;
+
+				_att_sp.timestamp = hrt_absolute_time();
+
+				/* publish attitude setpoint */
+				if (_att_sp_pub > 0) {
+					orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
+
+				} else {
+					_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+				}
+
+			} else {
+				/* run position & altitude controllers, calculate velocity setpoint */
+				math::Vector<3> pos_err;
+				float err_x, err_y;
+				get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]);
+				pos_err(2) = -(_alt_sp - alt);
+
+				_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
+
+				if (!_control_mode.flag_control_altitude_enabled) {
+					_reset_alt_sp = true;
+					_vel_sp(2) = 0.0f;
+				}
+
+				if (!_control_mode.flag_control_position_enabled) {
+					_reset_lat_lon_sp = true;
+					_vel_sp(0) = 0.0f;
+					_vel_sp(1) = 0.0f;
+				}
+
+				/* use constant descend rate when landing, ignore altitude setpoint */
+				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+					_vel_sp(2) = _params.land_speed;
+				}
+
+				if (!_control_mode.flag_control_manual_enabled) {
+					/* limit 3D speed only in non-manual modes */
+					float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length();
+
+					if (vel_sp_norm > 1.0f) {
+						_vel_sp /= vel_sp_norm;
+					}
+				}
+
+				_global_vel_sp.vx = _vel_sp(0);
+				_global_vel_sp.vy = _vel_sp(1);
+				_global_vel_sp.vz = _vel_sp(2);
+
+				/* publish velocity setpoint */
+				if (_global_vel_sp_pub > 0) {
+					orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
+
+				} else {
+					_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
+				}
+
+				if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
+					/* reset integrals if needed */
+					if (_control_mode.flag_control_climb_rate_enabled) {
+						if (reset_int_z) {
+							reset_int_z = false;
+							float i = _params.thr_min;
+
+							if (reset_int_z_manual) {
+								i = _manual.throttle;
+
+								if (i < _params.thr_min) {
+									i = _params.thr_min;
+
+								} else if (i > _params.thr_max) {
+									i = _params.thr_max;
+								}
+							}
+
+							thrust_int(2) = -i;
+							mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
+						}
+
+					} else {
+						reset_int_z = true;
+					}
+
+					if (_control_mode.flag_control_velocity_enabled) {
+						if (reset_int_xy) {
+							reset_int_xy = false;
+							thrust_int(0) = 0.0f;
+							thrust_int(1) = 0.0f;
+							mavlink_log_info(_mavlink_fd, "[mpc] reset xy vel integral");
+						}
+
+					} else {
+						reset_int_xy = true;
+					}
+
+					/* velocity error */
+					math::Vector<3> vel_err = _vel_sp - _vel;
+
+					/* derivative of velocity error, not includes setpoint acceleration */
+					math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
+					_vel_prev = _vel;
+
+					/* thrust vector in NED frame */
+					math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;
+
+					if (!_control_mode.flag_control_velocity_enabled) {
+						thrust_sp(0) = 0.0f;
+						thrust_sp(1) = 0.0f;
+					}
+
+					if (!_control_mode.flag_control_climb_rate_enabled) {
+						thrust_sp(2) = 0.0f;
+					}
+
+					/* limit thrust vector and check for saturation */
+					bool saturation_xy = false;
+					bool saturation_z = false;
+
+					/* limit min lift */
+					float thr_min = _params.thr_min;
+
+					if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
+						/* don't allow downside thrust direction in manual attitude mode */
+						thr_min = 0.0f;
+					}
+
+					float tilt_max = _params.tilt_max;
+
+					/* adjust limits for landing mode */
+					if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
+					    _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+						/* limit max tilt and min lift when landing */
+						tilt_max = _params.land_tilt_max;
+
+						if (thr_min < 0.0f)
+							thr_min = 0.0f;
+					}
+
+					/* limit min lift */
+					if (-thrust_sp(2) < thr_min) {
+						thrust_sp(2) = -thr_min;
+						saturation_z = true;
+					}
+
+					if (_control_mode.flag_control_velocity_enabled) {
+						/* limit max tilt */
+						if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) {
+							/* absolute horizontal thrust */
+							float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+
+							if (thrust_sp_xy_len > 0.01f) {
+								/* max horizontal thrust for given vertical thrust*/
+								float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
+
+								if (thrust_sp_xy_len > thrust_xy_max) {
+									float k = thrust_xy_max / thrust_sp_xy_len;
+									thrust_sp(0) *= k;
+									thrust_sp(1) *= k;
+									saturation_xy = true;
+								}
+							}
+						}
+
+					} else {
+						/* thrust compensation for altitude only control mode */
+						float att_comp;
+
+						if (_att.R[2][2] > TILT_COS_MAX) {
+							att_comp = 1.0f / _att.R[2][2];
+
+						} else if (_att.R[2][2] > 0.0f) {
+							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
+							saturation_z = true;
+
+						} else {
+							att_comp = 1.0f;
+							saturation_z = true;
+						}
+
+						thrust_sp(2) *= att_comp;
+					}
+
+					/* limit max thrust */
+					float thrust_abs = thrust_sp.length();
+
+					if (thrust_abs > _params.thr_max) {
+						if (thrust_sp(2) < 0.0f) {
+							if (-thrust_sp(2) > _params.thr_max) {
+								/* thrust Z component is too large, limit it */
+								thrust_sp(0) = 0.0f;
+								thrust_sp(1) = 0.0f;
+								thrust_sp(2) = -_params.thr_max;
+								saturation_xy = true;
+								saturation_z = true;
+
+							} else {
+								/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
+								float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2));
+								float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+								float k = thrust_xy_max / thrust_xy_abs;
+								thrust_sp(0) *= k;
+								thrust_sp(1) *= k;
+								saturation_xy = true;
+							}
+
+						} else {
+							/* Z component is negative, going down, simply limit thrust vector */
+							float k = _params.thr_max / thrust_abs;
+							thrust_sp *= k;
+							saturation_xy = true;
+							saturation_z = true;
+						}
+
+						thrust_abs = _params.thr_max;
+					}
+
+					/* update integrals */
+					if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
+						thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
+						thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
+					}
+
+					if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
+						thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
+
+						/* protection against flipping on ground when landing */
+						if (thrust_int(2) > 0.0f)
+							thrust_int(2) = 0.0f;
+					}
+
+					/* calculate attitude setpoint from thrust vector */
+					if (_control_mode.flag_control_velocity_enabled) {
+						/* desired body_z axis = -normalize(thrust_vector) */
+						math::Vector<3> body_x;
+						math::Vector<3> body_y;
+						math::Vector<3> body_z;
+
+						if (thrust_abs > SIGMA) {
+							body_z = -thrust_sp / thrust_abs;
+
+						} else {
+							/* no thrust, set Z axis to safe value */
+							body_z.zero();
+							body_z(2) = 1.0f;
+						}
+
+						/* vector of desired yaw direction in XY plane, rotated by PI/2 */
+						math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
+
+						if (fabsf(body_z(2)) > SIGMA) {
+							/* desired body_x axis, orthogonal to body_z */
+							body_x = y_C % body_z;
+
+							/* keep nose to front while inverted upside down */
+							if (body_z(2) < 0.0f) {
+								body_x = -body_x;
+							}
+
+							body_x.normalize();
+
+						} else {
+							/* desired thrust is in XY plane, set X downside to construct correct matrix,
+							 * but yaw component will not be used actually */
+							body_x.zero();
+							body_x(2) = 1.0f;
+						}
+
+						/* desired body_y axis */
+						body_y = body_z % body_x;
+
+						/* fill rotation matrix */
+						for (int i = 0; i < 3; i++) {
+							R(i, 0) = body_x(i);
+							R(i, 1) = body_y(i);
+							R(i, 2) = body_z(i);
+						}
+
+						/* copy rotation matrix to attitude setpoint topic */
+						memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+						_att_sp.R_valid = true;
+
+						/* calculate euler angles, for logging only, must not be used for control */
+						math::Vector<3> euler = R.to_euler();
+						_att_sp.roll_body = euler(0);
+						_att_sp.pitch_body = euler(1);
+						/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
+					}
+
+					_att_sp.thrust = thrust_abs;
+
+					_att_sp.timestamp = hrt_absolute_time();
+
+					/* publish attitude setpoint */
+					if (_att_sp_pub > 0) {
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
+
+					} else {
+						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+					}
+
+				} else {
+					reset_int_z = true;
+				}
+			}
+
+		} else {
+			/* position controller disabled, reset setpoints */
+			_reset_alt_sp = true;
+			_reset_lat_lon_sp = true;
+			reset_int_z = true;
+			reset_int_xy = true;
+		}
+
+		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
+		reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled;
+	}
+
+	warnx("stopped");
+	mavlink_log_info(_mavlink_fd, "[mpc] stopped");
+
+	_control_task = -1;
+	_exit(0);
+}
+
+int
+MulticopterPositionControl::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("mc_pos_control",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2048,
+				       (main_t)&MulticopterPositionControl::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+int mc_pos_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		errx(1, "usage: mc_pos_control {start|stop|status}");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (pos_control::g_control != nullptr)
+			errx(1, "already running");
+
+		pos_control::g_control = new MulticopterPositionControl;
+
+		if (pos_control::g_control == nullptr)
+			errx(1, "alloc failed");
+
+		if (OK != pos_control::g_control->start()) {
+			delete pos_control::g_control;
+			pos_control::g_control = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (pos_control::g_control == nullptr)
+			errx(1, "not running");
+
+		delete pos_control::g_control;
+		pos_control::g_control = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (pos_control::g_control) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/mc_pos_control/mc_pos_control_params.c
index 5e169c1ba..9eb56545d 100644
--- a/src/modules/multirotor_pos_control/thrust_pid.h
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *   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
@@ -33,44 +33,26 @@
  ****************************************************************************/
 
 /**
- * @file thrust_pid.h
- *
- * Definition of thrust control PID interface.
- *
- * @author Anton Babushkin <anton.babushkin@me.com>
+ * @file mc_pos_control_params.c
+ * Multicopter position controller parameters.
  */
 
-#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);
-__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i);
-
-__END_DECLS
-
-#endif /* THRUST_PID_H_ */
+#include <systemlib/param/param.h>
+
+PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.02f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
+PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
+PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f);
diff --git a/src/modules/mc_pos_control/module.mk b/src/modules/mc_pos_control/module.mk
new file mode 100644
index 000000000..0b566d7bd
--- /dev/null
+++ b/src/modules/mc_pos_control/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.
+#
+############################################################################
+
+#
+# Build multicopter position controller
+#
+
+MODULE_COMMAND	= mc_pos_control
+
+SRCS		= mc_pos_control_main.cpp \
+			  mc_pos_control_params.c
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
deleted file mode 100644
index 60a211817..000000000
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ /dev/null
@@ -1,465 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file multirotor_att_control_main.c
- *
- * Implementation of multirotor attitude control main loop.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-#include <getopt.h>
-#include <time.h>
-#include <math.h>
-#include <poll.h>
-#include <sys/prctl.h>
-#include <drivers/drv_hrt.h>
-#include <uORB/uORB.h>
-#include <drivers/drv_gyro.h>
-#include <uORB/topics/vehicle_control_mode.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_status.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/parameter_update.h>
-
-#include <systemlib/perf_counter.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/param/param.h>
-
-#include "multirotor_attitude_control.h"
-#include "multirotor_rate_control.h"
-
-__EXPORT int multirotor_att_control_main(int argc, char *argv[]);
-
-static bool thread_should_exit;
-static int mc_task;
-static bool motor_test_mode = false;
-static const float min_takeoff_throttle = 0.3f;
-static const float yaw_deadzone = 0.01f;
-
-static int
-mc_thread_main(int argc, char *argv[])
-{
-	/* declare and safely initialize all structs */
-	struct vehicle_attitude_s att;
-	memset(&att, 0, sizeof(att));
-	struct vehicle_attitude_setpoint_s att_sp;
-	memset(&att_sp, 0, sizeof(att_sp));
-	struct offboard_control_setpoint_s offboard_sp;
-	memset(&offboard_sp, 0, sizeof(offboard_sp));
-	struct vehicle_control_mode_s control_mode;
-	memset(&control_mode, 0, sizeof(control_mode));
-	struct manual_control_setpoint_s manual;
-	memset(&manual, 0, sizeof(manual));
-	struct sensor_combined_s sensor;
-	memset(&sensor, 0, sizeof(sensor));
-	struct vehicle_rates_setpoint_s rates_sp;
-	memset(&rates_sp, 0, sizeof(rates_sp));
-	struct vehicle_status_s status;
-	memset(&status, 0, sizeof(status));
-	struct actuator_controls_s actuators;
-	memset(&actuators, 0, sizeof(actuators));
-
-	/* subscribe */
-	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
-	int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	int offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	int vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
-	int vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-
-	/* publish actuator controls */
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
-		actuators.control[i] = 0.0f;
-	}
-
-	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);
-
-	/* register the perf counter */
-	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
-	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval");
-	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
-
-	warnx("starting");
-
-	/* store last control mode to detect mode switches */
-	bool control_yaw_position = true;
-	bool reset_yaw_sp = true;
-
-	struct pollfd fds[1] = {
-		{ .fd = vehicle_attitude_sub, .events = POLLIN },
-	};
-
-	while (!thread_should_exit) {
-
-		/* wait for a sensor update, check for exit condition every 500 ms */
-		int ret = poll(fds, 1, 500);
-
-		if (ret < 0) {
-			/* poll error, count it in perf */
-			perf_count(mc_err_perf);
-
-		} else if (ret > 0) {
-			/* only run controller if attitude changed */
-			perf_begin(mc_loop_perf);
-
-			/* attitude */
-			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
-
-			bool updated;
-
-			/* parameters */
-			orb_check(parameter_update_sub, &updated);
-
-			if (updated) {
-				struct parameter_update_s update;
-				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
-				/* update parameters */
-			}
-
-			/* control mode */
-			orb_check(vehicle_control_mode_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub, &control_mode);
-			}
-
-			/* manual control setpoint */
-			orb_check(manual_control_setpoint_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
-			}
-
-			/* attitude setpoint */
-			orb_check(vehicle_attitude_setpoint_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
-			}
-
-			/* offboard control setpoint */
-			orb_check(offboard_control_setpoint_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp);
-			}
-
-			/* vehicle status */
-			orb_check(vehicle_status_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status);
-			}
-
-			/* sensors */
-			orb_check(sensor_combined_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
-			}
-
-			/* set flag to safe value */
-			control_yaw_position = true;
-
-			/* reset yaw setpoint if not armed */
-			if (!control_mode.flag_armed) {
-				reset_yaw_sp = true;
-			}
-
-			/* define which input is the dominating control input */
-			if (control_mode.flag_control_offboard_enabled) {
-				/* offboard inputs */
-				if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
-					rates_sp.roll = offboard_sp.p1;
-					rates_sp.pitch = offboard_sp.p2;
-					rates_sp.yaw = offboard_sp.p3;
-					rates_sp.thrust = offboard_sp.p4;
-					rates_sp.timestamp = hrt_absolute_time();
-					orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-
-				} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
-					att_sp.roll_body = offboard_sp.p1;
-					att_sp.pitch_body = offboard_sp.p2;
-					att_sp.yaw_body = offboard_sp.p3;
-					att_sp.thrust = offboard_sp.p4;
-					att_sp.timestamp = hrt_absolute_time();
-					/* publish the result to the vehicle actuators */
-					orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-				}
-
-				/* reset yaw setpoint after offboard control */
-				reset_yaw_sp = true;
-
-			} else if (control_mode.flag_control_manual_enabled) {
-				/* manual input */
-				if (control_mode.flag_control_attitude_enabled) {
-					/* control attitude, update attitude setpoint depending on mode */
-					if (att_sp.thrust < 0.1f) {
-						/* no thrust, don't try to control yaw */
-						rates_sp.yaw = 0.0f;
-						control_yaw_position = false;
-
-						if (status.condition_landed) {
-							/* reset yaw setpoint if on ground */
-							reset_yaw_sp = true;
-						}
-
-					} else {
-						/* only move yaw setpoint if manual input is != 0 */
-						if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
-							/* control yaw rate */
-							control_yaw_position = false;
-							rates_sp.yaw = manual.yaw;
-							reset_yaw_sp = true;	// has no effect on control, just for beautiful log
-
-						} else {
-							control_yaw_position = true;
-						}
-					}
-
-					if (!control_mode.flag_control_velocity_enabled) {
-						/* update attitude setpoint if not in position control mode */
-						att_sp.roll_body = manual.roll;
-						att_sp.pitch_body = manual.pitch;
-
-						if (!control_mode.flag_control_climb_rate_enabled) {
-							/* pass throttle directly if not in altitude control mode */
-							att_sp.thrust = manual.throttle;
-						}
-					}
-
-					/* reset yaw setpint to current position if needed */
-					if (reset_yaw_sp) {
-						att_sp.yaw_body = att.yaw;
-						reset_yaw_sp = false;
-					}
-
-					if (motor_test_mode) {
-						printf("testmode");
-						att_sp.roll_body = 0.0f;
-						att_sp.pitch_body = 0.0f;
-						att_sp.yaw_body = 0.0f;
-						att_sp.thrust = 0.1f;
-					}
-
-					att_sp.timestamp = hrt_absolute_time();
-
-					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-
-				} 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();
-					}
-
-					/* reset yaw setpoint after ACRO */
-					reset_yaw_sp = true;
-				}
-
-			} else {
-				if (!control_mode.flag_control_auto_enabled) {
-					/* no control, try to stay on place */
-					if (!control_mode.flag_control_velocity_enabled) {
-						/* no velocity control, reset attitude setpoint */
-						att_sp.roll_body = 0.0f;
-						att_sp.pitch_body = 0.0f;
-						att_sp.timestamp = hrt_absolute_time();
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-					}
-				}
-
-				/* reset yaw setpoint after non-manual control */
-				reset_yaw_sp = true;
-			}
-
-			/* check if we should we reset integrals */
-			bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f;	// TODO use landed status instead of throttle
-
-			/* run attitude controller if needed */
-			if (control_mode.flag_control_attitude_enabled) {
-				multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral);
-				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-			}
-
-			/* measure in what intervals the controller runs */
-			perf_count(mc_interval_perf);
-
-			/* run rates controller if needed */
-			if (control_mode.flag_control_rates_enabled) {
-				/* get current rate setpoint */
-				bool rates_sp_updated = false;
-				orb_check(vehicle_rates_setpoint_sub, &rates_sp_updated);
-
-				if (rates_sp_updated) {
-					orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp);
-				}
-
-				/* apply controller */
-				float rates[3];
-				rates[0] = att.rollspeed;
-				rates[1] = att.pitchspeed;
-				rates[2] = att.yawspeed;
-				multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
-
-			} else {
-				/* rates controller disabled, set actuators to zero for safety */
-				actuators.control[0] = 0.0f;
-				actuators.control[1] = 0.0f;
-				actuators.control[2] = 0.0f;
-				actuators.control[3] = 0.0f;
-			}
-
-			/* fill in manual control values */
-			actuators.control[4] = manual.flaps;
-			actuators.control[5] = manual.aux1;
-			actuators.control[6] = manual.aux2;
-			actuators.control[7] = manual.aux3;
-
-			actuators.timestamp = hrt_absolute_time();
-			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-
-			perf_end(mc_loop_perf);
-		}
-	}
-
-	warnx("stopping, disarming motors");
-
-	/* kill all outputs */
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
-		actuators.control[i] = 0.0f;
-
-	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-
-	close(vehicle_attitude_sub);
-	close(vehicle_control_mode_sub);
-	close(manual_control_setpoint_sub);
-	close(actuator_pub);
-	close(att_sp_pub);
-
-	perf_print_counter(mc_loop_perf);
-	perf_free(mc_loop_perf);
-
-	fflush(stdout);
-	exit(0);
-}
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: multirotor_att_control [-m <mode>] [-t] {start|status|stop}\n");
-	fprintf(stderr, "    <mode> is 'rates' or 'attitude'\n");
-	fprintf(stderr, "    -t enables motor test mode with 10%% thrust\n");
-	exit(1);
-}
-
-int multirotor_att_control_main(int argc, char *argv[])
-{
-	int	ch;
-	unsigned int optioncount = 0;
-
-	while ((ch = getopt(argc, argv, "tm:")) != EOF) {
-		switch (ch) {
-		case 't':
-			motor_test_mode = true;
-			optioncount += 1;
-			break;
-
-		case ':':
-			usage("missing parameter");
-			break;
-
-		default:
-			fprintf(stderr, "option: -%c\n", ch);
-			usage("unrecognized option");
-			break;
-		}
-	}
-
-	argc -= optioncount;
-	//argv += optioncount;
-
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1 + optioncount], "start")) {
-
-		thread_should_exit = false;
-		mc_task = task_spawn_cmd("multirotor_att_control",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 15,
-					 2048,
-					 mc_thread_main,
-					 NULL);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1 + optioncount], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
deleted file mode 100644
index 8245aa560..000000000
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.c
+++ /dev/null
@@ -1,254 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           Lorenz Meier <lm@inf.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_attitude_control.c
- *
- * Implementation of attitude controller for multirotors.
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include "multirotor_attitude_control.h"
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <float.h>
-#include <math.h>
-#include <systemlib/pid/pid.h>
-#include <systemlib/param/param.h>
-#include <drivers/drv_hrt.h>
-
-PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f);
-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, 6.8f);
-PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
-PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f);
-//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;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-static int parameters_init(struct mc_att_control_param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p);
-
-
-static int parameters_init(struct mc_att_control_param_handles *h)
-{
-	/* PID parameters */
-	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;
-}
-
-static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p)
-{
-	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, bool reset_integral)
-{
-	static uint64_t last_run = 0;
-	static uint64_t last_input = 0;
-	float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-	last_run = hrt_absolute_time();
-
-	if (last_input != att_sp->timestamp) {
-		last_input = att_sp->timestamp;
-	}
-
-	static int motor_skip_counter = 0;
-
-	static PID_t pitch_controller;
-	static PID_t roll_controller;
-
-	static struct mc_att_control_params p;
-	static struct mc_att_control_param_handles h;
-
-	static bool initialized = false;
-
-	static float yaw_error;
-
-	/* initialize the pid controllers when the function is called for the first time */
-	if (initialized == false) {
-		parameters_init(&h);
-		parameters_update(&h, &p);
-
-		pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
-		pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
-
-		initialized = true;
-	}
-
-	/* load new parameters with lower rate */
-	if (motor_skip_counter % 500 == 0) {
-		/* update parameters from storage */
-		parameters_update(&h, &p);
-
-		/* 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);
-	}
-
-	/* reset integrals if needed */
-	if (reset_integral) {
-		pid_reset_integral(&pitch_controller);
-		pid_reset_integral(&roll_controller);
-		//TODO pid_reset_integral(&yaw_controller);
-	}
-
-	/* calculate current control outputs */
-
-	/* control pitch (forward) output */
-	rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body ,
-					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);
-
-	if (control_yaw_position) {
-		/* control yaw rate */
-		// TODO use pid lib
-
-		/* positive error: rotate to right, negative error, rotate to left (NED frame) */
-		// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);
-
-		yaw_error = att_sp->yaw_body - att->yaw;
-
-		if (yaw_error > M_PI_F) {
-			yaw_error -= M_TWOPI_F;
-
-		} else if (yaw_error < -M_PI_F) {
-			yaw_error += M_TWOPI_F;
-		}
-
-		rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
-	}
-
-	rates_sp->thrust = att_sp->thrust;
-    //need to update the timestamp now that we've touched rates_sp
-    rates_sp->timestamp = hrt_absolute_time();
-
-	motor_skip_counter++;
-}
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
deleted file mode 100644
index 86ac0e4ff..000000000
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ /dev/null
@@ -1,196 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file multirotor_rate_control.c
- *
- * Implementation of rate controller for multirotors.
- *
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#include "multirotor_rate_control.h"
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <float.h>
-#include <math.h>
-#include <systemlib/pid/pid.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <drivers/drv_hrt.h>
-
-PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
-PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f);
-PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f);
-
-PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
-
-struct mc_rate_control_params {
-
-	float yawrate_p;
-	float yawrate_d;
-	float yawrate_i;
-
-	float attrate_p;
-	float attrate_d;
-	float attrate_i;
-
-	float rate_lim;
-};
-
-struct mc_rate_control_param_handles {
-
-	param_t yawrate_p;
-	param_t yawrate_i;
-	param_t yawrate_d;
-
-	param_t attrate_p;
-	param_t attrate_i;
-	param_t attrate_d;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-static int parameters_init(struct mc_rate_control_param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p);
-
-
-static int parameters_init(struct mc_rate_control_param_handles *h)
-{
-	/* PID parameters */
-	h->yawrate_p 	=	param_find("MC_YAWRATE_P");
-	h->yawrate_i 	=	param_find("MC_YAWRATE_I");
-	h->yawrate_d 	=	param_find("MC_YAWRATE_D");
-
-	h->attrate_p 	= 	param_find("MC_ATTRATE_P");
-	h->attrate_i 	= 	param_find("MC_ATTRATE_I");
-	h->attrate_d 	= 	param_find("MC_ATTRATE_D");
-
-	return OK;
-}
-
-static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p)
-{
-	param_get(h->yawrate_p, &(p->yawrate_p));
-	param_get(h->yawrate_i, &(p->yawrate_i));
-	param_get(h->yawrate_d, &(p->yawrate_d));
-
-	param_get(h->attrate_p, &(p->attrate_p));
-	param_get(h->attrate_i, &(p->attrate_i));
-	param_get(h->attrate_d, &(p->attrate_d));
-
-	return OK;
-}
-
-void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators, bool reset_integral)
-{
-	static uint64_t last_run = 0;
-	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-	static uint64_t last_input = 0;
-
-	if (last_input != rate_sp->timestamp) {
-		last_input = rate_sp->timestamp;
-	}
-
-	last_run = hrt_absolute_time();
-
-	static int motor_skip_counter = 0;
-
-	static PID_t pitch_rate_controller;
-	static PID_t roll_rate_controller;
-	static PID_t yaw_rate_controller;
-
-	static struct mc_rate_control_params p;
-	static struct mc_rate_control_param_handles h;
-
-	static bool initialized = false;
-
-	/* initialize the pid controllers when the function is called for the first time */
-	if (initialized == false) {
-		parameters_init(&h);
-		parameters_update(&h, &p);
-		initialized = true;
-
-		pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
-		pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
-		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
-	}
-
-	/* load new parameters with lower rate */
-	if (motor_skip_counter % 2500 == 0) {
-		/* update parameters from storage */
-		parameters_update(&h, &p);
-		pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
-		pid_set_parameters(&roll_rate_controller,  p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
-		pid_set_parameters(&yaw_rate_controller,  p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f);
-	}
-
-	/* reset integrals if needed */
-	if (reset_integral) {
-		pid_reset_integral(&pitch_rate_controller);
-		pid_reset_integral(&roll_rate_controller);
-		pid_reset_integral(&yaw_rate_controller);
-	}
-
-	/* run pitch, roll and yaw controllers */
-	float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
-	float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
-	float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
-
-	actuators->control[0] = roll_control;
-	actuators->control[1] = pitch_control;
-	actuators->control[2] = yaw_control;
-	actuators->control[3] = rate_sp->thrust;
-
-	motor_skip_counter++;
-}
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
deleted file mode 100644
index 3d23d0c09..000000000
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ /dev/null
@@ -1,690 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file multirotor_pos_control.c
- *
- * Multirotor position controller
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-#include <stdbool.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-#include <termios.h>
-#include <time.h>
-#include <sys/prctl.h>
-#include <drivers/drv_hrt.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
-#include <uORB/topics/vehicle_global_velocity_setpoint.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/pid/pid.h>
-#include <mavlink/mavlink_log.h>
-
-#include "multirotor_pos_control_params.h"
-#include "thrust_pid.h"
-
-
-static bool thread_should_exit = false;		/**< Deamon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
-static int deamon_task;				/**< Handle of deamon task / thread */
-
-__EXPORT int multirotor_pos_control_main(int argc, char *argv[]);
-
-/**
- * Mainloop of position controller.
- */
-static int multirotor_pos_control_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static float scale_control(float ctl, float end, float dz);
-
-static float norm(float x, float y);
-
-static void usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n");
-	exit(1);
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_spawn().
- */
-int multirotor_pos_control_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			warnx("already running");
-			/* this is not an error */
-			exit(0);
-		}
-
-		warnx("start");
-		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("multirotor_pos_control",
-					     SCHED_DEFAULT,
-					     SCHED_PRIORITY_MAX - 60,
-					     4096,
-					     multirotor_pos_control_thread_main,
-					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		warnx("stop");
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("app is running");
-
-		} else {
-			warnx("app not started");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-static float scale_control(float ctl, float end, float dz)
-{
-	if (ctl > dz) {
-		return (ctl - dz) / (end - dz);
-
-	} else if (ctl < -dz) {
-		return (ctl + dz) / (end - dz);
-
-	} else {
-		return 0.0f;
-	}
-}
-
-static float norm(float x, float y)
-{
-	return sqrtf(x * x + y * y);
-}
-
-static int multirotor_pos_control_thread_main(int argc, char *argv[])
-{
-	/* welcome user */
-	warnx("started");
-	static int mavlink_fd;
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(mavlink_fd, "[mpc] started");
-
-	/* structures */
-	struct vehicle_control_mode_s control_mode;
-	memset(&control_mode, 0, sizeof(control_mode));
-	struct vehicle_attitude_s att;
-	memset(&att, 0, sizeof(att));
-	struct vehicle_attitude_setpoint_s att_sp;
-	memset(&att_sp, 0, sizeof(att_sp));
-	struct manual_control_setpoint_s manual;
-	memset(&manual, 0, sizeof(manual));
-	struct vehicle_local_position_s local_pos;
-	memset(&local_pos, 0, sizeof(local_pos));
-	struct vehicle_local_position_setpoint_s local_pos_sp;
-	memset(&local_pos_sp, 0, sizeof(local_pos_sp));
-	struct vehicle_global_position_setpoint_s global_pos_sp;
-	memset(&global_pos_sp, 0, sizeof(global_pos_sp));
-	struct vehicle_global_velocity_setpoint_s global_vel_sp;
-	memset(&global_vel_sp, 0, sizeof(global_vel_sp));
-
-	/* subscribe to attitude, motor setpoints and system state */
-	int param_sub = orb_subscribe(ORB_ID(parameter_update));
-	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
-	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
-
-	/* publish setpoint */
-	orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
-	orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
-	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
-
-	bool reset_mission_sp = false;
-	bool global_pos_sp_valid = false;
-	bool reset_man_sp_z = true;
-	bool reset_man_sp_xy = true;
-	bool reset_int_z = true;
-	bool reset_int_z_manual = false;
-	bool reset_int_xy = true;
-	bool was_armed = false;
-	bool reset_auto_sp_xy = true;
-	bool reset_auto_sp_z = true;
-	bool reset_takeoff_sp = true;
-
-	hrt_abstime t_prev = 0;
-	const float alt_ctl_dz = 0.2f;
-	const float pos_ctl_dz = 0.05f;
-
-	float ref_alt = 0.0f;
-	hrt_abstime ref_alt_t = 0;
-	uint64_t local_ref_timestamp = 0;
-
-	PID_t xy_pos_pids[2];
-	PID_t xy_vel_pids[2];
-	PID_t z_pos_pid;
-	thrust_pid_t z_vel_pid;
-
-	thread_running = true;
-
-	struct multirotor_position_control_params params;
-	struct multirotor_position_control_param_handles params_h;
-	parameters_init(&params_h);
-	parameters_update(&params_h, &params);
-
-
-	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, params.z_vel_max, 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);
-
-	while (!thread_should_exit) {
-
-		bool param_updated;
-		orb_check(param_sub, &param_updated);
-
-		if (param_updated) {
-			/* clear updated flag */
-			struct parameter_update_s ps;
-			orb_copy(ORB_ID(parameter_update), param_sub, &ps);
-			/* update params */
-			parameters_update(&params_h, &params);
-
-			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.0f) {
-					i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
-
-				} else {
-					i_limit = 0.0f;	// not used
-				}
-
-				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);
-		}
-
-		bool updated;
-
-		orb_check(control_mode_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
-		}
-
-		orb_check(global_pos_sp_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
-			global_pos_sp_valid = true;
-			reset_mission_sp = true;
-		}
-
-		hrt_abstime t = hrt_absolute_time();
-		float dt;
-
-		if (t_prev != 0) {
-			dt = (t - t_prev) * 0.000001f;
-
-		} else {
-			dt = 0.0f;
-		}
-
-		if (control_mode.flag_armed && !was_armed) {
-			/* reset setpoints and integrals on arming */
-			reset_man_sp_z = true;
-			reset_man_sp_xy = true;
-			reset_auto_sp_z = true;
-			reset_auto_sp_xy = true;
-			reset_takeoff_sp = true;
-			reset_int_z = true;
-			reset_int_xy = true;
-		}
-
-		was_armed = control_mode.flag_armed;
-
-		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);
-
-			float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
-			float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
-			float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
-
-			if (control_mode.flag_control_manual_enabled) {
-				/* manual control */
-				/* check for reference point updates and correct setpoint */
-				if (local_pos.ref_timestamp != ref_alt_t) {
-					if (ref_alt_t != 0) {
-						/* home alt changed, don't follow large ground level changes in manual flight */
-						local_pos_sp.z += local_pos.ref_alt - ref_alt;
-					}
-
-					ref_alt_t = local_pos.ref_timestamp;
-					ref_alt = local_pos.ref_alt;
-					// TODO also correct XY setpoint
-				}
-
-				/* reset setpoints to current position if needed */
-				if (control_mode.flag_control_altitude_enabled) {
-					if (reset_man_sp_z) {
-						reset_man_sp_z = false;
-						local_pos_sp.z = local_pos.z;
-						mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
-					}
-
-					/* move altitude setpoint with throttle stick */
-					float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
-
-					if (z_sp_ctl != 0.0f) {
-						sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
-						local_pos_sp.z += sp_move_rate[2] * dt;
-
-						if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
-							local_pos_sp.z = local_pos.z + z_sp_offs_max;
-
-						} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
-							local_pos_sp.z = local_pos.z - z_sp_offs_max;
-						}
-					}
-				}
-
-				if (control_mode.flag_control_position_enabled) {
-					if (reset_man_sp_xy) {
-						reset_man_sp_xy = false;
-						local_pos_sp.x = local_pos.x;
-						local_pos_sp.y = local_pos.y;
-						pid_reset_integral(&xy_vel_pids[0]);
-						pid_reset_integral(&xy_vel_pids[1]);
-						mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
-					}
-
-					/* move position setpoint with roll/pitch stick */
-					float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
-					float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
-
-					if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
-						/* calculate direction and increment of control in NED frame */
-						float xy_sp_ctl_dir = att.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;
-						}
-					}
-				}
-
-				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
-				local_pos_sp.yaw = att_sp.yaw_body;
-
-				/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
-				reset_auto_sp_xy = !control_mode.flag_control_position_enabled;
-				reset_auto_sp_z = !control_mode.flag_control_altitude_enabled;
-				reset_takeoff_sp = true;
-
-				/* force reprojection of global setpoint after manual mode */
-				reset_mission_sp = true;
-
-			} else if (control_mode.flag_control_auto_enabled) {
-				/* AUTO mode, use global setpoint */
-				if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
-					reset_auto_sp_xy = true;
-					reset_auto_sp_z = true;
-
-				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
-					if (reset_takeoff_sp) {
-						reset_takeoff_sp = false;
-						local_pos_sp.x = local_pos.x;
-						local_pos_sp.y = local_pos.y;
-						local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
-						att_sp.yaw_body = att.yaw;
-						mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
-					}
-
-					reset_auto_sp_xy = false;
-					reset_auto_sp_z = true;
-
-				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
-					// TODO
-					reset_auto_sp_xy = true;
-					reset_auto_sp_z = true;
-
-				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
-					/* init local projection using local position ref */
-					if (local_pos.ref_timestamp != local_ref_timestamp) {
-						reset_mission_sp = true;
-						local_ref_timestamp = local_pos.ref_timestamp;
-						double lat_home = local_pos.ref_lat * 1e-7;
-						double lon_home = local_pos.ref_lon * 1e-7;
-						map_projection_init(lat_home, lon_home);
-						mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
-					}
-
-					if (reset_mission_sp) {
-						reset_mission_sp = false;
-						/* update global setpoint projection */
-
-						if (global_pos_sp_valid) {
-							/* global position setpoint valid, use it */
-							double sp_lat = global_pos_sp.lat * 1e-7;
-							double sp_lon = global_pos_sp.lon * 1e-7;
-							/* project global setpoint to local setpoint */
-							map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
-
-							if (global_pos_sp.altitude_is_relative) {
-								local_pos_sp.z = -global_pos_sp.altitude;
-
-							} else {
-								local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
-							}
-							/* update yaw setpoint only if value is valid */
-							if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
-								att_sp.yaw_body = global_pos_sp.yaw;
-							}
-
-							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
-
-						} else {
-							if (reset_auto_sp_xy) {
-								reset_auto_sp_xy = false;
-								/* local position setpoint is invalid,
-								 * use current position as setpoint for loiter */
-								local_pos_sp.x = local_pos.x;
-								local_pos_sp.y = local_pos.y;
-								local_pos_sp.yaw = att.yaw;
-							}
-
-							if (reset_auto_sp_z) {
-								reset_auto_sp_z = false;
-								local_pos_sp.z = local_pos.z;
-							}
-
-							mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
-						}
-					}
-
-					reset_auto_sp_xy = true;
-					reset_auto_sp_z = true;
-				}
-
-				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) {
-					reset_takeoff_sp = true;
-				}
-
-				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
-					reset_mission_sp = true;
-				}
-
-				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
-				local_pos_sp.yaw = att_sp.yaw_body;
-
-				/* reset setpoints after AUTO mode */
-				reset_man_sp_xy = true;
-				reset_man_sp_z = true;
-
-			} else {
-				/* no control (failsafe), loiter or stay on ground */
-				if (local_pos.landed) {
-					/* landed: move setpoint down */
-					/* in air: hold altitude */
-					if (local_pos_sp.z < 5.0f) {
-						/* set altitude setpoint to 5m under ground,
-						 * don't set it too deep to avoid unexpected landing in case of false "landed" signal */
-						local_pos_sp.z = 5.0f;
-						mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
-					}
-
-					reset_man_sp_z = true;
-
-				} else {
-					/* in air: hold altitude */
-					if (reset_man_sp_z) {
-						reset_man_sp_z = false;
-						local_pos_sp.z = local_pos.z;
-						mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
-					}
-
-					reset_auto_sp_z = false;
-				}
-
-				if (control_mode.flag_control_position_enabled) {
-					if (reset_man_sp_xy) {
-						reset_man_sp_xy = false;
-						local_pos_sp.x = local_pos.x;
-						local_pos_sp.y = local_pos.y;
-						local_pos_sp.yaw = att.yaw;
-						att_sp.yaw_body = att.yaw;
-						mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
-					}
-
-					reset_auto_sp_xy = false;
-				}
-			}
-
-			/* publish local position setpoint */
-			orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
-
-			/* run position & altitude controllers, calculate velocity setpoint */
-			if (control_mode.flag_control_altitude_enabled) {
-				global_vel_sp.vz = 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 {
-				reset_man_sp_z = true;
-				global_vel_sp.vz = 0.0f;
-			}
-
-			if (control_mode.flag_control_position_enabled) {
-				/* calculate velocity set point in NED frame */
-				global_vel_sp.vx = 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_man_sp_xy = 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 subscribe to velocity setpoint if altitude/position control disabled
-
-			if (control_mode.flag_control_climb_rate_enabled || 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_climb_rate_enabled) {
-					if (reset_int_z) {
-						reset_int_z = false;
-						float i = params.thr_min;
-
-						if (reset_int_z_manual) {
-							i = manual.throttle;
-
-							if (i < params.thr_min) {
-								i = params.thr_min;
-
-							} else if (i > params.thr_max) {
-								i = params.thr_max;
-							}
-						}
-
-						thrust_pid_set_integral(&z_vel_pid, -i);
-						mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
-					}
-
-					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];
-
-				} else {
-					/* reset thrust integral when altitude control enabled */
-					reset_int_z = true;
-				}
-
-				if (control_mode.flag_control_velocity_enabled) {
-					/* calculate thrust set point in NED frame */
-					if (reset_int_xy) {
-						reset_int_xy = false;
-						pid_reset_integral(&xy_vel_pids[0]);
-						pid_reset_integral(&xy_vel_pids[1]);
-						mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
-					}
-
-					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);
-
-				} else {
-					reset_int_xy = true;
-				}
-
-				att_sp.timestamp = hrt_absolute_time();
-
-				/* publish new attitude setpoint */
-				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-			}
-
-		} else {
-			/* position controller disabled, reset setpoints */
-			reset_man_sp_z = true;
-			reset_man_sp_xy = true;
-			reset_int_z = true;
-			reset_int_xy = true;
-			reset_mission_sp = true;
-			reset_auto_sp_xy = true;
-			reset_auto_sp_z = true;
-		}
-
-		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
-		reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;
-
-		/* run at approximately 50 Hz */
-		usleep(20000);
-	}
-
-	warnx("stopped");
-	mavlink_log_info(mavlink_fd, "[mpc] stopped");
-
-	thread_running = false;
-
-	fflush(stdout);
-	return 0;
-}
-
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
deleted file mode 100644
index b7041e4d5..000000000
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ /dev/null
@@ -1,112 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/*
- * @file multirotor_pos_control_params.c
- *
- * Parameters for multirotor_pos_control
- */
-
-#include "multirotor_pos_control_params.h"
-
-/* controller parameters */
-PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f);
-PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f);
-PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
-PARAM_DEFINE_FLOAT(MPC_Z_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, 5.0f);
-PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
-
-int parameters_init(struct multirotor_position_control_param_handles *h)
-{
-	h->takeoff_alt = param_find("NAV_TAKEOFF_ALT");
-	h->takeoff_gap = param_find("NAV_TAKEOFF_GAP");
-	h->thr_min 	=	param_find("MPC_THR_MIN");
-	h->thr_max 	=	param_find("MPC_THR_MAX");
-	h->z_p 	=	param_find("MPC_Z_P");
-	h->z_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->takeoff_alt, &(p->takeoff_alt));
-	param_get(h->takeoff_gap, &(p->takeoff_gap));
-	param_get(h->thr_min, &(p->thr_min));
-	param_get(h->thr_max, &(p->thr_max));
-	param_get(h->z_p, &(p->z_p));
-	param_get(h->z_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/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c
deleted file mode 100644
index b985630ae..000000000
--- a/src/modules/multirotor_pos_control/thrust_pid.c
+++ /dev/null
@@ -1,189 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file thrust_pid.c
- *
- * Implementation of thrust control PID.
- *
- * @author Anton Babushkin <anton.babushkin@me.com>
- */
-
-#include "thrust_pid.h"
-#include <math.h>
-
-__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;
-}
-
-__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i)
-{
-	pid->integral = i;
-}
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
new file mode 100644
index 000000000..9bbaf167a
--- /dev/null
+++ b/src/modules/navigator/geofence.cpp
@@ -0,0 +1,299 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Jean Cyr <jean.m.cyr@gmail.com>
+ *           @author Thomas Gubler <thomasgubler@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 geofence.cpp
+ * Provides functions for handling the geofence
+ */
+#include "geofence.h"
+
+#include <uORB/topics/vehicle_global_position.h>
+#include <string.h>
+#include <dataman/dataman.h>
+#include <systemlib/err.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <ctype.h>
+#include <nuttx/config.h>
+#include <unistd.h>
+
+
+/* Oddly, ERROR is not defined for C++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+Geofence::Geofence() : _fence_pub(-1),
+		_altitude_min(0),
+		_altitude_max(0),
+		_verticesCount(0),
+		param_geofence_on(NULL, "GF_ON", false)
+{
+	/* Load initial params */
+	updateParams();
+}
+
+Geofence::~Geofence()
+{
+
+}
+
+
+bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
+{
+	double lat = vehicle->lat / 1e7d;
+	double lon = vehicle->lon / 1e7d;
+	float	alt = vehicle->alt;
+
+	return inside(lat, lon, vehicle->alt);
+}
+
+bool Geofence::inside(double lat, double lon, float altitude)
+{
+	/* Return true if geofence is disabled */
+	if (param_geofence_on.get() != 1)
+		return true;
+
+	if (valid()) {
+
+		if (!isEmpty()) {
+			/* Vertical check */
+			if (altitude > _altitude_max || altitude < _altitude_min)
+				return false;
+
+			/*Horizontal check */
+			/* Adaptation of algorithm originally presented as
+			 * PNPOLY - Point Inclusion in Polygon Test
+			 * W. Randolph Franklin (WRF) */
+
+			bool c = false;
+
+			struct fence_vertex_s temp_vertex_i;
+			struct fence_vertex_s temp_vertex_j;
+
+			/* Red until fence is finished */
+			for (unsigned i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) {
+				if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
+					break;
+				}
+				if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
+					break;
+				}
+
+				// skip vertex 0 (return point)
+				if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) &&
+							(lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) /
+							 (temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) {
+							c = !c;
+				}
+
+			}
+
+			return c;
+		} else {
+			/* Empty fence --> accept all points */
+			return true;
+		}
+
+	} else {
+		/* Invalid fence --> accept all points */
+		return true;
+	}
+}
+
+bool
+Geofence::valid()
+{
+	// NULL fence is valid
+	if (isEmpty())
+		return true;
+
+	// Otherwise
+	if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) {
+		warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1);
+		return false;
+	}
+
+	return true;
+}
+
+void
+Geofence::addPoint(int argc, char *argv[])
+{
+	int ix, last;
+	double lon, lat;
+	struct fence_vertex_s vertex;
+	char *end;
+
+	if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) {
+		dm_clear(DM_KEY_FENCE_POINTS);
+		publishFence(0);
+		return;
+	}
+
+	if (argc < 3)
+		errx(1, "Specify: -clear | sequence latitude longitude [-publish]");
+
+	ix = atoi(argv[0]);
+	if (ix >= DM_KEY_FENCE_POINTS_MAX)
+		errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX);
+
+	lat = strtod(argv[1], &end);
+	lon = strtod(argv[2], &end);
+
+	last = 0;
+	if ((argc > 3) && (strcmp(argv[3], "-publish") == 0))
+		last = 1;
+
+	vertex.lat = (float)lat;
+	vertex.lon = (float)lon;
+
+	if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) {
+		if (last)
+			publishFence((unsigned)ix + 1);
+		return;
+	}
+
+	errx(1, "can't store fence point");
+}
+
+void
+Geofence::publishFence(unsigned vertices)
+{
+	if (_fence_pub == -1)
+		_fence_pub = orb_advertise(ORB_ID(fence), &vertices);
+	else
+		orb_publish(ORB_ID(fence), _fence_pub, &vertices);
+}
+
+int
+Geofence::loadFromFile(const char *filename)
+{
+	FILE		*fp;
+	char		line[120];
+	int			pointCounter = 0;
+	bool		gotVertical = false;
+	const char commentChar = '#';
+
+	/* Make sure no data is left in the datamanager */
+	clearDm();
+
+	/* open the mixer definition file */
+	fp = fopen(GEOFENCE_FILENAME, "r");
+	if (fp == NULL) {
+		return ERROR;
+	}
+
+	/* create geofence points from valid lines and store in DM */
+	for (;;) {
+
+		/* get a line, bail on error/EOF */
+		if (fgets(line, sizeof(line), fp) == NULL)
+			break;
+
+		/* Trim leading whitespace */
+		size_t textStart = 0;
+		while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++;
+
+		/* if the line starts with #, skip */
+		if (line[textStart] == commentChar)
+			continue;
+
+		if (gotVertical) {
+			/* Parse the line as a geofence point */
+			struct fence_vertex_s vertex;
+
+			/* if the line starts with DMS, this means that the coordinate is given as degree minute second instead of decimal degrees */
+			if (line[textStart] == 'D' && line[textStart + 1] == 'M' && line[textStart + 2] == 'S') {
+				/* Handle degree minute second format */
+				float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s;
+
+				if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6)
+					return ERROR;
+
+//				warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s);
+
+				vertex.lat = lat_d + lat_m/60.0f + lat_s/3600.0f;
+				vertex.lon = lon_d + lon_m/60.0f + lon_s/3600.0f;
+
+			} else {
+				/* Handle decimal degree format */
+
+				if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2)
+					return ERROR;
+			}
+
+			if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex))
+								return ERROR;
+
+			warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter,  (double)vertex.lat, (double)vertex.lon);
+
+			pointCounter++;
+		} else {
+			/* Parse the line as the vertical limits */
+			if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2)
+				return ERROR;
+
+
+			warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max);
+			gotVertical = true;
+		}
+
+
+	}
+
+	fclose(fp);
+
+	/* Check if import was successful */
+	if(gotVertical && pointCounter > 0)
+	{
+		_verticesCount = pointCounter;
+		warnx("Geofence: imported successfully");
+	} else {
+		warnx("Geofence: import error");
+	}
+
+	return ERROR;
+}
+
+int Geofence::clearDm()
+{
+	dm_clear(DM_KEY_FENCE_POINTS);
+}
+
+void Geofence::updateParams()
+{
+	param_geofence_on.update();
+}
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
new file mode 100644
index 000000000..5b56ebc7a
--- /dev/null
+++ b/src/modules/navigator/geofence.h
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Jean Cyr <jean.m.cyr@gmail.com>
+ *           @author Thomas Gubler <thomasgubler@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 geofence.h
+ * Provides functions for handling the geofence
+ */
+
+#ifndef GEOFENCE_H_
+#define GEOFENCE_H_
+
+#include <uORB/topics/fence.h>
+#include <controllib/block/BlockParam.hpp>
+
+#define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt"
+
+class Geofence {
+private:
+	orb_advert_t	_fence_pub;			/**< publish fence topic */
+
+	float			_altitude_min;
+	float			_altitude_max;
+
+	unsigned 			_verticesCount;
+
+	/* Params */
+	control::BlockParamInt param_geofence_on;
+public:
+	Geofence();
+	~Geofence();
+
+	/**
+	 * Return whether craft is inside geofence.
+	 *
+	 * Calculate whether point is inside arbitrary polygon
+	 * @param craft pointer craft coordinates
+	 * @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected
+	 * @return true: craft is inside fence, false:craft is outside fence
+	 */
+	bool inside(const struct vehicle_global_position_s *craft);
+	bool inside(double lat, double lon, float altitude);
+
+	int clearDm();
+
+	bool valid();
+
+	/**
+	 * Specify fence vertex position.
+	 */
+	void addPoint(int argc, char *argv[]);
+
+	void publishFence(unsigned vertices);
+
+	int loadFromFile(const char *filename);
+
+	bool isEmpty() {return _verticesCount == 0;}
+
+	void updateParams();
+};
+
+
+#endif /* GEOFENCE_H_ */
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/navigator/geofence_params.c
index fc658dadb..5831a0ca9 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/navigator/geofence_params.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -18,7 +18,7 @@
  *    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
+ * 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,
@@ -32,70 +32,30 @@
  *
  ****************************************************************************/
 
-/*
- * @file multirotor_pos_control_params.h
+/**
+ * @file geofence_params.c
+ *
+ * Parameters for geofence
  *
- * Parameters for multirotor_pos_control
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <systemlib/param/param.h>
-
-struct multirotor_position_control_params {
-	float takeoff_alt;
-	float takeoff_gap;
-	float thr_min;
-	float thr_max;
-	float z_p;
-	float z_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;
-};
+#include <nuttx/config.h>
 
-struct multirotor_position_control_param_handles {
-	param_t takeoff_alt;
-	param_t takeoff_gap;
-	param_t thr_min;
-	param_t thr_max;
-	param_t z_p;
-	param_t z_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;
-};
+#include <systemlib/param/param.h>
 
-/**
- * Initialize all parameter handles and values
- *
+/*
+ * Geofence parameters, accessible via MAVLink
  */
-int parameters_init(struct multirotor_position_control_param_handles *h);
 
 /**
- * Update all parameters
+ * Enable geofence.
+ *
+ * Set to 1 to enable geofence.
+ * Defaults to 1 because geofence is only enabled when the geofence.txt file is present.
  *
+ * @min 0
+ * @max 1
+ * @group Geofence
  */
-int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p);
+PARAM_DEFINE_INT32(GF_ON, 1);
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
new file mode 100644
index 000000000..eaafa217d
--- /dev/null
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -0,0 +1,202 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Thomas Gubler <thomasgubler@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 mission_feasibility_checker.cpp
+ * Provides checks if mission is feasible given the navigation capabilities
+ */
+
+#include "mission_feasibility_checker.h"
+
+#include <geo/geo.h>
+#include <math.h>
+#include <mathlib/mathlib.h>
+#include <mavlink/mavlink_log.h>
+#include <fw_pos_control_l1/landingslope.h>
+#include <systemlib/err.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <uORB/topics/fence.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false)
+{
+	_nav_caps = {0};
+}
+
+
+bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
+{
+	/* Init if not done yet */
+	init();
+
+	/* Open mavlink fd */
+	if (_mavlink_fd < 0) {
+		/* try to open the mavlink log device every once in a while */
+		_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	}
+
+
+	if (isRotarywing)
+		return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence);
+	else
+		return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence);
+}
+
+bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
+{
+
+	return checkGeofence(dm_current, nMissionItems, geofence);
+}
+
+bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
+{
+	/* Update fixed wing navigation capabilites */
+	updateNavigationCapabilities();
+//	warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
+
+	return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence));
+}
+
+bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
+{
+	/* Check if all mission items are inside the geofence (if we have a valid geofence) */
+	if (geofence.valid()) {
+		for (size_t i = 0; i < nMissionItems; i++) {
+			static struct mission_item_s missionitem;
+			const ssize_t len = sizeof(struct mission_item_s);
+
+			if (dm_read(dm_current, i, &missionitem, len) != len) {
+				/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+				return false;
+			}
+
+			if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude
+				mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i);
+				return false;
+			}
+		}
+	}
+
+	return true;
+}
+
+bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems)
+{
+	/* Go through all mission items and search for a landing waypoint
+	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
+
+
+	for (size_t i = 0; i < nMissionItems; i++) {
+		static struct mission_item_s missionitem;
+		const ssize_t len = sizeof(struct mission_item_s);
+		if (dm_read(dm_current, i, &missionitem, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return false;
+		}
+
+		if (missionitem.nav_cmd == NAV_CMD_LAND) {
+			struct mission_item_s missionitem_previous;
+			if (i != 0) {
+				if (dm_read(dm_current, i-1, &missionitem_previous, len) != len) {
+					/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+					return false;
+				}
+
+				float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon);
+				float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
+				float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
+				float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
+//				warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f",
+//						wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req);
+//				warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f",
+//						_nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length);
+
+				if (wp_distance > _nav_caps.landing_flare_length) {
+					/* Last wp is before flare region */
+
+					if (delta_altitude < 0) {
+						if (missionitem_previous.altitude <= slope_alt_req) {
+							/* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */
+							return true;
+						} else {
+							/* Landing waypoint is above altitude of slope at the given waypoint distance */
+							mavlink_log_info(_mavlink_fd, "#audio: Landing: last waypoint too high/too close");
+							mavlink_log_info(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm",
+									(double)(slope_alt_req),
+									(double)(wp_distance_req - wp_distance));
+							return false;
+						}
+					} else {
+						/* Landing waypoint is above last waypoint */
+						mavlink_log_info(_mavlink_fd, "#audio: Landing waypoint above last nav waypoint");
+						return false;
+					}
+				} else {
+					/* Last wp is in flare region */
+					//xxx give recommendations
+					mavlink_log_info(_mavlink_fd, "#audio: Warning: Landing: last waypoint in flare region");
+					return false;
+				}
+			} else {
+				mavlink_log_info(_mavlink_fd, "#audio: Warning: starting with land waypoint");
+				return false;
+			}
+		}
+	}
+
+
+//	float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
+}
+
+void MissionFeasibilityChecker::updateNavigationCapabilities()
+{
+	int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
+}
+
+void MissionFeasibilityChecker::init()
+{
+	if (!_initDone) {
+
+		_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
+
+		_initDone = true;
+	}
+}
diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h
new file mode 100644
index 000000000..7a0b2a296
--- /dev/null
+++ b/src/modules/navigator/mission_feasibility_checker.h
@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Thomas Gubler <thomasgubler@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 mission_feasibility_checker.h
+ * Provides checks if mission is feasible given the navigation capabilities
+ */
+#ifndef MISSION_FEASIBILITY_CHECKER_H_
+#define MISSION_FEASIBILITY_CHECKER_H_
+
+#include <unistd.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/navigation_capabilities.h>
+#include <dataman/dataman.h>
+#include "geofence.h"
+
+
+class MissionFeasibilityChecker
+{
+private:
+	int		_mavlink_fd;
+
+	int _capabilities_sub;
+	struct navigation_capabilities_s _nav_caps;
+
+	bool _initDone;
+	void init();
+
+	/* Checks for all airframes */
+	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
+
+	/* Checks specific to fixedwing airframes */
+	bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
+	bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
+	void updateNavigationCapabilities();
+
+	/* Checks specific to rotarywing airframes */
+	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
+public:
+
+	MissionFeasibilityChecker();
+	~MissionFeasibilityChecker() {}
+
+	/*
+	 * Returns true if mission is feasible and false otherwise
+	 */
+	bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
+
+};
+
+
+#endif /* MISSION_FEASIBILITY_CHECKER_H_ */
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 0404b06c7..55f8a4caa 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -38,4 +38,10 @@
 MODULE_COMMAND	= navigator
 
 SRCS		= navigator_main.cpp \
-		  navigator_params.c
+		  navigator_params.c \
+		  navigator_mission.cpp \
+		  mission_feasibility_checker.cpp \
+		  geofence.cpp \
+		  geofence_params.c
+
+INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index f6c44444a..260356eca 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1,7 +1,10 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: 	Lorenz Meier
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Jean Cyr <jean.m.cyr@gmail.com>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *           @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,6 +39,12 @@
  * Implementation of the main navigation state machine.
  *
  * Handles missions, geo fencing and failsafe navigation behavior.
+ * Published the mission item triplet for the position controller.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Jean Cyr <jean.m.cyr@gmail.com>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
@@ -48,26 +57,44 @@
 #include <math.h>
 #include <poll.h>
 #include <time.h>
+#include <sys/ioctl.h>
+#include <drivers/device/device.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/airspeed.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/mission_result.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission.h>
+#include <uORB/topics/fence.h>
+#include <uORB/topics/navigation_capabilities.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
-#include <geo/geo.h>
+#include <systemlib/state_table.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
+#include <geo/geo.h>
 #include <mathlib/mathlib.h>
+#include <dataman/dataman.h>
+#include <mavlink/mavlink_log.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include "navigator_state.h"
+#include "navigator_mission.h"
+#include "mission_feasibility_checker.h"
+#include "geofence.h"
+
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
 
 /**
  * navigator app start / stop handling function
@@ -76,7 +103,7 @@
  */
 extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
 
-class Navigator
+class Navigator : public StateTable
 {
 public:
 	/**
@@ -85,103 +112,175 @@ public:
 	Navigator();
 
 	/**
-	 * Destructor, also kills the sensors task.
+	 * Destructor, also kills the navigators task.
 	 */
 	~Navigator();
 
 	/**
-	 * Start the sensors task.
+	* Start the navigator task.
 	 *
 	 * @return		OK on success.
 	 */
 	int		start();
 
+	/**
+	 * Display the navigator status.
+	 */
+	void		status();
+
+	/**
+	 * Add point to geofence
+	 */
+	void add_fence_point(int argc, char *argv[]);
+
+	/**
+	 * Load fence from file
+	 */
+	void load_fence_from_file(const char *filename);
+
 private:
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
-	int		_navigator_task;			/**< task handle for sensor task */
+	int		_navigator_task;		/**< task handle for sensor task */
 
-	int		_global_pos_sub;
-	int		_att_sub;			/**< vehicle attitude subscription */
-	int		_attitude_sub;			/**< raw rc channels data subscription */
-	int		_airspeed_sub;			/**< airspeed subscription */
+	int		_mavlink_fd;
+
+	int		_global_pos_sub;		/**< global position subscription */
+	int		_home_pos_sub;			/**< home position subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
-	int 		_params_sub;			/**< notification of parameter updates */
-	int 		_manual_control_sub;		/**< notification of manual control updates */
-	int		_mission_sub;
+	int		_params_sub;			/**< notification of parameter updates */
+	int		_offboard_mission_sub;		/**< notification of offboard mission updates */
+	int 		_onboard_mission_sub;		/**< notification of onboard mission updates */
+	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
+	int		_control_mode_sub;		/**< vehicle control mode subscription */
 
-	orb_advert_t	_triplet_pub;			/**< position setpoint */
+	orb_advert_t	_pos_sp_triplet_pub;			/**< publish position setpoint triplet */
+	orb_advert_t	_mission_result_pub;		/**< publish mission result topic */
 
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
-	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct airspeed_s				_airspeed;		/**< airspeed */
 	struct vehicle_status_s				_vstatus;		/**< vehicle status */
+	struct vehicle_control_mode_s		_control_mode;		/**< vehicle control mode */
 	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
-	struct vehicle_global_position_set_triplet_s	_global_triplet;	/**< triplet of global setpoints */
+	struct home_position_s				_home_pos;		/**< home position for RTL */
+	struct position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
+	struct mission_result_s				_mission_result;	/**< mission result for commander/mavlink */
+	struct mission_item_s				_mission_item;	/**< current mission item */
+	bool		_mission_item_valid;	/**< current mission item valid */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-	unsigned	_mission_items_maxcount;				/**< maximum number of mission items supported */
-	struct	mission_item_s 				* _mission_items;	/**< storage for mission items */
-	bool		_mission_valid;						/**< flag if mission is valid */
+	Geofence					_geofence;
+	bool						_geofence_violation_warning_sent;
+
+	bool						_fence_valid;		/**< flag if fence is valid */
+	bool						_inside_fence;		/**< vehicle is inside fence */
+
+	struct navigation_capabilities_s		_nav_caps;
+
+	class 		Mission				_mission;
+
+	bool		_global_pos_valid;				/**< track changes of global_position.global_valid flag */
+	bool		_reset_loiter_pos;				/**< if true then loiter position should be set to current position */
+	bool		_waypoint_position_reached;
+	bool		_waypoint_yaw_reached;
+	uint64_t	_time_first_inside_orbit;
+	bool		_need_takeoff;		/**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */
+	bool		_do_takeoff;		/**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */
+
+	MissionFeasibilityChecker missionFeasiblityChecker;
 
-	/** manual control states */
-	float		_seatbelt_hold_heading;		/**< heading the system should hold in seatbelt mode */
-	float		_loiter_hold_lat;
-	float		_loiter_hold_lon;
-	float		_loiter_hold_alt;
-	bool		_loiter_hold;
+	uint64_t	_set_nav_state_timestamp;		/**< timestamp of last handled navigation state request */
+
+	bool		_pos_sp_triplet_updated;
+
+	char *nav_states_str[NAV_STATE_MAX];
 
 	struct {
-		float throttle_cruise;
-	}		_parameters;			/**< local copies of interesting parameters */
+		float min_altitude;
+		float acceptance_radius;
+		float loiter_radius;
+		int onboard_mission_enabled;
+		float takeoff_alt;
+		float land_alt;
+		float rtl_alt;
+		float rtl_land_delay;
+	}		_parameters;			/**< local copies of parameters */
 
 	struct {
-		param_t throttle_cruise;
+		param_t min_altitude;
+		param_t acceptance_radius;
+		param_t loiter_radius;
+		param_t onboard_mission_enabled;
+		param_t takeoff_alt;
+		param_t land_alt;
+		param_t rtl_alt;
+		param_t rtl_land_delay;
+	}		_parameter_handles;		/**< handles for parameters */
+
+	enum Event {
+		EVENT_NONE_REQUESTED,
+		EVENT_READY_REQUESTED,
+		EVENT_LOITER_REQUESTED,
+		EVENT_MISSION_REQUESTED,
+		EVENT_RTL_REQUESTED,
+		EVENT_LAND_REQUESTED,
+		EVENT_MISSION_CHANGED,
+		EVENT_HOME_POSITION_CHANGED,
+		MAX_EVENT
+	};
 
-	}		_parameter_handles;		/**< handles for interesting parameters */
+	/**
+	 * State machine transition table
+	 */
+	static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT];
 
+	enum RTLState {
+		RTL_STATE_NONE = 0,
+		RTL_STATE_CLIMB,
+		RTL_STATE_RETURN,
+		RTL_STATE_DESCEND
+	};
+
+	enum RTLState _rtl_state;
 
 	/**
 	 * Update our local parameter cache.
 	 */
-	int		parameters_update();
+	void		parameters_update();
 
 	/**
-	 * Update control outputs
-	 *
+	 * Retrieve global position
 	 */
-	void		control_update();
+	void		global_position_update();
 
 	/**
-	 * Check for changes in vehicle status.
+	 * Retrieve home position
 	 */
-	void		vehicle_status_poll();
+	void		home_position_update();
 
 	/**
-	 * Check for position updates.
+	 * Retreive navigation capabilities
 	 */
-	void		vehicle_attitude_poll();
+	void		navigation_capabilities_update();
 
 	/**
-	 * Check for set triplet updates.
+	 * Retrieve offboard mission.
 	 */
-	void		mission_poll();
+	void		offboard_mission_update(bool isrotaryWing);
 
 	/**
-	 * Control throttle.
+	 * Retrieve onboard mission.
 	 */
-	float		control_throttle(float energy_error);
+	void		onboard_mission_update();
 
 	/**
-	 * Control pitch.
+	 * Retrieve vehicle status
 	 */
-	float		control_pitch(float altitude_error);
+	void		vehicle_status_update();
 
-	void calculate_airspeed_errors();
-	void calculate_gndspeed_undershoot();
-	void calculate_altitude_error();
+	/**
+	 * Retrieve vehicle control mode
+	 */
+	void		vehicle_control_mode_update();
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -192,6 +291,64 @@ private:
 	 * Main sensor collection task.
 	 */
 	void		task_main() __attribute__((noreturn));
+
+	void		publish_safepoints(unsigned points);
+
+	/**
+	 * Functions that are triggered when a new state is entered.
+	 */
+	void		start_none();
+	void		start_ready();
+	void		start_loiter();
+	void		start_mission();
+	void		start_rtl();
+	void		start_land();
+	void		start_land_home();
+
+	/**
+	 * Guards offboard mission
+	 */
+	bool		offboard_mission_available(unsigned relative_index);
+
+	/**
+	 * Guards onboard mission
+	 */
+	bool		onboard_mission_available(unsigned relative_index);
+
+	/**
+	 * Check if current mission item has been reached.
+	 */
+	bool		check_mission_item_reached();
+
+	/**
+	 * Perform actions when current mission item reached.
+	 */
+	void		on_mission_item_reached();
+
+	/**
+	 * Move to next waypoint
+	 */
+	void		set_mission_item();
+
+	/**
+	 * Switch to next RTL state
+	 */
+	void		set_rtl_item();
+
+	/**
+	 * Set position setpoint for mission item
+	 */
+	void		position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item);
+
+	/**
+	 * Helper function to get a takeoff item
+	 */
+	void		get_takeoff_setpoint(position_setpoint_s *pos_sp);
+
+	/**
+	 * Publish a new mission item triplet for position controller
+	 */
+	void		publish_position_setpoint_triplet();
 };
 
 namespace navigator
@@ -208,37 +365,71 @@ Navigator	*g_navigator;
 
 Navigator::Navigator() :
 
+/* state machine transition table */
+	StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
+
 	_task_should_exit(false),
 	_navigator_task(-1),
+	_mavlink_fd(-1),
 
 /* subscriptions */
 	_global_pos_sub(-1),
-	_att_sub(-1),
-	_airspeed_sub(-1),
+	_home_pos_sub(-1),
 	_vstatus_sub(-1),
+	_control_mode_sub(-1),
 	_params_sub(-1),
-	_manual_control_sub(-1),
+	_offboard_mission_sub(-1),
+	_onboard_mission_sub(-1),
+	_capabilities_sub(-1),
 
 /* publications */
-	_triplet_pub(-1),
+	_pos_sp_triplet_pub(-1),
+	_mission_result_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
-/* states */
-	_mission_items_maxcount(20),
-	_mission_valid(false),
-	_loiter_hold(false)
-{
-	_mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_items_maxcount);
-	if (!_mission_items) {
-		_mission_items_maxcount = 0;
-		warnx("no free RAM to allocate mission, rejecting any waypoints");
-	}
-
-	_parameter_handles.throttle_cruise = param_find("NAV_DUMMY");
 
-	/* fetch initial parameter values */
-	parameters_update();
+/* states */
+	_rtl_state(RTL_STATE_NONE),
+	_fence_valid(false),
+	_inside_fence(true),
+	_mission(),
+	_global_pos_valid(false),
+	_reset_loiter_pos(true),
+	_waypoint_position_reached(false),
+	_waypoint_yaw_reached(false),
+	_time_first_inside_orbit(0),
+	_set_nav_state_timestamp(0),
+	_mission_item_valid(false),
+	_need_takeoff(true),
+	_do_takeoff(false),
+	_pos_sp_triplet_updated(false),
+	_geofence_violation_warning_sent(false)
+{
+	_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
+	_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
+	_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
+	_parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN");
+	_parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT");
+	_parameter_handles.land_alt = param_find("NAV_LAND_ALT");
+	_parameter_handles.rtl_alt = param_find("NAV_RTL_ALT");
+	_parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T");
+
+	memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s));
+	memset(&_mission_result, 0, sizeof(struct mission_result_s));
+	memset(&_mission_item, 0, sizeof(struct mission_item_s));
+
+	memset(&nav_states_str, 0, sizeof(nav_states_str));
+	nav_states_str[0] = "NONE";
+	nav_states_str[1] = "READY";
+	nav_states_str[2] = "LOITER";
+	nav_states_str[3] = "MISSION";
+	nav_states_str[4] = "RTL";
+	nav_states_str[5] = "LAND";
+
+	/* Initialize state machine */
+	myState = NAV_STATE_NONE;
+	start_none();
 }
 
 Navigator::~Navigator()
@@ -266,69 +457,108 @@ Navigator::~Navigator()
 	navigator::g_navigator = nullptr;
 }
 
-int
+void
 Navigator::parameters_update()
 {
-
-	//param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
-
-	return OK;
+	/* read from param to clear updated flag */
+	struct parameter_update_s update;
+	orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+
+	param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude));
+	param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius));
+	param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius));
+	param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled));
+	param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));
+	param_get(_parameter_handles.land_alt, &(_parameters.land_alt));
+	param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt));
+	param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay));
+
+	_mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled);
+
+	_geofence.updateParams();
 }
 
 void
-Navigator::vehicle_status_poll()
+Navigator::global_position_update()
 {
-	bool vstatus_updated;
-
-	/* Check HIL state if vehicle status has changed */
-	orb_check(_vstatus_sub, &vstatus_updated);
+	orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
+}
 
-	if (vstatus_updated) {
+void
+Navigator::home_position_update()
+{
+	orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
+}
 
-		orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
-	}
+void
+Navigator::navigation_capabilities_update()
+{
+	orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
 }
 
+
 void
-Navigator::vehicle_attitude_poll()
+Navigator::offboard_mission_update(bool isrotaryWing)
 {
-	/* check if there is a new position */
-	bool att_updated;
-	orb_check(_att_sub, &att_updated);
+	struct mission_s offboard_mission;
+
+	if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
+
+		/* Check mission feasibility, for now do not handle the return value,
+		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
+		dm_item_t dm_current;
+
+		if (offboard_mission.dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
 
-	if (att_updated) {
-		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+
+		missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence);
+
+		_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
+		_mission.set_current_offboard_mission_index(offboard_mission.current_index);
+		_mission.set_offboard_mission_count(offboard_mission.count);
+
+	} else {
+		_mission.set_current_offboard_mission_index(0);
+		_mission.set_offboard_mission_count(0);
 	}
 }
 
 void
-Navigator::mission_poll()
+Navigator::onboard_mission_update()
 {
-	/* check if there is a new setpoint */
-	bool mission_updated;
-	orb_check(_mission_sub, &mission_updated);
-
-	if (mission_updated) {
+	struct mission_s onboard_mission;
 
-		struct mission_s mission;
-		orb_copy(ORB_ID(mission), _mission_sub, &mission);
+	if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
 
-		// XXX this is not optimal yet, but a first prototype /
-		// test implementation
+		_mission.set_current_onboard_mission_index(onboard_mission.current_index);
+		_mission.set_onboard_mission_count(onboard_mission.count);
 
-		if (mission.count <= _mission_items_maxcount) {
-			/*
-			 * Perform an atomic copy & state update
-			 */
-			irqstate_t flags = irqsave();
+	} else {
+		_mission.set_current_onboard_mission_index(0);
+		_mission.set_onboard_mission_count(0);
+	}
+}
 
-			memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s));
-			_mission_valid = true;
+void
+Navigator::vehicle_status_update()
+{
+	if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {
+		/* in case the commander is not be running */
+		_vstatus.arming_state = ARMING_STATE_STANDBY;
+	}
+}
 
-			irqrestore(flags);
-		} else {
-			warnx("mission larger than storage space");
-		}
+void
+Navigator::vehicle_control_mode_update()
+{
+	if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) != OK) {
+		/* in case the commander is not be running */
+		_control_mode.flag_control_auto_enabled = false;
+		_control_mode.flag_armed = false;
 	}
 }
 
@@ -341,46 +571,90 @@ Navigator::task_main_trampoline(int argc, char *argv[])
 void
 Navigator::task_main()
 {
-
 	/* inform about start */
 	warnx("Initializing..");
 	fflush(stdout);
 
+	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	mavlink_log_info(_mavlink_fd, "[navigator] started");
+
+	/* Try to load the geofence:
+	 * if /fs/microsd/etc/geofence.txt load from this file
+	 * else clear geofence data in datamanager
+	 */
+	struct stat buffer;
+
+	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
+		warnx("Try to load geofence.txt");
+		_geofence.loadFromFile(GEOFENCE_FILENAME);
+
+	} else {
+		if (_geofence.clearDm() > 0)
+			warnx("Geofence cleared");
+		else
+			warnx("Could not clear geofence");
+	}
+
 	/*
 	 * do subscriptions
 	 */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	_mission_sub = orb_subscribe(ORB_ID(mission));
-	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	_offboard_mission_sub = orb_subscribe(ORB_ID(mission));
+	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
+	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
-	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
+
+	/* copy all topics first time */
+	vehicle_status_update();
+	vehicle_control_mode_update();
+	parameters_update();
+	global_position_update();
+	home_position_update();
+	navigation_capabilities_update();
+	offboard_mission_update(_vstatus.is_rotary_wing);
+	onboard_mission_update();
 
-	/* rate limit vehicle status updates to 5Hz */
-	orb_set_interval(_vstatus_sub, 200);
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
 
-	parameters_update();
+	unsigned prevState = NAV_STATE_NONE;
+	hrt_abstime mavlink_open_time = 0;
+	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[2];
+	struct pollfd fds[8];
 
 	/* Setup of loop */
 	fds[0].fd = _params_sub;
 	fds[0].events = POLLIN;
 	fds[1].fd = _global_pos_sub;
 	fds[1].events = POLLIN;
+	fds[2].fd = _home_pos_sub;
+	fds[2].events = POLLIN;
+	fds[3].fd = _capabilities_sub;
+	fds[3].events = POLLIN;
+	fds[4].fd = _offboard_mission_sub;
+	fds[4].events = POLLIN;
+	fds[5].fd = _onboard_mission_sub;
+	fds[5].events = POLLIN;
+	fds[6].fd = _vstatus_sub;
+	fds[6].events = POLLIN;
+	fds[7].fd = _control_mode_sub;
+	fds[7].events = POLLIN;
 
 	while (!_task_should_exit) {
 
-		/* wait for up to 500ms for data */
+		/* wait for up to 100ms for data */
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit, etc. */
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -390,147 +664,218 @@ Navigator::task_main()
 
 		perf_begin(_loop_perf);
 
-		/* check vehicle status for changes to publication state */
-		vehicle_status_poll();
-
-		/* only update parameters if they changed */
-		if (fds[0].revents & POLLIN) {
-			/* read from param to clear updated flag */
-			struct parameter_update_s update;
-			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+		if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) {
+			/* try to reopen the mavlink log device with specified interval */
+			mavlink_open_time = hrt_abstime() + mavlink_open_interval;
+			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+		}
 
-			/* update parameters from storage */
-			parameters_update();
+		/* vehicle control mode updated */
+		if (fds[7].revents & POLLIN) {
+			vehicle_control_mode_update();
 		}
 
-		/* only run controller if position changed */
-		if (fds[1].revents & POLLIN) {
+		/* vehicle status updated */
+		if (fds[6].revents & POLLIN) {
+			vehicle_status_update();
 
+			/* evaluate state machine from commander and set the navigator mode accordingly */
+			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
+				bool stick_mode = false;
 
-			static uint64_t last_run = 0;
-			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-			last_run = hrt_absolute_time();
+				if (!_vstatus.rc_signal_lost) {
+					/* RC signal available, use control switches to set mode */
+					/* RETURN switch, overrides MISSION switch */
+					if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
+						/* switch to RTL if not already landed after RTL and home position set */
+						if (!(_rtl_state == RTL_STATE_DESCEND &&
+						      (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
+						    _vstatus.condition_home_position_valid) {
+							dispatch(EVENT_RTL_REQUESTED);
+						}
 
-			/* guard against too large deltaT's */
-			if (deltaT > 1.0f)
-				deltaT = 0.01f;
+						stick_mode = true;
 
-			/* load local copies */
-			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
+					} else {
+						/* MISSION switch */
+						if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
+							dispatch(EVENT_LOITER_REQUESTED);
+							stick_mode = true;
+
+						} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
+							/* switch to mission only if available */
+							if (_mission.current_mission_available()) {
+								dispatch(EVENT_MISSION_REQUESTED);
+
+							} else {
+								dispatch(EVENT_LOITER_REQUESTED);
+							}
+
+							stick_mode = true;
+						}
+
+						if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
+							/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
+							dispatch(EVENT_LOITER_REQUESTED);
+							stick_mode = true;
+						}
+					}
+				}
 
-			vehicle_attitude_poll();
+				if (!stick_mode) {
+					if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
+						/* commander requested new navigation mode, try to set it */
+						_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
 
-			mission_poll();
+						switch (_vstatus.set_nav_state) {
+						case NAV_STATE_NONE:
+							/* nothing to do */
+							break;
 
-			math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
-			// Current waypoint
-			math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
-			// Global position
-			math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
+						case NAV_STATE_LOITER:
+							dispatch(EVENT_LOITER_REQUESTED);
+							break;
 
-			/* AUTONOMOUS FLIGHT */
+						case NAV_STATE_MISSION:
+							if (_mission.current_mission_available()) {
+								dispatch(EVENT_MISSION_REQUESTED);
 
-			if (1 /* autonomous flight */) {
+							} else {
+								dispatch(EVENT_LOITER_REQUESTED);
+							}
 
-				/* execute navigation once we have a setpoint */
-				if (_mission_valid) {
+							break;
 
-					// Next waypoint
-					math::Vector2f prev_wp;
+						case NAV_STATE_RTL:
+							if (!(_rtl_state == RTL_STATE_DESCEND &&
+							      (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
+							    _vstatus.condition_home_position_valid) {
+								dispatch(EVENT_RTL_REQUESTED);
+							}
 
-					if (_global_triplet.previous_valid) {
-						prev_wp.setX(_global_triplet.previous.lat / 1e7f);
-						prev_wp.setY(_global_triplet.previous.lon / 1e7f);
+							break;
 
-					} else {
-						/*
-						 * No valid next waypoint, go for heading hold.
-						 * This is automatically handled by the L1 library.
-						 */
-						prev_wp.setX(_global_triplet.current.lat / 1e7f);
-						prev_wp.setY(_global_triplet.current.lon / 1e7f);
+						case NAV_STATE_LAND:
+							if (myState != NAV_STATE_READY) {
+								dispatch(EVENT_LAND_REQUESTED);
+							}
 
-					}
+							break;
 
+						default:
+							warnx("ERROR: Requested navigation state not supported");
+							break;
+						}
 
+					} else {
+						/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
+						if (myState == NAV_STATE_NONE) {
+							if (_mission.current_mission_available()) {
+								dispatch(EVENT_MISSION_REQUESTED);
+
+							} else {
+								dispatch(EVENT_LOITER_REQUESTED);
+							}
+						}
+					}
+				}
 
-					/********   MAIN NAVIGATION STATE MACHINE   ********/
+			} else {
+				/* navigator shouldn't act */
+				dispatch(EVENT_NONE_REQUESTED);
+			}
+		}
 
-					// XXX to be put in its own class
+		/* parameters updated */
+		if (fds[0].revents & POLLIN) {
+			parameters_update();
+			/* note that these new parameters won't be in effect until a mission triplet is published again */
+		}
 
-					if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
-						/* waypoint is a plain navigation waypoint */
-						
+		/* navigation capabilities updated */
+		if (fds[3].revents & POLLIN) {
+			navigation_capabilities_update();
+		}
 
-					} else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-						_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-						_global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+		/* offboard mission updated */
+		if (fds[4].revents & POLLIN) {
+			offboard_mission_update(_vstatus.is_rotary_wing);
+			// XXX check if mission really changed
+			dispatch(EVENT_MISSION_CHANGED);
+		}
 
-						/* waypoint is a loiter waypoint */
-						
-					}
+		/* onboard mission updated */
+		if (fds[5].revents & POLLIN) {
+			onboard_mission_update();
+			// XXX check if mission really changed
+			dispatch(EVENT_MISSION_CHANGED);
+		}
 
-					// XXX at this point we always want no loiter hold if a
-					// mission is active
-					_loiter_hold = false;
+		/* home position updated */
+		if (fds[2].revents & POLLIN) {
+			home_position_update();
+			// XXX check if home position really changed
+			dispatch(EVENT_HOME_POSITION_CHANGED);
+		}
 
-				} else {
+		/* global position updated */
+		if (fds[1].revents & POLLIN) {
+			global_position_update();
 
-					if (!_loiter_hold) {
-						_loiter_hold_lat = _global_pos.lat / 1e7f;
-						_loiter_hold_lon = _global_pos.lon / 1e7f;
-						_loiter_hold_alt = _global_pos.alt;
-						_loiter_hold = true;
-					}
+			/* publish position setpoint triplet on each position update if navigator active */
+			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
+				_pos_sp_triplet_updated = true;
 
-					//_parameters.loiter_hold_radius
+				if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) {
+					/* got global position when landing, update setpoint */
+					start_land();
 				}
 
-			} else if (0/* seatbelt mode enabled */) {
-
-				/** SEATBELT FLIGHT **/
-				continue;
-
-			} else {
-
-				/** MANUAL FLIGHT **/
+				_global_pos_valid = _global_pos.global_valid;
 
-				/* no flight mode applies, do not publish an attitude setpoint */
-				continue;
+				/* check if waypoint has been reached in MISSION, RTL and LAND modes */
+				if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
+					if (check_mission_item_reached()) {
+						on_mission_item_reached();
+					}
+				}
 			}
 
-			/********   MAIN NAVIGATION STATE MACHINE   ********/
-
-			if (_global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-				// XXX define launch position and return
-
-			} else if (_global_triplet.current.nav_cmd == NAV_CMD_LAND) {
-				// XXX flared descent on final approach
+			/* Check geofence violation */
+			if (!_geofence.inside(&_global_pos)) {
+				//xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination)
 
-			} else if (_global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
-
-				/* apply minimum pitch if altitude has not yet been reached */
-				if (_global_pos.alt < _global_triplet.current.altitude) {
-					_att_sp.pitch_body = math::max(_att_sp.pitch_body, _global_triplet.current.param1);
+				/* Issue a warning about the geofence violation once */
+				if (!_geofence_violation_warning_sent) {
+					mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation");
+					_geofence_violation_warning_sent = true;
 				}
-			}
-
-			/* lazily publish the setpoint only once available */
-			if (_triplet_pub > 0) {
-				/* publish the attitude setpoint */
-				orb_publish(ORB_ID(vehicle_global_position_set_triplet), _triplet_pub, &_global_triplet);
 
 			} else {
-				/* advertise and publish */
-				_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &_global_triplet);
+				/* Reset the _geofence_violation_warning_sent field */
+				_geofence_violation_warning_sent = false;
 			}
+		}
+
+		/* publish position setpoint triplet if updated */
+		if (_pos_sp_triplet_updated) {
+			_pos_sp_triplet_updated = false;
+			publish_position_setpoint_triplet();
+		}
+
+		/* notify user about state changes */
+		if (myState != prevState) {
+			mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]);
+			prevState = myState;
 
+			/* reset time counter on state changes */
+			_time_first_inside_orbit = 0;
 		}
 
 		perf_end(_loop_perf);
 	}
 
-	warnx("exiting.\n");
+	warnx("exiting.");
 
 	_navigator_task = -1;
 	_exit(0);
@@ -543,11 +888,11 @@ Navigator::start()
 
 	/* start the task */
 	_navigator_task = task_spawn_cmd("navigator",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       2048,
-				       (main_t)&Navigator::task_main_trampoline,
-				       nullptr);
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 5,
+					 2048,
+					 (main_t)&Navigator::task_main_trampoline,
+					 nullptr);
 
 	if (_navigator_task < 0) {
 		warn("task start failed");
@@ -557,20 +902,745 @@ Navigator::start()
 	return OK;
 }
 
+void
+Navigator::status()
+{
+	warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in");
+
+	if (_global_pos.global_valid) {
+		warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
+		warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
+		      (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
+		warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f",
+		      (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d);
+		warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
+	}
+
+	if (_fence_valid) {
+		warnx("Geofence is valid");
+//		warnx("Vertex longitude latitude");
+//		for (unsigned i = 0; i < _fence.count; i++)
+//		warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
+
+	} else {
+		warnx("Geofence not set");
+	}
+
+	switch (myState) {
+	case NAV_STATE_NONE:
+		warnx("State: None");
+		break;
+
+	case NAV_STATE_LOITER:
+		warnx("State: Loiter");
+		break;
+
+	case NAV_STATE_MISSION:
+		warnx("State: Mission");
+		break;
+
+	case NAV_STATE_RTL:
+		warnx("State: RTL");
+		break;
+
+	default:
+		warnx("State: Unknown");
+		break;
+	}
+}
+
+StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
+	{
+		/* NAV_STATE_NONE */
+		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_NONE},
+		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
+		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_NONE},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_NONE},
+	},
+	{
+		/* NAV_STATE_READY */
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
+		/* EVENT_READY_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
+		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_LAND_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_READY},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_READY},
+	},
+	{
+		/* NAV_STATE_LOITER */
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
+		/* EVENT_READY_REQUESTED */		{NO_ACTION, NAV_STATE_LOITER},
+		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_LOITER},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_LOITER},
+	},
+	{
+		/* NAV_STATE_MISSION */
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
+		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
+		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, NAV_STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
+		/* EVENT_MISSION_CHANGED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_MISSION},
+	},
+	{
+		/* NAV_STATE_RTL */
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
+		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
+		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_RTL},
+		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land_home), NAV_STATE_LAND},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_RTL},
+		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},	// TODO need to reset rtl_state
+	},
+	{
+		/* NAV_STATE_LAND */
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
+		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
+		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_LAND_REQUESTED */		{NO_ACTION, NAV_STATE_LAND},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_LAND},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_LAND},
+	},
+};
+
+void
+Navigator::start_none()
+{
+	_pos_sp_triplet.previous.valid = false;
+	_pos_sp_triplet.current.valid = false;
+	_pos_sp_triplet.next.valid = false;
+	_mission_item_valid = false;
+
+	_reset_loiter_pos = true;
+	_do_takeoff = false;
+	_rtl_state = RTL_STATE_NONE;
+
+	_pos_sp_triplet_updated = true;
+}
+
+void
+Navigator::start_ready()
+{
+	_pos_sp_triplet.previous.valid = false;
+	_pos_sp_triplet.current.valid = true;
+	_pos_sp_triplet.next.valid = false;
+
+	_pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;
+
+	_mission_item_valid = false;
+
+	_reset_loiter_pos = true;
+	_do_takeoff = false;
+
+	if (_rtl_state != RTL_STATE_DESCEND) {
+		/* reset RTL state if landed not at home */
+		_rtl_state = RTL_STATE_NONE;
+	}
+
+	_pos_sp_triplet_updated = true;
+}
+
+void
+Navigator::start_loiter()
+{
+	_do_takeoff = false;
+
+	/* set loiter position if needed */
+	if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) {
+		_reset_loiter_pos = false;
+
+		_pos_sp_triplet.current.lat = _global_pos.lat;
+		_pos_sp_triplet.current.lon = _global_pos.lon;
+		_pos_sp_triplet.current.yaw = NAN;	// NAN means to use current yaw
+
+		float min_alt_amsl = _parameters.min_altitude + _home_pos.alt;
+
+		/* use current altitude if above min altitude set by parameter */
+		if (_global_pos.alt < min_alt_amsl) {
+			_pos_sp_triplet.current.alt = min_alt_amsl;
+			mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
+
+		} else {
+			_pos_sp_triplet.current.alt = _global_pos.alt;
+			mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
+		}
+
+	}
+	_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
+	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
+	_pos_sp_triplet.current.loiter_direction = 1;
+	_pos_sp_triplet.previous.valid = false;
+	_pos_sp_triplet.current.valid = true;
+	_pos_sp_triplet.next.valid = false;
+	_mission_item_valid = false;
+
+	_pos_sp_triplet_updated = true;
+}
+
+void
+Navigator::start_mission()
+{
+	_need_takeoff = true;
+
+	set_mission_item();
+}
+
+void
+Navigator::set_mission_item()
+{
+	/* copy current mission to previous item */
+	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+
+	_reset_loiter_pos = true;
+	_do_takeoff = false;
+
+	int ret;
+	bool onboard;
+	unsigned index;
+
+	ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index);
+
+	if (ret == OK) {
+		/* reset time counter for new item */
+		_time_first_inside_orbit = 0;
+
+		_mission_item_valid = true;
+		position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
+
+		if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
+		    _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
+		    _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
+		    _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) {
+			/* don't reset RTL state on RTL or LOITER items */
+			_rtl_state = RTL_STATE_NONE;
+		}
+
+		if (_vstatus.is_rotary_wing) {
+			if (_need_takeoff && (
+				    _mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
+				    _mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
+				    _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
+				    _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+				    _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+				    _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED
+			    )) {
+				/* do special TAKEOFF handling for VTOL */
+				_need_takeoff = false;
+
+				/* calculate desired takeoff altitude AMSL */
+				float takeoff_alt_amsl = _pos_sp_triplet.current.alt;
+
+				if (_vstatus.condition_landed) {
+					/* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */
+					takeoff_alt_amsl = fmaxf(takeoff_alt_amsl, _global_pos.alt + _parameters.takeoff_alt);
+				}
+
+				/* check if we really need takeoff */
+				if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) {
+					/* force TAKEOFF if landed or waypoint altitude is more than current */
+					_do_takeoff = true;
+
+					/* move current position setpoint to next */
+					memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+
+					/* set current setpoint to takeoff */
+
+					_pos_sp_triplet.current.lat = _global_pos.lat;
+					_pos_sp_triplet.current.lon = _global_pos.lon;
+					_pos_sp_triplet.current.alt = takeoff_alt_amsl;
+					_pos_sp_triplet.current.yaw = NAN;
+					_pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF;
+				}
+
+			} else if (_mission_item.nav_cmd == NAV_CMD_LAND) {
+				/* will need takeoff after landing */
+				_need_takeoff = true;
+			}
+		}
+
+		if (_do_takeoff) {
+			mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt);
+
+		} else {
+			if (onboard) {
+				mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
+
+			} else {
+				mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index);
+			}
+		}
+
+	} else {
+		/* since a mission is not advanced without WPs available, this is not supposed to happen */
+		_mission_item_valid = false;
+		_pos_sp_triplet.current.valid = false;
+		warnx("ERROR: current WP can't be set");
+	}
+
+	if (!_do_takeoff) {
+		mission_item_s item_next;
+		ret = _mission.get_next_mission_item(&item_next);
+
+		if (ret == OK) {
+			position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next);
+
+		} else {
+			/* this will fail for the last WP */
+			_pos_sp_triplet.next.valid = false;
+		}
+	}
+
+	_pos_sp_triplet_updated = true;
+}
+
+void
+Navigator::start_rtl()
+{
+	_do_takeoff = false;
+
+	/* decide if we need climb */
+	if (_rtl_state == RTL_STATE_NONE) {
+		if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) {
+			_rtl_state = RTL_STATE_CLIMB;
+
+		} else {
+			_rtl_state = RTL_STATE_RETURN;
+		}
+	}
+
+	/* if switching directly to return state, reset altitude setpoint */
+	if (_rtl_state == RTL_STATE_RETURN) {
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _global_pos.alt;
+	}
+
+	_reset_loiter_pos = true;
+	set_rtl_item();
+}
+
+void
+Navigator::start_land()
+{
+	/* this state can be requested by commander even if no global position available,
+	 * in his case controller must perform landing without position control */
+	_do_takeoff = false;
+	_reset_loiter_pos = true;
+
+	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+
+	_mission_item_valid = true;
+
+	_mission_item.lat = _global_pos.lat;
+	_mission_item.lon = _global_pos.lon;
+	_mission_item.altitude_is_relative = false;
+	_mission_item.altitude = _global_pos.alt;
+	_mission_item.yaw = NAN;
+	_mission_item.loiter_radius = _parameters.loiter_radius;
+	_mission_item.loiter_direction = 1;
+	_mission_item.nav_cmd = NAV_CMD_LAND;
+	_mission_item.acceptance_radius = _parameters.acceptance_radius;
+	_mission_item.time_inside = 0.0f;
+	_mission_item.pitch_min = 0.0f;
+	_mission_item.autocontinue = true;
+	_mission_item.origin = ORIGIN_ONBOARD;
+
+	position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
+
+	_pos_sp_triplet.next.valid = false;
+}
+
+void
+Navigator::start_land_home()
+{
+	/* land to home position, should be called when hovering above home, from RTL state */
+	_do_takeoff = false;
+	_reset_loiter_pos = true;
+
+	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+
+	_mission_item_valid = true;
+
+	_mission_item.lat = _home_pos.lat;
+	_mission_item.lon = _home_pos.lon;
+	_mission_item.altitude_is_relative = false;
+	_mission_item.altitude = _home_pos.alt;
+	_mission_item.yaw = NAN;
+	_mission_item.loiter_radius = _parameters.loiter_radius;
+	_mission_item.loiter_direction = 1;
+	_mission_item.nav_cmd = NAV_CMD_LAND;
+	_mission_item.acceptance_radius = _parameters.acceptance_radius;
+	_mission_item.time_inside = 0.0f;
+	_mission_item.pitch_min = 0.0f;
+	_mission_item.autocontinue = true;
+	_mission_item.origin = ORIGIN_ONBOARD;
+
+	position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
+
+	_pos_sp_triplet.next.valid = false;
+}
+
+void
+Navigator::set_rtl_item()
+{
+	/*reset time counter for new RTL item */
+	_time_first_inside_orbit = 0;
+
+	switch (_rtl_state) {
+	case RTL_STATE_CLIMB: {
+			memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+
+			float climb_alt = _home_pos.alt + _parameters.rtl_alt;
+
+			if (_vstatus.condition_landed) {
+				climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
+			}
+
+			_mission_item_valid = true;
+
+			_mission_item.lat = _global_pos.lat;
+			_mission_item.lon = _global_pos.lon;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.altitude = climb_alt;
+			_mission_item.yaw = NAN;
+			_mission_item.loiter_radius = _parameters.loiter_radius;
+			_mission_item.loiter_direction = 1;
+			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
+			_mission_item.acceptance_radius = _parameters.acceptance_radius;
+			_mission_item.time_inside = 0.0f;
+			_mission_item.pitch_min = 0.0f;
+			_mission_item.autocontinue = true;
+			_mission_item.origin = ORIGIN_ONBOARD;
+
+			position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
+
+			_pos_sp_triplet.next.valid = false;
+
+			mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt);
+			break;
+		}
+
+	case RTL_STATE_RETURN: {
+			memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+
+			_mission_item_valid = true;
+
+			_mission_item.lat = _home_pos.lat;
+			_mission_item.lon = _home_pos.lon;
+			// don't change altitude
+			_mission_item.yaw = NAN;	// TODO set heading to home
+			_mission_item.loiter_radius = _parameters.loiter_radius;
+			_mission_item.loiter_direction = 1;
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			_mission_item.acceptance_radius = _parameters.acceptance_radius;
+			_mission_item.time_inside = 0.0f;
+			_mission_item.pitch_min = 0.0f;
+			_mission_item.autocontinue = true;
+			_mission_item.origin = ORIGIN_ONBOARD;
+
+			position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
+
+			_pos_sp_triplet.next.valid = false;
+
+			mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt);
+			break;
+		}
+
+	case RTL_STATE_DESCEND: {
+			memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+
+			_mission_item_valid = true;
+
+			_mission_item.lat = _home_pos.lat;
+			_mission_item.lon = _home_pos.lon;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.altitude = _home_pos.alt + _parameters.land_alt;
+			_mission_item.yaw = NAN;
+			_mission_item.loiter_radius = _parameters.loiter_radius;
+			_mission_item.loiter_direction = 1;
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			_mission_item.acceptance_radius = _parameters.acceptance_radius;
+			_mission_item.time_inside = _parameters.rtl_land_delay < 0.0 ? 0.0f : _parameters.rtl_land_delay;
+			_mission_item.pitch_min = 0.0f;
+			_mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f;
+			_mission_item.origin = ORIGIN_ONBOARD;
+
+			position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
+
+			_pos_sp_triplet.next.valid = false;
+
+			mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt);
+			break;
+		}
+
+	default: {
+			mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state);
+			start_loiter();
+			break;
+		}
+	}
+
+	_pos_sp_triplet_updated = true;
+}
+
+void
+Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item)
+{
+	sp->valid = true;
+
+	if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
+		/* set home position for RTL item */
+		sp->lat = _home_pos.lat;
+		sp->lon = _home_pos.lon;
+		sp->alt = _home_pos.alt + _parameters.rtl_alt;
+
+	} else {
+		sp->lat = item->lat;
+		sp->lon = item->lon;
+		sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude;
+	}
+
+	sp->yaw = item->yaw;
+	sp->loiter_radius = item->loiter_radius;
+	sp->loiter_direction = item->loiter_direction;
+	sp->pitch_min = item->pitch_min;
+
+	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
+		sp->type = SETPOINT_TYPE_TAKEOFF;
+
+	} else if (item->nav_cmd == NAV_CMD_LAND) {
+		sp->type = SETPOINT_TYPE_LAND;
+
+	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+		sp->type = SETPOINT_TYPE_LOITER;
+
+	} else {
+		sp->type = SETPOINT_TYPE_NORMAL;
+	}
+}
+
+bool
+Navigator::check_mission_item_reached()
+{
+	/* only check if there is actually a mission item to check */
+	if (!_mission_item_valid) {
+		return false;
+	}
+
+	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
+		return _vstatus.condition_landed;
+	}
+
+	/* XXX TODO count turns */
+	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
+	    _mission_item.loiter_radius > 0.01f) {
+
+		return false;
+	}
+
+	uint64_t now = hrt_absolute_time();
+
+	if (!_waypoint_position_reached) {
+		float acceptance_radius;
+
+		if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) {
+			acceptance_radius = _mission_item.acceptance_radius;
+
+		} else {
+			acceptance_radius = _parameters.acceptance_radius;
+		}
+
+		float dist = -1.0f;
+		float dist_xy = -1.0f;
+		float dist_z = -1.0f;
+
+		/* calculate AMSL altitude for this waypoint */
+		float wp_alt_amsl = _mission_item.altitude;
+
+		if (_mission_item.altitude_is_relative)
+			wp_alt_amsl += _home_pos.alt;
+
+		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
+				(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
+				&dist_xy, &dist_z);
+
+		if (_do_takeoff) {
+			if (_global_pos.alt > wp_alt_amsl - acceptance_radius) {
+				/* require only altitude for takeoff */
+				_waypoint_position_reached = true;
+			}
+
+		} else {
+			if (dist >= 0.0f && dist <= acceptance_radius) {
+				_waypoint_position_reached = true;
+			}
+		}
+	}
+
+	if (!_waypoint_yaw_reached) {
+		if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) {
+			/* check yaw if defined only for rotary wing except takeoff */
+			float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
+
+			if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
+				_waypoint_yaw_reached = true;
+			}
+
+		} else {
+			_waypoint_yaw_reached = true;
+		}
+	}
+
+	/* check if the current waypoint was reached */
+	if (_waypoint_position_reached && _waypoint_yaw_reached) {
+		if (_time_first_inside_orbit == 0) {
+			_time_first_inside_orbit = now;
+
+			if (_mission_item.time_inside > 0.01f) {
+				mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside);
+			}
+		}
+
+		/* check if the MAV was long enough inside the waypoint orbit */
+		if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6)
+		    || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+			_time_first_inside_orbit = 0;
+			_waypoint_yaw_reached = false;
+			_waypoint_position_reached = false;
+			return true;
+		}
+	}
+
+	return false;
+
+}
+
+void
+Navigator::on_mission_item_reached()
+{
+	if (myState == NAV_STATE_MISSION) {
+		if (_do_takeoff) {
+			/* takeoff completed */
+			_do_takeoff = false;
+			mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed");
+
+		} else {
+			/* advance by one mission item */
+			_mission.move_to_next();
+		}
+
+		if (_mission.current_mission_available()) {
+			set_mission_item();
+
+		} else {
+			/* if no more mission items available then finish mission */
+			/* loiter at last waypoint */
+			_reset_loiter_pos = false;
+			mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
+
+			if (_vstatus.condition_landed) {
+				dispatch(EVENT_READY_REQUESTED);
+
+			} else {
+				dispatch(EVENT_LOITER_REQUESTED);
+			}
+		}
+
+	} else if (myState == NAV_STATE_RTL) {
+		/* RTL completed */
+		if (_rtl_state == RTL_STATE_DESCEND) {
+			/* hovering above home position, land if needed or loiter */
+			mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");
+
+			if (_mission_item.autocontinue) {
+				dispatch(EVENT_LAND_REQUESTED);
+
+			} else {
+				_reset_loiter_pos = false;
+				dispatch(EVENT_LOITER_REQUESTED);
+			}
+
+		} else {
+			/* next RTL step */
+			_rtl_state = (RTLState)(_rtl_state + 1);
+			set_rtl_item();
+		}
+
+	} else if (myState == NAV_STATE_LAND) {
+		/* landing completed */
+		mavlink_log_info(_mavlink_fd, "[navigator] landing completed");
+		dispatch(EVENT_READY_REQUESTED);
+	}
+}
+
+void
+Navigator::publish_position_setpoint_triplet()
+{
+	/* update navigation state */
+	_pos_sp_triplet.nav_state = static_cast<nav_state_t>(myState);
+
+	/* lazily publish the position setpoint triplet only once available */
+	if (_pos_sp_triplet_pub > 0) {
+		/* publish the position setpoint triplet */
+		orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
+
+	} else {
+		/* advertise and publish */
+		_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
+	}
+}
+
+void Navigator::add_fence_point(int argc, char *argv[])
+{
+	_geofence.addPoint(argc, argv);
+}
+
+void Navigator::load_fence_from_file(const char *filename)
+{
+	_geofence.loadFromFile(filename);
+}
+
+
+static void usage()
+{
+	errx(1, "usage: navigator {start|stop|status|fence|fencefile}");
+}
+
 int navigator_main(int argc, char *argv[])
 {
-	if (argc < 1)
-		errx(1, "usage: navigator {start|stop|status}");
+	if (argc < 2) {
+		usage();
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (navigator::g_navigator != nullptr)
+		if (navigator::g_navigator != nullptr) {
 			errx(1, "already running");
+		}
 
 		navigator::g_navigator = new Navigator;
 
-		if (navigator::g_navigator == nullptr)
+		if (navigator::g_navigator == nullptr) {
 			errx(1, "alloc failed");
+		}
 
 		if (OK != navigator::g_navigator->start()) {
 			delete navigator::g_navigator;
@@ -578,27 +1648,28 @@ int navigator_main(int argc, char *argv[])
 			err(1, "start failed");
 		}
 
-		exit(0);
+		return 0;
 	}
 
-	if (!strcmp(argv[1], "stop")) {
-		if (navigator::g_navigator == nullptr)
-			errx(1, "not running");
+	if (navigator::g_navigator == nullptr)
+		errx(1, "not running");
 
+	if (!strcmp(argv[1], "stop")) {
 		delete navigator::g_navigator;
 		navigator::g_navigator = nullptr;
-		exit(0);
-	}
 
-	if (!strcmp(argv[1], "status")) {
-		if (navigator::g_navigator) {
-			errx(0, "running");
+	} else if (!strcmp(argv[1], "status")) {
+		navigator::g_navigator->status();
 
-		} else {
-			errx(1, "not running");
-		}
+	} else if (!strcmp(argv[1], "fence")) {
+		navigator::g_navigator->add_fence_point(argc - 2, argv + 2);
+
+	} else if (!strcmp(argv[1], "fencefile")) {
+		navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME);
+
+	} else {
+		usage();
 	}
 
-	warnx("unrecognized command");
-	return 1;
+	return 0;
 }
diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp
new file mode 100644
index 000000000..e72caf98e
--- /dev/null
+++ b/src/modules/navigator/navigator_mission.cpp
@@ -0,0 +1,269 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file navigator_mission.cpp
+ * Helper class to access missions
+ */
+
+// #include <stdio.h>
+// #include <stdlib.h>
+// #include <string.h>
+// #include <unistd.h>
+
+#include <stdlib.h>
+#include <dataman/dataman.h>
+#include "navigator_mission.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+
+Mission::Mission() :
+
+	_offboard_dataman_id(-1),
+	_current_offboard_mission_index(0),
+	_current_onboard_mission_index(0),
+	_offboard_mission_item_count(0),
+	_onboard_mission_item_count(0),
+	_onboard_mission_allowed(false),
+	_current_mission_type(MISSION_TYPE_NONE)
+{}
+
+Mission::~Mission()
+{
+
+}
+
+void
+Mission::set_offboard_dataman_id(int new_id)
+{
+	_offboard_dataman_id = new_id;
+}
+
+void
+Mission::set_current_offboard_mission_index(int new_index)
+{
+	if (new_index != -1) {
+		_current_offboard_mission_index = (unsigned)new_index;
+	}
+}
+
+void
+Mission::set_current_onboard_mission_index(int new_index)
+{
+	if (new_index != -1) {
+		_current_onboard_mission_index = (unsigned)new_index;
+	}
+}
+
+void
+Mission::set_offboard_mission_count(unsigned new_count)
+{
+	_offboard_mission_item_count = new_count;
+}
+
+void
+Mission::set_onboard_mission_count(unsigned new_count)
+{
+	_onboard_mission_item_count = new_count;
+}
+
+void
+Mission::set_onboard_mission_allowed(bool allowed)
+{
+	_onboard_mission_allowed = allowed;
+}
+
+bool
+Mission::current_mission_available()
+{
+	return (current_onboard_mission_available() || current_offboard_mission_available());
+
+}
+
+bool
+Mission::next_mission_available()
+{
+	return (next_onboard_mission_available() || next_offboard_mission_available());
+}
+
+int
+Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index)
+{
+	/* try onboard mission first */
+	if (current_onboard_mission_available()) {
+
+		const ssize_t len = sizeof(struct mission_item_s);
+
+		if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return ERROR;
+		}
+
+		_current_mission_type = MISSION_TYPE_ONBOARD;
+		*onboard = true;
+		*index = _current_onboard_mission_index;
+
+		/* otherwise fallback to offboard */
+
+	} else if (current_offboard_mission_available()) {
+
+		dm_item_t dm_current;
+
+		if (_offboard_dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+
+		const ssize_t len = sizeof(struct mission_item_s);
+
+		if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			_current_mission_type = MISSION_TYPE_NONE;
+			return ERROR;
+		}
+
+		_current_mission_type = MISSION_TYPE_OFFBOARD;
+		*onboard = false;
+		*index = _current_offboard_mission_index;
+
+	} else {
+		/* happens when no more mission items can be added as a next item */
+		_current_mission_type = MISSION_TYPE_NONE;
+		return ERROR;
+	}
+
+	return OK;
+}
+
+int
+Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
+{
+	/* try onboard mission first */
+	if (next_onboard_mission_available()) {
+
+		const ssize_t len = sizeof(struct mission_item_s);
+
+		if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return ERROR;
+		}
+
+		/* otherwise fallback to offboard */
+
+	} else if (next_offboard_mission_available()) {
+
+		dm_item_t dm_current;
+
+		if (_offboard_dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+
+		const ssize_t len = sizeof(struct mission_item_s);
+
+		if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return ERROR;
+		}
+
+	} else {
+		/* happens when no more mission items can be added as a next item */
+		return ERROR;
+	}
+
+	return OK;
+}
+
+
+bool
+Mission::current_onboard_mission_available()
+{
+	return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed;
+}
+
+bool
+Mission::current_offboard_mission_available()
+{
+	return _offboard_mission_item_count > _current_offboard_mission_index;
+}
+
+bool
+Mission::next_onboard_mission_available()
+{
+	unsigned next = 0;
+
+	if (_current_mission_type != MISSION_TYPE_ONBOARD) {
+		next = 1;
+	}
+
+	return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed;
+}
+
+bool
+Mission::next_offboard_mission_available()
+{
+	unsigned next = 0;
+
+	if (_current_mission_type != MISSION_TYPE_OFFBOARD) {
+		next = 1;
+	}
+
+	return _offboard_mission_item_count > (_current_offboard_mission_index + next);
+}
+
+void
+Mission::move_to_next()
+{
+	switch (_current_mission_type) {
+	case MISSION_TYPE_ONBOARD:
+		_current_onboard_mission_index++;
+		break;
+
+	case MISSION_TYPE_OFFBOARD:
+		_current_offboard_mission_index++;
+		break;
+
+	case MISSION_TYPE_NONE:
+	default:
+		break;
+	}
+}
\ No newline at end of file
diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h
new file mode 100644
index 000000000..15d4e86bf
--- /dev/null
+++ b/src/modules/navigator/navigator_mission.h
@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file navigator_mission.h
+ * Helper class to access missions
+ */
+
+#ifndef NAVIGATOR_MISSION_H
+#define NAVIGATOR_MISSION_H
+
+#include <uORB/topics/mission.h>
+
+
+class __EXPORT Mission
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Mission();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~Mission();
+
+	void		set_offboard_dataman_id(int new_id);
+	void		set_current_offboard_mission_index(int new_index);
+	void		set_current_onboard_mission_index(int new_index);
+	void		set_offboard_mission_count(unsigned new_count);
+	void		set_onboard_mission_count(unsigned new_count);
+
+	void		set_onboard_mission_allowed(bool allowed);
+
+	bool		current_mission_available();
+	bool		next_mission_available();
+
+	int		get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index);
+	int		get_next_mission_item(struct mission_item_s *mission_item);
+
+	void		move_to_next();
+
+	void		add_home_pos(struct mission_item_s *new_mission_item);
+
+private:
+	bool		current_onboard_mission_available();
+	bool		current_offboard_mission_available();
+	bool		next_onboard_mission_available();
+	bool		next_offboard_mission_available();
+
+	int 		_offboard_dataman_id;
+	unsigned	_current_offboard_mission_index;
+	unsigned	_current_onboard_mission_index;
+	unsigned	_offboard_mission_item_count;		/** number of offboard mission items available */
+	unsigned	_onboard_mission_item_count;		/** number of onboard mission items available */
+
+	bool		_onboard_mission_allowed;
+
+	enum {
+		MISSION_TYPE_NONE,
+		MISSION_TYPE_ONBOARD,
+		MISSION_TYPE_OFFBOARD,
+	} 		_current_mission_type;
+};
+
+#endif
\ No newline at end of file
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 06df9a452..ec7a4e229 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -1,7 +1,9 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *           @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
@@ -38,6 +40,8 @@
  * Parameters defined by the navigator task.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
@@ -46,8 +50,72 @@
 
 /*
  * Navigator parameters, accessible via MAVLink
+ */
+
+/**
+ * Minimum altitude
+ *
+ * @group Navigation
+ */
+PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
+
+/**
+ * Waypoint acceptance radius.
+ *
+ * @group Navigation
+ */
+PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
+
+/**
+ * Loiter radius.
  *
+ * @group Navigation
  */
+PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
 
-PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f);
+/**
+ * @group Navigation
+ */
+PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
 
+/**
+ * Default take-off altitude.
+ *
+ * @group Navigation
+ */
+PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);	// default TAKEOFF altitude
+
+/**
+ * Landing altitude.
+ *
+ * Slowly descend from this altitude when landing.
+ *
+ * @group Navigation
+ */
+PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);		// slow descend from this altitude when landing
+
+/**
+ * Return-to-land altitude.
+ *
+ * Minimum altitude for going home in RTL mode.
+ *
+ * @group Navigation
+ */
+PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);		// min altitude for going home in RTL mode
+
+/**
+ * Return-to-land delay.
+ *
+ * Delay after descend before landing.
+ * If set to -1 the system will not land but loiter at NAV_LAND_ALT.
+ *
+ * @group Navigation
+ */
+PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);
+
+/**
+ * Enable parachute deployment.
+ *
+ * @group Navigation
+ */
+PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0);
diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h
new file mode 100644
index 000000000..6a1475c9b
--- /dev/null
+++ b/src/modules/navigator/navigator_state.h
@@ -0,0 +1,21 @@
+/*
+ * navigator_state.h
+ *
+ *  Created on: 27.01.2014
+ *      Author: ton
+ */
+
+#ifndef NAVIGATOR_STATE_H_
+#define NAVIGATOR_STATE_H_
+
+typedef enum {
+	NAV_STATE_NONE = 0,
+	NAV_STATE_READY,
+	NAV_STATE_LOITER,
+	NAV_STATE_MISSION,
+	NAV_STATE_RTL,
+	NAV_STATE_LAND,
+	NAV_STATE_MAX
+} nav_state_t;
+
+#endif /* NAVIGATOR_STATE_H_ */
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 1270ff5cf..0863ed0f2 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -71,15 +71,21 @@
 #include "position_estimator_inav_params.h"
 #include "inertial_filter.h"
 
+#define MIN_VALID_W 0.00001f
+
 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;
 
-static const hrt_abstime gps_timeout = 1000000;	// GPS timeout = 1s
-static const hrt_abstime flow_timeout = 1000000;	// optical flow timeout = 1s
+static const hrt_abstime gps_topic_timeout = 1000000;		// GPS topic timeout = 1s
+static const hrt_abstime flow_topic_timeout = 1000000;	// optical flow topic timeout = 1s
+static const hrt_abstime sonar_timeout = 150000;	// sonar timeout = 150ms
+static const hrt_abstime sonar_valid_timeout = 1000000;	// estimate sonar distance during this time after sonar loss
+static const hrt_abstime xy_src_timeout = 2000000;	// estimate position during this time after position sources loss
 static const uint32_t updates_counter_len = 1000000;
-static const uint32_t pub_interval = 4000;	// limit publish rate to 250 Hz
+static const uint32_t pub_interval = 10000;	// limit publish rate to 100 Hz
+static const float max_flow = 1.0f;	// max flow value that can be used, rad/s
 
 __EXPORT int position_estimator_inav_main(int argc, char *argv[]);
 
@@ -95,8 +101,7 @@ 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");
+	fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n");
 	exit(1);
 }
 
@@ -115,7 +120,7 @@ int position_estimator_inav_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "start")) {
 		if (thread_running) {
-			printf("position_estimator_inav already running\n");
+			warnx("already running");
 			/* this is not an error */
 			exit(0);
 		}
@@ -135,16 +140,23 @@ int position_estimator_inav_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
+		if (thread_running) {
+			warnx("stop");
+			thread_should_exit = true;
+
+		} else {
+			warnx("app not started");
+		}
+
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tposition_estimator_inav is running\n");
+			warnx("app is running");
 
 		} else {
-			printf("\tposition_estimator_inav not started\n");
+			warnx("app not started");
 		}
 
 		exit(0);
@@ -154,32 +166,90 @@ int position_estimator_inav_main(int argc, char *argv[])
 	exit(1);
 }
 
+void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) {
+	FILE *f = fopen("/fs/microsd/inav.log", "a");
+	if (f) {
+		char *s = malloc(256);
+		snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]);
+		fputs(f, s);
+		snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
+		fputs(f, s);
+		free(s);
+	}
+	fclose(f);
+}
+
 /****************************************************************************
  * main
  ****************************************************************************/
 int position_estimator_inav_thread_main(int argc, char *argv[])
 {
-	warnx("started.");
+	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 */
+	float baro_offset = 0.0f;		// baro offset for reference altitude, initialized on start, then adjusted
+	float surface_offset = 0.0f;	// ground level offset from reference altitude
+	float surface_offset_rate = 0.0f;	// surface offset change rate
 	float alt_avg = 0.0f;
 	bool landed = true;
 	hrt_abstime landed_time = 0;
-	bool flag_armed = false;
 
 	hrt_abstime accel_timestamp = 0;
 	hrt_abstime baro_timestamp = 0;
 
+	bool ref_inited = false;
+	hrt_abstime ref_init_start = 0;
+	const hrt_abstime ref_init_delay = 1000000;	// wait for 1s after 3D fix
+
+	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();
+	hrt_abstime pub_last = hrt_absolute_time();
+
+	hrt_abstime t_prev = 0;
+
+	/* 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 corr_acc[] = { 0.0f, 0.0f, 0.0f };	// N E D
+	float acc_bias[] = { 0.0f, 0.0f, 0.0f };	// body frame
+	float corr_baro = 0.0f;		// D
+	float corr_gps[3][2] = {
+		{ 0.0f, 0.0f },		// N (pos, vel)
+		{ 0.0f, 0.0f },		// E (pos, vel)
+		{ 0.0f, 0.0f },		// D (pos, vel)
+	};
+	float w_gps_xy = 1.0f;
+	float w_gps_z = 1.0f;
+	float corr_sonar = 0.0f;
+	float corr_sonar_filtered = 0.0f;
+
+	float corr_flow[] = { 0.0f, 0.0f };	// N E
+	float w_flow = 0.0f;
+
+	float sonar_prev = 0.0f;
+	hrt_abstime sonar_time = 0;			// time of last sonar measurement (not filtered)
+	hrt_abstime sonar_valid_time = 0;	// time of last sonar measurement used for correction (filtered)
+	hrt_abstime xy_src_time = 0;		// time of last available position data
+
+	bool gps_valid = false;			// GPS is valid
+	bool sonar_valid = false;		// sonar is valid
+	bool flow_valid = false;		// flow is valid
+	bool flow_accurate = false;		// flow should be accurate (this flag not updated if flow_valid == false)
+
 	/* declare and safely initialize all structs */
 	struct actuator_controls_s actuator;
 	memset(&actuator, 0, sizeof(actuator));
@@ -247,75 +317,30 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 					/* mean calculation over several measurements */
 					if (baro_init_cnt < baro_init_num) {
-						baro_alt0 += sensor.baro_alt_meter;
+						baro_offset += 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.ref_alt = baro_alt0;
-						local_pos.ref_timestamp = hrt_absolute_time();
+						baro_offset /= (float) baro_init_cnt;
+						warnx("baro offs: %.2f", baro_offset);
+						mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
 						local_pos.z_valid = true;
 						local_pos.v_z_valid = true;
-						local_pos.z_global = true;
+						global_pos.baro_valid = true;
 					}
 				}
 			}
 		}
 	}
 
-	bool ref_xy_inited = false;
-	hrt_abstime ref_xy_init_start = 0;
-	const hrt_abstime ref_xy_init_delay = 5000000;	// wait for 5s after 3D fix
-
-	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();
-	hrt_abstime pub_last = hrt_absolute_time();
-
-	/* 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[7] = {
-		{ .fd = parameter_update_sub, .events = POLLIN },
-		{ .fd = actuator_sub, .events = POLLIN },
-		{ .fd = armed_sub, .events = POLLIN },
+	struct pollfd fds[1] = {
 		{ .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, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate
+		int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
 		hrt_abstime t = hrt_absolute_time();
 
 		if (ret < 0) {
@@ -324,40 +349,49 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			continue;
 
 		} else if (ret > 0) {
+			/* act on attitude updates */
+
+			/* vehicle attitude */
+			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+			attitude_updates++;
+
+			bool updated;
+
 			/* parameter update */
-			if (fds[0].revents & POLLIN) {
-				/* read from param to clear updated flag */
+			orb_check(parameter_update_sub, &updated);
+
+			if (updated) {
 				struct parameter_update_s update;
-				orb_copy(ORB_ID(parameter_update), parameter_update_sub,
-					 &update);
-				/* update parameters */
+				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
 				parameters_update(&pos_inav_param_handles, &params);
 			}
 
 			/* actuator */
-			if (fds[1].revents & POLLIN) {
+			orb_check(actuator_sub, &updated);
+
+			if (updated) {
 				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
 			}
 
 			/* armed */
-			if (fds[2].revents & POLLIN) {
-				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-			}
+			orb_check(armed_sub, &updated);
 
-			/* vehicle attitude */
-			if (fds[3].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
-				attitude_updates++;
+			if (updated) {
+				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
 			}
 
 			/* sensor combined */
-			if (fds[4].revents & POLLIN) {
+			orb_check(sensor_combined_sub, &updated);
+
+			if (updated) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
 				if (sensor.accelerometer_timestamp != accel_timestamp) {
 					if (att.R_valid) {
-						/* correct accel bias, now only for Z */
-						sensor.accelerometer_m_s2[2] -= accel_bias[2];
+						/* correct accel bias */
+						sensor.accelerometer_m_s2[0] -= acc_bias[0];
+						sensor.accelerometer_m_s2[1] -= acc_bias[1];
+						sensor.accelerometer_m_s2[2] -= acc_bias[2];
 
 						/* transform acceleration vector from body frame to NED frame */
 						for (int i = 0; i < 3; i++) {
@@ -368,12 +402,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							}
 						}
 
-						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];
+						corr_acc[0] = accel_NED[0] - x_est[2];
+						corr_acc[1] = accel_NED[1] - y_est[2];
+						corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
 
 					} else {
-						memset(accel_corr, 0, sizeof(accel_corr));
+						memset(corr_acc, 0, sizeof(corr_acc));
 					}
 
 					accel_timestamp = sensor.accelerometer_timestamp;
@@ -381,180 +415,353 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				}
 
 				if (sensor.baro_timestamp != baro_timestamp) {
-					baro_corr = - sensor.baro_alt_meter - z_est[0];
+					corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
 					baro_timestamp = sensor.baro_timestamp;
 					baro_updates++;
 				}
 			}
 
 			/* optical flow */
-			if (fds[5].revents & POLLIN) {
+			orb_check(optical_flow_sub, &updated);
+
+			if (updated) {
 				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;
-								mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
-								local_pos.ref_alt = baro_alt0;
-								local_pos.ref_timestamp = hrt_absolute_time();
-								z_est[0] += sonar_corr;
-								sonar_corr = 0.0f;
-								sonar_corr_filtered = 0.0f;
-							}
+				if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) {
+					sonar_time = t;
+					sonar_prev = flow.ground_distance_m;
+					corr_sonar = flow.ground_distance_m + surface_offset + z_est[0];
+					corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt;
+
+					if (fabsf(corr_sonar) > params.sonar_err) {
+						/* correction is too large: spike or new ground level? */
+						if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) {
+							/* spike detected, ignore */
+							corr_sonar = 0.0f;
+							sonar_valid = false;
+
+						} else {
+							/* new ground level */
+							surface_offset -= corr_sonar;
+							surface_offset_rate = 0.0f;
+							corr_sonar = 0.0f;
+							corr_sonar_filtered = 0.0f;
+							sonar_valid_time = t;
+							sonar_valid = true;
+							local_pos.surface_bottom_timestamp = t;
+							mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", surface_offset);
 						}
+
+					} else {
+						/* correction is ok, use it */
+						sonar_valid_time = t;
+						sonar_valid = true;
+					}
+				}
+
+				float flow_q = flow.quality / 255.0f;
+				float dist_bottom = - z_est[0] - surface_offset;
+
+				if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) {
+					/* distance to surface */
+					float flow_dist = dist_bottom / att.R[2][2];
+					/* check if flow if too large for accurate measurements */
+					/* calculate estimated velocity in body frame */
+					float body_v_est[2] = { 0.0f, 0.0f };
+
+					for (int i = 0; i < 2; i++) {
+						body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1];
 					}
 
+					/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
+					flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
+							fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
+
+					/* convert raw flow to angular flow */
+					float flow_ang[2];
+					flow_ang[0] = flow.flow_raw_x * params.flow_k;
+					flow_ang[1] = flow.flow_raw_y * params.flow_k;
+					/* flow measurements vector */
+					float flow_m[3];
+					flow_m[0] = -flow_ang[0] * flow_dist;
+					flow_m[1] = -flow_ang[1] * flow_dist;
+					flow_m[2] = z_est[1];
+					/* velocity in NED */
+					float flow_v[2] = { 0.0f, 0.0f };
+
+					/* project measurements vector to NED basis, skip Z component */
+					for (int i = 0; i < 2; i++) {
+						for (int j = 0; j < 3; j++) {
+							flow_v[i] += att.R[i][j] * flow_m[j];
+						}
+					}
+
+					/* velocity correction */
+					corr_flow[0] = flow_v[0] - x_est[1];
+					corr_flow[1] = flow_v[1] - y_est[1];
+					/* adjust correction weight */
+					float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
+					w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist);
+
+					/* if flow is not accurate, reduce weight for it */
+					// TODO make this more fuzzy
+					if (!flow_accurate)
+						w_flow *= 0.05f;
+
+					flow_valid = true;
+
 				} else {
-					sonar_corr = 0.0f;
+					w_flow = 0.0f;
+					flow_valid = false;
 				}
 
 				flow_updates++;
 			}
 
 			/* vehicle GPS position */
-			if (fds[6].revents & POLLIN) {
+			orb_check(vehicle_gps_position_sub, &updated);
+
+			if (updated) {
 				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
 
-				if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) {
+				if (gps.fix_type >= 3) {
+					/* hysteresis for GPS quality */
+					if (gps_valid) {
+						if (gps.eph_m > 10.0f || gps.epv_m > 20.0f) {
+							gps_valid = false;
+							mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
+						}
+
+					} else {
+						if (gps.eph_m < 5.0f && gps.epv_m < 10.0f) {
+							gps_valid = true;
+							mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
+						}
+					}
+
+				} else {
+					gps_valid = false;
+				}
+
+				if (gps_valid) {
 					/* initialize reference position if needed */
-					if (!ref_xy_inited) {
-						/* require EPH < 10m */
-						if (gps.eph_m < 10.0f) {
-							if (ref_xy_init_start == 0) {
-								ref_xy_init_start = t;
-
-							} else if (t > ref_xy_init_start + ref_xy_init_delay) {
-								ref_xy_inited = true;
-								/* reference GPS position */
-								double lat = gps.lat * 1e-7;
-								double lon = gps.lon * 1e-7;
-
-								local_pos.ref_lat = gps.lat;
-								local_pos.ref_lon = gps.lon;
-								local_pos.ref_timestamp = t;
-
-								/* initialize projection */
-								map_projection_init(lat, lon);
-								warnx("init GPS: lat = %.10f,  lon = %.10f", lat, lon);
-								mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon);
-							}
-						} else {
-							ref_xy_init_start = 0;
+					if (!ref_inited) {
+						if (ref_init_start == 0) {
+							ref_init_start = t;
+
+						} else if (t > ref_init_start + ref_init_delay) {
+							ref_inited = true;
+							/* reference GPS position */
+							double lat = gps.lat * 1e-7;
+							double lon = gps.lon * 1e-7;
+							float alt = gps.alt * 1e-3;
+
+							local_pos.ref_lat = gps.lat;
+							local_pos.ref_lon = gps.lon;
+							local_pos.ref_alt = alt + z_est[0];
+							local_pos.ref_timestamp = t;
+
+							/* initialize projection */
+							map_projection_init(lat, lon);
+							warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
+							mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
 						}
 					}
 
-					if (ref_xy_inited) {
+					if (ref_inited) {
 						/* 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]));
 						/* calculate correction for position */
-						gps_corr[0][0] = gps_proj[0] - x_est[0];
-						gps_corr[1][0] = gps_proj[1] - y_est[0];
+						corr_gps[0][0] = gps_proj[0] - x_est[0];
+						corr_gps[1][0] = gps_proj[1] - y_est[0];
+						corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0];
 
 						/* calculate correction for velocity */
 						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];
+							corr_gps[0][1] = gps.vel_n_m_s - x_est[1];
+							corr_gps[1][1] = gps.vel_e_m_s - y_est[1];
+							corr_gps[2][1] = gps.vel_d_m_s - z_est[1];
 
 						} else {
-							gps_corr[0][1] = 0.0f;
-							gps_corr[1][1] = 0.0f;
+							corr_gps[0][1] = 0.0f;
+							corr_gps[1][1] = 0.0f;
+							corr_gps[2][1] = 0.0f;
 						}
+
+						w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m);
+						w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m);
 					}
 
 				} else {
 					/* no GPS lock */
-					memset(gps_corr, 0, sizeof(gps_corr));
-					ref_xy_init_start = 0;
+					memset(corr_gps, 0, sizeof(corr_gps));
+					ref_init_start = 0;
 				}
 
 				gps_updates++;
 			}
 		}
 
-		/* end of poll return value check */
+		/* check for timeout on FLOW topic */
+		if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) {
+			flow_valid = false;
+			sonar_valid = false;
+			warnx("FLOW timeout");
+			mavlink_log_info(mavlink_fd, "[inav] FLOW timeout");
+		}
+
+		/* check for timeout on GPS topic */
+		if (gps_valid && t > gps.timestamp_position + gps_topic_timeout) {
+			gps_valid = false;
+			warnx("GPS timeout");
+			mavlink_log_info(mavlink_fd, "[inav] GPS timeout");
+		}
+
+		/* check for sonar measurement timeout */
+		if (sonar_valid && t > sonar_time + sonar_timeout) {
+			corr_sonar = 0.0f;
+			sonar_valid = false;
+		}
 
 		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
+		dt = fmaxf(fminf(0.02, dt), 0.005);
 		t_prev = t;
 
-		/* reset ground level on arm */
-		if (armed.armed && !flag_armed) {
-			baro_alt0 -= z_est[0];
-			z_est[0] = 0.0f;
-			local_pos.ref_alt = baro_alt0;
-			local_pos.ref_timestamp = hrt_absolute_time();
-			mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0);
+		/* use GPS if it's valid and reference position initialized */
+		bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
+		bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
+		/* use flow if it's valid and (accurate or no GPS available) */
+		bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
+
+		/* try to estimate position during some time after position sources lost */
+		if (use_gps_xy || use_flow) {
+			xy_src_time = t;
+		}
+
+		bool can_estimate_xy = (t < xy_src_time + xy_src_timeout);
+
+		bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
+
+		if (dist_bottom_valid) {
+			/* surface distance prediction */
+			surface_offset += surface_offset_rate * dt;
+
+			/* surface distance correction */
+			if (sonar_valid) {
+				surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt;
+				surface_offset -= corr_sonar * params.w_z_sonar * dt;
+			}
+		}
+
+		float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
+		float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
+		float w_z_gps_p = params.w_z_gps_p * w_gps_z;
+
+		/* reduce GPS weight if optical flow is good */
+		if (use_flow && flow_accurate) {
+			w_xy_gps_p *= params.w_gps_flow;
+			w_xy_gps_v *= params.w_gps_flow;
+		}
+
+		/* baro offset correction */
+		if (use_gps_z) {
+			float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
+			baro_offset += offs_corr;
+			corr_baro += offs_corr;
 		}
 
-		/* 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;
+		/* accelerometer bias correction */
+		float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
+
+		if (use_gps_xy) {
+			accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;
+			accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;
+			accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;
+			accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;
+		}
+
+		if (use_gps_z) {
+			accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
+		}
+
+		if (use_flow) {
+			accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
+			accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
+		}
+
+		accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;
+
+		/* transform error vector from NED frame to body frame */
+		for (int i = 0; i < 3; i++) {
+			float c = 0.0f;
+
+			for (int j = 0; j < 3; j++) {
+				c += att.R[j][i] * accel_bias_corr[j];
+			}
+
+			acc_bias[i] += c * 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);
-
-		bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout;
-		bool flow_valid = false;	// TODO implement opt flow
-
-		/* try to estimate xy even if no absolute position source available,
-		 * if using optical flow velocity will be correct in this case */
-		bool can_estimate_xy = gps_valid || flow_valid;
+		inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
+		inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
+		inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc);
 
 		if (can_estimate_xy) {
 			/* inertial filter prediction for position */
 			inertial_filter_predict(dt, x_est);
 			inertial_filter_predict(dt, y_est);
 
+			if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
+				write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
+				thread_should_exit = true;
+			}
+
 			/* 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);
+			inertial_filter_correct(corr_acc[0], dt, x_est, 2, params.w_xy_acc);
+			inertial_filter_correct(corr_acc[1], dt, y_est, 2, params.w_xy_acc);
 
-			if (gps_valid) {
-				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 (use_flow) {
+				inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
+				inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
+			}
 
-				if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) {
-					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 (use_gps_xy) {
+				inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
+				inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);
+
+				if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) {
+					inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);
+					inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);
 				}
 			}
+
+			if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
+				write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
+				thread_should_exit = true;
+			}
 		}
 
 		/* detect land */
-		alt_avg += (z_est[0] - alt_avg) * dt / params.land_t;
-		float alt_disp = z_est[0] - alt_avg;
-		alt_disp = alt_disp * alt_disp;
+		alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t;
+		float alt_disp2 = - z_est[0] - alt_avg;
+		alt_disp2 = alt_disp2 * alt_disp2;
 		float land_disp2 = params.land_disp * params.land_disp;
 		/* get actual thrust output */
 		float thrust = armed.armed ? actuator.control[3] : 0.0f;
 
 		if (landed) {
-			if (alt_disp > land_disp2 && thrust > params.land_thr) {
+			if (alt_disp2 > land_disp2 && thrust > params.land_thr) {
 				landed = false;
 				landed_time = 0;
 			}
 
 		} else {
-			if (alt_disp < land_disp2 && thrust < params.land_thr) {
+			if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
 				if (landed_time == 0) {
 					landed_time = t;    // land detected first time
 
@@ -593,10 +800,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		if (t > pub_last + pub_interval) {
 			pub_last = t;
 			/* publish local position */
-			local_pos.timestamp = t;
-			local_pos.xy_valid = can_estimate_xy && gps_valid;
+			local_pos.xy_valid = can_estimate_xy && use_gps_xy;
 			local_pos.v_xy_valid = can_estimate_xy;
-			local_pos.xy_global = local_pos.xy_valid && gps_valid;	// will make sense when local position sources (e.g. vicon) will be implemented
+			local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
+			local_pos.z_global = local_pos.z_valid && use_gps_z;
 			local_pos.x = x_est[0];
 			local_pos.vx = x_est[1];
 			local_pos.y = y_est[0];
@@ -605,48 +812,56 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			local_pos.vz = z_est[1];
 			local_pos.landed = landed;
 			local_pos.yaw = att.yaw;
+			local_pos.dist_bottom_valid = dist_bottom_valid;
+
+			if (local_pos.dist_bottom_valid) {
+				local_pos.dist_bottom = -z_est[0] - surface_offset;
+				local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate;
+			}
+
+			local_pos.timestamp = t;
 
 			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
 
 			/* publish global position */
-			global_pos.valid = local_pos.xy_global;
+			global_pos.global_valid = local_pos.xy_global;
 
 			if (local_pos.xy_global) {
 				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.lat = est_lat;
+				global_pos.lon = est_lon;
 				global_pos.time_gps_usec = gps.time_gps_usec;
 			}
 
 			/* set valid values even if position is not valid */
 			if (local_pos.v_xy_valid) {
-				global_pos.vx = local_pos.vx;
-				global_pos.vy = local_pos.vy;
-			}
-
-			if (local_pos.z_valid) {
-				global_pos.relative_alt = -local_pos.z;
+				global_pos.vel_n = local_pos.vx;
+				global_pos.vel_e = local_pos.vy;
 			}
 
 			if (local_pos.z_global) {
 				global_pos.alt = local_pos.ref_alt - local_pos.z;
 			}
 
+			if (local_pos.z_valid) {
+				global_pos.baro_alt = baro_offset - local_pos.z;
+			}
+
 			if (local_pos.v_z_valid) {
-				global_pos.vz = local_pos.vz;
+				global_pos.vel_d = local_pos.vz;
 			}
+
 			global_pos.yaw = local_pos.yaw;
 
 			global_pos.timestamp = t;
 
 			orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
 		}
-		flag_armed = armed.armed;
 	}
 
-	warnx("exiting.");
-	mavlink_log_info(mavlink_fd, "[inav] exiting");
+	warnx("stopped");
+	mavlink_log_info(mavlink_fd, "[inav] stopped");
 	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
index 4f9ddd009..b71f9472f 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -40,16 +40,19 @@
 
 #include "position_estimator_inav_params.h"
 
-PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f);
-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_W_Z_BARO, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
+PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
+PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
+PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f);
+PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
 PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
 PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
@@ -57,15 +60,18 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f);
 
 int parameters_init(struct position_estimator_inav_param_handles *h)
 {
-	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_z_baro = param_find("INAV_W_Z_BARO");
+	h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
+	h->w_z_acc = param_find("INAV_W_Z_ACC");
+	h->w_z_sonar = param_find("INAV_W_Z_SONAR");
+	h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
+	h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
+	h->w_xy_acc = param_find("INAV_W_XY_ACC");
+	h->w_xy_flow = param_find("INAV_W_XY_FLOW");
+	h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
 	h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
 	h->flow_k = param_find("INAV_FLOW_K");
+	h->flow_q_min = param_find("INAV_FLOW_Q_MIN");
 	h->sonar_filt = param_find("INAV_SONAR_FILT");
 	h->sonar_err = param_find("INAV_SONAR_ERR");
 	h->land_t = param_find("INAV_LAND_T");
@@ -77,15 +83,18 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
 
 int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
 {
-	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_z_baro, &(p->w_z_baro));
+	param_get(h->w_z_gps_p, &(p->w_z_gps_p));
+	param_get(h->w_z_acc, &(p->w_z_acc));
+	param_get(h->w_z_sonar, &(p->w_z_sonar));
+	param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
+	param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
+	param_get(h->w_xy_acc, &(p->w_xy_acc));
+	param_get(h->w_xy_flow, &(p->w_xy_flow));
+	param_get(h->w_gps_flow, &(p->w_gps_flow));
 	param_get(h->w_acc_bias, &(p->w_acc_bias));
 	param_get(h->flow_k, &(p->flow_k));
+	param_get(h->flow_q_min, &(p->flow_q_min));
 	param_get(h->sonar_filt, &(p->sonar_filt));
 	param_get(h->sonar_err, &(p->sonar_err));
 	param_get(h->land_t, &(p->land_t));
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
index 61570aea7..e2be079d3 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -41,15 +41,18 @@
 #include <systemlib/param/param.h>
 
 struct position_estimator_inav_params {
-	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_z_baro;
+	float w_z_gps_p;
+	float w_z_acc;
+	float w_z_sonar;
+	float w_xy_gps_p;
+	float w_xy_gps_v;
+	float w_xy_acc;
+	float w_xy_flow;
+	float w_gps_flow;
 	float w_acc_bias;
 	float flow_k;
+	float flow_q_min;
 	float sonar_filt;
 	float sonar_err;
 	float land_t;
@@ -58,15 +61,18 @@ struct position_estimator_inav_params {
 };
 
 struct position_estimator_inav_param_handles {
-	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_z_baro;
+	param_t w_z_gps_p;
+	param_t w_z_acc;
+	param_t w_z_sonar;
+	param_t w_xy_gps_p;
+	param_t w_xy_gps_v;
+	param_t w_xy_acc;
+	param_t w_xy_flow;
+	param_t w_gps_flow;
 	param_t w_acc_bias;
 	param_t flow_k;
+	param_t flow_q_min;
 	param_t sonar_filt;
 	param_t sonar_err;
 	param_t land_t;
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ac6a120fa..5cde94431 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -72,7 +72,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
@@ -82,6 +82,7 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/esc_status.h>
+#include <uORB/topics/telemetry_status.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -748,7 +749,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_local_position_s local_pos;
 		struct vehicle_local_position_setpoint_s local_pos_sp;
 		struct vehicle_global_position_s global_pos;
-		struct vehicle_global_position_setpoint_s global_pos_sp;
+		struct position_setpoint_triplet_s triplet;
 		struct vehicle_gps_position_s gps_pos;
 		struct vehicle_vicon_position_s vicon_pos;
 		struct optical_flow_s flow;
@@ -758,6 +759,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct esc_status_s esc;
 		struct vehicle_global_velocity_setpoint_s global_vel_sp;
 		struct battery_status_s battery;
+		struct telemetry_status_s telemetry;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -774,7 +776,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int local_pos_sub;
 		int local_pos_sp_sub;
 		int global_pos_sub;
-		int global_pos_sp_sub;
+		int triplet_sub;
 		int gps_pos_sub;
 		int vicon_pos_sub;
 		int flow_sub;
@@ -783,6 +785,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int esc_sub;
 		int global_vel_sp_sub;
 		int battery_sub;
+		int telemetry_sub;
 	} subs;
 
 	/* log message buffer: header + body */
@@ -810,6 +813,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_ESC_s log_ESC;
 			struct log_GVSP_s log_GVSP;
 			struct log_BATT_s log_BATT;
+			struct log_DIST_s log_DIST;
+			struct log_TELE_s log_TELE;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -898,8 +903,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fdsc_count++;
 
 	/* --- GLOBAL POSITION SETPOINT--- */
-	subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
-	fds[fdsc_count].fd = subs.global_pos_sp_sub;
+	subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+	fds[fdsc_count].fd = subs.triplet_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
@@ -945,6 +950,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- TELEMETRY STATUS --- */
+	subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
+	fds[fdsc_count].fd = subs.telemetry_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.
@@ -973,6 +984,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 	hrt_abstime barometer_timestamp = 0;
 	hrt_abstime differential_pressure_timestamp = 0;
 
+	/* track changes in distance status */
+	bool dist_bottom_present = false;
+
 	/* enable logging on start if needed */
 	if (log_on_start) {
 		/* check GPS topic to get GPS time */
@@ -989,7 +1003,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* decide use usleep() or blocking poll() */
 		bool use_sleep = sleep_delay > 0 && logging_enabled;
 
-		/* poll all topics if logging enabled or only management (first 2) if not */
+		/* poll all topics if logging enabled or only management (first 3) if not */
 		int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout);
 
 		/* handle the poll result */
@@ -1008,6 +1022,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			/* --- VEHICLE COMMAND - LOG MANAGEMENT --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
+
 				handle_command(&buf.cmd);
 				handled_topics++;
 			}
@@ -1038,7 +1053,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				continue;
 			}
 
-			ifds = 1;	// begin from fds[1] again
+			ifds = 1;	// begin from VEHICLE STATUS again
 
 			pthread_mutex_lock(&logbuffer_mutex);
 
@@ -1049,10 +1064,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 			/* --- VEHICLE STATUS --- */
 			if (fds[ifds++].revents & POLLIN) {
-				// Don't orb_copy, it's already done few lines above
+				/* don't orb_copy, it's already done few lines above */
 				log_msg.msg_type = LOG_STAT_MSG;
 				log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
-				log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
 				log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
 				log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
 				log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
@@ -1062,7 +1076,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 			/* --- GPS POSITION --- */
 			if (fds[ifds++].revents & POLLIN) {
-				// Don't orb_copy, it's already done few lines above
+				/* don't orb_copy, it's already done few lines above */
 				log_msg.msg_type = LOG_GPS_MSG;
 				log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec;
 				log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type;
@@ -1143,6 +1157,9 @@ 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.gx = buf.att.g_comp[0];
+				log_msg.body.log_ATT.gy = buf.att.g_comp[1];
+				log_msg.body.log_ATT.gz = buf.att.g_comp[2];
 				LOGBUFFER_WRITE_AND_COUNT(ATT);
 			}
 
@@ -1203,6 +1220,17 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
 				log_msg.body.log_LPOS.landed = buf.local_pos.landed;
 				LOGBUFFER_WRITE_AND_COUNT(LPOS);
+
+				if (buf.local_pos.dist_bottom_valid) {
+					dist_bottom_present = true;
+				}
+				if (dist_bottom_present) {
+					log_msg.msg_type = LOG_DIST_MSG;
+					log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom;
+					log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate;
+					log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
+					LOGBUFFER_WRITE_AND_COUNT(DIST);
+				}
 			}
 
 			/* --- LOCAL POSITION SETPOINT --- */
@@ -1220,31 +1248,30 @@ int sdlog2_thread_main(int argc, char *argv[])
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
 				log_msg.msg_type = LOG_GPOS_MSG;
-				log_msg.body.log_GPOS.lat = buf.global_pos.lat;
-				log_msg.body.log_GPOS.lon = buf.global_pos.lon;
+				log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7;
+				log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7;
 				log_msg.body.log_GPOS.alt = buf.global_pos.alt;
-				log_msg.body.log_GPOS.vel_n = buf.global_pos.vx;
-				log_msg.body.log_GPOS.vel_e = buf.global_pos.vy;
-				log_msg.body.log_GPOS.vel_d = buf.global_pos.vz;
+				log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n;
+				log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e;
+				log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
+				log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt;
+				log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0);
 				LOGBUFFER_WRITE_AND_COUNT(GPOS);
 			}
 
 			/* --- GLOBAL POSITION SETPOINT --- */
 			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp);
+				orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet);
 				log_msg.msg_type = LOG_GPSP_MSG;
-				log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative;
-				log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat;
-				log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon;
-				log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude;
-				log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw;
-				log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius;
-				log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction;
-				log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd;
-				log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1;
-				log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2;
-				log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3;
-				log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4;
+				log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
+				log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
+				log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
+				log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
+				log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
+				log_msg.body.log_GPSP.type = buf.triplet.current.type;
+				log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
+				log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
+				log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
 				LOGBUFFER_WRITE_AND_COUNT(GPSP);
 			}
 
@@ -1330,6 +1357,20 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(BATT);
 			}
 
+			/* --- TELEMETRY --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry);
+				log_msg.msg_type = LOG_TELE_MSG;
+				log_msg.body.log_TELE.rssi = buf.telemetry.rssi;
+				log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi;
+				log_msg.body.log_TELE.noise = buf.telemetry.noise;
+				log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise;
+				log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors;
+				log_msg.body.log_TELE.fixed = buf.telemetry.fixed;
+				log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf;
+				LOGBUFFER_WRITE_AND_COUNT(TELE);
+			}
+
 			/* signal the other thread new data, but not yet unlock */
 			if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
 				/* only request write if several packets can be written at once */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index a784a1f30..16bfc355d 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -57,6 +57,9 @@ struct log_ATT_s {
 	float roll_rate;
 	float pitch_rate;
 	float yaw_rate;
+	float gx;
+	float gy;
+	float gz;
 };
 
 /* --- ATSP - ATTITUDE SET POINT --- */
@@ -146,7 +149,6 @@ struct log_ATTC_s {
 #define LOG_STAT_MSG 10
 struct log_STAT_s {
 	uint8_t main_state;
-	uint8_t navigation_state;
 	uint8_t arming_state;
 	float battery_remaining;
 	uint8_t battery_warning;
@@ -202,23 +204,22 @@ struct log_GPOS_s {
 	float vel_n;
 	float vel_e;
 	float vel_d;
+	float baro_alt;
+	uint8_t flags;
 };
 
 /* --- GPSP - GLOBAL POSITION SETPOINT --- */
 #define LOG_GPSP_MSG 17
 struct log_GPSP_s {
-	uint8_t altitude_is_relative;
+	uint8_t nav_state;
 	int32_t lat;
 	int32_t lon;
-	float altitude;
+	float alt;
 	float yaw;
+	uint8_t type;
 	float loiter_radius;
 	int8_t loiter_direction;
-	uint8_t nav_cmd;
-	float param1;
-	float param2;
-	float param3;
-	float param4;
+	float pitch_min;
 };
 
 /* --- ESC - ESC STATE --- */
@@ -255,6 +256,28 @@ struct log_BATT_s {
 	float discharged;
 };
 
+/* --- DIST - DISTANCE TO SURFACE --- */
+#define LOG_DIST_MSG 21
+struct log_DIST_s {
+	float bottom;
+	float bottom_rate;
+	uint8_t flags;
+};
+
+/* --- TELE - TELEMETRY STATUS --- */
+#define LOG_TELE_MSG 22
+struct log_TELE_s {
+    uint8_t rssi;
+    uint8_t remote_rssi;
+    uint8_t noise;
+    uint8_t remote_noise;
+    uint16_t rxerrors;
+    uint16_t fixed;
+    uint8_t txbuf;
+};
+
+/********** SYSTEM MESSAGES, ID > 0x80 **********/
+
 /* --- TIME - TIME STAMP --- */
 #define LOG_TIME_MSG 129
 struct log_TIME_s {
@@ -280,7 +303,7 @@ struct log_PARM_s {
 /* construct list of all message formats */
 static const struct log_format_s log_formats[] = {
 	/* business-level messages, ID < 0x80 */
-	LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
+	LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
 	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"),
@@ -288,17 +311,20 @@ 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, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"),
+	LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
 	LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	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(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
+	LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
 	LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
+	LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
+	LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
+
 	/* system-level messages, ID >= 0x80 */
 	// FMT: don't write format of format message, it's useless
 	LOG_FORMAT(TIME, "Q", "StartTime"),
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 30659fd3a..288c6e00a 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -42,13 +42,10 @@
  */
 
 #include <nuttx/config.h>
-
 #include <systemlib/param/param.h>
 
 /**
- * Gyro X offset
- *
- * This is an X-axis offset for the gyro. Adjust it according to the calibration data.
+ * Gyro X-axis offset
  *
  * @min -10.0
  * @max 10.0
@@ -57,7 +54,7 @@
 PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
 
 /**
- * Gyro Y offset
+ * Gyro Y-axis offset
  *
  * @min -10.0
  * @max 10.0
@@ -66,7 +63,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
 
 /**
- * Gyro Z offset
+ * Gyro Z-axis offset
  *
  * @min -5.0
  * @max 5.0
@@ -75,9 +72,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
 
 /**
- * Gyro X scaling
- *
- * X-axis scaling.
+ * Gyro X-axis scaling factor
  *
  * @min -1.5
  * @max 1.5
@@ -86,9 +81,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
 
 /**
- * Gyro Y scaling
- *
- * Y-axis scaling.
+ * Gyro Y-axis scaling factor
  *
  * @min -1.5
  * @max 1.5
@@ -97,9 +90,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
 
 /**
- * Gyro Z scaling
- *
- * Z-axis scaling.
+ * Gyro Z-axis scaling factor
  *
  * @min -1.5
  * @max 1.5
@@ -107,10 +98,9 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
 
+
 /**
- * Magnetometer X offset
- *
- * This is an X-axis offset for the magnetometer.
+ * Magnetometer X-axis offset
  *
  * @min -500.0
  * @max 500.0
@@ -119,9 +109,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
 
 /**
- * Magnetometer Y offset
- *
- * This is an Y-axis offset for the magnetometer.
+ * Magnetometer Y-axis offset
  *
  * @min -500.0
  * @max 500.0
@@ -130,9 +118,7 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
 
 /**
- * Magnetometer Z offset
- *
- * This is an Z-axis offset for the magnetometer.
+ * Magnetometer Z-axis offset
  *
  * @min -500.0
  * @max 500.0
@@ -140,24 +126,134 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
 
+/**
+ * Magnetometer X-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f);
+
+/**
+ * Magnetometer Y-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f);
+
+/**
+ * Magnetometer Z-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f);
 
+
+/**
+ * Accelerometer X-axis offset
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f);
+
+/**
+ * Accelerometer Y-axis offset
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f);
+
+/**
+ * Accelerometer Z-axis offset
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f);
 
+/**
+ * Accelerometer X-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
+
+/**
+ * Accelerometer Y-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
+
+/**
+ * Accelerometer Z-axis scaling factor
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
+
+/**
+ * Differential pressure sensor offset
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
+
+/**
+ * Differential pressure sensor analog enabled
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
 
+
+/**
+ * Board rotation
+ *
+ * This parameter defines the rotation of the FMU board relative to the platform.
+ * Possible values are:
+ *    0 = No rotation
+ *    1 = Yaw 45°
+ *    2 = Yaw 90°
+ *    3 = Yaw 135°
+ *    4 = Yaw 180°
+ *    5 = Yaw 225°
+ *    6 = Yaw 270°
+ *    7 = Yaw 315°
+ *    8 = Roll 180°
+ *    9 = Roll 180°, Yaw 45°
+ *   10 = Roll 180°, Yaw 90°
+ *   11 = Roll 180°, Yaw 135°
+ *   12 = Pitch 180°
+ *   13 = Roll 180°, Yaw 225°
+ *   14 = Roll 180°, Yaw 270°
+ *   15 = Roll 180°, Yaw 315°
+ *   16 = Roll 90°
+ *   17 = Roll 90°, Yaw 45°
+ *   18 = Roll 90°, Yaw 90°
+ *   19 = Roll 90°, Yaw 135°
+ *   20 = Roll 270°
+ *   21 = Roll 270°, Yaw 45°
+ *   22 = Roll 270°, Yaw 90°
+ *   23 = Roll 270°, Yaw 135°
+ *   24 = Pitch 90°
+ *   25 = Pitch 270°
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
+
+/**
+ * External magnetometer rotation
+ *
+ * This parameter defines the rotation of the external magnetometer relative
+ * to the platform (not relative to the FMU).
+ * See SENS_BOARD_ROT for possible values.
+ *
+ * @group Sensor Calibration
+ */
 PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
 
+
 /**
  * RC Channel 1 Minimum
  *
@@ -367,20 +463,52 @@ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f);
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
 #endif
-PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */
 
+/**
+ * DSM binding trigger.
+ *
+ * -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind
+ *
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_DSM_BIND, -1);
+
+
+/**
+ * Scaling factor for battery voltage sensor on PX4IO.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
+
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+/**
+ * Scaling factor for battery voltage sensor on FMU v2.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
 #else
-/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
-/* PX4IOAR: 0.00838095238 */
-/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */
-/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
+/**
+ * Scaling factor for battery voltage sensor on FMU v1.
+ *
+ * FMUv1 standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659
+ * FMUv1 with PX4IO: 0.00459340659
+ * FMUv1 with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) = 0.00838095238
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
 #endif
+
+/**
+ * Scaling factor for battery current sensor.
+ *
+ * @group Battery Calibration
+ */
 PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124);	/* scaling for 3DR power brick */
 
+
 /**
  * Roll control channel mapping.
  *
@@ -446,22 +574,127 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
  * @group Radio Calibration
  */
 PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
+
+/**
+ * Return switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
+
+/**
+ * Assist switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
+
+/**
+ * Mission switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
 
 //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
+/**
+ * Flaps channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
 
-PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);	/**< default function: camera pitch */
+/**
+ * Auxiliary switch 1 channel mapping.
+ *
+ * Default function: Camera pitch
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);
+
+/**
+ * Auxiliary switch 2 channel mapping.
+ *
+ * Default function: Camera roll
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_AUX2, 0);	/**< default function: camera roll */
-PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);	/**< default function: camera azimuth / yaw */
 
+/**
+ * Auxiliary switch 3 channel mapping.
+ *
+ * Default function: Camera azimuth / yaw
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);
+
+
+/**
+ * Roll scaling factor
+ *
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
+
+/**
+ * Pitch scaling factor
+ *
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
+
+/**
+ * Yaw scaling factor
+ *
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
 
-PARAM_DEFINE_INT32(RC_FS_CH, 0);	/**< RC failsafe channel, 0 = disable */
-PARAM_DEFINE_INT32(RC_FS_MODE, 0);	/**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */
-PARAM_DEFINE_FLOAT(RC_FS_THR, 800);	/**< RC failsafe PWM threshold */
+
+/**
+ * Failsafe channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_FS_CH, 0);
+
+/**
+ * Failsafe channel mode.
+ *
+ * 0 = too low means signal loss,
+ * 1 = too high means signal loss
+ *
+ * @min 0
+ * @max 1
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_FS_MODE, 0);
+
+/**
+ * Failsafe channel PWM threshold.
+ *
+ * @min 0
+ * @max 1
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_FLOAT(RC_FS_THR, 800);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 46eb7cc85..dc5bd95c6 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -211,8 +211,8 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 
-	math::Matrix	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
-	math::Matrix	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
+	math::Matrix<3,3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
+	math::Matrix<3,3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
 	bool		_mag_is_external;		/**< true if the active mag is on an external board */
 
 	uint64_t _battery_discharged;			/**< battery discharged current in mA*ms */
@@ -470,8 +470,6 @@ Sensors::Sensors() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
 
-	_board_rotation(3, 3),
-	_external_mag_rotation(3, 3),
 	_mag_is_external(false),
 	_battery_discharged(0),
 	_battery_current_timestamp(0)
@@ -639,41 +637,43 @@ Sensors::parameters_update()
 	if (!rc_valid)
 		warnx("WARNING     WARNING     WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
 
+	const char *paramerr = "FAIL PARM LOAD";
+
 	/* channel mapping */
 	if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
-		warnx("Failed getting roll chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) {
-		warnx("Failed getting pitch chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) {
-		warnx("Failed getting yaw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
-		warnx("Failed getting throttle chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
-		warnx("Failed getting mode sw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
-		warnx("Failed getting return sw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
-		warnx("Failed getting assisted sw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
-		warnx("Failed getting mission sw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
-		warnx("Failed getting flaps chan index");
+		warnx(paramerr);
 	}
 
 //	if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
@@ -745,12 +745,12 @@ Sensors::parameters_update()
 
 	/* scaling of ADC ticks to battery voltage */
 	if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
-		warnx("Failed updating voltage scaling param");
+		warnx(paramerr);
 	}
 
 	/* scaling of ADC ticks to battery current */
 	if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
-		warnx("Failed updating current scaling param");
+		warnx(paramerr);
 	}
 
 	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
@@ -931,7 +931,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
-		math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
+		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
 		vect = _board_rotation * vect;
 
 		raw.accelerometer_m_s2[0] = vect(0);
@@ -957,7 +957,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
-		math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
+		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
 		vect = _board_rotation * vect;
 
 		raw.gyro_rad_s[0] = vect(0);
@@ -983,7 +983,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
-		math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
+		math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
 
 		if (_mag_is_external)
 			vect = _external_mag_rotation * vect;
@@ -1416,24 +1416,26 @@ Sensors::rc_poll()
 			}
 		}
 
+		/* mode switch input */
 		if (_rc.function[MODE] >= 0) {
 			manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
 		}
 
+		/* assisted switch input */
+		if (_rc.function[ASSISTED] >= 0) {
+			manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
+		}
+
+		/* mission switch input */
 		if (_rc.function[MISSION] >= 0) {
 			manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
 		}
 
-		/* land switch input */
+		/* return switch input */
 		if (_rc.function[RETURN] >= 0) {
 			manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
 		}
 
-		/* assisted switch input */
-		if (_rc.function[ASSISTED] >= 0) {
-			manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
-		}
-
 //		if (_rc.function[OFFBOARD_MODE] >= 0) {
 //			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
 //		}
@@ -1648,17 +1650,17 @@ int sensors_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (sensors::g_sensors != nullptr)
-			errx(0, "sensors task already running");
+			errx(0, "already running");
 
 		sensors::g_sensors = new Sensors;
 
 		if (sensors::g_sensors == nullptr)
-			errx(1, "sensors task alloc failed");
+			errx(1, "alloc failed");
 
 		if (OK != sensors::g_sensors->start()) {
 			delete sensors::g_sensors;
 			sensors::g_sensors = nullptr;
-			err(1, "sensors task start failed");
+			err(1, "start failed");
 		}
 
 		exit(0);
@@ -1666,7 +1668,7 @@ int sensors_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "stop")) {
 		if (sensors::g_sensors == nullptr)
-			errx(1, "sensors task not running");
+			errx(1, "not running");
 
 		delete sensors::g_sensors;
 		sensors::g_sensors = nullptr;
@@ -1675,10 +1677,10 @@ int sensors_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (sensors::g_sensors) {
-			errx(0, "task is running");
+			errx(0, "is running");
 
 		} else {
-			errx(1, "task is not running");
+			errx(1, "not running");
 		}
 	}
 
diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp
index cce46bf5f..20b1f18ed 100644
--- a/src/modules/systemlib/mixer/mixer.cpp
+++ b/src/modules/systemlib/mixer/mixer.cpp
@@ -171,7 +171,6 @@ NullMixer *
 NullMixer::from_text(const char *buf, unsigned &buflen)
 {
 	NullMixer *nm = nullptr;
-	const char *end = buf + buflen;
 
 	/* enforce that the mixer ends with space or a new line */
 	for (int i = buflen - 1; i >= 0; i--) {
diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c
index a55ddf8a3..b05273c0d 100644
--- a/src/modules/systemlib/mixer/mixer_load.c
+++ b/src/modules/systemlib/mixer/mixer_load.c
@@ -52,7 +52,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 	/* open the mixer definition file */
 	fp = fopen(fname, "r");
 	if (fp == NULL) {
-		return 1;
+		warnx("file not found");
+		return -1;
 	}
 
 	/* read valid lines from the file into a buffer */
@@ -88,7 +89,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 
 		/* if the line is too long to fit in the buffer, bail */
 		if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
-			return 1;
+			warnx("line too long");
+			return -1;
 		}
 
 		/* add the line to the buffer */
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 77c952f52..6a4e9392a 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -39,7 +39,7 @@
 /**
  * @file pid.c
  *
- * Implementation of generic PID control interface.
+ * Implementation of generic PID controller.
  *
  * @author Laurens Mackay <mackayl@student.ethz.ch>
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
@@ -53,24 +53,21 @@
 
 #define SIGMA 0.000001f
 
-__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
-		       float limit, uint8_t mode, float dt_min)
+__EXPORT void pid_init(PID_t *pid, uint8_t mode, float dt_min)
 {
-	pid->kp = kp;
-	pid->ki = ki;
-	pid->kd = kd;
-	pid->intmax = intmax;
-	pid->limit = limit;
 	pid->mode = mode;
 	pid->dt_min = dt_min;
-	pid->count = 0.0f;
-	pid->saturated = 0.0f;
-	pid->last_output = 0.0f;
-	pid->sp = 0.0f;
-	pid->error_previous = 0.0f;
+	pid->kp = 0.0f;
+	pid->ki = 0.0f;
+	pid->kd = 0.0f;
 	pid->integral = 0.0f;
+	pid->integral_limit = 0.0f;
+	pid->output_limit = 0.0f;
+	pid->error_previous = 0.0f;
+	pid->last_output = 0.0f;
 }
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
+
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
 {
 	int ret = 0;
 
@@ -95,15 +92,15 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
 		ret = 1;
 	}
 
-	if (isfinite(intmax)) {
-		pid->intmax = intmax;
+	if (isfinite(integral_limit)) {
+		pid->integral_limit = integral_limit;
 
 	}  else {
 		ret = 1;
 	}
 
-	if (isfinite(limit)) {
-		pid->limit = limit;
+	if (isfinite(output_limit)) {
+		pid->output_limit = output_limit;
 
 	}  else {
 		ret = 1;
@@ -112,42 +109,18 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
 	return ret;
 }
 
-//void pid_set(PID_t *pid, float sp)
-//{
-//	pid->sp = sp;
-//	pid->error_previous = 0;
-//	pid->integral = 0;
-//}
-
-/**
- *
- * @param pid
- * @param val
- * @param dt
- * @return
- */
 __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
 {
-	/*  error = setpoint - actual_position
-	 integral = integral + (error*dt)
-	 derivative = (error - previous_error)/dt
-	 output = (Kp*error) + (Ki*integral) + (Kd*derivative)
-	 previous_error = error
-	 wait(dt)
-	 goto start
-	 */
-
 	if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) {
 		return pid->last_output;
 	}
 
 	float i, d;
-	pid->sp = sp;
 
-	// Calculated current error value
-	float error = pid->sp - val;
+	/* current error value */
+	float error = sp - val;
 
-	// Calculate or measured current error derivative
+	/* current error derivative */
 	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
 		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
 		pid->error_previous = error;
@@ -167,39 +140,34 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		d = 0.0f;
 	}
 
-	if (pid->ki > 0.0f) {
+	/* calculate PD output */
+	float output = (error * pid->kp) + (d * pid->kd);
+
+	if (pid->ki > SIGMA) {
 		// Calculate the error integral and check for saturation
 		i = pid->integral + (error * dt);
 
-		if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) ||
-		    fabsf(i) > pid->intmax) {
-			i = pid->integral;		// If saturated then do not update integral value
-			pid->saturated = 1;
-
-		} else {
-			if (!isfinite(i)) {
-				i = 0.0f;
+		/* check for saturation */
+		if (isfinite(i)) {
+			if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
+			    fabsf(i) <= pid->integral_limit) {
+				/* not saturated, use new integral value */
+				pid->integral = i;
 			}
-
-			pid->integral = i;
-			pid->saturated = 0;
 		}
 
-	} else {
-		i = 0.0f;
-		pid->saturated = 0;
+		/* add I component to output */
+		output += pid->integral * pid->ki;
 	}
 
-	// Calculate the output.  Limit output magnitude to pid->limit
-	float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
-
+	/* limit output */
 	if (isfinite(output)) {
-		if (pid->limit > SIGMA) {
-			if (output > pid->limit) {
-				output = pid->limit;
+		if (pid->output_limit > SIGMA) {
+			if (output > pid->output_limit) {
+				output = pid->output_limit;
 
-			} else if (output < -pid->limit) {
-				output = -pid->limit;
+			} else if (output < -pid->output_limit) {
+				output = -pid->output_limit;
 			}
 		}
 
@@ -212,5 +180,5 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 
 __EXPORT void pid_reset_integral(PID_t *pid)
 {
-	pid->integral = 0;
+	pid->integral = 0.0f;
 }
diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h
index eca228464..e8b1aac4f 100644
--- a/src/modules/systemlib/pid/pid.h
+++ b/src/modules/systemlib/pid/pid.h
@@ -39,7 +39,7 @@
 /**
  * @file pid.h
  *
- * Definition of generic PID control interface.
+ * Definition of generic PID controller.
  *
  * @author Laurens Mackay <mackayl@student.ethz.ch>
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
@@ -55,38 +55,35 @@
 
 __BEGIN_DECLS
 
-/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error
- * val_dot in pid_calculate() will be ignored */
-#define PID_MODE_DERIVATIV_CALC	0
-/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored
- * val_dot in pid_calculate() will be ignored */
-#define PID_MODE_DERIVATIV_CALC_NO_SP	1
-/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
-#define PID_MODE_DERIVATIV_SET	2
-// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
-#define PID_MODE_DERIVATIV_NONE 9
+typedef enum PID_MODE {
+	/* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */
+	PID_MODE_DERIVATIV_NONE = 0,
+	/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
+	 * val_dot in pid_calculate() will be ignored */
+	PID_MODE_DERIVATIV_CALC,
+	/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value,
+	 * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */
+	PID_MODE_DERIVATIV_CALC_NO_SP,
+	/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
+	PID_MODE_DERIVATIV_SET
+} pid_mode_t;
 
 typedef struct {
+	pid_mode_t mode;
+	float dt_min;
 	float kp;
 	float ki;
 	float kd;
-	float intmax;
-	float sp;
 	float integral;
+	float integral_limit;
+	float output_limit;
 	float error_previous;
 	float last_output;
-	float limit;
-	float dt_min;
-	uint8_t mode;
-	uint8_t count;
-	uint8_t saturated;
 } PID_t;
 
-__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 void pid_init(PID_t *pid, pid_mode_t mode, float dt_min);
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);
 __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
-
 __EXPORT void pid_reset_integral(PID_t *pid);
 
 __END_DECLS
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/systemlib/state_table.h
index 431a435f7..f2709d29f 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.h
+++ b/src/modules/systemlib/state_table.h
@@ -1,12 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,29 +31,45 @@
  *
  ****************************************************************************/
 
-/*
- * @file multirotor_attitude_control.h
- *
- * Definition of attitude controller for multirotors.
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
+/**
+ * @file state_table.h
+ * 
+ * Finite-State-Machine helper class for state table
  */
 
-#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_
-#define MULTIROTOR_ATTITUDE_CONTROL_H_
+#ifndef __SYSTEMLIB_STATE_TABLE_H
+#define __SYSTEMLIB_STATE_TABLE_H
+
+class StateTable
+{
+public:
+	typedef void (StateTable::*Action)();
+	struct Tran {
+		Action action;
+		unsigned nextState;
+	};
+	
+	StateTable(Tran const *table, unsigned nStates, unsigned nSignals)
+	: myTable(table), myNsignals(nSignals), myNstates(nStates) {}
+	
+	#define NO_ACTION &StateTable::doNothing
+	#define ACTION(_target) static_cast<StateTable::Action>(_target)
 
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
+	virtual ~StateTable() {}
+	
+	void dispatch(unsigned const sig) {
+		register Tran const *t = myTable + myState*myNsignals + sig;
+		(this->*(t->action))();
 
-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, bool reset_integral);
+		myState = t->nextState;
+	}
+	void doNothing() {}
+protected:
+	unsigned myState;
+private:
+	Tran const *myTable;
+	unsigned myNsignals;
+	unsigned myNstates;
+};
 
-#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
+#endif
\ No newline at end of file
diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
index 75be090f8..cb35a2541 100644
--- a/src/modules/systemlib/system_params.c
+++ b/src/modules/systemlib/system_params.c
@@ -40,8 +40,23 @@
 #include <nuttx/config.h>
 #include <systemlib/param/param.h>
 
-// Auto-start script with index #n
+/**
+ * Auto-start script index.
+ *
+ * Defines the auto-start script used to bootstrap the system.
+ *
+ * @group System
+ */
 PARAM_DEFINE_INT32(SYS_AUTOSTART, 0);
 
-// Automatically configure default values
+/**
+ * Automatically configure default values.
+ *
+ * Set to 1 to set platform-specific parameters to their default
+ * values on next system startup. 
+ *
+ * @min 0
+ * @max 1
+ * @group System
+ */
 PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index c6a252b55..4c84c1f25 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -117,17 +117,21 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi
 #include "topics/vehicle_bodyframe_speed_setpoint.h"
 ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s);
 
-#include "topics/vehicle_global_position_setpoint.h"
-ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s);
-
-#include "topics/vehicle_global_position_set_triplet.h"
-ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s);
+#include "topics/position_setpoint_triplet.h"
+ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_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);
+ORB_DEFINE(onboard_mission, struct mission_s);
+
+#include "topics/mission_result.h"
+ORB_DEFINE(mission_result, struct mission_result_s);
+
+#include "topics/fence.h"
+ORB_DEFINE(fence, unsigned);
 
 #include "topics/vehicle_attitude_setpoint.h"
 ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
diff --git a/src/modules/mavlink/missionlib.h b/src/modules/uORB/topics/fence.h
index c7d8f90c5..6f16c51cf 100644
--- a/src/modules/mavlink/missionlib.h
+++ b/src/modules/uORB/topics/fence.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Author: @author Jean Cyr <jean.m.cyr@gmail.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,20 +33,48 @@
  ****************************************************************************/
 
 /**
- * @file missionlib.h
- * MAVLink mission helper library
+ * @file fence.h
+ * Definition of geofence.
  */
 
-#pragma once
+#ifndef TOPIC_FENCE_H_
+#define TOPIC_FENCE_H_
 
-#include "mavlink_bridge_header.h"
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
 
-//extern void	mavlink_wpm_send_message(mavlink_message_t *msg);
-//extern void	mavlink_wpm_send_gcs_string(const char *string);
-//extern uint64_t	mavlink_wpm_get_system_timestamp(void);
-extern int	mavlink_missionlib_send_message(mavlink_message_t *msg);
-extern int	mavlink_missionlib_send_gcs_string(const char *string);
-extern uint64_t	mavlink_missionlib_get_system_timestamp(void);
-extern void	mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
-		float param2, float param3, float param4, float param5_lat_x,
-		float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+#define GEOFENCE_MAX_VERTICES 16
+
+/**
+ * This is the position of a geofence vertex
+ *
+ */
+struct fence_vertex_s {
+	// Worst case float precision gives us 2 meter resolution at the equator
+	float lat;			/**< latitude in degrees */
+	float lon;			/**< longitude in degrees */
+};
+
+/**
+ * This is the position of a geofence
+ *
+ */
+struct fence_s { 
+	unsigned count;     /**< number of actual vertices */
+	struct fence_vertex_s vertices[GEOFENCE_MAX_VERTICES];
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(fence);
+
+#endif
diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h
index 09f5140b3..08d11abae 100644
--- a/src/modules/uORB/topics/home_position.h
+++ b/src/modules/uORB/topics/home_position.h
@@ -2,6 +2,7 @@
  *
  *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,9 +35,10 @@
 
 /**
  * @file home_position.h
- * Definition of the GPS home position uORB topic.
+ * Definition of the home position uORB topic.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #ifndef TOPIC_HOME_POSITION_H_
@@ -53,17 +55,14 @@
 /**
  * GPS home position in WGS84 coordinates.
  */
-struct home_position_s {
-	uint64_t timestamp;             /**< Timestamp (microseconds since system boot)   */
-	uint64_t time_gps_usec;         /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module   */
+struct home_position_s
+{
+	uint64_t timestamp;			/**< Timestamp (microseconds since system boot)	*/
 
-	int32_t lat;                    /**< Latitude in 1E7 degrees */
-	int32_t lon;                    /**< Longitude in 1E7 degrees */
-	int32_t alt;                    /**< Altitude in 1E3 meters (millimeters) above MSL */
-	float eph_m;                   /**< GPS HDOP horizontal dilution of position in m */
-	float epv_m;                   /**< GPS VDOP horizontal dilution of position in m */
-	float s_variance_m_s;               /**< speed accuracy estimate m/s */
-	float p_variance_m;               /**< position accuracy estimate m */
+	//bool altitude_is_relative;	// TODO what means home relative altitude? we need clear definition of reference altitude then
+	double lat;				/**< Latitude in degrees 			*/
+	double lon;				/**< Longitude in degrees			*/
+	float alt;				/**< Altitude in meters				*/
 };
 
 /**
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index 04cbf73e1..9594c4c6a 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -35,8 +35,8 @@
  ****************************************************************************/
 
 /**
- * @file mission_item.h
- * Definition of one mission item.
+ * @file mission.h
+ * Definition of a mission consisting of mission items.
  */
 
 #ifndef TOPIC_MISSION_H_
@@ -46,14 +46,24 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
+#define NUM_MISSIONS_SUPPORTED 256
+
+/* compatible to mavlink MAV_CMD */
 enum NAV_CMD {
-	NAV_CMD_WAYPOINT = 0,
-	NAV_CMD_LOITER_TURN_COUNT,
-	NAV_CMD_LOITER_TIME_LIMIT,
-	NAV_CMD_LOITER_UNLIMITED,
-	NAV_CMD_RETURN_TO_LAUNCH,
-	NAV_CMD_LAND,
-	NAV_CMD_TAKEOFF
+	NAV_CMD_WAYPOINT=16,
+	NAV_CMD_LOITER_UNLIMITED=17,
+	NAV_CMD_LOITER_TURN_COUNT=18,
+	NAV_CMD_LOITER_TIME_LIMIT=19,
+	NAV_CMD_RETURN_TO_LAUNCH=20,
+	NAV_CMD_LAND=21,
+	NAV_CMD_TAKEOFF=22,
+	NAV_CMD_ROI=80,
+	NAV_CMD_PATHPLANNING=81
+};
+
+enum ORIGIN {
+	ORIGIN_MAVLINK = 0,
+	ORIGIN_ONBOARD
 };
 
 /**
@@ -69,22 +79,25 @@ enum NAV_CMD {
  */
 struct mission_item_s {
 	bool altitude_is_relative;	/**< true if altitude is relative from start point	*/
-	double lat;			/**< latitude in degrees * 1E7				*/
-	double lon;			/**< longitude in degrees * 1E7				*/
+	double lat;			/**< latitude in degrees				*/
+	double lon;			/**< longitude in degrees				*/
 	float altitude;			/**< altitude in meters					*/
-	float yaw;			/**< in radians NED -PI..+PI 				*/
+	float yaw;			/**< in radians NED -PI..+PI, NAN means don't change yaw		*/
 	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
-	uint8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
-	enum NAV_CMD nav_cmd;		/**< true if loitering is enabled			*/
-	float param1;
-	float param2;
-	float param3;
-	float param4;
+	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
+	enum NAV_CMD nav_cmd;		/**< navigation command					*/
+	float acceptance_radius;	/**< default radius in which the mission is accepted as reached in meters */
+	float time_inside;		/**< time that the MAV should stay inside the radius before advancing in seconds */
+	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
+	bool autocontinue;		/**< true if next waypoint should follow after this one */
+	enum ORIGIN origin;		/**< where the waypoint has been generated		*/
 };
 
-struct mission_s {
-	struct mission_item_s *items;
-	unsigned count;
+struct mission_s
+{
+	int dataman_id;			/**< default -1, there are two offboard storage places in the dataman: 0 or 1 */
+	unsigned count;			/**< count of the missions stored in the datamanager */
+	int current_index;		/**< default -1, start at the one changed latest */
 };
 
 /**
@@ -93,5 +106,6 @@ struct mission_s {
 
 /* register this as object request broker structure */
 ORB_DECLARE(mission);
+ORB_DECLARE(onboard_mission);
 
 #endif
diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/mission_result.h
index e8565d887..c99706b97 100644
--- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -35,36 +35,26 @@
  ****************************************************************************/
 
 /**
- * @file vehicle_global_position_setpoint.h
- * Definition of the global WGS84 position setpoint uORB topic.
+ * @file mission_result.h
+ * Mission results that navigator needs to pass on to commander and mavlink.
  */
 
-#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_
-#define TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_
+#ifndef TOPIC_MISSION_RESULT_H
+#define TOPIC_MISSION_RESULT_H
 
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
 
-#include "vehicle_global_position_setpoint.h"
-
 /**
  * @addtogroup topics
  * @{
  */
 
-/**
- * Global position setpoint triplet in WGS84 coordinates.
- *
- * This are the three next waypoints (or just the next two or one).
- */
-struct vehicle_global_position_set_triplet_s {
-	bool previous_valid;					/**< flag indicating previous position is valid */
-	bool next_valid;					/**< flag indicating next position is valid */
-
-	struct vehicle_global_position_setpoint_s previous;
-	struct vehicle_global_position_setpoint_s current;
-	struct vehicle_global_position_setpoint_s next;
+struct mission_result_s
+{
+	bool mission_reached;		/**< true if mission has been reached			*/
+	unsigned mission_index;		/**< index of the mission which has been reached	*/
 };
 
 /**
@@ -72,6 +62,6 @@ struct vehicle_global_position_set_triplet_s {
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(vehicle_global_position_set_triplet);
+ORB_DECLARE(mission_result);
 
 #endif
diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h
index 3a367b8cf..7a5ae9891 100644
--- a/src/modules/uORB/topics/navigation_capabilities.h
+++ b/src/modules/uORB/topics/navigation_capabilities.h
@@ -52,7 +52,12 @@
  * Airspeed
  */
 struct navigation_capabilities_s {
-	float turn_distance;    /**< the optimal distance to a waypoint to switch to the next */
+	float turn_distance;		/**< the optimal distance to a waypoint to switch to the next */
+
+	/* Landing parameters: see fw_pos_control_l1/landingslope.h */
+	float landing_horizontal_slope_displacement;
+	float landing_slope_angle_rad;
+	float landing_flare_length;
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 8102e367d..34aaa30dd 100644
--- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
  *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
  *           @author Julian Oes <joes@student.ethz.ch>
  *           @author Lorenz Meier <lm@inf.ethz.ch>
@@ -35,44 +35,57 @@
  ****************************************************************************/
 
 /**
- * @file vehicle_global_position_setpoint.h
+ * @file mission_item_triplet.h
  * Definition of the global WGS84 position setpoint uORB topic.
  */
 
-#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_
-#define TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_
+#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_
+#define TOPIC_MISSION_ITEM_TRIPLET_H_
 
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
-#include "mission.h"
+#include <navigator/navigator_state.h>
 
 /**
  * @addtogroup topics
  * @{
  */
 
+enum SETPOINT_TYPE
+{
+	SETPOINT_TYPE_NORMAL = 0,		/**< normal setpoint */
+	SETPOINT_TYPE_LOITER,			/**< loiter setpoint */
+	SETPOINT_TYPE_TAKEOFF,			/**< takeoff setpoint */
+	SETPOINT_TYPE_LAND,			/**< land setpoint, altitude must be ignored, vehicle must descend until landing */
+	SETPOINT_TYPE_IDLE,			/**< do nothing, switch off motors or keep at idle speed (MC) */
+};
+
+struct position_setpoint_s
+{
+	bool valid;			/**< true if setpoint is valid */
+	enum SETPOINT_TYPE type;	/**< setpoint type to adjust behavior of position controller */
+	double lat;			/**< latitude, in deg */
+	double lon;			/**< longitude, in deg */
+	float alt;			/**< altitude AMSL, in m */
+	float yaw;			/**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */
+	float loiter_radius;		/**< loiter radius (only for fixed wing), in m */
+	int8_t loiter_direction;	/**< loiter direction: 1 = CW, -1 = CCW */
+	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
+};
+
 /**
- * Global position setpoint in WGS84 coordinates.
+ * Global position setpoint triplet in WGS84 coordinates.
  *
- * This is the position the MAV is heading towards. If it of type loiter,
- * the MAV is circling around it with the given loiter radius in meters.
+ * This are the three next waypoints (or just the next two or one).
  */
-struct vehicle_global_position_setpoint_s {
-	bool altitude_is_relative;	/**< true if altitude is relative from start point	*/
-	int32_t lat;			/**< latitude in degrees * 1E7				*/
-	int32_t lon;			/**< longitude in degrees * 1E7				*/
-	float altitude;			/**< altitude in meters					*/
-	float yaw;			/**< in radians NED -PI..+PI 				*/
-	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
-	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
-	enum NAV_CMD nav_cmd;		/**< true if loitering is enabled			*/
-	float param1;
-	float param2;
-	float param3;
-	float param4;
-	float turn_distance_xy;		/**< The distance on the plane which will mark this as reached */
-	float turn_distance_z;		/**< The distance in Z direction which will mark this as reached */
+struct position_setpoint_triplet_s
+{
+	struct position_setpoint_s previous;
+	struct position_setpoint_s current;
+	struct position_setpoint_s next;
+
+	nav_state_t nav_state;			/**< navigation state */
 };
 
 /**
@@ -80,6 +93,6 @@ struct vehicle_global_position_setpoint_s {
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(vehicle_global_position_setpoint);
+ORB_DECLARE(position_setpoint_triplet);
 
 #endif
diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h
index c181ec04f..76693c46e 100644
--- a/src/modules/uORB/topics/telemetry_status.h
+++ b/src/modules/uORB/topics/telemetry_status.h
@@ -57,14 +57,14 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
 
 struct telemetry_status_s {
 	uint64_t timestamp;
-	enum TELEMETRY_STATUS_RADIO_TYPE type;  /**< type of the radio hardware     */
-	unsigned rssi;              /**< local signal strength                      */
-	unsigned remote_rssi;       /**< remote signal strength                     */
-	unsigned rxerrors;          /**< receive errors                             */
-	unsigned fixed;             /**< count of error corrected packets           */
-	uint8_t noise;              /**< background noise level                     */
-	uint8_t remote_noise;       /**< remote background noise level              */
-	uint8_t txbuf;              /**< how full the tx buffer is as a percentage  */
+	enum TELEMETRY_STATUS_RADIO_TYPE type;	/**< type of the radio hardware     */
+	uint8_t rssi;				/**< local signal strength                      */
+	uint8_t remote_rssi;			/**< remote signal strength                     */
+	uint16_t rxerrors;			/**< receive errors                             */
+	uint16_t fixed;				/**< count of error corrected packets           */
+	uint8_t noise;				/**< background noise level                     */
+	uint8_t remote_noise;			/**< remote background noise level              */
+	uint8_t txbuf;				/**< how full the tx buffer is as a percentage  */
 };
 
 /**
@@ -73,4 +73,4 @@ struct telemetry_status_s {
 
 ORB_DECLARE(telemetry_status);
 
-#endif /* TOPIC_TELEMETRY_STATUS_H */
\ No newline at end of file
+#endif /* TOPIC_TELEMETRY_STATUS_H */
diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h
index 4380a5ee7..e5a35ff9b 100755
--- a/src/modules/uORB/topics/vehicle_attitude.h
+++ b/src/modules/uORB/topics/vehicle_attitude.h
@@ -76,6 +76,7 @@ struct vehicle_attitude_s {
 	float rate_offsets[3];	/**< Offsets of the body angular rates from zero		*/
 	float R[3][3];		/**< Rotation matrix body to world, (Tait-Bryan, NED)		*/
 	float q[4];		/**< Quaternion (NED)						*/
+	float g_comp[3];	/**< Compensated gravity vector				*/
 	bool R_valid;		/**< Rotation matrix valid					*/
 	bool q_valid;		/**< Quaternion valid						*/
 
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index fda3cd75e..d526a8ff2 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -60,7 +60,7 @@ struct vehicle_attitude_setpoint_s {
 	float yaw_body;					/**< body angle in NED frame		*/
 	//float body_valid;				/**< Set to true if body angles are valid */
 
-	float R_body[9];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
+	float R_body[3][3];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
 	bool R_valid;					/**< Set to true if rotation matrix is valid */
 
 	//! For quaternion-based attitude control
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 4c31f92b5..ea554006f 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -1,10 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,6 +36,11 @@
  * Definition of the vehicle_control_mode uORB topic.
  *
  * All control apps should depend their actions based on the flags set here.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #ifndef VEHICLE_CONTROL_MODE
@@ -48,6 +49,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
+#include "vehicle_status.h"
 
 /**
  * @addtogroup topics @{
@@ -59,6 +61,7 @@
  *
  * Encodes the complete system state and is set by the commander app.
  */
+
 struct vehicle_control_mode_s {
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
@@ -70,16 +73,14 @@ struct vehicle_control_mode_s {
 	bool flag_system_hil_enabled;
 
 	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
-	bool flag_control_offboard_enabled;		/**< true if offboard control input is on */
+	bool flag_control_auto_enabled;			/**< true if onboard autopilot should act */
 	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
 	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
 	bool flag_control_velocity_enabled;		/**< true if horizontal velocity (implies direction) is controlled */
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
-	bool flag_control_climb_rate_enabled;		/**< true if climb rate is controlled */
-
-	bool flag_control_auto_enabled;		// TEMP
-	uint8_t auto_state;	// TEMP navigation state for AUTO modes
+	bool flag_control_climb_rate_enabled;	/**< true if climb rate is controlled */
+	bool flag_control_termination_enabled;	/**< true if flighttermination is enabled */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index 4376fa1ba..adcd028f0 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,10 @@
 /**
  * @file vehicle_global_position.h
  * Definition of the global fused WGS84 position uORB topic.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef VEHICLE_GLOBAL_POSITION_T_H_
@@ -51,27 +52,30 @@
  * @{
  */
 
-/**
-* Fused global position in WGS84.
-*
-* This struct contains the system's believ about its position. It is not the raw GPS
-* measurement (@see vehicle_gps_position). This topic is usually published by the position
-* estimator, which will take more sources of information into account than just GPS,
-* e.g. control inputs of the vehicle in a Kalman-filter implementation.
-*/
+ /**
+ * Fused global position in WGS84.
+ *
+ * This struct contains global position estimation. It is not the raw GPS
+ * measurement (@see vehicle_gps_position). This topic is usually published by the position
+ * estimator, which will take more sources of information into account than just GPS,
+ * e.g. control inputs of the vehicle in a Kalman-filter implementation.
+ */
 struct vehicle_global_position_s {
-	uint64_t timestamp;		/**< time of this estimate, in microseconds since system start */
-	uint64_t time_gps_usec; 	/**< GPS timestamp in microseconds							   */
-	bool valid;			/**< true if position satisfies validity criteria of estimator */
+	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start */
+
+	bool global_valid;		/**< true if position satisfies validity criteria of estimator */
+	bool baro_valid;		/**< true if baro_alt is valid (vel_d is also valid in this case) */
+
+	uint64_t time_gps_usec; /**< GPS timestamp in microseconds					   */
+	double lat;			/**< Latitude in degrees							 	   */
+	double lon;			/**< Longitude in degrees							 	   */
+	float alt;			/**< Altitude AMSL in meters						 	   */
+	float vel_n; 		/**< Ground north velocity, m/s				 			   */
+	float vel_e;		/**< Ground east velocity, m/s							   */
+	float vel_d;		/**< Ground downside velocity, m/s						   */
+	float yaw; 			/**< Yaw in radians -PI..+PI.							   */
 
-	int32_t lat;			/**< Latitude in 1E7 degrees							 	   */
-	int32_t lon;			/**< Longitude in 1E7 degrees							 	   */
-	float alt;			/**< Altitude in meters									 	   */
-	float relative_alt;		/**< Altitude above home position in meters, 				   */
-	float vx; 			/**< Ground X velocity, m/s in NED				 			   */
-	float vy;			/**< Ground Y velocity, m/s in NED							   */
-	float vz;			/**< Ground Z velocity, m/s	in NED 							   */
-	float yaw; 			/**< Compass heading in radians -PI..+PI.					   */
+	float baro_alt;		/**< Barometric altitude (not raw baro but fused with accelerometer) */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 44c339711..db9637cd9 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -76,6 +76,11 @@ struct vehicle_local_position_s {
 	int32_t ref_lon;		/**< Reference point longitude in 1E7 degrees */
 	float ref_alt;			/**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
 	bool landed;			/**< true if vehicle is landed */
+	/* Distance to surface */
+	float dist_bottom;		/**< Distance to bottom surface (ground) */
+	float dist_bottom_rate;		/**< Distance to bottom surface (ground) change rate */
+	uint64_t surface_bottom_timestamp;		/**< Time when new bottom surface found */
+	bool dist_bottom_valid;	/**< true if distance to bottom surface is valid */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 65980233e..56be4d241 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -54,6 +54,8 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
+#include <navigator/navigator_state.h>
+
 /**
  * @addtogroup topics @{
  */
@@ -64,22 +66,9 @@ typedef enum {
 	MAIN_STATE_SEATBELT,
 	MAIN_STATE_EASY,
 	MAIN_STATE_AUTO,
+	MAIN_STATE_MAX
 } main_state_t;
 
-/* navigation state machine */
-typedef enum {
-	NAVIGATION_STATE_DIRECT = 0,		// true manual control, no any stabilization
-	NAVIGATION_STATE_STABILIZE,		// attitude stabilization
-	NAVIGATION_STATE_ALTHOLD,		// attitude + altitude stabilization
-	NAVIGATION_STATE_VECTOR,		// attitude + altitude + position stabilization
-	NAVIGATION_STATE_AUTO_READY,	// AUTO, landed, reeady for takeoff
-	NAVIGATION_STATE_AUTO_TAKEOFF,	// detect takeoff using land detector and switch to desired AUTO mode
-	NAVIGATION_STATE_AUTO_LOITER,	// pause mission
-	NAVIGATION_STATE_AUTO_MISSION,	// fly mission
-	NAVIGATION_STATE_AUTO_RTL,		// Return To Launch, when home position switch to LAND
-	NAVIGATION_STATE_AUTO_LAND		// land and switch to AUTO_READY when landed (detect using land detector)
-} navigation_state_t;
-
 typedef enum {
 	ARMING_STATE_INIT = 0,
 	ARMING_STATE_STANDBY,
@@ -87,7 +76,8 @@ typedef enum {
 	ARMING_STATE_ARMED_ERROR,
 	ARMING_STATE_STANDBY_ERROR,
 	ARMING_STATE_REBOOT,
-	ARMING_STATE_IN_AIR_RESTORE
+	ARMING_STATE_IN_AIR_RESTORE,
+	ARMING_STATE_MAX
 } arming_state_t;
 
 typedef enum {
@@ -96,6 +86,14 @@ typedef enum {
 } hil_state_t;
 
 typedef enum {
+	FAILSAFE_STATE_NORMAL = 0,		/**< Normal operation */
+	FAILSAFE_STATE_RTL,				/**< Return To Launch */
+	FAILSAFE_STATE_LAND,			/**< Land without position control */
+	FAILSAFE_STATE_TERMINATION,		/**< Disable motors and use parachute, can't be recovered */
+	FAILSAFE_STATE_MAX
+} failsafe_state_t;
+
+typedef enum {
 	MODE_SWITCH_MANUAL = 0,
 	MODE_SWITCH_ASSISTED,
 	MODE_SWITCH_AUTO
@@ -108,11 +106,13 @@ typedef enum {
 
 typedef enum {
 	RETURN_SWITCH_NONE = 0,
+	RETURN_SWITCH_NORMAL,
 	RETURN_SWITCH_RETURN
 } return_switch_pos_t;
 
 typedef enum {
 	MISSION_SWITCH_NONE = 0,
+	MISSION_SWITCH_LOITER,
 	MISSION_SWITCH_MISSION
 } mission_switch_pos_t;
 
@@ -175,9 +175,11 @@ struct vehicle_status_s {
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
 	main_state_t main_state;				/**< main state machine */
-	navigation_state_t navigation_state;	/**< navigation state machine */
+	unsigned int set_nav_state;	/**< set navigation state machine to specified value */
+	uint64_t set_nav_state_timestamp;	/**< timestamp of latest change of set_nav_state */
 	arming_state_t arming_state;			/**< current arming state */
 	hil_state_t hil_state;					/**< current hil state */
+	failsafe_state_t failsafe_state;		/**< current failsafe state */
 
 	int32_t system_type;				/**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
 	int32_t	system_id;				/**< system id, inspired by MAVLink's system ID field */