Merged master into power_enforce

101 files changed, 9354 insertions, 3594 deletions

diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index d6e638c0a..1f8d8b862 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -3,7 +3,7 @@
 # USB MAVLink start
 #
 
-mavlink start -r 10000 -d /dev/ttyACM0
+mavlink start -r 10000 -d /dev/ttyACM0 -x
 # Enable a number of interesting streams we want via USB
 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
 usleep 100000
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 6d06f897a..f70e0ed77 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -428,11 +428,10 @@ then
 	#
 	sh /etc/init.d/rc.sensors
 
-	if [ $HIL == no ]
-	then
-		echo "[init] Start logging"
-		sh /etc/init.d/rc.logging
-	fi
+	#
+	# Start logging in all modes, including HIL
+	#
+	sh /etc/init.d/rc.logging
 
 	if [ $GPS == yes ]
 	then
diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk
index d6902c4ee..53a2ad1ab 100644
--- a/makefiles/config_aerocore_default.mk
+++ b/makefiles/config_aerocore_default.mk
@@ -80,8 +80,8 @@ LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
-MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
+MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
 
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 810e008ae..4d727aa4d 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -52,7 +52,6 @@ MODULES		+= systemcmds/pwm
 MODULES		+= systemcmds/esc_calib
 MODULES		+= systemcmds/reboot
 MODULES		+= systemcmds/top
-MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/dumpfile
@@ -67,12 +66,11 @@ MODULES		+= modules/mavlink
 MODULES		+= modules/gpio_led
 
 #
-# Estimation modules (EKF/ SO3 / other filters)
+# Estimation modules (EKF / other filters)
 #
 MODULES		+= modules/attitude_estimator_ekf
 MODULES		+= modules/ekf_att_pos_estimator
 MODULES		+= modules/position_estimator_inav
-#MODULES		+= examples/flow_position_estimator
 
 #
 # Vehicle Control
@@ -81,8 +79,6 @@ MODULES		+= modules/fw_pos_control_l1
 MODULES		+= modules/fw_att_control
 MODULES		+= modules/mc_att_control
 MODULES		+= modules/mc_pos_control
-#MODULES		+= examples/flow_position_control
-#MODULES		+= examples/flow_speed_control
 
 #
 # Logging
@@ -111,8 +107,8 @@ LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
-MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
+MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
 
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index a68cc32f4..4b6b0a4d2 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -120,8 +120,8 @@ LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
-MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
+MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
 
diff --git a/mavlink/include/mavlink/v1.0/checksum.h b/mavlink/include/mavlink/v1.0/checksum.h
index 948e080a1..7d9b6ac0c 100644
--- a/mavlink/include/mavlink/v1.0/checksum.h
+++ b/mavlink/include/mavlink/v1.0/checksum.h
@@ -73,7 +73,7 @@ static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length)
  * @param data new bytes to hash
  * @param crcAccum the already accumulated checksum
  **/
-static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length)
+static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length)
 {
 	const uint8_t *p = (const uint8_t *)pBuffer;
 	while (length--) {
diff --git a/mavlink/include/mavlink/v1.0/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h
index 020211d40..de6e22011 100644
--- a/mavlink/include/mavlink/v1.0/common/common.h
+++ b/mavlink/include/mavlink/v1.0/common/common.h
@@ -5,6 +5,10 @@
 #ifndef COMMON_H
 #define COMMON_H
 
+#ifndef MAVLINK_H
+ #error Wrong include order: common.h MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set all defines from mavlink.h manually.
+#endif
+
 #ifdef __cplusplus
 extern "C" {
 #endif
diff --git a/mavlink/include/mavlink/v1.0/mavlink_helpers.h b/mavlink/include/mavlink/v1.0/mavlink_helpers.h
index 96672f847..1639a830b 100644
--- a/mavlink/include/mavlink/v1.0/mavlink_helpers.h
+++ b/mavlink/include/mavlink/v1.0/mavlink_helpers.h
@@ -73,7 +73,6 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui
 #endif
 {
 	// This code part is the same for all messages;
-	uint16_t checksum;
 	msg->magic = MAVLINK_STX;
 	msg->len = length;
 	msg->sysid = system_id;
@@ -81,12 +80,13 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui
 	// One sequence number per component
 	msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
 	mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
-	checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN);
+	msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN);
+	crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
 #if MAVLINK_CRC_EXTRA
-	crc_accumulate(crc_extra, &checksum);
+	crc_accumulate(crc_extra, &msg->checksum);
 #endif
-	mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF);
-	mavlink_ck_b(msg) = (uint8_t)(checksum >> 8);
+	mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
+	mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
 
 	return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 }
@@ -133,7 +133,7 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint
 	buf[4] = mavlink_system.compid;
 	buf[5] = msgid;
 	status->current_tx_seq++;
-	checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
+	checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
 	crc_accumulate_buffer(&checksum, packet, length);
 #if MAVLINK_CRC_EXTRA
 	crc_accumulate(crc_extra, &checksum);
@@ -158,6 +158,7 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m
 
 	ck[0] = (uint8_t)(msg->checksum & 0xFF);
 	ck[1] = (uint8_t)(msg->checksum >> 8);
+	// XXX use the right sequence here
 
 	MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
 	_mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
@@ -172,7 +173,13 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m
  */
 MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
 {
-	memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len);
+	memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
+
+	uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
+
+	ck[0] = (uint8_t)(msg->checksum & 0xFF);
+	ck[1] = (uint8_t)(msg->checksum >> 8);
+
 	return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
 }
 
diff --git a/mavlink/include/mavlink/v1.0/mavlink_types.h b/mavlink/include/mavlink/v1.0/mavlink_types.h
index 4019c619e..43658e629 100644
--- a/mavlink/include/mavlink/v1.0/mavlink_types.h
+++ b/mavlink/include/mavlink/v1.0/mavlink_types.h
@@ -28,6 +28,7 @@
 
 #define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
 
+#pragma pack(push, 1)
 typedef struct param_union {
 	union {
 		float param_float;
@@ -62,13 +63,12 @@ typedef struct __mavlink_message {
 	uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
 } mavlink_message_t;
 
-
 typedef struct __mavlink_extended_message {
        mavlink_message_t base_msg;
        int32_t extended_payload_len;   ///< Length of extended payload if any
        uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
 } mavlink_extended_message_t;
-
+#pragma pack(pop)
 
 typedef enum {
 	MAVLINK_TYPE_CHAR     = 0,
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 5c502f682..98c491ce6 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -655,13 +655,14 @@ BlinkM::led()
 							/* indicate main control state */
 							if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
 								led_color_4 = LED_GREEN;
-							else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
+							/* TODO: add other Auto modes */
+							else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION)
 								led_color_4 = LED_BLUE;
 							else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
 								led_color_4 = LED_YELLOW;
 							else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
 								led_color_4 = LED_WHITE;
-							else 
+							else
 								led_color_4 = LED_OFF;
 							led_color_5 = led_color_4;
 						}
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 5342ccf78..8560716e9 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -280,8 +280,8 @@ GPS::task_main()
 			_report.p_variance_m = 10.0f;
 			_report.c_variance_rad = 0.1f;
 			_report.fix_type = 3;
-			_report.eph_m = 0.9f;
-			_report.epv_m = 1.8f;
+			_report.eph = 0.9f;
+			_report.epv = 1.8f;
 			_report.timestamp_velocity = hrt_absolute_time();
 			_report.vel_n_m_s = 0.0f;
 			_report.vel_e_m_s = 0.0f;
@@ -451,7 +451,7 @@ GPS::print_info()
 		warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
 				_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
 		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
-		warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m);
+		warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv);
 		warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
 		warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
 		warnx("rate publication:\t%6.2f Hz", (double)_rate);
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index 680f00d97..41716cd97 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -251,16 +251,16 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 		_gps_position->lon = 0;
 
 		// Indicate this data is not usable and bail out
-		_gps_position->eph_m = 1000.0f;
-		_gps_position->epv_m = 1000.0f;
+		_gps_position->eph = 1000.0f;
+		_gps_position->epv = 1000.0f;
 		_gps_position->fix_type = 0;
 		return;
 	}
 
 	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
 	_gps_position->fix_type = packet.fix_type;
-	_gps_position->eph_m = packet.hdop / 100.0f; // from cm to m
-	_gps_position->epv_m = _gps_position->eph_m; // unknown in mtk custom mode, so we cheat with eph
+	_gps_position->eph = packet.hdop / 100.0f; // from cm to m
+	_gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph
 	_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
 	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
 	_gps_position->satellites_visible = packet.satellites;
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index c143eeb0c..404607571 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -439,8 +439,8 @@ UBX::handle_message()
 					_gps_position->lat = packet->lat;
 					_gps_position->lon = packet->lon;
 					_gps_position->alt = packet->height_msl;
-					_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
-					_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
+					_gps_position->eph = (float)packet->hAcc * 1e-3f; // from mm to m
+					_gps_position->epv = (float)packet->vAcc * 1e-3f; // from mm to m
 					_gps_position->timestamp_position = hrt_absolute_time();
 
 					_rate_count_lat_lon++;
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 5954c40da..1c81910f6 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -131,8 +131,8 @@ public:
 	int		set_motor_count(unsigned count);
 	int		set_motor_test(bool motortest);
 	int		set_overrideSecurityChecks(bool overrideSecurityChecks);
-	int		set_px4mode(int px4mode);
-	int		set_frametype(int frametype);
+	void		set_px4mode(int px4mode);
+	void		set_frametype(int frametype);
 	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C);
 
 private:
@@ -330,13 +330,13 @@ MK::set_update_rate(unsigned rate)
 	return OK;
 }
 
-int
+void
 MK::set_px4mode(int px4mode)
 {
 	_px4mode = px4mode;
 }
 
-int
+void
 MK::set_frametype(int frametype)
 {
 	_frametype = frametype;
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 321fdd173..0edec3d0e 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -544,7 +544,7 @@ void MPU6000::reset()
 	write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
         irqrestore(state);
 
-	up_udelay(1000);
+	usleep(1000);
 
 	// SAMPLE RATE
 	_set_sample_rate(_sample_rate);
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 8a4bfa18c..84ea9a3bc 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -741,7 +741,7 @@ PX4FMU::task_main()
 	}
 
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
-		if (_control_subs > 0) {
+		if (_control_subs[i] > 0) {
 			::close(_control_subs[i]);
 			_control_subs[i] = -1;
 		}
diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index dd5e4d3e0..fdaa7f27b 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -182,7 +182,10 @@ float RoboClaw::getMotorPosition(e_motor motor)
 		return _motor1Position;
 	} else if (motor == MOTOR_2) {
 		return _motor2Position;
-	}
+	} else {
+		warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
+		return NAN;
+    }
 }
 
 float RoboClaw::getMotorSpeed(e_motor motor)
@@ -191,7 +194,10 @@ float RoboClaw::getMotorSpeed(e_motor motor)
 		return _motor1Speed;
 	} else if (motor == MOTOR_2) {
 		return _motor2Speed;
-	}
+	} else {
+		warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
+		return NAN;
+    }
 }
 
 int RoboClaw::setMotorSpeed(e_motor motor, float value)
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index a0a18bc2e..46db788a6 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -151,9 +151,6 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_ff = 0;
-
 	/* input conditioning */
 	if (!isfinite(airspeed)) {
 		/* airspeed is NaN, +- INF or not available, pick center of band */
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index d2a231694..9894a34d7 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -114,9 +114,6 @@ float ECL_RollController::control_bodyrate(float pitch,
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_ff = 0; //xxx: param
-
 	/* input conditioning */
 //	warnx("airspeed pre %.4f", (double)airspeed);
 	if (!isfinite(airspeed)) {
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index 79184e2cd..fe03b8065 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -84,7 +84,7 @@ float ECL_YawController::control_attitude(float roll, float pitch,
 	_rate_setpoint = 0.0f;
 	if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) {
 		float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
-		if(denumerator != 0.0f) { //XXX: floating point comparison
+		if(fabsf(denumerator) > FLT_EPSILON) {
 			_rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
 //			warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
 		}
@@ -132,11 +132,6 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-
-//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_ff = 0;
-
-
 	/* input conditioning */
 	if (!isfinite(airspeed)) {
 	/* airspeed is NaN, +- INF or not available, pick center of band */
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 87c1cf460..8b286af36 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -50,7 +50,7 @@
 
 __BEGIN_DECLS
 
-#include "geo_mag_declination.h"
+#include "geo_lookup/geo_mag_declination.h"
 
 #include <stdbool.h>
 
diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk
index 9500a2bcc..d08ca4532 100644
--- a/src/lib/geo/module.mk
+++ b/src/lib/geo/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   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
@@ -35,5 +35,4 @@
 # Geo library
 #
 
-SRCS		=	geo.c \
-			geo_mag_declination.c
+SRCS		=	geo.c
diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo_lookup/geo_mag_declination.c
index 09eac38f4..09eac38f4 100644
--- a/src/lib/geo/geo_mag_declination.c
+++ b/src/lib/geo_lookup/geo_mag_declination.c
diff --git a/src/lib/geo/geo_mag_declination.h b/src/lib/geo_lookup/geo_mag_declination.h
index 0ac062d6d..0ac062d6d 100644
--- a/src/lib/geo/geo_mag_declination.h
+++ b/src/lib/geo_lookup/geo_mag_declination.h
diff --git a/src/lib/geo_lookup/module.mk b/src/lib/geo_lookup/module.mk
new file mode 100644
index 000000000..d7a10df2d
--- /dev/null
+++ b/src/lib/geo_lookup/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Geo lookup table / data library
+#
+
+SRCS		 =	geo_mag_declination.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk
index de0174e37..d92f0bb45 100644
--- a/src/lib/launchdetection/module.mk
+++ b/src/lib/launchdetection/module.mk
@@ -38,3 +38,5 @@
 SRCS		 = 	LaunchDetector.cpp \
 			CatapultLaunchMethod.cpp \
 			launchdetection_params.c
+
+MAXOPTIMIZATION	 = -Os
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 5bcce1b4d..35dc39ec6 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -342,7 +342,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 				if (gps_updated) {
 					orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);
 
-					if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
+					if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
 						mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
 
 						/* update mag declination rotation matrix */
@@ -401,7 +401,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 					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) {
+					if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) {
 						vel_valid = true;
 						if (gps_updated) {
 							vel_t = gps.timestamp_velocity;
@@ -410,7 +410,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 							vel(2) = gps.vel_d_m_s;
 						}
 
-					} else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
+					} else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
 						vel_valid = true;
 						if (global_pos_updated) {
 							vel_t = global_pos.timestamp;
@@ -477,7 +477,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						parameters_update(&ekf_param_handles, &ekf_params);
 
 						/* update mag declination rotation matrix */
-						if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
+						if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
 							mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
 
 						} else {
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 3ff3d9922..49a892609 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -61,11 +61,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
 /* offset estimation - UNUSED */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
 
-/* offsets in roll, pitch and yaw of sensor plane and body */
-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);
 
@@ -85,10 +80,6 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->r2 	=	param_find("EKF_ATT_V4_R2");
 	h->r3 	=	param_find("EKF_ATT_V4_R3");
 
-	h->roll_off  =	param_find("ATT_ROLL_OFF3");
-	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");
@@ -109,10 +100,6 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->r2, &(p->r[2]));
 	param_get(h->r3, &(p->r[3]));
 
-	param_get(h->roll_off, &(p->roll_off));
-	param_get(h->pitch_off, &(p->pitch_off));
-	param_get(h->yaw_off, &(p->yaw_off));
-
 	param_get(h->mag_decl, &(p->mag_decl));
 	p->mag_decl *= M_PI / 180.0f;
 
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 74a141609..5985541ca 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
@@ -54,7 +54,6 @@ struct attitude_estimator_ekf_params {
 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/commander.cpp b/src/modules/commander/commander.cpp
index 3c04b29e1..507f18e24 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1,11 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *           Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
+ *   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
@@ -38,8 +33,13 @@
 
 /**
  * @file commander.cpp
- * Main system state machine implementation.
+ * Main fail-safe handling.
  *
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
@@ -78,6 +78,8 @@
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/safety.h>
 #include <uORB/topics/system_power.h>
+#include <uORB/topics/mission_result.h>
+#include <uORB/topics/telemetry_status.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -89,6 +91,7 @@
 #include <systemlib/err.h>
 #include <systemlib/cpuload.h>
 #include <systemlib/rc_check.h>
+#include <systemlib/state_table.h>
 
 #include "px4_custom_mode.h"
 #include "commander_helper.h"
@@ -122,6 +125,7 @@ extern struct system_load_s system_load;
 #define POSITION_TIMEOUT		(600 * 1000)		/**< consider the local or global position estimate invalid after 600ms */
 #define FAILSAFE_DEFAULT_TIMEOUT	(3 * 1000 * 1000)	/**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
 #define RC_TIMEOUT			500000
+#define DL_TIMEOUT			5 * 1000* 1000
 #define DIFFPRESS_TIMEOUT		2000000
 
 #define PRINT_INTERVAL	5000000
@@ -389,109 +393,80 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed
 
 bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
 {
-	/* result of the command */
-	enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-	bool ret = false;
-
 	/* only handle commands that are meant to be handled by this system and component */
 	if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
 		return false;
 	}
 
-	/* only handle high-priority commands here */
+	/* result of the command */
+	enum VEHICLE_CMD_RESULT cmd_result = VEHICLE_CMD_RESULT_UNSUPPORTED;
 
 	/* request to set different system mode */
 	switch (cmd->command) {
 	case VEHICLE_CMD_DO_SET_MODE: {
-			uint8_t base_mode = (uint8_t) cmd->param1;
-			uint8_t custom_main_mode = (uint8_t) cmd->param2;
-			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+			uint8_t base_mode = (uint8_t)cmd->param1;
+			uint8_t custom_main_mode = (uint8_t)cmd->param2;
 
-			/* 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, mavlink_fd);
-
-			/* if HIL got enabled, reset battery status state */
-			if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
-				/* reset the arming mode to disarmed */
-				arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed, mavlink_fd);
+			transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
 
-				if (arming_res != TRANSITION_DENIED) {
-					mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
+			transition_result_t main_ret = TRANSITION_NOT_CHANGED;
 
-				} else {
-					mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
-				}
-			}
-
-			if (hil_ret == OK) {
-				ret = true;
-			}
+			/* set HIL state */
+			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
+			transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
 
 			// Transition the arming state
-			arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
-
-			if (arming_res == TRANSITION_CHANGED) {
-				ret = true;
-			}
-
-			/* set main state */
-			transition_result_t main_res = TRANSITION_DENIED;
+			arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
 
 			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
 				/* use autopilot-specific mode */
 				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
 					/* MANUAL */
-					main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+					main_ret = main_state_transition(status, MAIN_STATE_MANUAL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
 					/* ALTCTL */
-					main_res = main_state_transition(status, MAIN_STATE_ALTCTL);
+					main_ret = main_state_transition(status, MAIN_STATE_ALTCTL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
 					/* POSCTL */
-					main_res = main_state_transition(status, MAIN_STATE_POSCTL);
+					main_ret = main_state_transition(status, MAIN_STATE_POSCTL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					/* AUTO */
-					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+					main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
 					/* ACRO */
-					main_res = main_state_transition(status, MAIN_STATE_ACRO);
+					main_ret = main_state_transition(status, MAIN_STATE_ACRO);
 				}
 
 			} else {
 				/* use base mode */
 				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
 					/* AUTO */
-					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+					main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
 
 				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
 					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
 						/* POSCTL */
-						main_res = main_state_transition(status, MAIN_STATE_POSCTL);
+						main_ret = main_state_transition(status, MAIN_STATE_POSCTL);
 
 					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
 						/* MANUAL */
-						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+						main_ret = main_state_transition(status, MAIN_STATE_MANUAL);
 					}
 				}
 			}
 
-			if (main_res == TRANSITION_CHANGED) {
-				ret = true;
-			}
-
-			if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
+			if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) {
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
-
-			break;
 		}
+		break;
 
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
 			// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
@@ -510,10 +485,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 				if (arming_res == TRANSITION_DENIED) {
 					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
-					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+					cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 
 				} else {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+					cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 				}
 			}
 		}
@@ -524,18 +499,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			unsigned int mav_goto = cmd->param1;
 
 			if (mav_goto == 0) {	// MAV_GOTO_DO_HOLD
-				status->set_nav_state = NAV_STATE_LOITER;
-				status->set_nav_state_timestamp = hrt_absolute_time();
+				status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
 				mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-				ret = true;
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else if (mav_goto == 1) {	// MAV_GOTO_DO_CONTINUE
-				status->set_nav_state = NAV_STATE_MISSION;
-				status->set_nav_state_timestamp = hrt_absolute_time();
+				status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
 				mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-				ret = true;
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} 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);
@@ -543,6 +514,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 		break;
 
+#if 0
 	/* Flight termination */
 	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
 
@@ -550,16 +522,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			//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;
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
 				/* reject parachute depoyment not armed */
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
 
 		}
 		break;
+#endif
 
 	case VEHICLE_CMD_DO_SET_HOME: {
 			bool use_current = cmd->param1 > 0.5f;
@@ -573,10 +545,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 					home->timestamp = hrt_absolute_time();
 
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+					cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
-					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+					cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 				}
 
 			} else {
@@ -587,10 +559,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 				home->timestamp = hrt_absolute_time();
 
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 			}
 
-			if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
+			if (cmd_result == VEHICLE_CMD_RESULT_ACCEPTED) {
 				warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt);
 				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt);
 
@@ -622,17 +594,18 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		break;
 	}
 
-	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+	if (cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
 		/* already warned about unsupported commands in "default" case */
-		answer_command(*cmd, result);
+		answer_command(*cmd, cmd_result);
 	}
 
 	/* send any requested ACKs */
-	if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+	if (cmd->confirmation > 0 && cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
 		/* send acknowledge command */
 		// XXX TODO
 	}
 
+	return true;
 }
 
 int commander_thread_main(int argc, char *argv[])
@@ -649,31 +622,41 @@ int commander_thread_main(int argc, char *argv[])
 	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");
+	param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN");
 
 	/* welcome user */
 	warnx("starting");
 
 	char *main_states_str[MAIN_STATE_MAX];
-	main_states_str[0] = "MANUAL";
-	main_states_str[1] = "ALTCTL";
-	main_states_str[2] = "POSCTL";
-	main_states_str[3] = "AUTO";
-	main_states_str[4] = "ACRO";
+	main_states_str[MAIN_STATE_MANUAL]			= "MANUAL";
+	main_states_str[MAIN_STATE_ALTCTL]			= "ALTCTL";
+	main_states_str[MAIN_STATE_POSCTL]			= "POSCTL";
+	main_states_str[MAIN_STATE_AUTO_MISSION]		= "AUTO_MISSION";
+	main_states_str[MAIN_STATE_AUTO_LOITER]			= "AUTO_LOITER";
+	main_states_str[MAIN_STATE_AUTO_RTL]			= "AUTO_RTL";
+	main_states_str[MAIN_STATE_ACRO]			= "ACRO";
 
 	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";
+	arming_states_str[ARMING_STATE_INIT]			= "INIT";
+	arming_states_str[ARMING_STATE_STANDBY]			= "STANDBY";
+	arming_states_str[ARMING_STATE_ARMED]			= "ARMED";
+	arming_states_str[ARMING_STATE_ARMED_ERROR]		= "ARMED_ERROR";
+	arming_states_str[ARMING_STATE_STANDBY_ERROR]		= "STANDBY_ERROR";
+	arming_states_str[ARMING_STATE_REBOOT]			= "REBOOT";
+	arming_states_str[ARMING_STATE_IN_AIR_RESTORE]		= "IN_AIR_RESTORE";
+
+	char *nav_states_str[NAVIGATION_STATE_MAX];
+	nav_states_str[NAVIGATION_STATE_MANUAL]			= "MANUAL";
+	nav_states_str[NAVIGATION_STATE_ALTCTL]			= "ALTCTL";
+	nav_states_str[NAVIGATION_STATE_POSCTL]			= "POSCTL";
+	nav_states_str[NAVIGATION_STATE_AUTO_MISSION]		= "AUTO_MISSION";
+	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]		= "AUTO_LOITER";
+	nav_states_str[NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
+	nav_states_str[NAVIGATION_STATE_AUTO_RTGS]		= "AUTO_RTGS";
+	nav_states_str[NAVIGATION_STATE_ACRO]			= "ACRO";
+	nav_states_str[NAVIGATION_STATE_LAND]			= "LAND";
+	nav_states_str[NAVIGATION_STATE_DESCEND]		= "DESCEND";
+	nav_states_str[NAVIGATION_STATE_TERMINATION]		= "TERMINATION";
 
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
@@ -695,11 +678,10 @@ int commander_thread_main(int argc, char *argv[])
 	// We want to accept RC inputs as default
 	status.rc_input_blocked = false;
 	status.main_state = MAIN_STATE_MANUAL;
-	status.set_nav_state = NAV_STATE_NONE;
-	status.set_nav_state_timestamp = 0;
+	status.nav_state = NAVIGATION_STATE_MANUAL;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
-	status.failsafe_state = FAILSAFE_STATE_NORMAL;
+	status.failsafe = false;
 
 	/* neither manual nor offboard control commands have been received */
 	status.offboard_control_signal_found_once = false;
@@ -708,6 +690,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* mark all signals lost as long as they haven't been found */
 	status.rc_signal_lost = true;
 	status.offboard_control_signal_lost = true;
+	status.data_link_lost = true;
 
 	/* set battery warning flag */
 	status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
@@ -792,6 +775,11 @@ int commander_thread_main(int argc, char *argv[])
 	safety.safety_switch_available = false;
 	safety.safety_off = false;
 
+	/* Subscribe to mission result topic */
+	int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
+	struct mission_result_s mission_result;
+	memset(&mission_result, 0, sizeof(mission_result));
+
 	/* Subscribe to manual control data */
 	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	struct manual_control_setpoint_s sp_man;
@@ -802,6 +790,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct offboard_control_setpoint_s sp_offboard;
 	memset(&sp_offboard, 0, sizeof(sp_offboard));
 
+	/* Subscribe to telemetry status */
+	int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
+	struct telemetry_status_s telemetry;
+	memset(&telemetry, 0, sizeof(telemetry));
+
 	/* Subscribe to global position */
 	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	struct vehicle_global_position_s global_position;
@@ -874,6 +867,15 @@ int commander_thread_main(int argc, char *argv[])
 
 	start_time = hrt_absolute_time();
 
+	transition_result_t arming_ret;
+
+	int32_t datalink_loss_enabled = false;
+
+	/* check which state machines for changes, clear "changed" flag */
+	bool arming_state_changed = false;
+	bool main_state_changed = false;
+	bool failsafe_old = false;
+
 	while (!thread_should_exit) {
 
 		if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
@@ -881,6 +883,9 @@ int commander_thread_main(int argc, char *argv[])
 			mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 		}
 
+		arming_ret = TRANSITION_NOT_CHANGED;
+
+
 		/* update parameters */
 		orb_check(param_changed_sub, &updated);
 
@@ -923,6 +928,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* navigation parameters */
 			param_get(_param_takeoff_alt, &takeoff_alt);
 			param_get(_param_enable_parachute, &parachute_enabled);
+			param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
 		}
 
 		orb_check(sp_man_sub, &updated);
@@ -937,6 +943,12 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
 		}
 
+		orb_check(telemetry_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry);
+		}
+
 		orb_check(sensor_sub, &updated);
 
 		if (updated) {
@@ -980,7 +992,8 @@ int commander_thread_main(int argc, char *argv[])
 				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
 
 				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
-					mavlink_log_info(mavlink_fd, "#audio DISARMED by safety switch");
+					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
+					arming_state_changed = true;
 				}
 			}
 		}
@@ -993,6 +1006,14 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
 		}
 
+		/* update local position estimate */
+		orb_check(local_position_sub, &updated);
+
+		if (updated) {
+			/* position changed */
+			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
+		}
+
 		/* update condition_global_position_valid */
 		/* hysteresis for EPH/EPV */
 		bool eph_epv_good;
@@ -1026,6 +1047,10 @@ int commander_thread_main(int argc, char *argv[])
 			home.lon = global_position.lon;
 			home.alt = global_position.alt;
 
+			home.x = local_position.x;
+			home.y = local_position.y;
+			home.z = local_position.z;
+
 			warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
 			mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
 
@@ -1042,14 +1067,6 @@ int commander_thread_main(int argc, char *argv[])
 			tune_positive(true);
 		}
 
-		/* update local position estimate */
-		orb_check(local_position_sub, &updated);
-
-		if (updated) {
-			/* position changed */
-			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
-		}
-
 		/* update condition_local_position_valid and condition_local_altitude_valid */
 		/* hysteresis for EPH */
 		bool local_eph_good;
@@ -1073,13 +1090,10 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(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_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");
@@ -1088,13 +1102,6 @@ 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 */
@@ -1183,12 +1190,19 @@ int commander_thread_main(int argc, char *argv[])
 			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
 
 			if (armed.armed) {
-				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
+
+				if (arming_ret == TRANSITION_CHANGED) {
+					arming_state_changed = true;
+				}
 
 			} else {
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
-			}
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
 
+				if (arming_ret == TRANSITION_CHANGED) {
+					arming_state_changed = true;
+				}
+			}
 			status_changed = true;
 		}
 
@@ -1196,11 +1210,15 @@ 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, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+			/* TODO: check for sensors */
+			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+
+			if (arming_ret == TRANSITION_CHANGED) {
+				arming_state_changed = true;
+			}
 
 		} else {
-			// XXX: Add emergency stuff if sensors are lost
+			/* TODO: Add emergency stuff if sensors are lost */
 		}
 
 
@@ -1219,7 +1237,13 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
 		}
 
-		/* start RC input check */
+		orb_check(mission_result_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
+		}
+
+		/* 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) {
@@ -1236,11 +1260,6 @@ int commander_thread_main(int argc, char *argv[])
 
 			status.rc_signal_lost = false;
 
-			transition_result_t arming_res;	// store all transitions results here
-
-			/* arm/disarm by RC */
-			arming_res = TRANSITION_NOT_CHANGED;
-
 			/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
 			 * do it only for rotary wings */
 			if (status.is_rotary_wing &&
@@ -1251,7 +1270,10 @@ int commander_thread_main(int argc, char *argv[])
 				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);
-					arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
+					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
+					if (arming_ret == TRANSITION_CHANGED) {
+						arming_state_changed = true;
+					}
 					stick_off_counter = 0;
 
 				} else {
@@ -1273,7 +1295,10 @@ int commander_thread_main(int argc, char *argv[])
 						print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
 
 					} else {
-						arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
+						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
+						if (arming_ret == TRANSITION_CHANGED) {
+							arming_state_changed = true;
+						}
 					}
 
 					stick_on_counter = 0;
@@ -1286,124 +1311,55 @@ int commander_thread_main(int argc, char *argv[])
 				stick_on_counter = 0;
 			}
 
-			if (arming_res == TRANSITION_CHANGED) {
+			if (arming_ret == TRANSITION_CHANGED) {
 				if (status.arming_state == ARMING_STATE_ARMED) {
 					mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
 
 				} else {
 					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
 				}
+				arming_state_changed = true;
 
-			} else if (arming_res == TRANSITION_DENIED) {
+			} else if (arming_ret == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
 				mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
 			}
 
-			if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
-				/* recover from failsafe */
-				(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
-			}
-
 			/* evaluate the main state machine according to mode switches */
 			transition_result_t main_res = set_main_state_rc(&status, &sp_man);
 
 			/* play tune on mode change only if armed, blink LED always */
 			if (main_res == TRANSITION_CHANGED) {
 				tune_positive(armed.armed);
+				main_state_changed = true;
 
 			} else if (main_res == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
 				mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
 			}
 
-			/* set navigation state */
-			/* RETURN switch, overrides MISSION switch */
-			if (sp_man.return_switch == SWITCH_POS_ON) {
-				/* switch to RTL if not already landed after RTL and home position set */
-				status.set_nav_state = NAV_STATE_RTL;
-				status.set_nav_state_timestamp = hrt_absolute_time();
-
-			} else {
-
-				/* LOITER switch */
-				if (sp_man.loiter_switch == SWITCH_POS_ON) {
-					/* stick is in LOITER position */
-					status.set_nav_state = NAV_STATE_LOITER;
-					status.set_nav_state_timestamp = hrt_absolute_time();
-
-				} else if (sp_man.loiter_switch != SWITCH_POS_NONE) {
-					/* stick is in MISSION position */
-					status.set_nav_state = NAV_STATE_MISSION;
-					status.set_nav_state_timestamp = hrt_absolute_time();
-
-				} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
-					   pos_sp_triplet.nav_state == NAV_STATE_RTL) {
-					/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
-					status.set_nav_state = NAV_STATE_MISSION;
-					status.set_nav_state_timestamp = hrt_absolute_time();
-				}
-			}
-
 		} 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 auto_res = main_state_transition(&status, MAIN_STATE_AUTO);
-
-					if (auto_res == TRANSITION_NOT_CHANGED) {
-						last_auto_state_valid = hrt_absolute_time();
-					}
-
-					/* still invalid state after the timeout interval, execute failsafe */
-					if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) {
-						/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
-						auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
-
-						if (auto_res == TRANSITION_DENIED) {
-							/* LAND not allowed, set TERMINATION state */
-							(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
-						}
-					}
-
-				} else {
-					/* failsafe for manual modes */
-					transition_result_t manual_res = TRANSITION_DENIED;
-
-					if (!status.condition_landed) {
-						/* vehicle is not landed, try to perform RTL */
-						manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
-					}
-
-					if (manual_res == TRANSITION_DENIED) {
-						/* RTL not allowed (no global position estimate) or not wanted, try LAND */
-						manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
-
-						if (manual_res == TRANSITION_DENIED) {
-							/* LAND not allowed, set TERMINATION state */
-							(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
-						}
-					}
-				}
-
-			} else {
-				if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
-					/* reset failsafe when disarmed */
-					(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
-				}
+		/* data link check */
+		if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) {
+			/* handle the case where data link was regained */
+			if (status.data_link_lost) {
+				mavlink_log_critical(mavlink_fd, "#audio: data link regained");
+				status.data_link_lost = false;
+				status_changed = true;
 			}
-		}
 
-		// TODO remove this hack
-		/* flight termination in manual mode if assist switch is on POSCTL position */
-		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) {
-			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
-				tune_positive(armed.armed);
+		} else {
+			if (!status.data_link_lost) {
+				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST");
+				status.data_link_lost = true;
+				status_changed = true;
 			}
 		}
 
@@ -1420,11 +1376,6 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-		/* 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 failsafe_state_changed = check_failsafe_state_changed();
-
 		hrt_abstime t1 = hrt_absolute_time();
 
 		/* print new state */
@@ -1441,6 +1392,10 @@ int commander_thread_main(int argc, char *argv[])
 				home.lon = global_position.lon;
 				home.alt = global_position.alt;
 
+				home.x = local_position.x;
+				home.y = local_position.y;
+				home.z = local_position.z;
+
 				warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
 				mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
 
@@ -1455,18 +1410,33 @@ int commander_thread_main(int argc, char *argv[])
 				/* mark home position as set */
 				status.condition_home_position_valid = true;
 			}
+			arming_state_changed = false;
 		}
 
 		was_armed = armed.armed;
 
+		/* now set navigation state according to failsafe and main state */
+		bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
+						       mission_result.mission_finished);
+
+		// TODO handle mode changes by commands
 		if (main_state_changed) {
 			status_changed = true;
+			warnx("main state: %s", main_states_str[status.main_state]);
 			mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
+			main_state_changed = false;
 		}
 
-		if (failsafe_state_changed) {
+		if (status.failsafe != failsafe_old) {
 			status_changed = true;
-			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]);
+			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
+			failsafe_old = status.failsafe;
+		}
+
+		if (nav_state_changed) {
+			status_changed = true;
+			warnx("nav state: %s", nav_states_str[status.nav_state]);
+			mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]);
 		}
 
 		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
@@ -1492,7 +1462,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* play tune on battery critical */
 			set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
 
-		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
 			/* play tune on battery warning or failsafe */
 			set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
 
@@ -1596,7 +1566,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 
 		if (set_normal_color) {
 			/* set color */
-			if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) {
+			if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) {
 				rgbled_set_color(RGBLED_COLOR_AMBER);
 				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
 
@@ -1675,10 +1645,10 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 				break;	// changed successfully or already in this state
 			}
 
-			// else fallback to ALTCTL
 			print_reject_mode(status, "POSCTL");
 		}
 
+        // fallback to ALTCTL
 		res = main_state_transition(status, MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
@@ -1689,27 +1659,62 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 			print_reject_mode(status, "ALTCTL");
 		}
 
-		// else fallback to MANUAL
+		// fallback to MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
 	case SWITCH_POS_ON:			// AUTO
-		res = main_state_transition(status, MAIN_STATE_AUTO);
+		if (sp_man->return_switch == SWITCH_POS_ON) {
+			res = main_state_transition(status, MAIN_STATE_AUTO_RTL);
 
-		if (res != TRANSITION_DENIED) {
-			break;	// changed successfully or already in this state
+			if (res != TRANSITION_DENIED) {
+				break;	// changed successfully or already in this state
+			}
+
+            print_reject_mode(status, "AUTO_RTL");
+
+            // fallback to LOITER if home position not set
+            res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
+
+            if (res != TRANSITION_DENIED) {
+                break;  // changed successfully or already in this state
+            }
+
+		} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
+			res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
+
+			if (res != TRANSITION_DENIED) {
+				break;	// changed successfully or already in this state
+			}
+
+            print_reject_mode(status, "AUTO_LOITER");
+
+		} else {
+			res = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
+
+			if (res != TRANSITION_DENIED) {
+				break;	// changed successfully or already in this state
+			}
+
+            print_reject_mode(status, "AUTO_MISSION");
 		}
 
-		// else fallback to ALTCTL (POSCTL likely will not work too)
-		print_reject_mode(status, "AUTO");
+        // fallback to POSCTL
+        res = main_state_transition(status, MAIN_STATE_POSCTL);
+
+        if (res != TRANSITION_DENIED) {
+            break;  // changed successfully or already in this state
+        }
+
+		// fallback to ALTCTL
 		res = main_state_transition(status, MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
 		}
 
-		// else fallback to MANUAL
+		// fallback to MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
@@ -1722,85 +1727,92 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 }
 
 void
-
 set_control_mode()
 {
-	/* set vehicle_control_mode according to main state and failsafe state */
+	/* set vehicle_control_mode according to set_navigation_state */
 	control_mode.flag_armed = armed.armed;
+	/* TODO: check this */
 	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_ALTCTL:
-			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_POSCTL:
-			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;
-			break;
+	switch (status.nav_state) {
+	case NAVIGATION_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;
+		control_mode.flag_control_termination_enabled = false;
+		break;
 
-		case MAIN_STATE_ACRO:
-			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 = false;
-			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 NAVIGATION_STATE_ACRO:
+		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 = false;
+		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;
+		control_mode.flag_control_termination_enabled = false;
+		break;
 
-		default:
-			break;
-		}
+	case NAVIGATION_STATE_ALTCTL:
+		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;
+		control_mode.flag_control_termination_enabled = false;
+		break;
 
+	case NAVIGATION_STATE_POSCTL:
+		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;
+		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case FAILSAFE_STATE_RTL:
-		navigator_enabled = true;
+	case NAVIGATION_STATE_AUTO_MISSION:
+	case NAVIGATION_STATE_AUTO_LOITER:
+	case NAVIGATION_STATE_AUTO_RTL:
+	case NAVIGATION_STATE_AUTO_RTGS:
+		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_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;
+		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case FAILSAFE_STATE_LAND:
-		navigator_enabled = true;
+	case NAVIGATION_STATE_LAND:
+		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;
+		/* in failsafe LAND mode position may be not available */
+		control_mode.flag_control_position_enabled = status.condition_local_position_valid;
+		control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case FAILSAFE_STATE_TERMINATION:
+	case NAVIGATION_STATE_TERMINATION:
 		/* disable all controllers on termination */
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = false;
@@ -1816,21 +1828,6 @@ set_control_mode()
 	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;
-
-		/* in failsafe LAND mode position may be not available */
-		control_mode.flag_control_position_enabled = status.condition_local_position_valid;
-		control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
-
-		control_mode.flag_control_altitude_enabled = true;
-		control_mode.flag_control_climb_rate_enabled = true;
-	}
 }
 
 void
@@ -1973,8 +1970,6 @@ void *commander_low_prio_loop(void *arg)
 				int calib_ret = ERROR;
 
 				/* try to go to INIT/PREFLIGHT arming state */
-
-				// XXX disable interrupts in arming_state_transition
 				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 80ca68f21..4750f9d5c 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -39,7 +39,7 @@
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include <nuttx/config.h>
@@ -84,3 +84,14 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
  * @group Battery Calibration
  */
 PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
+
+/**
+ * Datalink loss mode enabled.
+ *
+ * Set to 1 to enable actions triggered when the datalink is lost.
+ *
+ * @group commander
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index e0f8dc95d..7f5f93801 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -25,6 +25,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
 	PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
 	PX4_CUSTOM_SUB_MODE_AUTO_RTL,
 	PX4_CUSTOM_SUB_MODE_AUTO_LAND,
+	PX4_CUSTOM_SUB_MODE_AUTO_RTGS
 };
 
 union px4_custom_mode {
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 84f4d03af..1997729d4 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.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
@@ -36,6 +34,9 @@
 /**
  * @file state_machine_helper.cpp
  * State machine helper functions implementations
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include <stdio.h>
@@ -59,30 +60,20 @@
 #include "state_machine_helper.h"
 #include "commander_helper.h"
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
-static bool arming_state_changed = true;
-static bool main_state_changed = true;
-static bool failsafe_state_changed = true;
-
 // This array defines the arming state transitions. The rows are the new state, and the columns
 // are the current state. Using new state and current  state you can index into the array which
 // will be true for a valid transition or false for a invalid transition. In some cases even
 // though the transition is marked as true additional checks must be made. See arming_state_transition
 // code for those checks.
 static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
-	//                                  INIT,   STANDBY,    ARMED,  ARMED_ERROR,    STANDBY_ERROR,  REBOOT,     IN_AIR_RESTORE
-	{ /* ARMING_STATE_INIT */           true,   true,       false,  false,          false,          false,      false },
-	{ /* ARMING_STATE_STANDBY */        true,   true,       true,   true,           false,          false,      false },
-	{ /* ARMING_STATE_ARMED */          false,  true,       true,   false,          false,          false,      true },
-	{ /* ARMING_STATE_ARMED_ERROR */    false,  false,      true,   true,           false,          false,      false },
-	{ /* ARMING_STATE_STANDBY_ERROR */  true,   true,       false,  true,           true,           false,      false },
-	{ /* ARMING_STATE_REBOOT */         true,   true,       false,  false,          true,           true,       true },
-	{ /* ARMING_STATE_IN_AIR_RESTORE */ false,  false,      false,  false,          false,          false,      false }, // NYI
+	//                                  INIT,  STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
+	{ /* ARMING_STATE_INIT */           true,  true,    false, false,       false,         false,  false },
+	{ /* ARMING_STATE_STANDBY */        true,  true,    true,  true,        false,         false,  false },
+	{ /* ARMING_STATE_ARMED */          false, true,    true,  false,       false,         false,  true },
+	{ /* ARMING_STATE_ARMED_ERROR */    false, false,   true,  true,        false,         false,  false },
+	{ /* ARMING_STATE_STANDBY_ERROR */  true,  true,    false, true,        true,          false,  false },
+	{ /* ARMING_STATE_REBOOT */         true,  true,    false, false,       true,          true,   true },
+	{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,       false,         false,  false }, // NYI
 };
 
 // You can index into the array with an arming_state_t in order to get it's textual representation
@@ -191,7 +182,6 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 			armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
 			ret = TRANSITION_CHANGED;
 			status->arming_state = new_arming_state;
-			arming_state_changed = true;
 		}
 	}
 
@@ -225,69 +215,58 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet
 	}
 }
 
-bool
-check_arming_state_changed()
-{
-	if (arming_state_changed) {
-		arming_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
 transition_result_t
 main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
-	/* transition may be denied even if requested the same state because conditions may be changed */
+	/* transition may be denied even if the same state is requested because conditions may have changed */
 	switch (new_main_state) {
 	case MAIN_STATE_MANUAL:
-		ret = TRANSITION_CHANGED;
-		break;
-
 	case MAIN_STATE_ACRO:
 		ret = TRANSITION_CHANGED;
 		break;
 
 	case MAIN_STATE_ALTCTL:
-
 		/* need at minimum altitude estimate */
+		/* TODO: add this for fixedwing as well */
 		if (!status->is_rotary_wing ||
 		    (status->condition_local_altitude_valid ||
 		     status->condition_global_position_valid)) {
 			ret = TRANSITION_CHANGED;
 		}
-
 		break;
 
 	case MAIN_STATE_POSCTL:
-
 		/* need at minimum local position estimate */
 		if (status->condition_local_position_valid ||
 		    status->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
-
 		break;
 
-	case MAIN_STATE_AUTO:
-
+	case MAIN_STATE_AUTO_MISSION:
+	case MAIN_STATE_AUTO_LOITER:
 		/* need global position estimate */
 		if (status->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
+		break;
 
+	case MAIN_STATE_AUTO_RTL:
+		/* need global position and home position */
+		if (status->condition_global_position_valid && status->condition_home_position_valid) {
+			ret = TRANSITION_CHANGED;
+		}
 		break;
-	}
 
+	case MAIN_STATE_MAX:
+	default:
+		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;
 		}
@@ -296,70 +275,35 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 	return ret;
 }
 
-bool
-check_main_state_changed()
-{
-	if (main_state_changed) {
-		main_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
-bool
-check_failsafe_state_changed()
-{
-	if (failsafe_state_changed) {
-		failsafe_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
 /**
-* 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, const int mavlink_fd)
+ * Transition from one hil state to another
+ */
+transition_result_t 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;
+	transition_result_t ret = TRANSITION_DENIED;
 
 	if (current_status->hil_state == new_state) {
-		valid_transition = true;
+		ret = TRANSITION_NOT_CHANGED;
 
 	} else {
-
 		switch (new_state) {
-
 		case HIL_STATE_OFF:
-
 			/* we're in HIL and unexpected things can happen if we disable HIL now */
 			mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
-			valid_transition = false;
-
+			ret = TRANSITION_DENIED;
 			break;
 
 		case HIL_STATE_ON:
-
 			if (current_status->arming_state == ARMING_STATE_INIT
 			    || current_status->arming_state == ARMING_STATE_STANDBY
 			    || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
 
-				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
-				valid_transition = true;
-
-				// Disable publication of all attached sensors
-
+				/* Disable publication of all attached sensors */
 				/* list directory */
 				DIR		*d;
 				d = opendir("/dev");
 
 				if (d) {
-
 					struct dirent	*direntry;
 					char devname[24];
 
@@ -414,290 +358,210 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 
 						printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
 					}
-
 					closedir(d);
+					ret = TRANSITION_CHANGED;
+					mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+
 
 				} else {
 					/* failed opening dir */
-					warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
-					return 1;
+					mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
+					ret = TRANSITION_DENIED;
 				}
+			} else {
+				mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
+				ret = TRANSITION_DENIED;
 			}
-
 			break;
 
 		default:
-			warnx("Unknown hil state");
+			warnx("Unknown HIL state");
 			break;
 		}
 	}
 
-	if (valid_transition) {
+	if (ret == TRANSITION_CHANGED) {
 		current_status->hil_state = new_state;
-
 		current_status->timestamp = hrt_absolute_time();
-		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
 		// XXX also set lockdown here
-
-		ret = OK;
-
-	} else {
-		mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
+		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
 	}
-
 	return ret;
 }
 
-
 /**
-* Transition from one failsafe state to another
-*/
-transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state)
+ * Check failsafe and main status and set navigation status for navigator accordingly
+ */
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished)
 {
-	transition_result_t ret = TRANSITION_DENIED;
+	navigation_state_t nav_state_old = status->nav_state;
 
-	/* 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;
+	bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR);
+	status->failsafe = false;
+
+	/* evaluate main state to decide in normal (non-failsafe) mode */
+	switch (status->main_state) {
+	case MAIN_STATE_ACRO:
+	case MAIN_STATE_MANUAL:
+	case MAIN_STATE_ALTCTL:
+	case MAIN_STATE_POSCTL:
+		/* require RC for all manual modes */
+		if (status->rc_signal_lost && armed) {
+			status->failsafe = true;
+
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
+
+		} else {
+			switch (status->main_state) {
+			case MAIN_STATE_ACRO:
+				status->nav_state = NAVIGATION_STATE_ACRO;
+				break;
+
+			case MAIN_STATE_MANUAL:
+				status->nav_state = NAVIGATION_STATE_MANUAL;
+				break;
+
+			case MAIN_STATE_ALTCTL:
+				status->nav_state = NAVIGATION_STATE_ALTCTL;
+				break;
+
+			case MAIN_STATE_POSCTL:
+				status->nav_state = NAVIGATION_STATE_POSCTL;
+				break;
+
+			default:
+				status->nav_state = NAVIGATION_STATE_MANUAL;
+				break;
+			}
 		}
+		break;
 
-	} else {
-		switch (new_failsafe_state) {
-		case FAILSAFE_STATE_NORMAL:
-			/* always allowed (except from TERMINATION state) */
-			ret = TRANSITION_CHANGED;
-			break;
+	case MAIN_STATE_AUTO_MISSION:
+		/* go into failsafe
+		 * - if either the datalink is enabled and lost as well as RC is lost
+		 * - if there is no datalink and the mission is finished */
+		if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
+		    (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
+			status->failsafe = true;
 
-		case FAILSAFE_STATE_RTL:
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
+
+		/* also go into failsafe if just datalink is lost */
+		} else if (status->data_link_lost && data_link_loss_enabled) {
+			status->failsafe = true;
 
-			/* 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;
+				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-			break;
+		/* don't bother if RC is lost and mission is not yet finished */
+		} else if (status->rc_signal_lost) {
 
-		case FAILSAFE_STATE_LAND:
+			/* this mode is ok, we don't need RC for missions */
+			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+		} else {
+			/* everything is perfect */
+			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+		}
+		break;
+
+	case MAIN_STATE_AUTO_LOITER:
+		/* go into failsafe if datalink and RC is lost */
+		if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
+			status->failsafe = true;
 
-			/* 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;
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-			break;
+		/* also go into failsafe if just datalink is lost */
+		} else if (status->data_link_lost && data_link_loss_enabled) {
+			status->failsafe = true;
 
-		case FAILSAFE_STATE_TERMINATION:
-			/* always allowed */
-			ret = TRANSITION_CHANGED;
-			break;
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
 
-		default:
-			break;
+		/* go into failsafe if RC is lost and datalink loss is not set up */
+		} else if (status->rc_signal_lost && !data_link_loss_enabled) {
+			status->failsafe = true;
+
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
+
+		/* don't bother if RC is lost if datalink is connected */
+		} else if (status->rc_signal_lost) {
+
+			/* this mode is ok, we don't need RC for loitering */
+			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+		} else {
+			/* everything is perfect */
+			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
 		}
+		break;
 
-		if (ret == TRANSITION_CHANGED) {
-			if (status->failsafe_state != new_failsafe_state) {
-				status->failsafe_state = new_failsafe_state;
-				failsafe_state_changed = true;
+	case MAIN_STATE_AUTO_RTL:
+		/* require global position and home */
+		if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) {
+			status->failsafe = true;
 
+			if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
 			} else {
-				ret = TRANSITION_NOT_CHANGED;
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
+		} else {
+			status->nav_state = NAVIGATION_STATE_AUTO_RTL;
 		}
+		break;
+
+	default:
+		break;
 	}
 
-	return ret;
+	return status->nav_state != nav_state_old;
 }
 
-
-
-// /*
-//  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
-//  */
-
-// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
-//  *
-//  * START SUBSYSTEM/EMERGENCY FUNCTIONS
-//  * */
-
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was removed something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was disabled something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-
-///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
-//
-//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
-//{
-//	int ret = 1;
-//
-////	/* Switch on HIL if in standby and not already in HIL mode */
-////	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
-////	    && !current_status->flag_hil_enabled) {
-////		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
-////			/* Enable HIL on request */
-////			current_status->flag_hil_enabled = true;
-////			ret = OK;
-////			state_machine_publish(status_pub, current_status, mavlink_fd);
-////			publish_armed_status(current_status);
-////			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
-////
-////		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
-////			   current_status->flag_fmu_armed) {
-////
-////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
-////
-////		} else {
-////
-////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
-////		}
-////	}
-//
-//	/* switch manual / auto */
-//	if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
-//		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
-//
-//	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
-//		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
-//
-//	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
-//		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
-//
-//	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-//		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
-//	}
-//
-//	/* vehicle is disarmed, mode requests arming */
-//	if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-//		/* only arm in standby state */
-//		// XXX REMOVE
-//		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-//			ret = OK;
-//			printf("[cmd] arming due to command request\n");
-//		}
-//	}
-//
-//	/* vehicle is armed, mode requests disarming */
-//	if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-//		/* only disarm in ground ready */
-//		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-//			ret = OK;
-//			printf("[cmd] disarming due to command request\n");
-//		}
-//	}
-//
-//	/* NEVER actually switch off HIL without reboot */
-//	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
-//		warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
-//		mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
-//		ret = ERROR;
-//	}
-//
-//	return ret;
-//}
-
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index abb917873..11072403e 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -56,25 +56,15 @@ typedef enum {
 
 } transition_result_t;
 
-transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
-
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
-bool check_arming_state_changed();
+transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
+		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
 
 transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
 
-bool check_main_state_changed();
-
-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();
+transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, 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);
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
 
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 0a6d3fa8f..e4f805dc0 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -49,11 +49,11 @@
 #include <math.h>
 #include <poll.h>
 #include <time.h>
-#include <drivers/drv_hrt.h>
+#include <float.h>
 
 #define SENSOR_COMBINED_SUB
 
-
+#include <drivers/drv_hrt.h>
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_mag.h>
@@ -74,6 +74,7 @@
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/home_position.h>
+#include <uORB/topics/wind_estimate.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
@@ -82,7 +83,7 @@
 #include <mathlib/mathlib.h>
 #include <mavlink/mavlink_log.h>
 
-#include "estimator.h"
+#include "estimator_23states.h"
 
 
 
@@ -95,7 +96,6 @@ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
 
 __EXPORT uint32_t millis();
 
-static uint64_t last_run = 0;
 static uint64_t IMUmsec = 0;
 static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
 
@@ -120,7 +120,7 @@ public:
 	/**
 	 * Start the sensors task.
 	 *
-	 * @return		OK on success.
+	 * @return	OK on success.
 	 */
 	int		start();
 
@@ -134,6 +134,20 @@ public:
 	 */
 	int		trip_nan();
 
+	/**
+	 * Enable logging.
+	 *
+	 * @param	enable Set to true to enable logging, false to disable
+	 */
+	int		enable_logging(bool enable);
+
+	/**
+	 * Set debug level.
+	 *
+	 * @param	debug Desired debug level - 0 to disable.
+	 */
+	int		set_debuglevel(unsigned debug) { _debug = debug; return 0; }
+
 private:
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
@@ -158,6 +172,7 @@ private:
 	orb_advert_t	_global_pos_pub;		/**< global position */
 	orb_advert_t	_local_pos_pub;			/**< position in local frame */
 	orb_advert_t	_estimator_status_pub;		/**< status of the estimator */
+	orb_advert_t	_wind_pub;			/**< wind estimate */
 
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
 	struct gyro_report				_gyro;
@@ -169,6 +184,7 @@ private:
 	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
 	struct vehicle_local_position_s			_local_pos;		/**< local vehicle position */
 	struct vehicle_gps_position_s			_gps;			/**< GPS position */
+	struct wind_estimate_s				_wind;			/**< Wind estimate */
 
 	struct gyro_scale				_gyro_offsets;
 	struct accel_scale				_accel_offsets;
@@ -181,24 +197,28 @@ private:
 	struct map_projection_reference_s	_pos_ref;
 
 	float						_baro_ref;		/**< barometer reference altitude */
-	float						_baro_gps_offset;	/**< offset between GPS and baro */
+	float						_baro_ref_offset;	/**< offset between initial baro reference and GPS init baro altitude */
+	float						_baro_gps_offset;	/**< offset between baro altitude (at GPS init time) and GPS altitude */
 
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t	_loop_perf;			///< loop performance counter
 	perf_counter_t	_perf_gyro;			///<local performance counter for gyro updates
-	perf_counter_t	_perf_accel;			///<local performance counter for accel updates
 	perf_counter_t	_perf_mag;			///<local performance counter for mag updates
 	perf_counter_t	_perf_gps;			///<local performance counter for gps updates
 	perf_counter_t	_perf_baro;			///<local performance counter for baro updates
 	perf_counter_t	_perf_airspeed;			///<local performance counter for airspeed updates
+	perf_counter_t	_perf_reset;			///<local performance counter for filter resets
 
-	bool						_initialized;
 	bool						_baro_init;
 	bool						_gps_initialized;
-	uint64_t					_gps_start_time;
-	uint64_t					_filter_start_time;
+	hrt_abstime					_gps_start_time;
+	hrt_abstime					_filter_start_time;
+	hrt_abstime					_last_sensor_timestamp;
+	hrt_abstime					_last_run;
 	bool						_gyro_valid;
 	bool						_accel_valid;
 	bool						_mag_valid;
+	bool						_ekf_logging;		///< log EKF state
+	unsigned					_debug;			///< debug level - default 0
 
 	int						_mavlink_fd;
 
@@ -246,6 +266,10 @@ private:
 
 	AttPosEKF					*_ekf;
 
+	float						_velocity_xy_filtered;
+	float						_velocity_z_filtered;
+	float						_airspeed_filtered;
+
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -268,9 +292,16 @@ private:
 	static void	task_main_trampoline(int argc, char *argv[]);
 
 	/**
-	 * Main sensor collection task.
+	 * Main filter task.
 	 */
 	void		task_main();
+
+	/**
+	 * Check filter sanity state
+	 *
+	 * @return zero if ok, non-zero for a filter error condition.
+	 */
+	int		check_filter_state();
 };
 
 namespace estimator
@@ -312,6 +343,7 @@ FixedwingEstimator::FixedwingEstimator() :
 	_global_pos_pub(-1),
 	_local_pos_pub(-1),
 	_estimator_status_pub(-1),
+	_wind_pub(-1),
 
 	_att({}),
 	_gyro({}),
@@ -323,6 +355,7 @@ FixedwingEstimator::FixedwingEstimator() :
 	_global_pos({}),
 	_local_pos({}),
 	_gps({}),
+	_wind({}),
 
 	_gyro_offsets({}),
 	_accel_offsets({}),
@@ -333,29 +366,34 @@ FixedwingEstimator::FixedwingEstimator() :
 	#endif
 
 	_baro_ref(0.0f),
+	_baro_ref_offset(0.0f),
 	_baro_gps_offset(0.0f),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_COUNT, "ekf_att_pos_estimator")),
-	_perf_gyro(perf_alloc(PC_COUNT, "ekf_att_pos_gyro_upd")),
-	_perf_accel(perf_alloc(PC_COUNT, "ekf_att_pos_accel_upd")),
-	_perf_mag(perf_alloc(PC_COUNT, "ekf_att_pos_mag_upd")),
-	_perf_gps(perf_alloc(PC_COUNT, "ekf_att_pos_gps_upd")),
-	_perf_baro(perf_alloc(PC_COUNT, "ekf_att_pos_baro_upd")),
-	_perf_airspeed(perf_alloc(PC_COUNT, "ekf_att_pos_aspd_upd")),
+	_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
+	_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
+	_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
+	_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
+	_perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
+	_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
+	_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
 
 /* states */
-	_initialized(false),
 	_baro_init(false),
 	_gps_initialized(false),
 	_gyro_valid(false),
 	_accel_valid(false),
 	_mag_valid(false),
+	_ekf_logging(true),
+	_debug(0),
 	_mavlink_fd(-1),
-	_ekf(nullptr)
+	_ekf(nullptr),
+	_velocity_xy_filtered(0.0f),
+	_velocity_z_filtered(0.0f),
+	_airspeed_filtered(0.0f)
 {
 
-	last_run = hrt_absolute_time();
+	_last_run = hrt_absolute_time();
 
 	_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
 	_parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS");
@@ -439,10 +477,20 @@ FixedwingEstimator::~FixedwingEstimator()
 		} while (_estimator_task != -1);
 	}
 
+	delete _ekf;
+
 	estimator::g_estimator = nullptr;
 }
 
 int
+FixedwingEstimator::enable_logging(bool logging)
+{
+	_ekf_logging = logging;
+
+	return 0;
+}
+
+int
 FixedwingEstimator::parameters_update()
 {
 
@@ -500,6 +548,151 @@ FixedwingEstimator::vehicle_status_poll()
 	}
 }
 
+int
+FixedwingEstimator::check_filter_state()
+{
+	/*
+	 *    CHECK IF THE INPUT DATA IS SANE
+	 */
+
+	struct ekf_status_report ekf_report;
+
+	int check = _ekf->CheckAndBound(&ekf_report);
+
+	const char* ekfname = "att pos estimator: ";
+
+	switch (check) {
+		case 0:
+			/* all ok */
+			break;
+		case 1:
+		{
+			const char* str = "NaN in states, resetting";
+			warnx("%s", str);
+			mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
+			break;
+		}
+		case 2:
+		{
+			const char* str = "stale IMU data, resetting";
+			warnx("%s", str);
+			mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
+			break;
+		}
+		case 3:
+		{
+			const char* str = "switching to dynamic state";
+			warnx("%s", str);
+			mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
+			break;
+		}
+		case 4:
+		{
+			const char* str = "excessive gyro offsets";
+			warnx("%s", str);
+			mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
+			break;
+		}
+		case 5:
+		{
+			const char* str = "GPS velocity divergence";
+			warnx("%s", str);
+			mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
+			break;
+		}
+		case 6:
+		{
+			const char* str = "excessive covariances";
+			warnx("%s", str);
+			mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
+			break;
+		}
+
+		default:
+		{
+			const char* str = "unknown reset condition";
+			warnx("%s", str);
+			mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
+		}
+	}
+
+	struct estimator_status_report rep;
+	memset(&rep, 0, sizeof(rep));
+
+	// If error flag is set, we got a filter reset
+	if (check && ekf_report.error) {
+
+		// Count the reset condition
+		perf_count(_perf_reset);
+
+	} else if (_ekf_logging) {
+		_ekf->GetFilterState(&ekf_report);
+	}
+
+	if (_ekf_logging || check) {
+		rep.timestamp = hrt_absolute_time();
+
+		rep.nan_flags |= (((uint8_t)ekf_report.angNaN)		<< 0);
+		rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN)	<< 1);
+		rep.nan_flags |= (((uint8_t)ekf_report.KHNaN)		<< 2);
+		rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN)		<< 3);
+		rep.nan_flags |= (((uint8_t)ekf_report.PNaN)		<< 4);
+		rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN)	<< 5);
+		rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN)	<< 6);
+		rep.nan_flags |= (((uint8_t)ekf_report.statesNaN)	<< 7);
+
+		rep.health_flags |= (((uint8_t)ekf_report.velHealth)	<< 0);
+		rep.health_flags |= (((uint8_t)ekf_report.posHealth)	<< 1);
+		rep.health_flags |= (((uint8_t)ekf_report.hgtHealth)	<< 2);
+		rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive)	<< 3);
+
+		rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout)	<< 0);
+		rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout)	<< 1);
+		rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout)	<< 2);
+		rep.timeout_flags |= (((uint8_t)ekf_report.imuTimeout)	<< 3);
+
+		if (_debug > 10) {
+
+			if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) {
+				warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s",
+					((rep.health_flags & (1 << 0)) ? "OK" : "ERR"),
+					((rep.health_flags & (1 << 1)) ? "OK" : "ERR"),
+					((rep.health_flags & (1 << 2)) ? "OK" : "ERR"),
+					((rep.health_flags & (1 << 3)) ? "OK" : "ERR"));
+			}
+
+			if (rep.timeout_flags) {
+				warnx("timeout: %s%s%s%s",
+					((rep.timeout_flags & (1 << 0)) ? "VEL " : ""),
+					((rep.timeout_flags & (1 << 1)) ? "POS " : ""),
+					((rep.timeout_flags & (1 << 2)) ? "HGT " : ""),
+					((rep.timeout_flags & (1 << 3)) ? "IMU " : ""));
+			}
+		}
+
+		// Copy all states or at least all that we can fit
+		unsigned ekf_n_states = ekf_report.n_states;
+		unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
+		rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;
+
+		for (unsigned i = 0; i < rep.n_states; i++) {
+			rep.states[i] = ekf_report.states[i];
+		}
+
+		for (unsigned i = 0; i < rep.n_states; i++) {
+			rep.states[i] = ekf_report.states[i];
+		}
+
+		if (_estimator_status_pub > 0) {
+			orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep);
+		} else {
+			_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
+		}
+	}
+
+	return check;
+}
+
 void
 FixedwingEstimator::task_main_trampoline(int argc, char *argv[])
 {
@@ -516,7 +709,7 @@ FixedwingEstimator::task_main()
 	_filter_start_time = hrt_absolute_time();
 
 	if (!_ekf) {
-		errx(1, "failed allocating EKF filter - out of RAM!");
+		errx(1, "OUT OF MEM!");
 	}
 
 	/*
@@ -544,7 +737,7 @@ FixedwingEstimator::task_main()
 #else
 	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	/* XXX remove this!, BUT increase the data buffer size! */
-	orb_set_interval(_sensor_combined_sub, 4);
+	orb_set_interval(_sensor_combined_sub, 9);
 #endif
 
 	/* sets also parameters in the EKF object */
@@ -572,6 +765,13 @@ FixedwingEstimator::task_main()
 	bool newAdsData = false;
 	bool newDataMag = false;
 
+	float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
+
+	uint64_t last_gps = 0;
+	_gps.vel_n_m_s = 0.0f;
+	_gps.vel_e_m_s = 0.0f;
+	_gps.vel_d_m_s = 0.0f;
+
 	while (!_task_should_exit) {
 
 		/* wait for up to 500ms for data */
@@ -583,7 +783,7 @@ FixedwingEstimator::task_main()
 
 		/* 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);
+			warn("POLL ERR %d, %d", pret, errno);
 			continue;
 		}
 
@@ -609,8 +809,6 @@ FixedwingEstimator::task_main()
 			bool accel_updated;
 			bool mag_updated;
 
-			hrt_abstime last_sensor_timestamp;
-
 			perf_count(_perf_gyro);
 
 			/* Reset baro reference if switching to HIL, reset sensor states */
@@ -634,12 +832,12 @@ FixedwingEstimator::task_main()
 
 				_baro_init = false;
 				_gps_initialized = false;
-				last_sensor_timestamp = hrt_absolute_time();
-				last_run = last_sensor_timestamp;
+				_last_sensor_timestamp = hrt_absolute_time();
+				_last_run = _last_sensor_timestamp;
 
 				_ekf->ZeroVariables();
 				_ekf->dtIMU = 0.01f;
-				_filter_start_time = last_sensor_timestamp;
+				_filter_start_time = _last_sensor_timestamp;
 
 				/* now skip this loop and get data on the next one, which will also re-init the filter */
 				continue;
@@ -657,15 +855,14 @@ FixedwingEstimator::task_main()
 			orb_check(_accel_sub, &accel_updated);
 
 			if (accel_updated) {
-				perf_count(_perf_accel);
 				orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
 			}
 
-			last_sensor_timestamp = _gyro.timestamp;
+			_last_sensor_timestamp = _gyro.timestamp;
 			IMUmsec = _gyro.timestamp / 1e3f;
 
-			float deltaT = (_gyro.timestamp - last_run) / 1e6f;
-			last_run = _gyro.timestamp;
+			float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
+			_last_run = _gyro.timestamp;
 
 			/* guard against too large deltaT's */
 			if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
@@ -725,17 +922,17 @@ FixedwingEstimator::task_main()
 
 
 			// Copy gyro and accel
-			last_sensor_timestamp = _sensor_combined.timestamp;
+			_last_sensor_timestamp = _sensor_combined.timestamp;
 			IMUmsec = _sensor_combined.timestamp / 1e3f;
 
-			float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f;
+			float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
 
 			/* guard against too large deltaT's */
 			if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
 				deltaT = 0.01f;
 			}
 
-			last_run = _sensor_combined.timestamp;
+			_last_run = _sensor_combined.timestamp;
 
 			// Always store data, independent of init status
 			/* fill in last data set */
@@ -753,6 +950,7 @@ FixedwingEstimator::task_main()
 				}
 
 				_gyro_valid = true;
+				perf_count(_perf_gyro);
 			}
 
 			if (accel_updated) {
@@ -784,6 +982,8 @@ FixedwingEstimator::task_main()
 
 #endif
 
+			//warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
+
 			bool airspeed_updated;
 			orb_check(_airspeed_sub, &airspeed_updated);
 
@@ -803,7 +1003,7 @@ FixedwingEstimator::task_main()
 
 			if (gps_updated) {
 
-				uint64_t last_gps = _gps.timestamp_position;
+				last_gps = _gps.timestamp_position;
 
 				orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
 				perf_count(_perf_gps);
@@ -871,6 +1071,8 @@ FixedwingEstimator::task_main()
 					warnx("ALT REF INIT");
 				}
 
+				perf_count(_perf_baro);
+
 				newHgtData = true;
 			} else {
 				newHgtData = false;
@@ -921,127 +1123,39 @@ FixedwingEstimator::task_main()
 				newDataMag = false;
 			}
 
-
-			/**
-			 *    CHECK IF THE INPUT DATA IS SANE
+			/*
+			 *    CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
 			 */
-			int check = _ekf->CheckAndBound();
-
-			const char* ekfname = "[ekf] ";
-
-			switch (check) {
-				case 0:
-					/* all ok */
-					break;
-				case 1:
-				{
-					const char* str = "NaN in states, resetting";
-					warnx("%s", str);
-					mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
-					break;
-				}
-				case 2:
-				{
-					const char* str = "stale IMU data, resetting";
-					warnx("%s", str);
-					mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
-					break;
-				}
-				case 3:
-				{
-					const char* str = "switching to dynamic state";
-					warnx("%s", str);
-					mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
-					break;
-				}
-
-				default:
-				{
-					const char* str = "unknown reset condition";
-					warnx("%s", str);
-					mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
-				}
-			}
-
-			// warn on fatal resets
-			if (check == 1) {
-				warnx("NUMERIC ERROR IN FILTER");
-			}
-
-			// If non-zero, we got a filter reset
-			if (check) {
-
-				struct ekf_status_report ekf_report;
-
-				_ekf->GetLastErrorState(&ekf_report);
-
-				struct estimator_status_report rep;
-				memset(&rep, 0, sizeof(rep));
-				rep.timestamp = hrt_absolute_time();
-
-				rep.states_nan = ekf_report.statesNaN;
-				rep.covariance_nan = ekf_report.covarianceNaN;
-				rep.kalman_gain_nan = ekf_report.kalmanGainsNaN;
-
-				// Copy all states or at least all that we can fit
-				unsigned i = 0;
-				unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0]));
-				unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
-				rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;
-
-				while ((i < ekf_n_states) && (i < max_states)) {
-
-					rep.states[i] = ekf_report.states[i];
-					i++;
-				}
-
-				if (_estimator_status_pub > 0) {
-					orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep);
-				} else {
-					_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
-				}
-
-				/* set sensors to de-initialized state */
-				_gyro_valid = false;
-				_accel_valid = false;
-				_mag_valid = false;
-
-				_baro_init = false;
-				_gps_initialized = false;
-				last_sensor_timestamp = hrt_absolute_time();
-				last_run = last_sensor_timestamp;
-
-				_ekf->ZeroVariables();
-				_ekf->dtIMU = 0.01f;
-
-				// Let the system re-initialize itself
+			if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) {
 				continue;
-
 			}
 
-
 			/**
 			 *    PART TWO: EXECUTE THE FILTER
+			 *
+			 *    We run the filter only once all data has been fetched
 			 **/
 
-			if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) {
+			if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
 
 				float initVelNED[3];
 
-				if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) {
-
-					initVelNED[0] = _gps.vel_n_m_s;
-					initVelNED[1] = _gps.vel_e_m_s;
-					initVelNED[2] = _gps.vel_d_m_s;
+				/* Initialize the filter first */
+				if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) {
 
 					// GPS is in scaled integers, convert
 					double lat = _gps.lat / 1.0e7;
 					double lon = _gps.lon / 1.0e7;
 					float gps_alt = _gps.alt / 1e3f;
 
+					initVelNED[0] = _gps.vel_n_m_s;
+					initVelNED[1] = _gps.vel_e_m_s;
+					initVelNED[2] = _gps.vel_d_m_s;
+
 					// Set up height correctly
 					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
-					_baro_gps_offset = _baro_ref - _baro.altitude;
+					_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
+					_baro_gps_offset = _baro.altitude - gps_alt;
 					_ekf->baroHgt = _baro.altitude;
 					_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
 
@@ -1065,10 +1179,13 @@ FixedwingEstimator::task_main()
 
 					map_projection_init(&_pos_ref, lat, lon);
 					mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
+
+					#if 0
 					warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
 						(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
-					warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset);
-					warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination));
+					warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset);
+					warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination));
+					#endif
 
 					_gps_initialized = true;
 
@@ -1077,263 +1194,322 @@ FixedwingEstimator::task_main()
 					initVelNED[0] = 0.0f;
 					initVelNED[1] = 0.0f;
 					initVelNED[2] = 0.0f;
-					_ekf->posNED[0] = 0.0f;
-					_ekf->posNED[1] = 0.0f;
-					_ekf->posNED[2] = 0.0f;
-
-					_ekf->posNE[0] = _ekf->posNED[0];
-					_ekf->posNE[1] = _ekf->posNED[1];
+					_ekf->posNE[0] = posNED[0];
+					_ekf->posNE[1] = posNED[1];
 
 					_local_pos.ref_alt = _baro_ref;
+					_baro_ref_offset = 0.0f;
 					_baro_gps_offset = 0.0f;
 
 					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
-				}
-			}
-
-			// If valid IMU data and states initialised, predict states and covariances
-			if (_ekf->statesInitialised) {
-				// Run the strapdown INS equations every IMU update
-				_ekf->UpdateStrapdownEquationsNED();
-#if 0
-				// debug code - could be tunred into a filter mnitoring/watchdog function
-				float tempQuat[4];
-
-				for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
-
-				quat2eul(eulerEst, tempQuat);
-
-				for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
-
-				if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
-
-				if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
-
-#endif
-				// store the predicted states for subsequent use by measurement fusion
-				_ekf->StoreStates(IMUmsec);
-				// Check if on ground - status is used by covariance prediction
-				_ekf->OnGroundCheck();
-				// sum delta angles and time used by covariance prediction
-				_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
-				_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
-				dt += _ekf->dtIMU;
-
-				// perform a covariance prediction if the total delta angle has exceeded the limit
-				// or the time limit will be exceeded at the next IMU update
-				if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
-					_ekf->CovariancePrediction(dt);
-					_ekf->summedDelAng.zero();
-					_ekf->summedDelVel.zero();
-					dt = 0.0f;
-				}
+				} else if (_ekf->statesInitialised) {
 
-				_initialized = true;
-			}
+					// We're apparently initialized in this case now
 
-			// Fuse GPS Measurements
-			if (newDataGps && _gps_initialized) {
-				// Convert GPS measurements to Pos NE, hgt and Vel NED
-				_ekf->velNED[0] = _gps.vel_n_m_s;
-				_ekf->velNED[1] = _gps.vel_e_m_s;
-				_ekf->velNED[2] = _gps.vel_d_m_s;
-				_ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
-
-				_ekf->posNE[0] = _ekf->posNED[0];
-				_ekf->posNE[1] = _ekf->posNED[1];
-				// set fusion flags
-				_ekf->fuseVelData = true;
-				_ekf->fusePosData = true;
-				// recall states stored at time of measurement after adjusting for delays
-				_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
-				_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
-				// run the fusion step
-				_ekf->FuseVelposNED();
-
-			} else if (_ekf->statesInitialised) {
-				// Convert GPS measurements to Pos NE, hgt and Vel NED
-				_ekf->velNED[0] = 0.0f;
-				_ekf->velNED[1] = 0.0f;
-				_ekf->velNED[2] = 0.0f;
-				_ekf->posNED[0] = 0.0f;
-				_ekf->posNED[1] = 0.0f;
-				_ekf->posNED[2] = 0.0f;
-
-				_ekf->posNE[0] = _ekf->posNED[0];
-				_ekf->posNE[1] = _ekf->posNED[1];
-				// set fusion flags
-				_ekf->fuseVelData = true;
-				_ekf->fusePosData = true;
-				// recall states stored at time of measurement after adjusting for delays
-				_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
-				_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
-				// run the fusion step
-				_ekf->FuseVelposNED();
+					int check = check_filter_state();
 
-			} else {
-				_ekf->fuseVelData = false;
-				_ekf->fusePosData = false;
-			}
+					if (check) {
+						// Let the system re-initialize itself
+						continue;
+					}
 
-			if (newHgtData && _ekf->statesInitialised) {
-				// Could use a blend of GPS and baro alt data if desired
-				_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
-				_ekf->fuseHgtData = true;
-				// recall states stored at time of measurement after adjusting for delays
-				_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
-				// run the fusion step
-				_ekf->FuseVelposNED();
 
-			} else {
-				_ekf->fuseHgtData = false;
-			}
+					// Run the strapdown INS equations every IMU update
+					_ekf->UpdateStrapdownEquationsNED();
+	#if 0
+					// debug code - could be tunred into a filter mnitoring/watchdog function
+					float tempQuat[4];
 
-			// Fuse Magnetometer Measurements
-			if (newDataMag && _ekf->statesInitialised) {
-				_ekf->fuseMagData = true;
-				_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
+					for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
 
-			} else {
-				_ekf->fuseMagData = false;
-			}
+					quat2eul(eulerEst, tempQuat);
 
-			if (_ekf->statesInitialised) _ekf->FuseMagnetometer();
+					for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
 
-			// Fuse Airspeed Measurements
-			if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
-				_ekf->fuseVtasData = true;
-				_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
-				_ekf->FuseAirspeed();
+					if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
 
-			} else {
-				_ekf->fuseVtasData = false;
-			}
+					if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
 
-			// Publish results
-			if (_initialized && (check == OK)) {
-
-
-
-				// State vector:
-				// 0-3: quaternions (q0, q1, q2, q3)
-				// 4-6: Velocity - m/sec (North, East, Down)
-				// 7-9: Position - m (North, East, Down)
-				// 10-12: Delta Angle bias - rad (X,Y,Z)
-				// 13-14: Wind Vector  - m/sec (North,East)
-				// 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down)
-				// 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z)
-
-				math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
-				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);
-
-				_att.timestamp = last_sensor_timestamp;
-				_att.q[0] = _ekf->states[0];
-				_att.q[1] = _ekf->states[1];
-				_att.q[2] = _ekf->states[2];
-				_att.q[3] = _ekf->states[3];
-				_att.q_valid = true;
-				_att.R_valid = true;
-
-				_att.timestamp = last_sensor_timestamp;
-				_att.roll = euler(0);
-				_att.pitch = euler(1);
-				_att.yaw = euler(2);
-
-				_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
-				_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
-				_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
-				// gyro offsets
-				_att.rate_offsets[0] = _ekf->states[10];
-				_att.rate_offsets[1] = _ekf->states[11];
-				_att.rate_offsets[2] = _ekf->states[12];
-
-				/* lazily publish the attitude only once available */
-				if (_att_pub > 0) {
-					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
+	#endif
+					// store the predicted states for subsequent use by measurement fusion
+					_ekf->StoreStates(IMUmsec);
+					// Check if on ground - status is used by covariance prediction
+					_ekf->OnGroundCheck();
+					// sum delta angles and time used by covariance prediction
+					_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
+					_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
+					dt += _ekf->dtIMU;
+
+					// perform a covariance prediction if the total delta angle has exceeded the limit
+					// or the time limit will be exceeded at the next IMU update
+					if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
+						_ekf->CovariancePrediction(dt);
+						_ekf->summedDelAng.zero();
+						_ekf->summedDelVel.zero();
+						dt = 0.0f;
+					}
 
-				} else {
-					/* advertise and publish */
-					_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
-				}
-			}
+					// Fuse GPS Measurements
+					if (newDataGps && _gps_initialized) {
+						// Convert GPS measurements to Pos NE, hgt and Vel NED
+
+						float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f;
+
+						// Calculate acceleration predicted by GPS velocity change
+						if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) ||
+							(fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) ||
+							(fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) {
+
+							_ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt;
+							_ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt;
+							_ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt;
+						}
+
+						_ekf->velNED[0] = _gps.vel_n_m_s;
+						_ekf->velNED[1] = _gps.vel_e_m_s;
+						_ekf->velNED[2] = _gps.vel_d_m_s;
+						_ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
+
+						_ekf->posNE[0] = posNED[0];
+						_ekf->posNE[1] = posNED[1];
+						// set fusion flags
+						_ekf->fuseVelData = true;
+						_ekf->fusePosData = true;
+						// recall states stored at time of measurement after adjusting for delays
+						_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+						_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+						// run the fusion step
+						_ekf->FuseVelposNED();
+
+					} else if (_ekf->statesInitialised) {
+						// Convert GPS measurements to Pos NE, hgt and Vel NED
+						_ekf->velNED[0] = 0.0f;
+						_ekf->velNED[1] = 0.0f;
+						_ekf->velNED[2] = 0.0f;
+
+						_ekf->posNE[0] = 0.0f;
+						_ekf->posNE[1] = 0.0f;
+						// set fusion flags
+						_ekf->fuseVelData = true;
+						_ekf->fusePosData = true;
+						// recall states stored at time of measurement after adjusting for delays
+						_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+						_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+						// run the fusion step
+						_ekf->FuseVelposNED();
+
+					} else {
+						_ekf->fuseVelData = false;
+						_ekf->fusePosData = false;
+					}
 
-			if (_gps_initialized) {
-				_local_pos.timestamp = last_sensor_timestamp;
-				_local_pos.x = _ekf->states[7];
-				_local_pos.y = _ekf->states[8];
-				// XXX need to announce change of Z reference somehow elegantly
-				_local_pos.z = _ekf->states[9] - _baro_gps_offset;
+					if (newHgtData && _ekf->statesInitialised) {
+						// Could use a blend of GPS and baro alt data if desired
+						_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
+						_ekf->fuseHgtData = true;
+						// recall states stored at time of measurement after adjusting for delays
+						_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
+						// run the fusion step
+						_ekf->FuseVelposNED();
+
+					} else {
+						_ekf->fuseHgtData = false;
+					}
 
-				_local_pos.vx = _ekf->states[4];
-				_local_pos.vy = _ekf->states[5];
-				_local_pos.vz = _ekf->states[6];
+					// Fuse Magnetometer Measurements
+					if (newDataMag && _ekf->statesInitialised) {
+						_ekf->fuseMagData = true;
+						_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
 
-				_local_pos.xy_valid = _gps_initialized;
-				_local_pos.z_valid = true;
-				_local_pos.v_xy_valid = _gps_initialized;
-				_local_pos.v_z_valid = true;
-				_local_pos.xy_global = true;
+						_ekf->magstate.obsIndex = 0;
+						_ekf->FuseMagnetometer();
+						_ekf->FuseMagnetometer();
+						_ekf->FuseMagnetometer();
 
-				_local_pos.z_global = false;
-				_local_pos.yaw = _att.yaw;
+					} else {
+						_ekf->fuseMagData = false;
+					}
 
-				/* lazily publish the local position only once available */
-				if (_local_pos_pub > 0) {
-					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);
+					// Fuse Airspeed Measurements
+					if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
+						_ekf->fuseVtasData = true;
+						_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
+						_ekf->FuseAirspeed();
 
-				} else {
-					/* advertise and publish */
-					_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
-				}
+					} else {
+						_ekf->fuseVtasData = false;
+					}
 
-				_global_pos.timestamp = _local_pos.timestamp;
 
-				if (_local_pos.xy_global) {
-					double est_lat, est_lon;
-					map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
-					_global_pos.lat = est_lat;
-					_global_pos.lon = est_lon;
-					_global_pos.time_gps_usec = _gps.time_gps_usec;
-					_global_pos.eph = _gps.eph_m;
-					_global_pos.epv = _gps.epv_m;
-				}
+					// Output results
+					math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
+					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);
+
+					_att.timestamp = _last_sensor_timestamp;
+					_att.q[0] = _ekf->states[0];
+					_att.q[1] = _ekf->states[1];
+					_att.q[2] = _ekf->states[2];
+					_att.q[3] = _ekf->states[3];
+					_att.q_valid = true;
+					_att.R_valid = true;
+
+					_att.timestamp = _last_sensor_timestamp;
+					_att.roll = euler(0);
+					_att.pitch = euler(1);
+					_att.yaw = euler(2);
+
+					_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
+					_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
+					_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
+					// gyro offsets
+					_att.rate_offsets[0] = _ekf->states[10];
+					_att.rate_offsets[1] = _ekf->states[11];
+					_att.rate_offsets[2] = _ekf->states[12];
+
+					/* lazily publish the attitude only once available */
+					if (_att_pub > 0) {
+						/* publish the attitude setpoint */
+						orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
+
+					} else {
+						/* advertise and publish */
+						_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
+					}
 
-				if (_local_pos.v_xy_valid) {
-					_global_pos.vel_n = _local_pos.vx;
-					_global_pos.vel_e = _local_pos.vy;
-				} else {
-					_global_pos.vel_n = 0.0f;
-					_global_pos.vel_e = 0.0f;
-				}
+					if (_gps_initialized) {
+						_local_pos.timestamp = _last_sensor_timestamp;
+						_local_pos.x = _ekf->states[7];
+						_local_pos.y = _ekf->states[8];
+						// XXX need to announce change of Z reference somehow elegantly
+						_local_pos.z = _ekf->states[9] - _baro_ref_offset;
+
+						_local_pos.vx = _ekf->states[4];
+						_local_pos.vy = _ekf->states[5];
+						_local_pos.vz = _ekf->states[6];
+
+						_local_pos.xy_valid = _gps_initialized;
+						_local_pos.z_valid = true;
+						_local_pos.v_xy_valid = _gps_initialized;
+						_local_pos.v_z_valid = true;
+						_local_pos.xy_global = true;
+
+						_velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
+						_velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
+						_airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s;
+
+
+						/* crude land detector for fixedwing only,
+						* TODO: adapt so that it works for both, maybe move to another location
+						*/
+						if (_velocity_xy_filtered < 5
+							&& _velocity_z_filtered < 10
+							&& _airspeed_filtered < 10) {
+							_local_pos.landed = true;
+						} else {
+							_local_pos.landed = false;
+						}
+
+						_local_pos.z_global = false;
+						_local_pos.yaw = _att.yaw;
+
+						/* lazily publish the local position only once available */
+						if (_local_pos_pub > 0) {
+							/* publish the attitude setpoint */
+							orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);
+
+						} else {
+							/* advertise and publish */
+							_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
+						}
+
+						_global_pos.timestamp = _local_pos.timestamp;
+
+						if (_local_pos.xy_global) {
+							double est_lat, est_lon;
+							map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
+							_global_pos.lat = est_lat;
+							_global_pos.lon = est_lon;
+							_global_pos.time_gps_usec = _gps.time_gps_usec;
+							_global_pos.eph = _gps.eph;
+							_global_pos.epv = _gps.epv;
+						}
+
+						if (_local_pos.v_xy_valid) {
+							_global_pos.vel_n = _local_pos.vx;
+							_global_pos.vel_e = _local_pos.vy;
+						} else {
+							_global_pos.vel_n = 0.0f;
+							_global_pos.vel_e = 0.0f;
+						}
+
+						/* local pos alt is negative, change sign and add alt offsets */
+						_global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset;
+
+						if (_local_pos.v_z_valid) {
+							_global_pos.vel_d = _local_pos.vz;
+						}
+
+
+						_global_pos.yaw = _local_pos.yaw;
+
+						_global_pos.eph = _gps.eph;
+						_global_pos.epv = _gps.epv;
+
+						_global_pos.timestamp = _local_pos.timestamp;
+
+						/* lazily publish the global position only once available */
+						if (_global_pos_pub > 0) {
+							/* publish the global position */
+							orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
+
+						} else {
+							/* advertise and publish */
+							_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
+						}
+
+						if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
+							_wind.timestamp = _global_pos.timestamp;
+							_wind.windspeed_north = _ekf->states[14];
+							_wind.windspeed_east = _ekf->states[15];
+							_wind.covariance_north = 0.0f; // XXX get form filter
+							_wind.covariance_east = 0.0f;
+
+							/* lazily publish the wind estimate only once available */
+							if (_wind_pub > 0) {
+								/* publish the wind estimate */
+								orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
+
+							} else {
+								/* advertise and publish */
+								_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
+							}
+
+						}
 
-				/* local pos alt is negative, change sign and add alt offsets */
-				_global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z);
+					}
 
-				if (_local_pos.v_z_valid) {
-					_global_pos.vel_d = _local_pos.vz;
 				}
 
-				_global_pos.yaw = _local_pos.yaw;
-
-				_global_pos.eph = _gps.eph_m;
-				_global_pos.epv = _gps.epv_m;
-
-				_global_pos.timestamp = _local_pos.timestamp;
-
-				/* lazily publish the global position only once available */
-				if (_global_pos_pub > 0) {
-					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
-
-				} else {
-					/* advertise and publish */
-					_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
+				if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
+					_wind.timestamp = _global_pos.timestamp;
+					_wind.windspeed_north = _ekf->states[14];
+					_wind.windspeed_east = _ekf->states[15];
+					_wind.covariance_north = _ekf->P[14][14];
+					_wind.covariance_east = _ekf->P[15][15];
+
+					/* lazily publish the wind estimate only once available */
+					if (_wind_pub > 0) {
+						/* publish the wind estimate */
+						orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
+
+					} else {
+						/* advertise and publish */
+						_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
+					}
 				}
 			}
 
@@ -1384,32 +1560,44 @@ FixedwingEstimator::print_status()
 	// 4-6: Velocity - m/sec (North, East, Down)
 	// 7-9: Position - m (North, East, Down)
 	// 10-12: Delta Angle bias - rad (X,Y,Z)
-	// 13-14: Wind Vector  - m/sec (North,East)
-	// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
-	// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
+	// 13:    Accelerometer offset
+	// 14-15: Wind Vector  - m/sec (North,East)
+	// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
+	// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
 
 	printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
-	printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt);
+	printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f));
+	printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, (double)_baro_gps_offset);
 	printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
 	printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
-	printf("states (quat)        [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
-	printf("states (vel m/s)     [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
-	printf("states (pos m)      [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
-	printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
-	printf("states (wind)      [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
-	printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
-	printf("states (body mag)  [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
+	printf("states (quat)        [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
+	printf("states (vel m/s)     [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
+	printf("states (pos m)      [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
+	printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
+
+	if (n_states == 23) {
+		printf("states (accel offs)   [13]: %8.4f\n", (double)_ekf->states[13]);
+		printf("states (wind)      [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]);
+		printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]);
+		printf("states (body mag)  [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]);
+		printf("states (terrain)      [22]: %8.4f\n", (double)_ekf->states[22]);
+
+	} else {
+		printf("states (wind)      [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
+		printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
+		printf("states (body mag)  [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
+	}
 	printf("states: %s %s %s %s %s %s %s %s %s %s\n",
-	       (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
-	       (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
-	       (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
-	       (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
-	       (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
-	       (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
-	       (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
-	       (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
-	       (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
-	       (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
+		       (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
+		       (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
+		       (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
+		       (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
+		       (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
+		       (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
+		       (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
+		       (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
+		       (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
+		       (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
 }
 
 int FixedwingEstimator::trip_nan() {
@@ -1464,7 +1652,7 @@ int FixedwingEstimator::trip_nan() {
 int ekf_att_pos_estimator_main(int argc, char *argv[])
 {
 	if (argc < 1)
-		errx(1, "usage: ekf_att_pos_estimator {start|stop|status}");
+		errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}");
 
 	if (!strcmp(argv[1], "start")) {
 
@@ -1518,6 +1706,29 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
 		}
 	}
 
+	if (!strcmp(argv[1], "logging")) {
+		if (estimator::g_estimator) {
+			int ret = estimator::g_estimator->enable_logging(true);
+
+			exit(ret);
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	if (!strcmp(argv[1], "debug")) {
+		if (estimator::g_estimator) {
+			int debug = strtoul(argv[2], NULL, 10);
+			int ret = estimator::g_estimator->set_debuglevel(debug);
+
+			exit(ret);
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
 	warnx("unrecognized command");
 	return 1;
 }
diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp
new file mode 100644
index 000000000..67bfec4ea
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp
@@ -0,0 +1,2142 @@
+#include "estimator_21states.h"
+
+#include <string.h>
+
+AttPosEKF::AttPosEKF() :
+    fusionModeGPS(0),
+    covSkipCount(0),
+    EAS2TAS(1.0f),
+    statesInitialised(false),
+    fuseVelData(false),
+    fusePosData(false),
+    fuseHgtData(false),
+    fuseMagData(false),
+    fuseVtasData(false),
+    onGround(true),
+    staticMode(true),
+    useAirspeed(true),
+    useCompass(true),
+    numericalProtection(true),
+    storeIndex(0),
+    magDeclination(0.0f)
+{
+    InitialiseParameters();
+}
+
+AttPosEKF::~AttPosEKF()
+{
+}
+
+void AttPosEKF::UpdateStrapdownEquationsNED()
+{
+    Vector3f delVelNav;
+    float q00;
+    float q11;
+    float q22;
+    float q33;
+    float q01;
+    float q02;
+    float q03;
+    float q12;
+    float q13;
+    float q23;
+    Mat3f Tbn;
+    Mat3f Tnb;
+    float rotationMag;
+    float qUpdated[4];
+    float quatMag;
+    double deltaQuat[4];
+    const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS};
+
+// Remove sensor bias errors
+    correctedDelAng.x = dAngIMU.x - states[10];
+    correctedDelAng.y = dAngIMU.y - states[11];
+    correctedDelAng.z = dAngIMU.z - states[12];
+    dVelIMU.x = dVelIMU.x;
+    dVelIMU.y = dVelIMU.y;
+    dVelIMU.z = dVelIMU.z;
+
+// Save current measurements
+    Vector3f  prevDelAng = correctedDelAng;
+
+// Apply corrections for earths rotation rate and coning errors
+// * and + operators have been overloaded
+    correctedDelAng   = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
+// Convert the rotation vector to its equivalent quaternion
+    rotationMag = correctedDelAng.length();
+    if (rotationMag < 1e-12f)
+    {
+        deltaQuat[0] = 1.0;
+        deltaQuat[1] = 0.0;
+        deltaQuat[2] = 0.0;
+        deltaQuat[3] = 0.0;
+    }
+    else
+    {
+        deltaQuat[0] = cos(0.5f*rotationMag);
+        double rotScaler = (sin(0.5f*rotationMag))/rotationMag;
+        deltaQuat[1] = correctedDelAng.x*rotScaler;
+        deltaQuat[2] = correctedDelAng.y*rotScaler;
+        deltaQuat[3] = correctedDelAng.z*rotScaler;
+    }
+
+// Update the quaternions by rotating from the previous attitude through
+// the delta angle rotation quaternion
+    qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3];
+    qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2];
+    qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3];
+    qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
+
+// Normalise the quaternions and update the quaternion states
+    quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
+    if (quatMag > 1e-16f)
+    {
+        float quatMagInv = 1.0f/quatMag;
+        states[0] = quatMagInv*qUpdated[0];
+        states[1] = quatMagInv*qUpdated[1];
+        states[2] = quatMagInv*qUpdated[2];
+        states[3] = quatMagInv*qUpdated[3];
+    }
+
+// Calculate the body to nav cosine matrix
+    q00 = sq(states[0]);
+    q11 = sq(states[1]);
+    q22 = sq(states[2]);
+    q33 = sq(states[3]);
+    q01 =  states[0]*states[1];
+    q02 =  states[0]*states[2];
+    q03 =  states[0]*states[3];
+    q12 =  states[1]*states[2];
+    q13 =  states[1]*states[3];
+    q23 =  states[2]*states[3];
+
+    Tbn.x.x = q00 + q11 - q22 - q33;
+    Tbn.y.y = q00 - q11 + q22 - q33;
+    Tbn.z.z = q00 - q11 - q22 + q33;
+    Tbn.x.y = 2*(q12 - q03);
+    Tbn.x.z = 2*(q13 + q02);
+    Tbn.y.x = 2*(q12 + q03);
+    Tbn.y.z = 2*(q23 - q01);
+    Tbn.z.x = 2*(q13 - q02);
+    Tbn.z.y = 2*(q23 + q01);
+
+    Tnb = Tbn.transpose();
+
+// transform body delta velocities to delta velocities in the nav frame
+// * and + operators have been overloaded
+    //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
+    delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU;
+    delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU;
+    delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU;
+
+// calculate the magnitude of the nav acceleration (required for GPS
+// variance estimation)
+    accNavMag = delVelNav.length()/dtIMU;
+
+// If calculating position save previous velocity
+    float lastVelocity[3];
+    lastVelocity[0] = states[4];
+    lastVelocity[1] = states[5];
+    lastVelocity[2] = states[6];
+
+// Sum delta velocities to get velocity
+    states[4] = states[4] + delVelNav.x;
+    states[5] = states[5] + delVelNav.y;
+    states[6] = states[6] + delVelNav.z;
+
+// If calculating postions, do a trapezoidal integration for position
+    states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU;
+    states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU;
+    states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU;
+
+    // Constrain states (to protect against filter divergence)
+    //ConstrainStates();
+}
+
+void AttPosEKF::CovariancePrediction(float dt)
+{
+    // scalars
+    float daxCov;
+    float dayCov;
+    float dazCov;
+    float dvxCov;
+    float dvyCov;
+    float dvzCov;
+    float dvx;
+    float dvy;
+    float dvz;
+    float dax;
+    float day;
+    float daz;
+    float q0;
+    float q1;
+    float q2;
+    float q3;
+    float dax_b;
+    float day_b;
+    float daz_b;
+
+    // arrays
+    float processNoise[21];
+    float SF[14];
+    float SG[8];
+    float SQ[11];
+    float SPP[13] = {0};
+    float nextP[21][21];
+
+    // calculate covariance prediction process noise
+    windVelSigma = dt*0.1f;
+    dAngBiasSigma = dt*5.0e-7f;
+    magEarthSigma = dt*3.0e-4f;
+    magBodySigma  = dt*3.0e-4f;
+    for (uint8_t i= 0; i<=9; i++) processNoise[i]  = 1.0e-9f;
+    for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma;
+    if (onGround) processNoise[12] = dAngBiasSigma * yawVarScale;
+    for (uint8_t i=13; i<=14; i++) processNoise[i] = windVelSigma;
+    for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma;
+    for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma;
+    for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]);
+
+    // set variables used to calculate covariance growth
+    dvx = summedDelVel.x;
+    dvy = summedDelVel.y;
+    dvz = summedDelVel.z;
+    dax = summedDelAng.x;
+    day = summedDelAng.y;
+    daz = summedDelAng.z;
+    q0 = states[0];
+    q1 = states[1];
+    q2 = states[2];
+    q3 = states[3];
+    dax_b = states[10];
+    day_b = states[11];
+    daz_b = states[12];
+    daxCov = sq(dt*1.4544411e-2f);
+    dayCov = sq(dt*1.4544411e-2f);
+    dazCov = sq(dt*1.4544411e-2f);
+    if (onGround) dazCov = dazCov * sq(yawVarScale);
+    dvxCov = sq(dt*0.5f);
+    dvyCov = sq(dt*0.5f);
+    dvzCov = sq(dt*0.5f);
+
+    // Predicted covariance calculation
+    SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3;
+    SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1;
+    SF[2] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2;
+    SF[3] = day/2 - day_b/2;
+    SF[4] = daz/2 - daz_b/2;
+    SF[5] = dax/2 - dax_b/2;
+    SF[6] = dax_b/2 - dax/2;
+    SF[7] = daz_b/2 - daz/2;
+    SF[8] = day_b/2 - day/2;
+    SF[9] = q1/2;
+    SF[10] = q2/2;
+    SF[11] = q3/2;
+    SF[12] = 2*dvz*q0;
+    SF[13] = 2*dvy*q1;
+
+    SG[0] = q0/2;
+    SG[1] = sq(q3);
+    SG[2] = sq(q2);
+    SG[3] = sq(q1);
+    SG[4] = sq(q0);
+    SG[5] = 2*q2*q3;
+    SG[6] = 2*q1*q3;
+    SG[7] = 2*q1*q2;
+
+    SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3);
+    SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3);
+    SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]);
+    SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4;
+    SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4;
+    SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4;
+    SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4;
+    SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2;
+    SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4;
+    SQ[9] = sq(SG[0]);
+    SQ[10] = sq(q1);
+
+    SPP[0] = SF[12] + SF[13] - 2*dvx*q2;
+    SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2;
+    SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1;
+    SPP[3] = SF[11];
+    SPP[4] = SF[10];
+    SPP[5] = SF[9];
+    SPP[6] = SF[7];
+    SPP[7] = SF[8];
+
+    nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4;
+    nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2;
+    nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2;
+    nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3] + SF[4]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[3]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[6]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[4]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) - SPP[5]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - (q0*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]))/2;
+    nextP[0][4] = P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3] + SF[2]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) - SPP[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]);
+    nextP[0][5] = P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3] + SF[1]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SF[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) - SPP[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]);
+    nextP[0][6] = P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3] + SF[1]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[0]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[0]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) - SPP[1]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]);
+    nextP[0][7] = P[0][7] + P[1][7]*SF[6] + P[2][7]*SPP[7] + P[3][7]*SPP[6] + P[10][7]*SPP[5] + P[11][7]*SPP[4] + P[12][7]*SPP[3] + dt*(P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3]);
+    nextP[0][8] = P[0][8] + P[1][8]*SF[6] + P[2][8]*SPP[7] + P[3][8]*SPP[6] + P[10][8]*SPP[5] + P[11][8]*SPP[4] + P[12][8]*SPP[3] + dt*(P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3]);
+    nextP[0][9] = P[0][9] + P[1][9]*SF[6] + P[2][9]*SPP[7] + P[3][9]*SPP[6] + P[10][9]*SPP[5] + P[11][9]*SPP[4] + P[12][9]*SPP[3] + dt*(P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3]);
+    nextP[0][10] = P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3];
+    nextP[0][11] = P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3];
+    nextP[0][12] = P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3];
+    nextP[0][13] = P[0][13] + P[1][13]*SF[6] + P[2][13]*SPP[7] + P[3][13]*SPP[6] + P[10][13]*SPP[5] + P[11][13]*SPP[4] + P[12][13]*SPP[3];
+    nextP[0][14] = P[0][14] + P[1][14]*SF[6] + P[2][14]*SPP[7] + P[3][14]*SPP[6] + P[10][14]*SPP[5] + P[11][14]*SPP[4] + P[12][14]*SPP[3];
+    nextP[0][15] = P[0][15] + P[1][15]*SF[6] + P[2][15]*SPP[7] + P[3][15]*SPP[6] + P[10][15]*SPP[5] + P[11][15]*SPP[4] + P[12][15]*SPP[3];
+    nextP[0][16] = P[0][16] + P[1][16]*SF[6] + P[2][16]*SPP[7] + P[3][16]*SPP[6] + P[10][16]*SPP[5] + P[11][16]*SPP[4] + P[12][16]*SPP[3];
+    nextP[0][17] = P[0][17] + P[1][17]*SF[6] + P[2][17]*SPP[7] + P[3][17]*SPP[6] + P[10][17]*SPP[5] + P[11][17]*SPP[4] + P[12][17]*SPP[3];
+    nextP[0][18] = P[0][18] + P[1][18]*SF[6] + P[2][18]*SPP[7] + P[3][18]*SPP[6] + P[10][18]*SPP[5] + P[11][18]*SPP[4] + P[12][18]*SPP[3];
+    nextP[0][19] = P[0][19] + P[1][19]*SF[6] + P[2][19]*SPP[7] + P[3][19]*SPP[6] + P[10][19]*SPP[5] + P[11][19]*SPP[4] + P[12][19]*SPP[3];
+    nextP[0][20] = P[0][20] + P[1][20]*SF[6] + P[2][20]*SPP[7] + P[3][20]*SPP[6] + P[10][20]*SPP[5] + P[11][20]*SPP[4] + P[12][20]*SPP[3];
+    nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2 + SF[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[7]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[6]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[5]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[4]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) + SPP[3]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2);
+    nextP[1][1] = P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[4]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[7]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[3]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - SPP[4]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2))/2;
+    nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) - SPP[3]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[5]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2))/2;
+    nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[6]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[4]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) - SPP[5]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2))/2;
+    nextP[1][4] = P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2);
+    nextP[1][5] = P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2 + SF[1]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SF[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2);
+    nextP[1][6] = P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2 + SF[1]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[0]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2);
+    nextP[1][7] = P[1][7] + P[0][7]*SF[5] + P[2][7]*SF[4] + P[3][7]*SPP[7] + P[11][7]*SPP[3] - P[12][7]*SPP[4] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2);
+    nextP[1][8] = P[1][8] + P[0][8]*SF[5] + P[2][8]*SF[4] + P[3][8]*SPP[7] + P[11][8]*SPP[3] - P[12][8]*SPP[4] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2);
+    nextP[1][9] = P[1][9] + P[0][9]*SF[5] + P[2][9]*SF[4] + P[3][9]*SPP[7] + P[11][9]*SPP[3] - P[12][9]*SPP[4] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2);
+    nextP[1][10] = P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2;
+    nextP[1][11] = P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2;
+    nextP[1][12] = P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2;
+    nextP[1][13] = P[1][13] + P[0][13]*SF[5] + P[2][13]*SF[4] + P[3][13]*SPP[7] + P[11][13]*SPP[3] - P[12][13]*SPP[4] - (P[10][13]*q0)/2;
+    nextP[1][14] = P[1][14] + P[0][14]*SF[5] + P[2][14]*SF[4] + P[3][14]*SPP[7] + P[11][14]*SPP[3] - P[12][14]*SPP[4] - (P[10][14]*q0)/2;
+    nextP[1][15] = P[1][15] + P[0][15]*SF[5] + P[2][15]*SF[4] + P[3][15]*SPP[7] + P[11][15]*SPP[3] - P[12][15]*SPP[4] - (P[10][15]*q0)/2;
+    nextP[1][16] = P[1][16] + P[0][16]*SF[5] + P[2][16]*SF[4] + P[3][16]*SPP[7] + P[11][16]*SPP[3] - P[12][16]*SPP[4] - (P[10][16]*q0)/2;
+    nextP[1][17] = P[1][17] + P[0][17]*SF[5] + P[2][17]*SF[4] + P[3][17]*SPP[7] + P[11][17]*SPP[3] - P[12][17]*SPP[4] - (P[10][17]*q0)/2;
+    nextP[1][18] = P[1][18] + P[0][18]*SF[5] + P[2][18]*SF[4] + P[3][18]*SPP[7] + P[11][18]*SPP[3] - P[12][18]*SPP[4] - (P[10][18]*q0)/2;
+    nextP[1][19] = P[1][19] + P[0][19]*SF[5] + P[2][19]*SF[4] + P[3][19]*SPP[7] + P[11][19]*SPP[3] - P[12][19]*SPP[4] - (P[10][19]*q0)/2;
+    nextP[1][20] = P[1][20] + P[0][20]*SF[5] + P[2][20]*SF[4] + P[3][20]*SPP[7] + P[11][20]*SPP[3] - P[12][20]*SPP[4] - (P[10][20]*q0)/2;
+    nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2 + SF[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[7]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[6]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[5]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[4]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) + SPP[3]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2);
+    nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[4]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[7]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[3]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - SPP[4]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2))/2;
+    nextP[2][2] = P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) - SPP[3]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[5]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2))/2;
+    nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[6]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[4]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) - SPP[5]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2))/2;
+    nextP[2][4] = P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2);
+    nextP[2][5] = P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2 + SF[1]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SF[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2);
+    nextP[2][6] = P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2 + SF[1]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[0]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2);
+    nextP[2][7] = P[2][7] + P[0][7]*SF[3] + P[3][7]*SF[5] + P[1][7]*SPP[6] - P[10][7]*SPP[3] + P[12][7]*SPP[5] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2);
+    nextP[2][8] = P[2][8] + P[0][8]*SF[3] + P[3][8]*SF[5] + P[1][8]*SPP[6] - P[10][8]*SPP[3] + P[12][8]*SPP[5] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2);
+    nextP[2][9] = P[2][9] + P[0][9]*SF[3] + P[3][9]*SF[5] + P[1][9]*SPP[6] - P[10][9]*SPP[3] + P[12][9]*SPP[5] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2);
+    nextP[2][10] = P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2;
+    nextP[2][11] = P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2;
+    nextP[2][12] = P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2;
+    nextP[2][13] = P[2][13] + P[0][13]*SF[3] + P[3][13]*SF[5] + P[1][13]*SPP[6] - P[10][13]*SPP[3] + P[12][13]*SPP[5] - (P[11][13]*q0)/2;
+    nextP[2][14] = P[2][14] + P[0][14]*SF[3] + P[3][14]*SF[5] + P[1][14]*SPP[6] - P[10][14]*SPP[3] + P[12][14]*SPP[5] - (P[11][14]*q0)/2;
+    nextP[2][15] = P[2][15] + P[0][15]*SF[3] + P[3][15]*SF[5] + P[1][15]*SPP[6] - P[10][15]*SPP[3] + P[12][15]*SPP[5] - (P[11][15]*q0)/2;
+    nextP[2][16] = P[2][16] + P[0][16]*SF[3] + P[3][16]*SF[5] + P[1][16]*SPP[6] - P[10][16]*SPP[3] + P[12][16]*SPP[5] - (P[11][16]*q0)/2;
+    nextP[2][17] = P[2][17] + P[0][17]*SF[3] + P[3][17]*SF[5] + P[1][17]*SPP[6] - P[10][17]*SPP[3] + P[12][17]*SPP[5] - (P[11][17]*q0)/2;
+    nextP[2][18] = P[2][18] + P[0][18]*SF[3] + P[3][18]*SF[5] + P[1][18]*SPP[6] - P[10][18]*SPP[3] + P[12][18]*SPP[5] - (P[11][18]*q0)/2;
+    nextP[2][19] = P[2][19] + P[0][19]*SF[3] + P[3][19]*SF[5] + P[1][19]*SPP[6] - P[10][19]*SPP[3] + P[12][19]*SPP[5] - (P[11][19]*q0)/2;
+    nextP[2][20] = P[2][20] + P[0][20]*SF[3] + P[3][20]*SF[5] + P[1][20]*SPP[6] - P[10][20]*SPP[3] + P[12][20]*SPP[5] - (P[11][20]*q0)/2;
+    nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2 + SF[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[7]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[6]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[5]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[4]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + SPP[3]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2);
+    nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[4]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[7]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[3]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) - SPP[4]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2))/2;
+    nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) - SPP[3]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[5]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2))/2;
+    nextP[3][3] = P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[6]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[4]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) - SPP[5]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2))/2;
+    nextP[3][4] = P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2);
+    nextP[3][5] = P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2 + SF[1]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SF[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2);
+    nextP[3][6] = P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2 + SF[1]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[0]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2);
+    nextP[3][7] = P[3][7] + P[0][7]*SF[4] + P[1][7]*SF[3] + P[2][7]*SF[6] + P[10][7]*SPP[4] - P[11][7]*SPP[5] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2);
+    nextP[3][8] = P[3][8] + P[0][8]*SF[4] + P[1][8]*SF[3] + P[2][8]*SF[6] + P[10][8]*SPP[4] - P[11][8]*SPP[5] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2);
+    nextP[3][9] = P[3][9] + P[0][9]*SF[4] + P[1][9]*SF[3] + P[2][9]*SF[6] + P[10][9]*SPP[4] - P[11][9]*SPP[5] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2);
+    nextP[3][10] = P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2;
+    nextP[3][11] = P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2;
+    nextP[3][12] = P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2;
+    nextP[3][13] = P[3][13] + P[0][13]*SF[4] + P[1][13]*SF[3] + P[2][13]*SF[6] + P[10][13]*SPP[4] - P[11][13]*SPP[5] - (P[12][13]*q0)/2;
+    nextP[3][14] = P[3][14] + P[0][14]*SF[4] + P[1][14]*SF[3] + P[2][14]*SF[6] + P[10][14]*SPP[4] - P[11][14]*SPP[5] - (P[12][14]*q0)/2;
+    nextP[3][15] = P[3][15] + P[0][15]*SF[4] + P[1][15]*SF[3] + P[2][15]*SF[6] + P[10][15]*SPP[4] - P[11][15]*SPP[5] - (P[12][15]*q0)/2;
+    nextP[3][16] = P[3][16] + P[0][16]*SF[4] + P[1][16]*SF[3] + P[2][16]*SF[6] + P[10][16]*SPP[4] - P[11][16]*SPP[5] - (P[12][16]*q0)/2;
+    nextP[3][17] = P[3][17] + P[0][17]*SF[4] + P[1][17]*SF[3] + P[2][17]*SF[6] + P[10][17]*SPP[4] - P[11][17]*SPP[5] - (P[12][17]*q0)/2;
+    nextP[3][18] = P[3][18] + P[0][18]*SF[4] + P[1][18]*SF[3] + P[2][18]*SF[6] + P[10][18]*SPP[4] - P[11][18]*SPP[5] - (P[12][18]*q0)/2;
+    nextP[3][19] = P[3][19] + P[0][19]*SF[4] + P[1][19]*SF[3] + P[2][19]*SF[6] + P[10][19]*SPP[4] - P[11][19]*SPP[5] - (P[12][19]*q0)/2;
+    nextP[3][20] = P[3][20] + P[0][20]*SF[4] + P[1][20]*SF[3] + P[2][20]*SF[6] + P[10][20]*SPP[4] - P[11][20]*SPP[5] - (P[12][20]*q0)/2;
+    nextP[4][0] = P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2] + SF[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[7]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[6]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[5]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[4]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) + SPP[3]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]);
+    nextP[4][1] = P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2] + SF[5]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[4]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[7]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[3]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - SPP[4]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]))/2;
+    nextP[4][2] = P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2] + SF[3]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[5]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) - SPP[3]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[5]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]))/2;
+    nextP[4][3] = P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2] + SF[4]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[3]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[6]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[4]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) - SPP[5]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - (q0*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]))/2;
+    nextP[4][4] = P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[2]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) - SPP[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]);
+    nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2] + SF[1]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SF[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) - SPP[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]);
+    nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2] + SF[1]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[0]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[0]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) - SPP[1]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]);
+    nextP[4][7] = P[4][7] + P[0][7]*SF[2] + P[1][7]*SF[0] + P[2][7]*SPP[0] - P[3][7]*SPP[2] + dt*(P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2]);
+    nextP[4][8] = P[4][8] + P[0][8]*SF[2] + P[1][8]*SF[0] + P[2][8]*SPP[0] - P[3][8]*SPP[2] + dt*(P[4][5] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2]);
+    nextP[4][9] = P[4][9] + P[0][9]*SF[2] + P[1][9]*SF[0] + P[2][9]*SPP[0] - P[3][9]*SPP[2] + dt*(P[4][6] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2]);
+    nextP[4][10] = P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2];
+    nextP[4][11] = P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2];
+    nextP[4][12] = P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2];
+    nextP[4][13] = P[4][13] + P[0][13]*SF[2] + P[1][13]*SF[0] + P[2][13]*SPP[0] - P[3][13]*SPP[2];
+    nextP[4][14] = P[4][14] + P[0][14]*SF[2] + P[1][14]*SF[0] + P[2][14]*SPP[0] - P[3][14]*SPP[2];
+    nextP[4][15] = P[4][15] + P[0][15]*SF[2] + P[1][15]*SF[0] + P[2][15]*SPP[0] - P[3][15]*SPP[2];
+    nextP[4][16] = P[4][16] + P[0][16]*SF[2] + P[1][16]*SF[0] + P[2][16]*SPP[0] - P[3][16]*SPP[2];
+    nextP[4][17] = P[4][17] + P[0][17]*SF[2] + P[1][17]*SF[0] + P[2][17]*SPP[0] - P[3][17]*SPP[2];
+    nextP[4][18] = P[4][18] + P[0][18]*SF[2] + P[1][18]*SF[0] + P[2][18]*SPP[0] - P[3][18]*SPP[2];
+    nextP[4][19] = P[4][19] + P[0][19]*SF[2] + P[1][19]*SF[0] + P[2][19]*SPP[0] - P[3][19]*SPP[2];
+    nextP[4][20] = P[4][20] + P[0][20]*SF[2] + P[1][20]*SF[0] + P[2][20]*SPP[0] - P[3][20]*SPP[2];
+    nextP[5][0] = P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0] + SF[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[7]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[6]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[5]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[4]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) + SPP[3]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]);
+    nextP[5][1] = P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0] + SF[5]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[4]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[7]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[3]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - SPP[4]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]))/2;
+    nextP[5][2] = P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0] + SF[3]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[5]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) - SPP[3]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[5]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]))/2;
+    nextP[5][3] = P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0] + SF[4]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[3]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[6]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[4]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) - SPP[5]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - (q0*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]))/2;
+    nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0] + SF[2]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) - SPP[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]);
+    nextP[5][5] = P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[1]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SF[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) - SPP[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]);
+    nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0] + SF[1]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[0]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[0]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) - SPP[1]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]);
+    nextP[5][7] = P[5][7] + P[0][7]*SF[1] + P[2][7]*SF[0] + P[3][7]*SF[2] - P[1][7]*SPP[0] + dt*(P[5][4] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0]);
+    nextP[5][8] = P[5][8] + P[0][8]*SF[1] + P[2][8]*SF[0] + P[3][8]*SF[2] - P[1][8]*SPP[0] + dt*(P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0]);
+    nextP[5][9] = P[5][9] + P[0][9]*SF[1] + P[2][9]*SF[0] + P[3][9]*SF[2] - P[1][9]*SPP[0] + dt*(P[5][6] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0]);
+    nextP[5][10] = P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0];
+    nextP[5][11] = P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0];
+    nextP[5][12] = P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0];
+    nextP[5][13] = P[5][13] + P[0][13]*SF[1] + P[2][13]*SF[0] + P[3][13]*SF[2] - P[1][13]*SPP[0];
+    nextP[5][14] = P[5][14] + P[0][14]*SF[1] + P[2][14]*SF[0] + P[3][14]*SF[2] - P[1][14]*SPP[0];
+    nextP[5][15] = P[5][15] + P[0][15]*SF[1] + P[2][15]*SF[0] + P[3][15]*SF[2] - P[1][15]*SPP[0];
+    nextP[5][16] = P[5][16] + P[0][16]*SF[1] + P[2][16]*SF[0] + P[3][16]*SF[2] - P[1][16]*SPP[0];
+    nextP[5][17] = P[5][17] + P[0][17]*SF[1] + P[2][17]*SF[0] + P[3][17]*SF[2] - P[1][17]*SPP[0];
+    nextP[5][18] = P[5][18] + P[0][18]*SF[1] + P[2][18]*SF[0] + P[3][18]*SF[2] - P[1][18]*SPP[0];
+    nextP[5][19] = P[5][19] + P[0][19]*SF[1] + P[2][19]*SF[0] + P[3][19]*SF[2] - P[1][19]*SPP[0];
+    nextP[5][20] = P[5][20] + P[0][20]*SF[1] + P[2][20]*SF[0] + P[3][20]*SF[2] - P[1][20]*SPP[0];
+    nextP[6][0] = P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1] + SF[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[7]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[6]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[5]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[4]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) + SPP[3]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]);
+    nextP[6][1] = P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1] + SF[5]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[4]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[7]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[3]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - SPP[4]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]))/2;
+    nextP[6][2] = P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1] + SF[3]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[5]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) - SPP[3]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[5]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]))/2;
+    nextP[6][3] = P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1] + SF[4]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[3]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[6]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[4]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) - SPP[5]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - (q0*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]))/2;
+    nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1] + SF[2]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) - SPP[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]);
+    nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1] + SF[1]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SF[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) - SPP[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]);
+    nextP[6][6] = P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1] + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) + SF[1]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[0]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) - SPP[1]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]);
+    nextP[6][7] = P[6][7] + P[1][7]*SF[1] + P[3][7]*SF[0] + P[0][7]*SPP[0] - P[2][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1]);
+    nextP[6][8] = P[6][8] + P[1][8]*SF[1] + P[3][8]*SF[0] + P[0][8]*SPP[0] - P[2][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1]);
+    nextP[6][9] = P[6][9] + P[1][9]*SF[1] + P[3][9]*SF[0] + P[0][9]*SPP[0] - P[2][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1]);
+    nextP[6][10] = P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1];
+    nextP[6][11] = P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1];
+    nextP[6][12] = P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1];
+    nextP[6][13] = P[6][13] + P[1][13]*SF[1] + P[3][13]*SF[0] + P[0][13]*SPP[0] - P[2][13]*SPP[1];
+    nextP[6][14] = P[6][14] + P[1][14]*SF[1] + P[3][14]*SF[0] + P[0][14]*SPP[0] - P[2][14]*SPP[1];
+    nextP[6][15] = P[6][15] + P[1][15]*SF[1] + P[3][15]*SF[0] + P[0][15]*SPP[0] - P[2][15]*SPP[1];
+    nextP[6][16] = P[6][16] + P[1][16]*SF[1] + P[3][16]*SF[0] + P[0][16]*SPP[0] - P[2][16]*SPP[1];
+    nextP[6][17] = P[6][17] + P[1][17]*SF[1] + P[3][17]*SF[0] + P[0][17]*SPP[0] - P[2][17]*SPP[1];
+    nextP[6][18] = P[6][18] + P[1][18]*SF[1] + P[3][18]*SF[0] + P[0][18]*SPP[0] - P[2][18]*SPP[1];
+    nextP[6][19] = P[6][19] + P[1][19]*SF[1] + P[3][19]*SF[0] + P[0][19]*SPP[0] - P[2][19]*SPP[1];
+    nextP[6][20] = P[6][20] + P[1][20]*SF[1] + P[3][20]*SF[0] + P[0][20]*SPP[0] - P[2][20]*SPP[1];
+    nextP[7][0] = P[7][0] + P[4][0]*dt + SF[6]*(P[7][1] + P[4][1]*dt) + SPP[7]*(P[7][2] + P[4][2]*dt) + SPP[6]*(P[7][3] + P[4][3]*dt) + SPP[5]*(P[7][10] + P[4][10]*dt) + SPP[4]*(P[7][11] + P[4][11]*dt) + SPP[3]*(P[7][12] + P[4][12]*dt);
+    nextP[7][1] = P[7][1] + P[4][1]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][2] + P[4][2]*dt) + SPP[7]*(P[7][3] + P[4][3]*dt) + SPP[3]*(P[7][11] + P[4][11]*dt) - SPP[4]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2;
+    nextP[7][2] = P[7][2] + P[4][2]*dt + SF[3]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][1] + P[4][1]*dt) - SPP[3]*(P[7][10] + P[4][10]*dt) + SPP[5]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2;
+    nextP[7][3] = P[7][3] + P[4][3]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[3]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][2] + P[4][2]*dt) + SPP[4]*(P[7][10] + P[4][10]*dt) - SPP[5]*(P[7][11] + P[4][11]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2;
+    nextP[7][4] = P[7][4] + P[4][4]*dt + SF[0]*(P[7][1] + P[4][1]*dt) + SF[2]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt);
+    nextP[7][5] = P[7][5] + P[4][5]*dt + SF[1]*(P[7][0] + P[4][0]*dt) + SF[0]*(P[7][2] + P[4][2]*dt) + SF[2]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt);
+    nextP[7][6] = P[7][6] + P[4][6]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[0]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt);
+    nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
+    nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt);
+    nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt);
+    nextP[7][10] = P[7][10] + P[4][10]*dt;
+    nextP[7][11] = P[7][11] + P[4][11]*dt;
+    nextP[7][12] = P[7][12] + P[4][12]*dt;
+    nextP[7][13] = P[7][13] + P[4][13]*dt;
+    nextP[7][14] = P[7][14] + P[4][14]*dt;
+    nextP[7][15] = P[7][15] + P[4][15]*dt;
+    nextP[7][16] = P[7][16] + P[4][16]*dt;
+    nextP[7][17] = P[7][17] + P[4][17]*dt;
+    nextP[7][18] = P[7][18] + P[4][18]*dt;
+    nextP[7][19] = P[7][19] + P[4][19]*dt;
+    nextP[7][20] = P[7][20] + P[4][20]*dt;
+    nextP[8][0] = P[8][0] + P[5][0]*dt + SF[6]*(P[8][1] + P[5][1]*dt) + SPP[7]*(P[8][2] + P[5][2]*dt) + SPP[6]*(P[8][3] + P[5][3]*dt) + SPP[5]*(P[8][10] + P[5][10]*dt) + SPP[4]*(P[8][11] + P[5][11]*dt) + SPP[3]*(P[8][12] + P[5][12]*dt);
+    nextP[8][1] = P[8][1] + P[5][1]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][2] + P[5][2]*dt) + SPP[7]*(P[8][3] + P[5][3]*dt) + SPP[3]*(P[8][11] + P[5][11]*dt) - SPP[4]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2;
+    nextP[8][2] = P[8][2] + P[5][2]*dt + SF[3]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][1] + P[5][1]*dt) - SPP[3]*(P[8][10] + P[5][10]*dt) + SPP[5]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2;
+    nextP[8][3] = P[8][3] + P[5][3]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[3]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][2] + P[5][2]*dt) + SPP[4]*(P[8][10] + P[5][10]*dt) - SPP[5]*(P[8][11] + P[5][11]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2;
+    nextP[8][4] = P[8][4] + P[5][4]*dt + SF[0]*(P[8][1] + P[5][1]*dt) + SF[2]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt);
+    nextP[8][5] = P[8][5] + P[5][5]*dt + SF[1]*(P[8][0] + P[5][0]*dt) + SF[0]*(P[8][2] + P[5][2]*dt) + SF[2]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt);
+    nextP[8][6] = P[8][6] + P[5][6]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[0]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt);
+    nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt);
+    nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt);
+    nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt);
+    nextP[8][10] = P[8][10] + P[5][10]*dt;
+    nextP[8][11] = P[8][11] + P[5][11]*dt;
+    nextP[8][12] = P[8][12] + P[5][12]*dt;
+    nextP[8][13] = P[8][13] + P[5][13]*dt;
+    nextP[8][14] = P[8][14] + P[5][14]*dt;
+    nextP[8][15] = P[8][15] + P[5][15]*dt;
+    nextP[8][16] = P[8][16] + P[5][16]*dt;
+    nextP[8][17] = P[8][17] + P[5][17]*dt;
+    nextP[8][18] = P[8][18] + P[5][18]*dt;
+    nextP[8][19] = P[8][19] + P[5][19]*dt;
+    nextP[8][20] = P[8][20] + P[5][20]*dt;
+    nextP[9][0] = P[9][0] + P[6][0]*dt + SF[6]*(P[9][1] + P[6][1]*dt) + SPP[7]*(P[9][2] + P[6][2]*dt) + SPP[6]*(P[9][3] + P[6][3]*dt) + SPP[5]*(P[9][10] + P[6][10]*dt) + SPP[4]*(P[9][11] + P[6][11]*dt) + SPP[3]*(P[9][12] + P[6][12]*dt);
+    nextP[9][1] = P[9][1] + P[6][1]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][2] + P[6][2]*dt) + SPP[7]*(P[9][3] + P[6][3]*dt) + SPP[3]*(P[9][11] + P[6][11]*dt) - SPP[4]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2;
+    nextP[9][2] = P[9][2] + P[6][2]*dt + SF[3]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][1] + P[6][1]*dt) - SPP[3]*(P[9][10] + P[6][10]*dt) + SPP[5]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2;
+    nextP[9][3] = P[9][3] + P[6][3]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[3]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][2] + P[6][2]*dt) + SPP[4]*(P[9][10] + P[6][10]*dt) - SPP[5]*(P[9][11] + P[6][11]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2;
+    nextP[9][4] = P[9][4] + P[6][4]*dt + SF[0]*(P[9][1] + P[6][1]*dt) + SF[2]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt);
+    nextP[9][5] = P[9][5] + P[6][5]*dt + SF[1]*(P[9][0] + P[6][0]*dt) + SF[0]*(P[9][2] + P[6][2]*dt) + SF[2]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt);
+    nextP[9][6] = P[9][6] + P[6][6]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[0]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt);
+    nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt);
+    nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt);
+    nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt);
+    nextP[9][10] = P[9][10] + P[6][10]*dt;
+    nextP[9][11] = P[9][11] + P[6][11]*dt;
+    nextP[9][12] = P[9][12] + P[6][12]*dt;
+    nextP[9][13] = P[9][13] + P[6][13]*dt;
+    nextP[9][14] = P[9][14] + P[6][14]*dt;
+    nextP[9][15] = P[9][15] + P[6][15]*dt;
+    nextP[9][16] = P[9][16] + P[6][16]*dt;
+    nextP[9][17] = P[9][17] + P[6][17]*dt;
+    nextP[9][18] = P[9][18] + P[6][18]*dt;
+    nextP[9][19] = P[9][19] + P[6][19]*dt;
+    nextP[9][20] = P[9][20] + P[6][20]*dt;
+    nextP[10][0] = P[10][0] + P[10][1]*SF[6] + P[10][2]*SPP[7] + P[10][3]*SPP[6] + P[10][10]*SPP[5] + P[10][11]*SPP[4] + P[10][12]*SPP[3];
+    nextP[10][1] = P[10][1] + P[10][0]*SF[5] + P[10][2]*SF[4] + P[10][3]*SPP[7] + P[10][11]*SPP[3] - P[10][12]*SPP[4] - (P[10][10]*q0)/2;
+    nextP[10][2] = P[10][2] + P[10][0]*SF[3] + P[10][3]*SF[5] + P[10][1]*SPP[6] - P[10][10]*SPP[3] + P[10][12]*SPP[5] - (P[10][11]*q0)/2;
+    nextP[10][3] = P[10][3] + P[10][0]*SF[4] + P[10][1]*SF[3] + P[10][2]*SF[6] + P[10][10]*SPP[4] - P[10][11]*SPP[5] - (P[10][12]*q0)/2;
+    nextP[10][4] = P[10][4] + P[10][1]*SF[0] + P[10][0]*SF[2] + P[10][2]*SPP[0] - P[10][3]*SPP[2];
+    nextP[10][5] = P[10][5] + P[10][0]*SF[1] + P[10][2]*SF[0] + P[10][3]*SF[2] - P[10][1]*SPP[0];
+    nextP[10][6] = P[10][6] + P[10][1]*SF[1] + P[10][3]*SF[0] + P[10][0]*SPP[0] - P[10][2]*SPP[1];
+    nextP[10][7] = P[10][7] + P[10][4]*dt;
+    nextP[10][8] = P[10][8] + P[10][5]*dt;
+    nextP[10][9] = P[10][9] + P[10][6]*dt;
+    nextP[10][10] = P[10][10];
+    nextP[10][11] = P[10][11];
+    nextP[10][12] = P[10][12];
+    nextP[10][13] = P[10][13];
+    nextP[10][14] = P[10][14];
+    nextP[10][15] = P[10][15];
+    nextP[10][16] = P[10][16];
+    nextP[10][17] = P[10][17];
+    nextP[10][18] = P[10][18];
+    nextP[10][19] = P[10][19];
+    nextP[10][20] = P[10][20];
+    nextP[11][0] = P[11][0] + P[11][1]*SF[6] + P[11][2]*SPP[7] + P[11][3]*SPP[6] + P[11][10]*SPP[5] + P[11][11]*SPP[4] + P[11][12]*SPP[3];
+    nextP[11][1] = P[11][1] + P[11][0]*SF[5] + P[11][2]*SF[4] + P[11][3]*SPP[7] + P[11][11]*SPP[3] - P[11][12]*SPP[4] - (P[11][10]*q0)/2;
+    nextP[11][2] = P[11][2] + P[11][0]*SF[3] + P[11][3]*SF[5] + P[11][1]*SPP[6] - P[11][10]*SPP[3] + P[11][12]*SPP[5] - (P[11][11]*q0)/2;
+    nextP[11][3] = P[11][3] + P[11][0]*SF[4] + P[11][1]*SF[3] + P[11][2]*SF[6] + P[11][10]*SPP[4] - P[11][11]*SPP[5] - (P[11][12]*q0)/2;
+    nextP[11][4] = P[11][4] + P[11][1]*SF[0] + P[11][0]*SF[2] + P[11][2]*SPP[0] - P[11][3]*SPP[2];
+    nextP[11][5] = P[11][5] + P[11][0]*SF[1] + P[11][2]*SF[0] + P[11][3]*SF[2] - P[11][1]*SPP[0];
+    nextP[11][6] = P[11][6] + P[11][1]*SF[1] + P[11][3]*SF[0] + P[11][0]*SPP[0] - P[11][2]*SPP[1];
+    nextP[11][7] = P[11][7] + P[11][4]*dt;
+    nextP[11][8] = P[11][8] + P[11][5]*dt;
+    nextP[11][9] = P[11][9] + P[11][6]*dt;
+    nextP[11][10] = P[11][10];
+    nextP[11][11] = P[11][11];
+    nextP[11][12] = P[11][12];
+    nextP[11][13] = P[11][13];
+    nextP[11][14] = P[11][14];
+    nextP[11][15] = P[11][15];
+    nextP[11][16] = P[11][16];
+    nextP[11][17] = P[11][17];
+    nextP[11][18] = P[11][18];
+    nextP[11][19] = P[11][19];
+    nextP[11][20] = P[11][20];
+    nextP[12][0] = P[12][0] + P[12][1]*SF[6] + P[12][2]*SPP[7] + P[12][3]*SPP[6] + P[12][10]*SPP[5] + P[12][11]*SPP[4] + P[12][12]*SPP[3];
+    nextP[12][1] = P[12][1] + P[12][0]*SF[5] + P[12][2]*SF[4] + P[12][3]*SPP[7] + P[12][11]*SPP[3] - P[12][12]*SPP[4] - (P[12][10]*q0)/2;
+    nextP[12][2] = P[12][2] + P[12][0]*SF[3] + P[12][3]*SF[5] + P[12][1]*SPP[6] - P[12][10]*SPP[3] + P[12][12]*SPP[5] - (P[12][11]*q0)/2;
+    nextP[12][3] = P[12][3] + P[12][0]*SF[4] + P[12][1]*SF[3] + P[12][2]*SF[6] + P[12][10]*SPP[4] - P[12][11]*SPP[5] - (P[12][12]*q0)/2;
+    nextP[12][4] = P[12][4] + P[12][1]*SF[0] + P[12][0]*SF[2] + P[12][2]*SPP[0] - P[12][3]*SPP[2];
+    nextP[12][5] = P[12][5] + P[12][0]*SF[1] + P[12][2]*SF[0] + P[12][3]*SF[2] - P[12][1]*SPP[0];
+    nextP[12][6] = P[12][6] + P[12][1]*SF[1] + P[12][3]*SF[0] + P[12][0]*SPP[0] - P[12][2]*SPP[1];
+    nextP[12][7] = P[12][7] + P[12][4]*dt;
+    nextP[12][8] = P[12][8] + P[12][5]*dt;
+    nextP[12][9] = P[12][9] + P[12][6]*dt;
+    nextP[12][10] = P[12][10];
+    nextP[12][11] = P[12][11];
+    nextP[12][12] = P[12][12];
+    nextP[12][13] = P[12][13];
+    nextP[12][14] = P[12][14];
+    nextP[12][15] = P[12][15];
+    nextP[12][16] = P[12][16];
+    nextP[12][17] = P[12][17];
+    nextP[12][18] = P[12][18];
+    nextP[12][19] = P[12][19];
+    nextP[12][20] = P[12][20];
+    nextP[13][0] = P[13][0] + P[13][1]*SF[6] + P[13][2]*SPP[7] + P[13][3]*SPP[6] + P[13][10]*SPP[5] + P[13][11]*SPP[4] + P[13][12]*SPP[3];
+    nextP[13][1] = P[13][1] + P[13][0]*SF[5] + P[13][2]*SF[4] + P[13][3]*SPP[7] + P[13][11]*SPP[3] - P[13][12]*SPP[4] - (P[13][10]*q0)/2;
+    nextP[13][2] = P[13][2] + P[13][0]*SF[3] + P[13][3]*SF[5] + P[13][1]*SPP[6] - P[13][10]*SPP[3] + P[13][12]*SPP[5] - (P[13][11]*q0)/2;
+    nextP[13][3] = P[13][3] + P[13][0]*SF[4] + P[13][1]*SF[3] + P[13][2]*SF[6] + P[13][10]*SPP[4] - P[13][11]*SPP[5] - (P[13][12]*q0)/2;
+    nextP[13][4] = P[13][4] + P[13][1]*SF[0] + P[13][0]*SF[2] + P[13][2]*SPP[0] - P[13][3]*SPP[2];
+    nextP[13][5] = P[13][5] + P[13][0]*SF[1] + P[13][2]*SF[0] + P[13][3]*SF[2] - P[13][1]*SPP[0];
+    nextP[13][6] = P[13][6] + P[13][1]*SF[1] + P[13][3]*SF[0] + P[13][0]*SPP[0] - P[13][2]*SPP[1];
+    nextP[13][7] = P[13][7] + P[13][4]*dt;
+    nextP[13][8] = P[13][8] + P[13][5]*dt;
+    nextP[13][9] = P[13][9] + P[13][6]*dt;
+    nextP[13][10] = P[13][10];
+    nextP[13][11] = P[13][11];
+    nextP[13][12] = P[13][12];
+    nextP[13][13] = P[13][13];
+    nextP[13][14] = P[13][14];
+    nextP[13][15] = P[13][15];
+    nextP[13][16] = P[13][16];
+    nextP[13][17] = P[13][17];
+    nextP[13][18] = P[13][18];
+    nextP[13][19] = P[13][19];
+    nextP[13][20] = P[13][20];
+    nextP[14][0] = P[14][0] + P[14][1]*SF[6] + P[14][2]*SPP[7] + P[14][3]*SPP[6] + P[14][10]*SPP[5] + P[14][11]*SPP[4] + P[14][12]*SPP[3];
+    nextP[14][1] = P[14][1] + P[14][0]*SF[5] + P[14][2]*SF[4] + P[14][3]*SPP[7] + P[14][11]*SPP[3] - P[14][12]*SPP[4] - (P[14][10]*q0)/2;
+    nextP[14][2] = P[14][2] + P[14][0]*SF[3] + P[14][3]*SF[5] + P[14][1]*SPP[6] - P[14][10]*SPP[3] + P[14][12]*SPP[5] - (P[14][11]*q0)/2;
+    nextP[14][3] = P[14][3] + P[14][0]*SF[4] + P[14][1]*SF[3] + P[14][2]*SF[6] + P[14][10]*SPP[4] - P[14][11]*SPP[5] - (P[14][12]*q0)/2;
+    nextP[14][4] = P[14][4] + P[14][1]*SF[0] + P[14][0]*SF[2] + P[14][2]*SPP[0] - P[14][3]*SPP[2];
+    nextP[14][5] = P[14][5] + P[14][0]*SF[1] + P[14][2]*SF[0] + P[14][3]*SF[2] - P[14][1]*SPP[0];
+    nextP[14][6] = P[14][6] + P[14][1]*SF[1] + P[14][3]*SF[0] + P[14][0]*SPP[0] - P[14][2]*SPP[1];
+    nextP[14][7] = P[14][7] + P[14][4]*dt;
+    nextP[14][8] = P[14][8] + P[14][5]*dt;
+    nextP[14][9] = P[14][9] + P[14][6]*dt;
+    nextP[14][10] = P[14][10];
+    nextP[14][11] = P[14][11];
+    nextP[14][12] = P[14][12];
+    nextP[14][13] = P[14][13];
+    nextP[14][14] = P[14][14];
+    nextP[14][15] = P[14][15];
+    nextP[14][16] = P[14][16];
+    nextP[14][17] = P[14][17];
+    nextP[14][18] = P[14][18];
+    nextP[14][19] = P[14][19];
+    nextP[14][20] = P[14][20];
+    nextP[15][0] = P[15][0] + P[15][1]*SF[6] + P[15][2]*SPP[7] + P[15][3]*SPP[6] + P[15][10]*SPP[5] + P[15][11]*SPP[4] + P[15][12]*SPP[3];
+    nextP[15][1] = P[15][1] + P[15][0]*SF[5] + P[15][2]*SF[4] + P[15][3]*SPP[7] + P[15][11]*SPP[3] - P[15][12]*SPP[4] - (P[15][10]*q0)/2;
+    nextP[15][2] = P[15][2] + P[15][0]*SF[3] + P[15][3]*SF[5] + P[15][1]*SPP[6] - P[15][10]*SPP[3] + P[15][12]*SPP[5] - (P[15][11]*q0)/2;
+    nextP[15][3] = P[15][3] + P[15][0]*SF[4] + P[15][1]*SF[3] + P[15][2]*SF[6] + P[15][10]*SPP[4] - P[15][11]*SPP[5] - (P[15][12]*q0)/2;
+    nextP[15][4] = P[15][4] + P[15][1]*SF[0] + P[15][0]*SF[2] + P[15][2]*SPP[0] - P[15][3]*SPP[2];
+    nextP[15][5] = P[15][5] + P[15][0]*SF[1] + P[15][2]*SF[0] + P[15][3]*SF[2] - P[15][1]*SPP[0];
+    nextP[15][6] = P[15][6] + P[15][1]*SF[1] + P[15][3]*SF[0] + P[15][0]*SPP[0] - P[15][2]*SPP[1];
+    nextP[15][7] = P[15][7] + P[15][4]*dt;
+    nextP[15][8] = P[15][8] + P[15][5]*dt;
+    nextP[15][9] = P[15][9] + P[15][6]*dt;
+    nextP[15][10] = P[15][10];
+    nextP[15][11] = P[15][11];
+    nextP[15][12] = P[15][12];
+    nextP[15][13] = P[15][13];
+    nextP[15][14] = P[15][14];
+    nextP[15][15] = P[15][15];
+    nextP[15][16] = P[15][16];
+    nextP[15][17] = P[15][17];
+    nextP[15][18] = P[15][18];
+    nextP[15][19] = P[15][19];
+    nextP[15][20] = P[15][20];
+    nextP[16][0] = P[16][0] + P[16][1]*SF[6] + P[16][2]*SPP[7] + P[16][3]*SPP[6] + P[16][10]*SPP[5] + P[16][11]*SPP[4] + P[16][12]*SPP[3];
+    nextP[16][1] = P[16][1] + P[16][0]*SF[5] + P[16][2]*SF[4] + P[16][3]*SPP[7] + P[16][11]*SPP[3] - P[16][12]*SPP[4] - (P[16][10]*q0)/2;
+    nextP[16][2] = P[16][2] + P[16][0]*SF[3] + P[16][3]*SF[5] + P[16][1]*SPP[6] - P[16][10]*SPP[3] + P[16][12]*SPP[5] - (P[16][11]*q0)/2;
+    nextP[16][3] = P[16][3] + P[16][0]*SF[4] + P[16][1]*SF[3] + P[16][2]*SF[6] + P[16][10]*SPP[4] - P[16][11]*SPP[5] - (P[16][12]*q0)/2;
+    nextP[16][4] = P[16][4] + P[16][1]*SF[0] + P[16][0]*SF[2] + P[16][2]*SPP[0] - P[16][3]*SPP[2];
+    nextP[16][5] = P[16][5] + P[16][0]*SF[1] + P[16][2]*SF[0] + P[16][3]*SF[2] - P[16][1]*SPP[0];
+    nextP[16][6] = P[16][6] + P[16][1]*SF[1] + P[16][3]*SF[0] + P[16][0]*SPP[0] - P[16][2]*SPP[1];
+    nextP[16][7] = P[16][7] + P[16][4]*dt;
+    nextP[16][8] = P[16][8] + P[16][5]*dt;
+    nextP[16][9] = P[16][9] + P[16][6]*dt;
+    nextP[16][10] = P[16][10];
+    nextP[16][11] = P[16][11];
+    nextP[16][12] = P[16][12];
+    nextP[16][13] = P[16][13];
+    nextP[16][14] = P[16][14];
+    nextP[16][15] = P[16][15];
+    nextP[16][16] = P[16][16];
+    nextP[16][17] = P[16][17];
+    nextP[16][18] = P[16][18];
+    nextP[16][19] = P[16][19];
+    nextP[16][20] = P[16][20];
+    nextP[17][0] = P[17][0] + P[17][1]*SF[6] + P[17][2]*SPP[7] + P[17][3]*SPP[6] + P[17][10]*SPP[5] + P[17][11]*SPP[4] + P[17][12]*SPP[3];
+    nextP[17][1] = P[17][1] + P[17][0]*SF[5] + P[17][2]*SF[4] + P[17][3]*SPP[7] + P[17][11]*SPP[3] - P[17][12]*SPP[4] - (P[17][10]*q0)/2;
+    nextP[17][2] = P[17][2] + P[17][0]*SF[3] + P[17][3]*SF[5] + P[17][1]*SPP[6] - P[17][10]*SPP[3] + P[17][12]*SPP[5] - (P[17][11]*q0)/2;
+    nextP[17][3] = P[17][3] + P[17][0]*SF[4] + P[17][1]*SF[3] + P[17][2]*SF[6] + P[17][10]*SPP[4] - P[17][11]*SPP[5] - (P[17][12]*q0)/2;
+    nextP[17][4] = P[17][4] + P[17][1]*SF[0] + P[17][0]*SF[2] + P[17][2]*SPP[0] - P[17][3]*SPP[2];
+    nextP[17][5] = P[17][5] + P[17][0]*SF[1] + P[17][2]*SF[0] + P[17][3]*SF[2] - P[17][1]*SPP[0];
+    nextP[17][6] = P[17][6] + P[17][1]*SF[1] + P[17][3]*SF[0] + P[17][0]*SPP[0] - P[17][2]*SPP[1];
+    nextP[17][7] = P[17][7] + P[17][4]*dt;
+    nextP[17][8] = P[17][8] + P[17][5]*dt;
+    nextP[17][9] = P[17][9] + P[17][6]*dt;
+    nextP[17][10] = P[17][10];
+    nextP[17][11] = P[17][11];
+    nextP[17][12] = P[17][12];
+    nextP[17][13] = P[17][13];
+    nextP[17][14] = P[17][14];
+    nextP[17][15] = P[17][15];
+    nextP[17][16] = P[17][16];
+    nextP[17][17] = P[17][17];
+    nextP[17][18] = P[17][18];
+    nextP[17][19] = P[17][19];
+    nextP[17][20] = P[17][20];
+    nextP[18][0] = P[18][0] + P[18][1]*SF[6] + P[18][2]*SPP[7] + P[18][3]*SPP[6] + P[18][10]*SPP[5] + P[18][11]*SPP[4] + P[18][12]*SPP[3];
+    nextP[18][1] = P[18][1] + P[18][0]*SF[5] + P[18][2]*SF[4] + P[18][3]*SPP[7] + P[18][11]*SPP[3] - P[18][12]*SPP[4] - (P[18][10]*q0)/2;
+    nextP[18][2] = P[18][2] + P[18][0]*SF[3] + P[18][3]*SF[5] + P[18][1]*SPP[6] - P[18][10]*SPP[3] + P[18][12]*SPP[5] - (P[18][11]*q0)/2;
+    nextP[18][3] = P[18][3] + P[18][0]*SF[4] + P[18][1]*SF[3] + P[18][2]*SF[6] + P[18][10]*SPP[4] - P[18][11]*SPP[5] - (P[18][12]*q0)/2;
+    nextP[18][4] = P[18][4] + P[18][1]*SF[0] + P[18][0]*SF[2] + P[18][2]*SPP[0] - P[18][3]*SPP[2];
+    nextP[18][5] = P[18][5] + P[18][0]*SF[1] + P[18][2]*SF[0] + P[18][3]*SF[2] - P[18][1]*SPP[0];
+    nextP[18][6] = P[18][6] + P[18][1]*SF[1] + P[18][3]*SF[0] + P[18][0]*SPP[0] - P[18][2]*SPP[1];
+    nextP[18][7] = P[18][7] + P[18][4]*dt;
+    nextP[18][8] = P[18][8] + P[18][5]*dt;
+    nextP[18][9] = P[18][9] + P[18][6]*dt;
+    nextP[18][10] = P[18][10];
+    nextP[18][11] = P[18][11];
+    nextP[18][12] = P[18][12];
+    nextP[18][13] = P[18][13];
+    nextP[18][14] = P[18][14];
+    nextP[18][15] = P[18][15];
+    nextP[18][16] = P[18][16];
+    nextP[18][17] = P[18][17];
+    nextP[18][18] = P[18][18];
+    nextP[18][19] = P[18][19];
+    nextP[18][20] = P[18][20];
+    nextP[19][0] = P[19][0] + P[19][1]*SF[6] + P[19][2]*SPP[7] + P[19][3]*SPP[6] + P[19][10]*SPP[5] + P[19][11]*SPP[4] + P[19][12]*SPP[3];
+    nextP[19][1] = P[19][1] + P[19][0]*SF[5] + P[19][2]*SF[4] + P[19][3]*SPP[7] + P[19][11]*SPP[3] - P[19][12]*SPP[4] - (P[19][10]*q0)/2;
+    nextP[19][2] = P[19][2] + P[19][0]*SF[3] + P[19][3]*SF[5] + P[19][1]*SPP[6] - P[19][10]*SPP[3] + P[19][12]*SPP[5] - (P[19][11]*q0)/2;
+    nextP[19][3] = P[19][3] + P[19][0]*SF[4] + P[19][1]*SF[3] + P[19][2]*SF[6] + P[19][10]*SPP[4] - P[19][11]*SPP[5] - (P[19][12]*q0)/2;
+    nextP[19][4] = P[19][4] + P[19][1]*SF[0] + P[19][0]*SF[2] + P[19][2]*SPP[0] - P[19][3]*SPP[2];
+    nextP[19][5] = P[19][5] + P[19][0]*SF[1] + P[19][2]*SF[0] + P[19][3]*SF[2] - P[19][1]*SPP[0];
+    nextP[19][6] = P[19][6] + P[19][1]*SF[1] + P[19][3]*SF[0] + P[19][0]*SPP[0] - P[19][2]*SPP[1];
+    nextP[19][7] = P[19][7] + P[19][4]*dt;
+    nextP[19][8] = P[19][8] + P[19][5]*dt;
+    nextP[19][9] = P[19][9] + P[19][6]*dt;
+    nextP[19][10] = P[19][10];
+    nextP[19][11] = P[19][11];
+    nextP[19][12] = P[19][12];
+    nextP[19][13] = P[19][13];
+    nextP[19][14] = P[19][14];
+    nextP[19][15] = P[19][15];
+    nextP[19][16] = P[19][16];
+    nextP[19][17] = P[19][17];
+    nextP[19][18] = P[19][18];
+    nextP[19][19] = P[19][19];
+    nextP[19][20] = P[19][20];
+    nextP[20][0] = P[20][0] + P[20][1]*SF[6] + P[20][2]*SPP[7] + P[20][3]*SPP[6] + P[20][10]*SPP[5] + P[20][11]*SPP[4] + P[20][12]*SPP[3];
+    nextP[20][1] = P[20][1] + P[20][0]*SF[5] + P[20][2]*SF[4] + P[20][3]*SPP[7] + P[20][11]*SPP[3] - P[20][12]*SPP[4] - (P[20][10]*q0)/2;
+    nextP[20][2] = P[20][2] + P[20][0]*SF[3] + P[20][3]*SF[5] + P[20][1]*SPP[6] - P[20][10]*SPP[3] + P[20][12]*SPP[5] - (P[20][11]*q0)/2;
+    nextP[20][3] = P[20][3] + P[20][0]*SF[4] + P[20][1]*SF[3] + P[20][2]*SF[6] + P[20][10]*SPP[4] - P[20][11]*SPP[5] - (P[20][12]*q0)/2;
+    nextP[20][4] = P[20][4] + P[20][1]*SF[0] + P[20][0]*SF[2] + P[20][2]*SPP[0] - P[20][3]*SPP[2];
+    nextP[20][5] = P[20][5] + P[20][0]*SF[1] + P[20][2]*SF[0] + P[20][3]*SF[2] - P[20][1]*SPP[0];
+    nextP[20][6] = P[20][6] + P[20][1]*SF[1] + P[20][3]*SF[0] + P[20][0]*SPP[0] - P[20][2]*SPP[1];
+    nextP[20][7] = P[20][7] + P[20][4]*dt;
+    nextP[20][8] = P[20][8] + P[20][5]*dt;
+    nextP[20][9] = P[20][9] + P[20][6]*dt;
+    nextP[20][10] = P[20][10];
+    nextP[20][11] = P[20][11];
+    nextP[20][12] = P[20][12];
+    nextP[20][13] = P[20][13];
+    nextP[20][14] = P[20][14];
+    nextP[20][15] = P[20][15];
+    nextP[20][16] = P[20][16];
+    nextP[20][17] = P[20][17];
+    nextP[20][18] = P[20][18];
+    nextP[20][19] = P[20][19];
+    nextP[20][20] = P[20][20];
+
+    for (unsigned i = 0; i < n_states; i++)
+    {
+        nextP[i][i] = nextP[i][i] + processNoise[i];
+    }
+
+    // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by
+    // setting the coresponding covariance terms to zero.
+    if (onGround || !useCompass)
+    {
+        zeroRows(nextP,15,20);
+        zeroCols(nextP,15,20);
+    }
+
+    // If on ground or not using airspeed sensing, inhibit wind velocity
+    // covariance growth.
+    if (onGround || !useAirspeed)
+    {
+        zeroRows(nextP,13,14);
+        zeroCols(nextP,13,14);
+    }
+
+    // If the total position variance exceds 1E6 (1000m), then stop covariance
+    // growth by setting the predicted to the previous values
+    // This prevent an ill conditioned matrix from occurring for long periods
+    // without GPS
+    if ((P[7][7] + P[8][8]) > 1E6f)
+    {
+        for (uint8_t i=7; i<=8; i++)
+        {
+            for (unsigned j = 0; j < n_states; j++)
+            {
+                nextP[i][j] = P[i][j];
+                nextP[j][i] = P[j][i];
+            }
+        }
+    }
+
+    if (onGround || staticMode) {
+        // copy the portion of the variances we want to
+        // propagate
+        for (unsigned i = 0; i < 14; i++) {
+            P[i][i] = nextP[i][i];
+
+            // force symmetry for observable states
+            // force zero for non-observable states
+            for (unsigned i = 1; i < n_states; i++)
+            {
+                for (uint8_t j = 0; j < i; j++)
+                {
+                    if ((i > 12) || (j > 12)) {
+                        P[i][j] = 0.0f;
+                    } else {
+                        P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
+                    }
+                    P[j][i] = P[i][j];
+                }
+            }
+        }
+
+    } else {
+
+        // Copy covariance
+        for (unsigned i = 0; i < n_states; i++) {
+            P[i][i] = nextP[i][i];
+        }
+
+        // force symmetry for observable states
+        for (unsigned i = 1; i < n_states; i++)
+        {
+            for (uint8_t j = 0; j < i; j++)
+            {
+                P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
+                P[j][i] = P[i][j];
+            }
+        }
+
+    }
+
+    ConstrainVariances();
+}
+
+void AttPosEKF::FuseVelposNED()
+{
+
+// declare variables used by fault isolation logic
+    uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure
+    uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available
+    uint32_t hgtRetryTime = 5000; // height measurement retry time
+    uint32_t horizRetryTime;
+
+// declare variables used to check measurement errors
+    float velInnov[3] = {0.0f,0.0f,0.0f};
+    float posInnov[2] = {0.0f,0.0f};
+    float hgtInnov = 0.0f;
+
+// declare variables used to control access to arrays
+    bool fuseData[6] = {false,false,false,false,false,false};
+    uint8_t stateIndex;
+    uint8_t obsIndex;
+    uint8_t indexLimit;
+
+// declare variables used by state and covariance update calculations
+    float velErr;
+    float posErr;
+    float R_OBS[6];
+    float observation[6];
+    float SK;
+    float quatMag;
+
+// Perform sequential fusion of GPS measurements. This assumes that the
+// errors in the different velocity and position components are
+// uncorrelated which is not true, however in the absence of covariance
+// data from the GPS receiver it is the only assumption we can make
+// so we might as well take advantage of the computational efficiencies
+// associated with sequential fusion
+    if (fuseVelData || fusePosData || fuseHgtData)
+    {
+        // set the GPS data timeout depending on whether airspeed data is present
+        if (useAirspeed) horizRetryTime = gpsRetryTime;
+        else horizRetryTime = gpsRetryTimeNoTAS;
+
+        // Form the observation vector
+        for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i];
+        for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3];
+        observation[5] = -(hgtMea);
+
+        // Estimate the GPS Velocity, GPS horiz position and height measurement variances.
+        velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
+        posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
+        R_OBS[0] = 0.04f + sq(velErr);
+        R_OBS[1] = R_OBS[0];
+        R_OBS[2] = 0.08f + sq(velErr);
+        R_OBS[3] = R_OBS[2];
+        R_OBS[4] = 4.0f  + sq(posErr);
+        R_OBS[5] = 4.0f;
+
+        // Set innovation variances to zero default
+        for (uint8_t i = 0; i<=5; i++)
+        {
+            varInnovVelPos[i] = 0.0f;
+        }
+        // calculate innovations and check GPS data validity using an innovation consistency check
+        if (fuseVelData)
+        {
+            // test velocity measurements
+            uint8_t imax = 2;
+            if (fusionModeGPS == 1) imax = 1;
+            for (uint8_t i = 0; i<=imax; i++)
+            {
+                velInnov[i] = statesAtVelTime[i+4] - velNED[i];
+                stateIndex = 4 + i;
+                varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
+            }
+            // apply a 5-sigma threshold
+            current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
+            current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
+            if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
+            {
+                current_ekf_state.velHealth = true;
+                current_ekf_state.velFailTime = millis();
+            }
+            else
+            {
+                current_ekf_state.velHealth = false;
+            }
+        }
+        if (fusePosData)
+        {
+            // test horizontal position measurements
+            posInnov[0] = statesAtPosTime[7] - posNE[0];
+            posInnov[1] = statesAtPosTime[8] - posNE[1];
+            varInnovVelPos[3] = P[7][7] + R_OBS[3];
+            varInnovVelPos[4] = P[8][8] + R_OBS[4];
+            // apply a 10-sigma threshold
+            current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]);
+            current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime;
+            if (current_ekf_state.posHealth || current_ekf_state.posTimeout)
+            {
+                current_ekf_state.posHealth = true;
+                current_ekf_state.posFailTime = millis();
+            }
+            else
+            {
+                current_ekf_state.posHealth = false;
+            }
+        }
+        // test height measurements
+        if (fuseHgtData)
+        {
+            hgtInnov = statesAtHgtTime[9] + hgtMea;
+            varInnovVelPos[5] = P[9][9] + R_OBS[5];
+            // apply a 10-sigma threshold
+            current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
+            current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime;
+            if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout)
+            {
+                current_ekf_state.hgtHealth = true;
+                current_ekf_state.hgtFailTime = millis();
+            }
+            else
+            {
+                current_ekf_state.hgtHealth = false;
+            }
+        }
+        // Set range for sequential fusion of velocity and position measurements depending
+        // on which data is available and its health
+        if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth)
+        {
+            fuseData[0] = true;
+            fuseData[1] = true;
+            fuseData[2] = true;
+        }
+        if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth)
+        {
+            fuseData[0] = true;
+            fuseData[1] = true;
+        }
+        if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth)
+        {
+            fuseData[3] = true;
+            fuseData[4] = true;
+        }
+        if (fuseHgtData && current_ekf_state.hgtHealth)
+        {
+            fuseData[5] = true;
+        }
+        // Limit range of states modified when on ground
+        if(!onGround)
+        {
+            indexLimit = 20;
+        }
+        else
+        {
+            indexLimit = 12;
+        }
+        // Fuse measurements sequentially
+        for (obsIndex=0; obsIndex<=5; obsIndex++)
+        {
+            if (fuseData[obsIndex])
+            {
+                stateIndex = 4 + obsIndex;
+                // Calculate the measurement innovation, using states from a
+                // different time coordinate if fusing height data
+                if (obsIndex >= 0 && obsIndex <= 2)
+                {
+                    innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex];
+                }
+                else if (obsIndex == 3 || obsIndex == 4)
+                {
+                    innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex];
+                }
+                else if (obsIndex == 5)
+                {
+                    innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex];
+                }
+                // Calculate the Kalman Gain
+                // Calculate innovation variances - also used for data logging
+                varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
+                SK = 1.0/varInnovVelPos[obsIndex];
+                for (uint8_t i= 0; i<=indexLimit; i++)
+                {
+                    Kfusion[i] = P[i][stateIndex]*SK;
+                }
+                // Calculate state corrections and re-normalise the quaternions
+                for (uint8_t i = 0; i<=indexLimit; i++)
+                {
+                    states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
+                }
+                quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+                if (quatMag > 1e-12f) // divide by  0 protection
+                {
+                    for (uint8_t i = 0; i<=3; i++)
+                    {
+                        states[i] = states[i] / quatMag;
+                    }
+                }
+                // Update the covariance - take advantage of direct observation of a
+                // single state at index = stateIndex to reduce computations
+                // Optimised implementation of standard equation P = (I - K*H)*P;
+                for (uint8_t i= 0; i<=indexLimit; i++)
+                {
+                    for (uint8_t j= 0; j<=indexLimit; j++)
+                    {
+                        KHP[i][j] = Kfusion[i] * P[stateIndex][j];
+                    }
+                }
+                for (uint8_t i= 0; i<=indexLimit; i++)
+                {
+                    for (uint8_t j= 0; j<=indexLimit; j++)
+                    {
+                        P[i][j] = P[i][j] - KHP[i][j];
+                    }
+                }
+            }
+        }
+    }
+
+    ForceSymmetry();
+    ConstrainVariances();
+
+    //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL"));
+}
+
+void AttPosEKF::FuseMagnetometer()
+{
+    uint8_t obsIndex;
+    uint8_t indexLimit;
+    float DCM[3][3] =
+    {
+        {1.0f,0.0f,0.0f} ,
+        {0.0f,1.0f,0.0f} ,
+        {0.0f,0.0f,1.0f}
+    };
+    float MagPred[3] = {0.0f,0.0f,0.0f};
+    float SK_MX[6];
+    float SK_MY[5];
+    float SK_MZ[6];
+    float SH_MAG[9] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
+
+// Perform sequential fusion of Magnetometer measurements.
+// This assumes that the errors in the different components are
+// uncorrelated which is not true, however in the absence of covariance
+// data fit is the only assumption we can make
+// so we might as well take advantage of the computational efficiencies
+// associated with sequential fusion
+    if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2))
+    {
+        // Limit range of states modified when on ground
+        if(!onGround)
+        {
+            indexLimit = 20;
+        }
+        else
+        {
+            indexLimit = 12;
+        }
+
+        static float q0 = 0.0f;
+        static float q1 = 0.0f;
+        static float q2 = 0.0f;
+        static float q3 = 1.0f;
+        static float magN = 0.4f;
+        static float magE = 0.0f;
+        static float magD = 0.3f;
+
+        static float R_MAG = 0.0025f;
+
+        float H_MAG[21] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
+
+        // Sequential fusion of XYZ components to spread processing load across
+        // three prediction time steps.
+
+        // Calculate observation jacobians and Kalman gains
+        if (fuseMagData)
+        {
+            static float magXbias = 0.0f;
+            static float magYbias = 0.0f;
+            static float magZbias = 0.0f;
+
+            // Copy required states to local variable names
+            q0       = statesAtMagMeasTime[0];
+            q1       = statesAtMagMeasTime[1];
+            q2       = statesAtMagMeasTime[2];
+            q3       = statesAtMagMeasTime[3];
+            magN     = statesAtMagMeasTime[15];
+            magE     = statesAtMagMeasTime[16];
+            magD     = statesAtMagMeasTime[17];
+            magXbias = statesAtMagMeasTime[18];
+            magYbias = statesAtMagMeasTime[19];
+            magZbias = statesAtMagMeasTime[20];
+
+            // rotate predicted earth components into body axes and calculate
+            // predicted measurments
+            DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
+            DCM[0][1] = 2*(q1*q2 + q0*q3);
+            DCM[0][2] = 2*(q1*q3-q0*q2);
+            DCM[1][0] = 2*(q1*q2 - q0*q3);
+            DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
+            DCM[1][2] = 2*(q2*q3 + q0*q1);
+            DCM[2][0] = 2*(q1*q3 + q0*q2);
+            DCM[2][1] = 2*(q2*q3 - q0*q1);
+            DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
+            MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE  + DCM[0][2]*magD + magXbias;
+            MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE  + DCM[1][2]*magD + magYbias;
+            MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE  + DCM[2][2]*magD + magZbias;
+
+            // scale magnetometer observation error with total angular rate
+            R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU);
+
+            // Calculate observation jacobians
+            SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
+            SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
+            SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
+            SH_MAG[3] = sq(q3);
+            SH_MAG[4] = sq(q2);
+            SH_MAG[5] = sq(q1);
+            SH_MAG[6] = sq(q0);
+            SH_MAG[7] = 2*magN*q0;
+            SH_MAG[8] = 2*magE*q3;
+
+            for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0;
+            H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            H_MAG[1] = SH_MAG[0];
+            H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2;
+            H_MAG[3] = SH_MAG[2];
+            H_MAG[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
+            H_MAG[16] = 2*q0*q3 + 2*q1*q2;
+            H_MAG[17] = 2*q1*q3 - 2*q0*q2;
+            H_MAG[18] = 1.0f;
+
+            // Calculate Kalman gain
+            SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
+            SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
+            SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            SK_MX[4] = 2*q0*q2 - 2*q1*q3;
+            SK_MX[5] = 2*q0*q3 + 2*q1*q2;
+            Kfusion[0] = SK_MX[0]*(P[0][18] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][15]*SK_MX[1] + P[0][16]*SK_MX[5] - P[0][17]*SK_MX[4]);
+            Kfusion[1] = SK_MX[0]*(P[1][18] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][15]*SK_MX[1] + P[1][16]*SK_MX[5] - P[1][17]*SK_MX[4]);
+            Kfusion[2] = SK_MX[0]*(P[2][18] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][15]*SK_MX[1] + P[2][16]*SK_MX[5] - P[2][17]*SK_MX[4]);
+            Kfusion[3] = SK_MX[0]*(P[3][18] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][15]*SK_MX[1] + P[3][16]*SK_MX[5] - P[3][17]*SK_MX[4]);
+            Kfusion[4] = SK_MX[0]*(P[4][18] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][15]*SK_MX[1] + P[4][16]*SK_MX[5] - P[4][17]*SK_MX[4]);
+            Kfusion[5] = SK_MX[0]*(P[5][18] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][15]*SK_MX[1] + P[5][16]*SK_MX[5] - P[5][17]*SK_MX[4]);
+            Kfusion[6] = SK_MX[0]*(P[6][18] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][15]*SK_MX[1] + P[6][16]*SK_MX[5] - P[6][17]*SK_MX[4]);
+            Kfusion[7] = SK_MX[0]*(P[7][18] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][15]*SK_MX[1] + P[7][16]*SK_MX[5] - P[7][17]*SK_MX[4]);
+            Kfusion[8] = SK_MX[0]*(P[8][18] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][15]*SK_MX[1] + P[8][16]*SK_MX[5] - P[8][17]*SK_MX[4]);
+            Kfusion[9] = SK_MX[0]*(P[9][18] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][15]*SK_MX[1] + P[9][16]*SK_MX[5] - P[9][17]*SK_MX[4]);
+            Kfusion[10] = SK_MX[0]*(P[10][18] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][15]*SK_MX[1] + P[10][16]*SK_MX[5] - P[10][17]*SK_MX[4]);
+            Kfusion[11] = SK_MX[0]*(P[11][18] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][15]*SK_MX[1] + P[11][16]*SK_MX[5] - P[11][17]*SK_MX[4]);
+            Kfusion[12] = SK_MX[0]*(P[12][18] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][15]*SK_MX[1] + P[12][16]*SK_MX[5] - P[12][17]*SK_MX[4]);
+            Kfusion[13] = SK_MX[0]*(P[13][18] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][15]*SK_MX[1] + P[13][16]*SK_MX[5] - P[13][17]*SK_MX[4]);
+            Kfusion[14] = SK_MX[0]*(P[14][18] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][15]*SK_MX[1] + P[14][16]*SK_MX[5] - P[14][17]*SK_MX[4]);
+            Kfusion[15] = SK_MX[0]*(P[15][18] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][15]*SK_MX[1] + P[15][16]*SK_MX[5] - P[15][17]*SK_MX[4]);
+            Kfusion[16] = SK_MX[0]*(P[16][18] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][15]*SK_MX[1] + P[16][16]*SK_MX[5] - P[16][17]*SK_MX[4]);
+            Kfusion[17] = SK_MX[0]*(P[17][18] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][15]*SK_MX[1] + P[17][16]*SK_MX[5] - P[17][17]*SK_MX[4]);
+            Kfusion[18] = SK_MX[0]*(P[18][18] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][15]*SK_MX[1] + P[18][16]*SK_MX[5] - P[18][17]*SK_MX[4]);
+            Kfusion[19] = SK_MX[0]*(P[19][18] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][15]*SK_MX[1] + P[19][16]*SK_MX[5] - P[19][17]*SK_MX[4]);
+            Kfusion[20] = SK_MX[0]*(P[20][18] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]);
+            varInnovMag[0] = 1.0f/SK_MX[0];
+            innovMag[0] = MagPred[0] - magData.x;
+
+            // reset the observation index to 0 (we start by fusing the X
+            // measurement)
+            obsIndex = 0;
+        }
+        else if (obsIndex == 1) // we are now fusing the Y measurement
+        {
+            // Calculate observation jacobians
+            for (unsigned int i=0; i<n_states; i++) H_MAG[i] = 0;
+            H_MAG[0] = SH_MAG[2];
+            H_MAG[1] = SH_MAG[1];
+            H_MAG[2] = SH_MAG[0];
+            H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7];
+            H_MAG[15] = 2*q1*q2 - 2*q0*q3;
+            H_MAG[16] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
+            H_MAG[17] = 2*q0*q1 + 2*q2*q3;
+            H_MAG[19] = 1;
+
+            // Calculate Kalman gain
+            SK_MY[0] = 1/(P[19][19] + R_MAG + P[0][19]*SH_MAG[2] + P[1][19]*SH_MAG[1] + P[2][19]*SH_MAG[0] - P[16][19]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[19][15] + P[0][15]*SH_MAG[2] + P[1][15]*SH_MAG[1] + P[2][15]*SH_MAG[0] - P[16][15]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][15]*(2*q0*q3 - 2*q1*q2) + P[17][15]*(2*q0*q1 + 2*q2*q3) - P[3][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[19][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[16][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][17]*(2*q0*q3 - 2*q1*q2) + P[17][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[16][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][3]*(2*q0*q3 - 2*q1*q2) + P[17][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[15][19]*(2*q0*q3 - 2*q1*q2) + P[17][19]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[19][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[16][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][0]*(2*q0*q3 - 2*q1*q2) + P[17][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[19][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[16][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][1]*(2*q0*q3 - 2*q1*q2) + P[17][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[16][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][2]*(2*q0*q3 - 2*q1*q2) + P[17][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[16][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][16]*(2*q0*q3 - 2*q1*q2) + P[17][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
+            SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            SK_MY[3] = 2*q0*q3 - 2*q1*q2;
+            SK_MY[4] = 2*q0*q1 + 2*q2*q3;
+            Kfusion[0] = SK_MY[0]*(P[0][19] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][16]*SK_MY[1] - P[0][15]*SK_MY[3] + P[0][17]*SK_MY[4]);
+            Kfusion[1] = SK_MY[0]*(P[1][19] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][16]*SK_MY[1] - P[1][15]*SK_MY[3] + P[1][17]*SK_MY[4]);
+            Kfusion[2] = SK_MY[0]*(P[2][19] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][16]*SK_MY[1] - P[2][15]*SK_MY[3] + P[2][17]*SK_MY[4]);
+            Kfusion[3] = SK_MY[0]*(P[3][19] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][16]*SK_MY[1] - P[3][15]*SK_MY[3] + P[3][17]*SK_MY[4]);
+            Kfusion[4] = SK_MY[0]*(P[4][19] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][16]*SK_MY[1] - P[4][15]*SK_MY[3] + P[4][17]*SK_MY[4]);
+            Kfusion[5] = SK_MY[0]*(P[5][19] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][16]*SK_MY[1] - P[5][15]*SK_MY[3] + P[5][17]*SK_MY[4]);
+            Kfusion[6] = SK_MY[0]*(P[6][19] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][16]*SK_MY[1] - P[6][15]*SK_MY[3] + P[6][17]*SK_MY[4]);
+            Kfusion[7] = SK_MY[0]*(P[7][19] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][16]*SK_MY[1] - P[7][15]*SK_MY[3] + P[7][17]*SK_MY[4]);
+            Kfusion[8] = SK_MY[0]*(P[8][19] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][16]*SK_MY[1] - P[8][15]*SK_MY[3] + P[8][17]*SK_MY[4]);
+            Kfusion[9] = SK_MY[0]*(P[9][19] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][16]*SK_MY[1] - P[9][15]*SK_MY[3] + P[9][17]*SK_MY[4]);
+            Kfusion[10] = SK_MY[0]*(P[10][19] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][16]*SK_MY[1] - P[10][15]*SK_MY[3] + P[10][17]*SK_MY[4]);
+            Kfusion[11] = SK_MY[0]*(P[11][19] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][16]*SK_MY[1] - P[11][15]*SK_MY[3] + P[11][17]*SK_MY[4]);
+            Kfusion[12] = SK_MY[0]*(P[12][19] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][16]*SK_MY[1] - P[12][15]*SK_MY[3] + P[12][17]*SK_MY[4]);
+            Kfusion[13] = SK_MY[0]*(P[13][19] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][16]*SK_MY[1] - P[13][15]*SK_MY[3] + P[13][17]*SK_MY[4]);
+            Kfusion[14] = SK_MY[0]*(P[14][19] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][16]*SK_MY[1] - P[14][15]*SK_MY[3] + P[14][17]*SK_MY[4]);
+            Kfusion[15] = SK_MY[0]*(P[15][19] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][16]*SK_MY[1] - P[15][15]*SK_MY[3] + P[15][17]*SK_MY[4]);
+            Kfusion[16] = SK_MY[0]*(P[16][19] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][16]*SK_MY[1] - P[16][15]*SK_MY[3] + P[16][17]*SK_MY[4]);
+            Kfusion[17] = SK_MY[0]*(P[17][19] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][16]*SK_MY[1] - P[17][15]*SK_MY[3] + P[17][17]*SK_MY[4]);
+            Kfusion[18] = SK_MY[0]*(P[18][19] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][16]*SK_MY[1] - P[18][15]*SK_MY[3] + P[18][17]*SK_MY[4]);
+            Kfusion[19] = SK_MY[0]*(P[19][19] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][16]*SK_MY[1] - P[19][15]*SK_MY[3] + P[19][17]*SK_MY[4]);
+            Kfusion[20] = SK_MY[0]*(P[20][19] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][16]*SK_MY[1] - P[20][15]*SK_MY[3] + P[20][17]*SK_MY[4]);
+            varInnovMag[1] = 1.0f/SK_MY[0];
+            innovMag[1] = MagPred[1] - magData.y;
+        }
+        else if (obsIndex == 2) // we are now fusing the Z measurement
+        {
+            // Calculate observation jacobians
+            for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0;
+            H_MAG[0] = SH_MAG[1];
+            H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1;
+            H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            H_MAG[3] = SH_MAG[0];
+            H_MAG[15] = 2*q0*q2 + 2*q1*q3;
+            H_MAG[16] = 2*q2*q3 - 2*q0*q1;
+            H_MAG[17] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
+            H_MAG[20] = 1;
+
+            // Calculate Kalman gain
+            SK_MZ[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[1] + P[3][20]*SH_MAG[0] + P[17][20]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[20][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[17][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][1]*(2*q0*q2 + 2*q1*q3) - P[16][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[17][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][2]*(2*q0*q2 + 2*q1*q3) - P[16][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[17][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][0]*(2*q0*q2 + 2*q1*q3) - P[16][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[17][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][3]*(2*q0*q2 + 2*q1*q3) - P[16][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[17][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][17]*(2*q0*q2 + 2*q1*q3) - P[16][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[15][20]*(2*q0*q2 + 2*q1*q3) - P[16][20]*(2*q0*q1 - 2*q2*q3) - P[1][20]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[20][15] + P[0][15]*SH_MAG[1] + P[3][15]*SH_MAG[0] + P[17][15]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][15]*(2*q0*q2 + 2*q1*q3) - P[16][15]*(2*q0*q1 - 2*q2*q3) - P[1][15]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[20][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[17][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][16]*(2*q0*q2 + 2*q1*q3) - P[16][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
+            SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
+            SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            SK_MZ[4] = 2*q0*q1 - 2*q2*q3;
+            SK_MZ[5] = 2*q0*q2 + 2*q1*q3;
+            Kfusion[0] = SK_MZ[0]*(P[0][20] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][17]*SK_MZ[1] + P[0][15]*SK_MZ[5] - P[0][16]*SK_MZ[4]);
+            Kfusion[1] = SK_MZ[0]*(P[1][20] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][17]*SK_MZ[1] + P[1][15]*SK_MZ[5] - P[1][16]*SK_MZ[4]);
+            Kfusion[2] = SK_MZ[0]*(P[2][20] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][17]*SK_MZ[1] + P[2][15]*SK_MZ[5] - P[2][16]*SK_MZ[4]);
+            Kfusion[3] = SK_MZ[0]*(P[3][20] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][17]*SK_MZ[1] + P[3][15]*SK_MZ[5] - P[3][16]*SK_MZ[4]);
+            Kfusion[4] = SK_MZ[0]*(P[4][20] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][17]*SK_MZ[1] + P[4][15]*SK_MZ[5] - P[4][16]*SK_MZ[4]);
+            Kfusion[5] = SK_MZ[0]*(P[5][20] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][17]*SK_MZ[1] + P[5][15]*SK_MZ[5] - P[5][16]*SK_MZ[4]);
+            Kfusion[6] = SK_MZ[0]*(P[6][20] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][17]*SK_MZ[1] + P[6][15]*SK_MZ[5] - P[6][16]*SK_MZ[4]);
+            Kfusion[7] = SK_MZ[0]*(P[7][20] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][17]*SK_MZ[1] + P[7][15]*SK_MZ[5] - P[7][16]*SK_MZ[4]);
+            Kfusion[8] = SK_MZ[0]*(P[8][20] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][17]*SK_MZ[1] + P[8][15]*SK_MZ[5] - P[8][16]*SK_MZ[4]);
+            Kfusion[9] = SK_MZ[0]*(P[9][20] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][17]*SK_MZ[1] + P[9][15]*SK_MZ[5] - P[9][16]*SK_MZ[4]);
+            Kfusion[10] = SK_MZ[0]*(P[10][20] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][17]*SK_MZ[1] + P[10][15]*SK_MZ[5] - P[10][16]*SK_MZ[4]);
+            Kfusion[11] = SK_MZ[0]*(P[11][20] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][17]*SK_MZ[1] + P[11][15]*SK_MZ[5] - P[11][16]*SK_MZ[4]);
+            Kfusion[12] = SK_MZ[0]*(P[12][20] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][17]*SK_MZ[1] + P[12][15]*SK_MZ[5] - P[12][16]*SK_MZ[4]);
+            Kfusion[13] = SK_MZ[0]*(P[13][20] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][17]*SK_MZ[1] + P[13][15]*SK_MZ[5] - P[13][16]*SK_MZ[4]);
+            Kfusion[14] = SK_MZ[0]*(P[14][20] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][17]*SK_MZ[1] + P[14][15]*SK_MZ[5] - P[14][16]*SK_MZ[4]);
+            Kfusion[15] = SK_MZ[0]*(P[15][20] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][17]*SK_MZ[1] + P[15][15]*SK_MZ[5] - P[15][16]*SK_MZ[4]);
+            Kfusion[16] = SK_MZ[0]*(P[16][20] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][17]*SK_MZ[1] + P[16][15]*SK_MZ[5] - P[16][16]*SK_MZ[4]);
+            Kfusion[17] = SK_MZ[0]*(P[17][20] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][17]*SK_MZ[1] + P[17][15]*SK_MZ[5] - P[17][16]*SK_MZ[4]);
+            Kfusion[18] = SK_MZ[0]*(P[18][20] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][17]*SK_MZ[1] + P[18][15]*SK_MZ[5] - P[18][16]*SK_MZ[4]);
+            Kfusion[19] = SK_MZ[0]*(P[19][20] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][17]*SK_MZ[1] + P[19][15]*SK_MZ[5] - P[19][16]*SK_MZ[4]);
+            Kfusion[20] = SK_MZ[0]*(P[20][20] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][17]*SK_MZ[1] + P[20][15]*SK_MZ[5] - P[20][16]*SK_MZ[4]);
+            varInnovMag[2] = 1.0f/SK_MZ[0];
+            innovMag[2] = MagPred[2] - magData.z;
+
+        }
+
+        // Check the innovation for consistency and don't fuse if > 5Sigma
+        if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0)
+        {
+            // correct the state vector
+            for (uint8_t j= 0; j<=indexLimit; j++)
+            {
+                states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
+            }
+            // normalise the quaternion states
+            float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+            if (quatMag > 1e-12)
+            {
+                for (uint8_t j= 0; j<=3; j++)
+                {
+                    float quatMagInv = 1.0f/quatMag;
+                    states[j] = states[j] * quatMagInv;
+                }
+            }
+            // correct the covariance P = (I - K*H)*P
+            // take advantage of the empty columns in KH to reduce the
+            // number of operations
+            for (uint8_t i = 0; i<=indexLimit; i++)
+            {
+                for (uint8_t j = 0; j<=3; j++)
+                {
+                    KH[i][j] = Kfusion[i] * H_MAG[j];
+                }
+                for (uint8_t j = 4; j<=17; j++) KH[i][j] = 0.0f;
+                if (!onGround)
+                {
+                    for (uint8_t j = 15; j<=20; j++)
+                    {
+                        KH[i][j] = Kfusion[i] * H_MAG[j];
+                    }
+                }
+                else
+                {
+                    for (uint8_t j = 15; j<=20; j++)
+                    {
+                        KH[i][j] = 0.0f;
+                    }
+                }
+            }
+            for (uint8_t i = 0; i<=indexLimit; i++)
+            {
+                for (uint8_t j = 0; j<=indexLimit; j++)
+                {
+                    KHP[i][j] = 0.0f;
+                    for (uint8_t k = 0; k<=3; k++)
+                    {
+                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                    }
+                    if (!onGround)
+                    {
+                        for (uint8_t k = 15; k<=20; k++)
+                        {
+                            KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                        }
+                    }
+                }
+            }
+        }
+        for (uint8_t i = 0; i<=indexLimit; i++)
+        {
+            for (uint8_t j = 0; j<=indexLimit; j++)
+            {
+                P[i][j] = P[i][j] - KHP[i][j];
+            }
+        }
+    }
+    obsIndex = obsIndex + 1;
+
+    ForceSymmetry();
+    ConstrainVariances();
+}
+
+void AttPosEKF::FuseAirspeed()
+{
+    float vn;
+    float ve;
+    float vd;
+    float vwn;
+    float vwe;
+    const float R_TAS = 2.0f;
+    float SH_TAS[3];
+    float Kfusion[21];
+    float VtasPred;
+
+    // Copy required states to local variable names
+    vn = statesAtVtasMeasTime[4];
+    ve = statesAtVtasMeasTime[5];
+    vd = statesAtVtasMeasTime[6];
+    vwn = statesAtVtasMeasTime[13];
+    vwe = statesAtVtasMeasTime[14];
+
+    // Need to check that it is flying before fusing airspeed data
+    // Calculate the predicted airspeed
+    VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
+    // Perform fusion of True Airspeed measurement
+    if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
+    {
+        // Calculate observation jacobians
+        SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
+        SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
+        SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
+        
+        float H_TAS[21];
+        for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f;
+        H_TAS[4] = SH_TAS[2];
+        H_TAS[5] = SH_TAS[1];
+        H_TAS[6] = vd*SH_TAS[0];
+        H_TAS[13] = -SH_TAS[2];
+        H_TAS[14] = -SH_TAS[1];
+
+        // Calculate Kalman gains
+        float SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[13][4]*SH_TAS[2] - P[14][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[13][5]*SH_TAS[2] - P[14][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][13]*SH_TAS[2] + P[5][13]*SH_TAS[1] - P[13][13]*SH_TAS[2] - P[14][13]*SH_TAS[1] + P[6][13]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[13][14]*SH_TAS[2] - P[14][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[13][6]*SH_TAS[2] - P[14][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
+        Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][13]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][14]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]);
+        Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][13]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][14]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]);
+        Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][13]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][14]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]);
+        Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][13]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][14]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]);
+        Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][13]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][14]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]);
+        Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][13]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][14]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]);
+        Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][13]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][14]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]);
+        Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][13]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][14]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]);
+        Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][13]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][14]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]);
+        Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][13]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][14]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]);
+        Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][13]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][14]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]);
+        Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][13]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][14]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]);
+        Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][13]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][14]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]);
+        Kfusion[13] = SK_TAS*(P[13][4]*SH_TAS[2] - P[13][13]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][14]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]);
+        Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][13]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][14]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]);
+        Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][13]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][14]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]);
+        Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][13]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][14]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]);
+        Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][13]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][14]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]);
+        Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][13]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][14]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]);
+        Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][13]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][14]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]);
+        Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][13]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][14]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
+        varInnovVtas = 1.0f/SK_TAS;
+
+        // Calculate the measurement innovation
+        innovVtas = VtasPred - VtasMeas;
+        // Check the innovation for consistency and don't fuse if > 5Sigma
+        if ((innovVtas*innovVtas*SK_TAS) < 25.0)
+        {
+            // correct the state vector
+            for (uint8_t j=0; j<=20; j++)
+            {
+                states[j] = states[j] - Kfusion[j] * innovVtas;
+            }
+            // normalise the quaternion states
+            float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+            if (quatMag > 1e-12f)
+            {
+                for (uint8_t j= 0; j<=3; j++)
+                {
+                    float quatMagInv = 1.0f/quatMag;
+                    states[j] = states[j] * quatMagInv;
+                }
+            }
+            // correct the covariance P = (I - K*H)*P
+            // take advantage of the empty columns in H to reduce the
+            // number of operations
+            for (uint8_t i = 0; i<=20; i++)
+            {
+                for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0;
+                for (uint8_t j = 4; j<=6; j++)
+                {
+                    KH[i][j] = Kfusion[i] * H_TAS[j];
+                }
+                for (uint8_t j = 7; j<=12; j++) KH[i][j] = 0.0;
+                for (uint8_t j = 13; j<=14; j++)
+                {
+                    KH[i][j] = Kfusion[i] * H_TAS[j];
+                }
+                for (uint8_t j = 15; j<=20; j++) KH[i][j] = 0.0;
+            }
+            for (uint8_t i = 0; i<=20; i++)
+            {
+                for (uint8_t j = 0; j<=20; j++)
+                {
+                    KHP[i][j] = 0.0;
+                    for (uint8_t k = 4; k<=6; k++)
+                    {
+                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                    }
+                    for (uint8_t k = 13; k<=14; k++)
+                    {
+                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                    }
+                }
+            }
+            for (uint8_t i = 0; i<=20; i++)
+            {
+                for (uint8_t j = 0; j<=20; j++)
+                {
+                    P[i][j] = P[i][j] - KHP[i][j];
+                }
+            }
+        }
+    }
+
+    ForceSymmetry();
+    ConstrainVariances();
+}
+
+void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
+{
+    uint8_t row;
+    uint8_t col;
+    for (row=first; row<=last; row++)
+    {
+        for (col=0; col<n_states; col++)
+        {
+            covMat[row][col] = 0.0;
+        }
+    }
+}
+
+void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
+{
+    uint8_t row;
+    uint8_t col;
+    for (col=first; col<=last; col++)
+    {
+        for (row=0; row < n_states; row++)
+        {
+            covMat[row][col] = 0.0;
+        }
+    }
+}
+
+float AttPosEKF::sq(float valIn)
+{
+    return valIn*valIn;
+}
+
+// Store states in a history array along with time stamp
+void AttPosEKF::StoreStates(uint64_t timestamp_ms)
+{
+    for (unsigned i=0; i<n_states; i++)
+        storedStates[i][storeIndex] = states[i];
+    statetimeStamp[storeIndex] = timestamp_ms;
+    storeIndex++;
+    if (storeIndex == data_buffer_size)
+        storeIndex = 0;
+}
+
+void AttPosEKF::ResetStoredStates()
+{
+    // reset all stored states
+    memset(&storedStates[0][0], 0, sizeof(storedStates));
+    memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
+
+    // reset store index to first
+    storeIndex = 0;
+
+    // overwrite all existing states
+    for (unsigned i = 0; i < n_states; i++) {
+        storedStates[i][storeIndex] = states[i];
+    }
+
+    statetimeStamp[storeIndex] = millis();
+
+    // increment to next storage index
+    storeIndex++;
+}
+
+// Output the state vector stored at the time that best matches that specified by msec
+int AttPosEKF::RecallStates(float statesForFusion[n_states], uint64_t msec)
+{
+    int ret = 0;
+
+    // int64_t bestTimeDelta = 200;
+    // unsigned bestStoreIndex = 0;
+    // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
+    // {
+    //     // The time delta can also end up as negative number,
+    //     // since we might compare future to past or past to future
+    //     // therefore cast to int64.
+    //     int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex];
+    //     if (timeDelta < 0) {
+    //         timeDelta = -timeDelta;
+    //     }
+
+    //     if (timeDelta < bestTimeDelta)
+    //     {
+    //         bestStoreIndex = storeIndex;
+    //         bestTimeDelta = timeDelta;
+    //     }
+    // }
+    // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
+    // {
+    //     for (uint8_t i=0; i < n_states; i++) {
+    //         if (isfinite(storedStates[i][bestStoreIndex])) {
+    //             statesForFusion[i] = storedStates[i][bestStoreIndex];
+    //         } else if (isfinite(states[i])) {
+    //             statesForFusion[i] = states[i];
+    //         } else {
+    //             // There is not much we can do here, except reporting the error we just
+    //             // found.
+    //             ret++;
+    //         }
+    // }
+    // else // otherwise output current state
+    // {
+        for (uint8_t i=0; i < n_states; i++) {
+            if (isfinite(states[i])) {
+                statesForFusion[i] = states[i];
+            } else {
+                ret++;
+            }
+        }
+    // }
+
+    return ret;
+}
+
+void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
+{
+    // Calculate the nav to body cosine matrix
+    float q00 = sq(quat[0]);
+    float q11 = sq(quat[1]);
+    float q22 = sq(quat[2]);
+    float q33 = sq(quat[3]);
+    float q01 =  quat[0]*quat[1];
+    float q02 =  quat[0]*quat[2];
+    float q03 =  quat[0]*quat[3];
+    float q12 =  quat[1]*quat[2];
+    float q13 =  quat[1]*quat[3];
+    float q23 =  quat[2]*quat[3];
+
+    Tnb.x.x = q00 + q11 - q22 - q33;
+    Tnb.y.y = q00 - q11 + q22 - q33;
+    Tnb.z.z = q00 - q11 - q22 + q33;
+    Tnb.y.x = 2*(q12 - q03);
+    Tnb.z.x = 2*(q13 + q02);
+    Tnb.x.y = 2*(q12 + q03);
+    Tnb.z.y = 2*(q23 - q01);
+    Tnb.x.z = 2*(q13 - q02);
+    Tnb.y.z = 2*(q23 + q01);
+}
+
+void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
+{
+    // Calculate the body to nav cosine matrix
+    float q00 = sq(quat[0]);
+    float q11 = sq(quat[1]);
+    float q22 = sq(quat[2]);
+    float q33 = sq(quat[3]);
+    float q01 =  quat[0]*quat[1];
+    float q02 =  quat[0]*quat[2];
+    float q03 =  quat[0]*quat[3];
+    float q12 =  quat[1]*quat[2];
+    float q13 =  quat[1]*quat[3];
+    float q23 =  quat[2]*quat[3];
+
+    Tbn.x.x = q00 + q11 - q22 - q33;
+    Tbn.y.y = q00 - q11 + q22 - q33;
+    Tbn.z.z = q00 - q11 - q22 + q33;
+    Tbn.x.y = 2*(q12 - q03);
+    Tbn.x.z = 2*(q13 + q02);
+    Tbn.y.x = 2*(q12 + q03);
+    Tbn.y.z = 2*(q23 - q01);
+    Tbn.z.x = 2*(q13 - q02);
+    Tbn.z.y = 2*(q23 + q01);
+}
+
+void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
+{
+    float u1 = cos(0.5f*eul[0]);
+    float u2 = cos(0.5f*eul[1]);
+    float u3 = cos(0.5f*eul[2]);
+    float u4 = sin(0.5f*eul[0]);
+    float u5 = sin(0.5f*eul[1]);
+    float u6 = sin(0.5f*eul[2]);
+    quat[0] = u1*u2*u3+u4*u5*u6;
+    quat[1] = u4*u2*u3-u1*u5*u6;
+    quat[2] = u1*u5*u3+u4*u2*u6;
+    quat[3] = u1*u2*u6-u4*u5*u3;
+}
+
+void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4])
+{
+    y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
+    y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2]));
+    y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
+}
+
+void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
+{
+    velNED[0] = gpsGndSpd*cosf(gpsCourse);
+    velNED[1] = gpsGndSpd*sinf(gpsCourse);
+    velNED[2] = gpsVelD;
+}
+
+void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
+{
+    posNED[0] = earthRadius * (lat - latRef);
+    posNED[1] = earthRadius * cos(latRef) * (lon - lonRef);
+    posNED[2] = -(hgt - hgtRef);
+}
+
+void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
+{
+    lat = latRef + posNED[0] * earthRadiusInv;
+    lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
+    hgt = hgtRef - posNED[2];
+}
+
+void AttPosEKF::OnGroundCheck()
+{
+    onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
+    if (staticMode) {
+        staticMode = !(GPSstatus > GPS_FIX_2D);
+    }
+}
+
+void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude)
+{
+    //Define Earth rotation vector in the NED navigation frame
+    omega.x  = earthRate*cosf(latitude);
+    omega.y  = 0.0f;
+    omega.z  = -earthRate*sinf(latitude);
+}
+
+void AttPosEKF::CovarianceInit()
+{
+    // Calculate the initial covariance matrix P
+    P[0][0]   = 0.25f * sq(1.0f*deg2rad);
+    P[1][1]   = 0.25f * sq(1.0f*deg2rad);
+    P[2][2]   = 0.25f * sq(1.0f*deg2rad);
+    P[3][3]   = 0.25f * sq(10.0f*deg2rad);
+    P[4][4]   = sq(0.7);
+    P[5][5]   = P[4][4];
+    P[6][6]   = sq(0.7);
+    P[7][7]   = sq(15.0);
+    P[8][8]   = P[7][7];
+    P[9][9]   = sq(5.0);
+    P[10][10] = sq(0.1*deg2rad*dtIMU);
+    P[11][11] = P[10][10];
+    P[12][12] = P[10][10];
+    P[13][13] = sq(8.0f);
+    P[14][4]  = P[13][13];
+    P[15][15] = sq(0.02f);
+    P[16][16] = P[15][15];
+    P[17][17] = P[15][15];
+    P[18][18] = sq(0.02f);
+    P[19][19] = P[18][18];
+    P[20][20] = P[18][18];
+}
+
+float AttPosEKF::ConstrainFloat(float val, float min, float max)
+{
+    return (val > max) ? max : ((val < min) ? min : val);
+}
+
+void AttPosEKF::ConstrainVariances()
+{
+    if (!numericalProtection) {
+        return;
+    }
+
+    // State vector:
+    // 0-3: quaternions (q0, q1, q2, q3)
+    // 4-6: Velocity - m/sec (North, East, Down)
+    // 7-9: Position - m (North, East, Down)
+    // 10-12: Delta Angle bias - rad (X,Y,Z)
+    // 13-14: Wind Vector  - m/sec (North,East)
+    // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
+    // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
+
+    // Constrain quaternion variances
+    for (unsigned i = 0; i < 4; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+    }
+
+    // Constrain velocitie variances
+    for (unsigned i = 4; i < 7; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
+    }
+
+    // Constrain position variances
+    for (unsigned i = 7; i < 10; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f);
+    }
+
+    // Angle bias variances
+    for (unsigned i = 10; i < 13; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.175f * dtIMU));
+    }
+
+    // Wind velocity variances
+    for (unsigned i = 13; i < 15; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
+    }
+
+    // Earth magnetic field variances
+    for (unsigned i = 15; i < 18; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+    }
+
+    // Body magnetic field variances
+    for (unsigned i = 18; i < 21; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+    }
+
+}
+
+void AttPosEKF::ConstrainStates()
+{
+    if (!numericalProtection) {
+        return;
+    }
+
+    // State vector:
+    // 0-3: quaternions (q0, q1, q2, q3)
+    // 4-6: Velocity - m/sec (North, East, Down)
+    // 7-9: Position - m (North, East, Down)
+    // 10-12: Delta Angle bias - rad (X,Y,Z)
+    // 13-14: Wind Vector  - m/sec (North,East)
+    // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
+    // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
+
+
+    // Constrain quaternion
+    for (unsigned i = 0; i < 4; i++) {
+        states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
+    }
+
+    // Constrain velocities to what GPS can do for us
+    for (unsigned i = 4; i < 7; i++) {
+        states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f);
+    }
+
+    // Constrain position to a reasonable vehicle range (in meters)
+    for (unsigned i = 7; i < 9; i++) {
+        states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f);
+    }
+
+    // Constrain altitude
+    states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f);
+
+    // Angle bias limit - set to 8 degrees / sec
+    for (unsigned i = 10; i < 13; i++) {
+        states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU);
+    }
+
+    // Wind velocity limits - assume 120 m/s max velocity
+    for (unsigned i = 13; i < 15; i++) {
+        states[i] = ConstrainFloat(states[i], -120.0f, 120.0f);
+    }
+
+    // Earth magnetic field limits (in Gauss)
+    for (unsigned i = 15; i < 18; i++) {
+        states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
+    }
+
+    // Body magnetic field variances (in Gauss).
+    // the max offset should be in this range.
+    for (unsigned i = 18; i < 21; i++) {
+        states[i] = ConstrainFloat(states[i], -0.5f, 0.5f);
+    }
+
+}
+
+void AttPosEKF::ForceSymmetry()
+{
+    if (!numericalProtection) {
+        return;
+    }
+
+    // Force symmetry on the covariance matrix to prevent ill-conditioning
+    // of the matrix which would cause the filter to blow-up
+    for (unsigned i = 1; i < n_states; i++)
+    {
+        for (uint8_t j = 0; j < i; j++)
+        {
+            P[i][j] = 0.5f * (P[i][j] + P[j][i]);
+            P[j][i] = P[i][j];
+        }
+    }
+}
+
+bool AttPosEKF::FilterHealthy()
+{
+    if (!statesInitialised) {
+        return false;
+    }
+
+    // XXX Check state vector for NaNs and ill-conditioning
+
+    // Check if any of the major inputs timed out
+    if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) {
+        return false;
+    }
+
+    // Nothing fired, return ok.
+    return true;
+}
+
+/**
+ * Reset the filter position states
+ *
+ * This resets the position to the last GPS measurement
+ * or to zero in case of static position.
+ */
+void AttPosEKF::ResetPosition(void)
+{
+    if (staticMode) {
+        states[7] = 0;
+        states[8] = 0;
+    } else if (GPSstatus >= GPS_FIX_3D) {
+
+        // reset the states from the GPS measurements
+        states[7] = posNE[0];
+        states[8] = posNE[1];
+    }
+}
+
+/**
+ * Reset the height state.
+ *
+ * This resets the height state with the last altitude measurements
+ */
+void AttPosEKF::ResetHeight(void)
+{
+    // write to the state vector
+    states[9]   = -hgtMea;
+}
+
+/**
+ * Reset the velocity state.
+ */
+void AttPosEKF::ResetVelocity(void)
+{
+    if (staticMode) {
+        states[4] = 0.0f;
+        states[5] = 0.0f;
+        states[6] = 0.0f;
+    } else if (GPSstatus >= GPS_FIX_3D) {
+
+        states[4]  = velNED[0]; // north velocity from last reading
+        states[5]  = velNED[1]; // east velocity from last reading
+        states[6]  = velNED[2]; // down velocity from last reading
+    }
+}
+
+
+void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
+{
+    for (int i = 0; i < n_states; i++)
+    {
+        err->states[i] = states[i];
+    }
+
+    err->velHealth = current_ekf_state.velHealth;
+    err->posHealth = current_ekf_state.posHealth;
+    err->hgtHealth = current_ekf_state.hgtHealth;
+    err->velTimeout = current_ekf_state.velTimeout;
+    err->posTimeout = current_ekf_state.posTimeout;
+    err->hgtTimeout = current_ekf_state.hgtTimeout;
+}
+
+bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
+    bool err = false;
+
+    // check all states and covariance matrices
+    for (unsigned i = 0; i < n_states; i++) {
+        for (unsigned j = 0; j < n_states; j++) {
+            if (!isfinite(KH[i][j])) {
+
+                err_report->covarianceNaN = true;
+                err = true;
+            } //  intermediate result used for covariance updates
+            if (!isfinite(KHP[i][j])) {
+
+                err_report->covarianceNaN = true;
+                err = true;
+            } // intermediate result used for covariance updates
+            if (!isfinite(P[i][j])) {
+
+                err_report->covarianceNaN = true;
+                err = true;
+            } // covariance matrix
+        }
+
+        if (!isfinite(Kfusion[i])) {
+
+            err_report->kalmanGainsNaN = true;
+            err = true;
+        } // Kalman gains
+
+        if (!isfinite(states[i])) {
+
+            err_report->statesNaN = true;
+            err = true;
+        } // state matrix
+    }
+
+    if (err) {
+        FillErrorReport(err_report);
+    }
+
+    return err;
+
+}
+
+/**
+ * Check the filter inputs and bound its operational state
+ *
+ * This check will reset the filter states if required
+ * due to a failure of consistency or timeout checks.
+ * it should be run after the measurement data has been
+ * updated, but before any of the fusion steps are
+ * executed.
+ */
+int AttPosEKF::CheckAndBound()
+{
+
+    // Store the old filter state
+    bool currStaticMode = staticMode;
+
+    // Reset the filter if the states went NaN
+    if (StatesNaN(&last_ekf_error)) {
+
+        InitializeDynamic(velNED, magDeclination);
+
+        return 1;
+    }
+
+    // Reset the filter if the IMU data is too old
+    if (dtIMU > 0.2f) {
+
+        ResetVelocity();
+        ResetPosition();
+        ResetHeight();
+        ResetStoredStates();
+
+        // that's all we can do here, return
+        return 2;
+    }
+
+    // Check if we're on ground - this also sets static mode.
+    OnGroundCheck();
+
+    // Check if we switched between states
+    if (currStaticMode != staticMode) {
+        ResetVelocity();
+        ResetPosition();
+        ResetHeight();
+        ResetStoredStates();
+
+        return 3;
+    }
+
+    return 0;
+}
+
+void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat)
+{
+    float initialRoll, initialPitch;
+    float cosRoll, sinRoll, cosPitch, sinPitch;
+    float magX, magY;
+    float initialHdg, cosHeading, sinHeading;
+
+    initialRoll = atan2(-ay, -az);
+    initialPitch = atan2(ax, -az);
+
+    cosRoll = cosf(initialRoll);
+    sinRoll = sinf(initialRoll);
+    cosPitch = cosf(initialPitch);
+    sinPitch = sinf(initialPitch);
+
+    magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
+
+    magY = my * cosRoll - mz * sinRoll;
+
+    initialHdg = atan2f(-magY, magX);
+    /* true heading is the mag heading minus declination */
+    initialHdg += declination;
+
+    cosRoll = cosf(initialRoll * 0.5f);
+    sinRoll = sinf(initialRoll * 0.5f);
+
+    cosPitch = cosf(initialPitch * 0.5f);
+    sinPitch = sinf(initialPitch * 0.5f);
+
+    cosHeading = cosf(initialHdg * 0.5f);
+    sinHeading = sinf(initialHdg * 0.5f);
+
+    initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
+    initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
+    initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
+    initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
+}
+
+void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
+{
+
+    // Clear the init flag
+    statesInitialised = false;
+
+    magDeclination = declination;
+
+    ZeroVariables();
+
+    // Calculate initial filter quaternion states from raw measurements
+    float initQuat[4];
+    Vector3f initMagXYZ;
+    initMagXYZ   = magData - magBias;
+    AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, magDeclination, initQuat);
+
+    // Calculate initial Tbn matrix and rotate Mag measurements into NED
+    // to set initial NED magnetic field states
+    Mat3f DCM;
+    quat2Tbn(DCM, initQuat);
+    Vector3f initMagNED;
+    initMagXYZ   = magData - magBias;
+    initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z;
+    initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
+    initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
+
+
+
+    // write to state vector
+    for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
+    for (uint8_t j=0; j<=2; j++) states[j+4] = initvelNED[j]; // velocities
+    for (uint8_t j=0; j<=7; j++) states[j+7] = 0.0f; // positiions, dAngBias, windVel
+    states[15] = initMagNED.x; // Magnetic Field North
+    states[16] = initMagNED.y; // Magnetic Field East
+    states[17] = initMagNED.z; // Magnetic Field Down
+    states[18] = magBias.x; // Magnetic Field Bias X
+    states[19] = magBias.y; // Magnetic Field Bias Y
+    states[20] = magBias.z; // Magnetic Field Bias Z
+
+    statesInitialised = true;
+
+    // initialise the covariance matrix
+    CovarianceInit();
+
+    //Define Earth rotation vector in the NED navigation frame
+    calcEarthRateNED(earthRateNED, latRef);
+
+    //Initialise summed variables used by covariance prediction
+    summedDelAng.x = 0.0f;
+    summedDelAng.y = 0.0f;
+    summedDelAng.z = 0.0f;
+    summedDelVel.x = 0.0f;
+    summedDelVel.y = 0.0f;
+    summedDelVel.z = 0.0f;
+}
+
+void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination)
+{
+    //store initial lat,long and height
+    latRef = referenceLat;
+    lonRef = referenceLon;
+    hgtRef = referenceHgt;
+
+    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
+
+    InitializeDynamic(initvelNED, declination);
+}
+
+void AttPosEKF::ZeroVariables()
+{
+    // Do the data structure init
+    for (unsigned i = 0; i < n_states; i++) {
+        for (unsigned j = 0; j < n_states; j++) {
+            KH[i][j] = 0.0f; //  intermediate result used for covariance updates
+            KHP[i][j] = 0.0f; // intermediate result used for covariance updates
+            P[i][j] = 0.0f; // covariance matrix
+        }
+
+        Kfusion[i] = 0.0f; // Kalman gains
+        states[i] = 0.0f; // state matrix
+    }
+
+    for (unsigned i = 0; i < data_buffer_size; i++) {
+
+        for (unsigned j = 0; j < n_states; j++) {
+            storedStates[j][i] = 0.0f;
+        }
+
+        statetimeStamp[i] = 0;
+    }
+
+    memset(&current_ekf_state, 0, sizeof(current_ekf_state));
+}
+
+void AttPosEKF::GetFilterState(struct ekf_status_report *state)
+{
+    memcpy(state, &current_ekf_state, sizeof(state));
+}
+
+void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
+{
+    memcpy(last_error, &last_ekf_error, sizeof(last_error));
+}
diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.h b/src/modules/ekf_att_pos_estimator/estimator_21states.h
new file mode 100644
index 000000000..a19ff1995
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/estimator_21states.h
@@ -0,0 +1,247 @@
+#pragma once
+
+#include "estimator_utilities.h"
+
+class AttPosEKF {
+
+public:
+
+    AttPosEKF();
+    ~AttPosEKF();
+
+    /* ##############################################
+     *
+     *   M A I N    F I L T E R    P A R A M E T E R S
+     *
+     * ########################################### */
+
+    /*
+     * parameters are defined here and initialised in
+     * the InitialiseParameters() (which is just 20 lines down)
+     */
+
+    float covTimeStepMax; // maximum time allowed between covariance predictions
+    float covDelAngMax; // maximum delta angle between covariance predictions
+    float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+
+    float yawVarScale;
+    float windVelSigma;
+    float dAngBiasSigma;
+    float dVelBiasSigma;
+    float magEarthSigma;
+    float magBodySigma;
+    float gndHgtSigma;
+
+    float vneSigma;
+    float vdSigma;
+    float posNeSigma;
+    float posDSigma;
+    float magMeasurementSigma;
+    float airspeedMeasurementSigma;
+
+    float gyroProcessNoise;
+    float accelProcessNoise;
+
+    float EAS2TAS; // ratio f true to equivalent airspeed
+
+    void InitialiseParameters()
+    {
+        covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
+        covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
+        rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+        EAS2TAS = 1.0f;
+
+        yawVarScale = 1.0f;
+        windVelSigma = 0.1f;
+        dAngBiasSigma = 5.0e-7f;
+        dVelBiasSigma = 1e-4f;
+        magEarthSigma = 3.0e-4f;
+        magBodySigma  = 3.0e-4f;
+        gndHgtSigma  = 0.02f; // assume 2% terrain gradient 1-sigma
+
+        vneSigma = 0.2f;
+        vdSigma = 0.3f;
+        posNeSigma = 2.0f;
+        posDSigma = 2.0f;
+
+        magMeasurementSigma = 0.05;
+        airspeedMeasurementSigma = 1.4f;
+        gyroProcessNoise = 1.4544411e-2f;
+        accelProcessNoise = 0.5f;
+    }
+
+    // Global variables
+    float KH[n_states][n_states]; //  intermediate result used for covariance updates
+    float KHP[n_states][n_states]; // intermediate result used for covariance updates
+    float P[n_states][n_states]; // covariance matrix
+    float Kfusion[n_states]; // Kalman gains
+    float states[n_states]; // state matrix
+    float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
+    uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
+
+    float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
+    float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
+    float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
+    float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
+    float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
+
+    Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
+    Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
+    Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
+    Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
+    float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
+    Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
+    Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
+    Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
+    Vector3f dVelIMU;
+    Vector3f dAngIMU;
+    float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
+    uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
+    float innovVelPos[6]; // innovation output
+    float varInnovVelPos[6]; // innovation variance output
+
+    float velNED[3]; // North, East, Down velocity obs (m/s)
+    float posNE[2]; // North, East position obs (m)
+    float hgtMea; //  measured height (m)
+    float posNED[3]; // North, East Down position (m)
+
+    float innovMag[3]; // innovation output
+    float varInnovMag[3]; // innovation variance output
+    Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
+    float innovVtas; // innovation output
+    float varInnovVtas; // innovation variance output
+    float VtasMeas; // true airspeed measurement (m/s)
+    float magDeclination;
+    float latRef; // WGS-84 latitude of reference point (rad)
+    float lonRef; // WGS-84 longitude of reference point (rad)
+    float hgtRef; // WGS-84 height of reference point (m)
+    Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
+    uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
+
+    // GPS input data variables
+    float gpsCourse;
+    float gpsVelD;
+    float gpsLat;
+    float gpsLon;
+    float gpsHgt;
+    uint8_t GPSstatus;
+
+    // Baro input
+    float baroHgt;
+
+    bool statesInitialised;
+
+    bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
+    bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
+    bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
+    bool fuseMagData; // boolean true when magnetometer data is to be fused
+    bool fuseVtasData; // boolean true when airspeed data is to be fused
+
+    bool onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
+    bool staticMode;    ///< boolean true if no position feedback is fused
+    bool useAirspeed;    ///< boolean true if airspeed data is being used
+    bool useCompass;    ///< boolean true if magnetometer data is being used
+
+    struct ekf_status_report current_ekf_state;
+    struct ekf_status_report last_ekf_error;
+
+    bool numericalProtection;
+
+    unsigned storeIndex;
+
+
+void  UpdateStrapdownEquationsNED();
+
+void CovariancePrediction(float dt);
+
+void FuseVelposNED();
+
+void FuseMagnetometer();
+
+void FuseAirspeed();
+
+void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
+
+void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
+
+void quatNorm(float (&quatOut)[4], const float quatIn[4]);
+
+// store staes along with system time stamp in msces
+void StoreStates(uint64_t timestamp_ms);
+
+/**
+ * Recall the state vector.
+ *
+ * Recalls the vector stored at closest time to the one specified by msec
+ *
+ * @return zero on success, integer indicating the number of invalid states on failure.
+ *         Does only copy valid states, if the statesForFusion vector was initialized
+ *         correctly by the caller, the result can be safely used, but is a mixture
+ *         time-wise where valid states were updated and invalid remained at the old
+ *         value.
+ */
+int RecallStates(float statesForFusion[n_states], uint64_t msec);
+
+void ResetStoredStates();
+
+void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
+
+void calcEarthRateNED(Vector3f &omega, float latitude);
+
+static void eul2quat(float (&quat)[4], const float (&eul)[3]);
+
+static void quat2eul(float (&eul)[3], const float (&quat)[4]);
+
+static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
+
+static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
+
+static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
+
+static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
+
+static float sq(float valIn);
+
+void OnGroundCheck();
+
+void CovarianceInit();
+
+void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
+
+float ConstrainFloat(float val, float min, float max);
+
+void ConstrainVariances();
+
+void ConstrainStates();
+
+void ForceSymmetry();
+
+int CheckAndBound();
+
+void ResetPosition();
+
+void ResetVelocity();
+
+void ZeroVariables();
+
+void GetFilterState(struct ekf_status_report *state);
+
+void GetLastErrorState(struct ekf_status_report *last_error);
+
+bool StatesNaN(struct ekf_status_report *err_report);
+void FillErrorReport(struct ekf_status_report *err);
+
+void InitializeDynamic(float (&initvelNED)[3], float declination);
+
+protected:
+
+bool FilterHealthy();
+
+void ResetHeight(void);
+
+void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
+
+};
+
+uint32_t millis();
+
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
index 5db1adbb3..9622f7e40 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -1,143 +1,9 @@
-#include "estimator.h"
+#include "estimator_23states.h"
 #include <string.h>
-#include <stdarg.h>
-
-// Define EKF_DEBUG here to enable the debug print calls
-// if the macro is not set, these will be completely
-// optimized out by the compiler.
-//#define EKF_DEBUG
-
-#ifdef EKF_DEBUG
 #include <stdio.h>
+#include <stdarg.h>
 
-static void
-ekf_debug_print(const char *fmt, va_list args)
-{
-    fprintf(stderr, "%s: ", "[ekf]");
-    vfprintf(stderr, fmt, args);
-
-    fprintf(stderr, "\n");
-}
-
-static void
-ekf_debug(const char *fmt, ...)
-{
-    va_list args;
-
-    va_start(args, fmt);
-    ekf_debug_print(fmt, args);
-}
-
-#else
-
-static void ekf_debug(const char *fmt, ...) { while(0){} }
-#endif
-
-float Vector3f::length(void) const
-{
-    return sqrt(x*x + y*y + z*z);
-}
-
-void Vector3f::zero(void)
-{
-    x = 0.0f;
-    y = 0.0f;
-    z = 0.0f;
-}
-
-Mat3f::Mat3f() {
-    identity();
-}
-
-void Mat3f::identity() {
-    x.x = 1.0f;
-    x.y = 0.0f;
-    x.z = 0.0f;
-
-    y.x = 0.0f;
-    y.y = 1.0f;
-    y.z = 0.0f;
-
-    z.x = 0.0f;
-    z.y = 0.0f;
-    z.z = 1.0f;
-}
-
-Mat3f Mat3f::transpose(void) const
-{
-    Mat3f ret = *this;
-    swap_var(ret.x.y, ret.y.x);
-    swap_var(ret.x.z, ret.z.x);
-    swap_var(ret.y.z, ret.z.y);
-    return ret;
-}
-
-// overload + operator to provide a vector addition
-Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
-{
-    Vector3f vecOut;
-    vecOut.x = vecIn1.x + vecIn2.x;
-    vecOut.y = vecIn1.y + vecIn2.y;
-    vecOut.z = vecIn1.z + vecIn2.z;
-    return vecOut;
-}
-
-// overload - operator to provide a vector subtraction
-Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
-{
-    Vector3f vecOut;
-    vecOut.x = vecIn1.x - vecIn2.x;
-    vecOut.y = vecIn1.y - vecIn2.y;
-    vecOut.z = vecIn1.z - vecIn2.z;
-    return vecOut;
-}
-
-// overload * operator to provide a matrix vector product
-Vector3f operator*( Mat3f matIn, Vector3f vecIn)
-{
-    Vector3f vecOut;
-    vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
-    vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
-    vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
-    return vecOut;
-}
-
-// overload % operator to provide a vector cross product
-Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
-{
-    Vector3f vecOut;
-    vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
-    vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
-    vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
-    return vecOut;
-}
-
-// overload * operator to provide a vector scaler product
-Vector3f operator*(Vector3f vecIn1, float sclIn1)
-{
-    Vector3f vecOut;
-    vecOut.x = vecIn1.x * sclIn1;
-    vecOut.y = vecIn1.y * sclIn1;
-    vecOut.z = vecIn1.z * sclIn1;
-    return vecOut;
-}
-
-// overload * operator to provide a vector scaler product
-Vector3f operator*(float sclIn1, Vector3f vecIn1)
-{
-    Vector3f vecOut;
-    vecOut.x = vecIn1.x * sclIn1;
-    vecOut.y = vecIn1.y * sclIn1;
-    vecOut.z = vecIn1.z * sclIn1;
-    return vecOut;
-}
-
-void swap_var(float &d1, float &d2)
-{
-    float tmp = d1;
-    d1 = d2;
-    d2 = tmp;
-}
+#define EKF_COVARIANCE_DIVERGED 1.0e8f
 
 AttPosEKF::AttPosEKF()
 
@@ -145,7 +11,42 @@ AttPosEKF::AttPosEKF()
      * instead to allow clean in-air re-initialization.
      */
 {
+    summedDelAng.zero();
+    summedDelVel.zero();
+
+    fusionModeGPS = 0;
+    fuseVelData = false;
+    fusePosData = false;
+    fuseHgtData = false;
+    fuseMagData = false;
+    fuseVtasData = false;
+    onGround = true;
+    staticMode = true;
+    useAirspeed = true;
+    useCompass = true;
+    useRangeFinder = true;
+    numericalProtection = true;
+    refSet = false;
+    storeIndex = 0;
+    gpsHgt = 0.0f;
+    baroHgt = 0.0f;
+    GPSstatus = 0;
+    VtasMeas = 0.0f;
+    magDeclination = 0.0f;
+    dAngIMU.zero();
+    dVelIMU.zero();
+    velNED[0] = 0.0f;
+    velNED[1] = 0.0f;
+    velNED[2] = 0.0f;
+    accelGPSNED[0] = 0.0f;
+    accelGPSNED[1] = 0.0f;
+    accelGPSNED[2] = 0.0f;
+    delAngTotal.zero();
+    ekfDiverged = false;
+    lastReset = 0;
 
+    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
+    memset(&current_ekf_state, 0, sizeof(current_ekf_state));
     ZeroVariables();
     InitialiseParameters();
 }
@@ -181,6 +82,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
     dVelIMU.y = dVelIMU.y;
     dVelIMU.z = dVelIMU.z - states[13];
 
+    delAngTotal.x += correctedDelAng.x;
+    delAngTotal.y += correctedDelAng.y;
+    delAngTotal.z += correctedDelAng.z;
+
 // Save current measurements
     Vector3f  prevDelAng = correctedDelAng;
 
@@ -199,8 +104,12 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
     }
     else
     {
-        deltaQuat[0] = cosf(0.5f*rotationMag);
-        float rotScaler = (sinf(0.5f*rotationMag))/rotationMag;
+        // We are using double here as we are unsure how small
+        // the angle differences are and if we get into numeric
+        // issues with float. The runtime impact is not measurable
+        // for these quantities.
+        deltaQuat[0] = cos(0.5*(double)rotationMag);
+        float rotScaler = (sin(0.5*(double)rotationMag))/(double)rotationMag;
         deltaQuat[1] = correctedDelAng.x*rotScaler;
         deltaQuat[2] = correctedDelAng.y*rotScaler;
         deltaQuat[3] = correctedDelAng.z*rotScaler;
@@ -312,7 +221,8 @@ void AttPosEKF::CovariancePrediction(float dt)
     float nextP[n_states][n_states];
 
     // calculate covariance prediction process noise
-    for (uint8_t i= 0; i<=9;  i++) processNoise[i] = 1.0e-9f;
+    for (uint8_t i= 0; i<4;  i++) processNoise[i] = 1.0e-9f;
+    for (uint8_t i= 4; i<10;  i++) processNoise[i] = 1.0e-9f;
     for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
     // scale gyro bias noise when on ground to allow for faster bias estimation
     for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
@@ -977,20 +887,20 @@ void AttPosEKF::CovariancePrediction(float dt)
         // propagate
         for (unsigned i = 0; i <= 13; i++) {
             P[i][i] = nextP[i][i];
+        }
 
-            // force symmetry for observable states
-            // force zero for non-observable states
-            for (unsigned i = 1; i < n_states; i++)
+        // force symmetry for observable states
+        // force zero for non-observable states
+        for (unsigned i = 1; i < n_states; i++)
+        {
+            for (uint8_t j = 0; j < i; j++)
             {
-                for (uint8_t j = 0; j < i; j++)
-                {
-                    if ((i > 13) || (j > 13)) {
-                        P[i][j] = 0.0f;
-                    } else {
-                        P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
-                    }
-                    P[j][i] = P[i][j];
+                if ((i > 13) || (j > 13)) {
+                    P[i][j] = 0.0f;
+                } else {
+                    P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
                 }
+                P[j][i] = P[i][j];
             }
         }
 
@@ -1020,9 +930,9 @@ void AttPosEKF::FuseVelposNED()
 {
 
 // declare variables used by fault isolation logic
-    uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure
-    uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available
-    uint32_t hgtRetryTime = 5000; // height measurement retry time
+    uint32_t gpsRetryTime = 3000; // time in msec before GPS fusion will be retried following innovation consistency failure
+    uint32_t gpsRetryTimeNoTAS = 500; // retry time if no TAS measurement available
+    uint32_t hgtRetryTime = 500; // height measurement retry time
     uint32_t horizRetryTime;
 
 // declare variables used to check measurement errors
@@ -1178,7 +1088,7 @@ void AttPosEKF::FuseVelposNED()
                 stateIndex = 4 + obsIndex;
                 // Calculate the measurement innovation, using states from a
                 // different time coordinate if fusing height data
-                if (obsIndex >= 0 && obsIndex <= 2)
+                if (obsIndex <= 2)
                 {
                     innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex];
                 }
@@ -1193,7 +1103,7 @@ void AttPosEKF::FuseVelposNED()
                 // Calculate the Kalman Gain
                 // Calculate innovation variances - also used for data logging
                 varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
-                SK = 1.0/varInnovVelPos[obsIndex];
+                SK = 1.0/(double)varInnovVelPos[obsIndex];
                 for (uint8_t i= 0; i<=indexLimit; i++)
                 {
                     Kfusion[i] = P[i][stateIndex]*SK;
@@ -1277,7 +1187,7 @@ void AttPosEKF::FuseMagnetometer()
 // data fit is the only assumption we can make
 // so we might as well take advantage of the computational efficiencies
 // associated with sequential fusion
-    if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2))
+    if (useCompass && fuseMagData && (obsIndex < 3))
     {
         // Limit range of states modified when on ground
         if(!onGround)
@@ -1293,7 +1203,7 @@ void AttPosEKF::FuseMagnetometer()
         // three prediction time steps.
 
         // Calculate observation jacobians and Kalman gains
-        if (fuseMagData)
+        if (obsIndex == 0)
         {
             // Copy required states to local variable names
             q0       = statesAtMagMeasTime[0];
@@ -1388,11 +1298,6 @@ void AttPosEKF::FuseMagnetometer()
             Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]);
             varInnovMag[0] = 1.0f/SK_MX[0];
             innovMag[0] = MagPred[0] - magData.x;
-
-            // reset the observation index to 0 (we start by fusing the X
-            // measurement)
-            obsIndex = 0;
-            fuseMagData = false;
         }
         else if (obsIndex == 1) // we are now fusing the Y measurement
         {
@@ -1508,7 +1413,7 @@ void AttPosEKF::FuseMagnetometer()
         }
 
         // Check the innovation for consistency and don't fuse if > 5Sigma
-        if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0)
+        if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f)
         {
             // correct the state vector
             for (uint8_t j= 0; j < indexLimit; j++)
@@ -1517,7 +1422,7 @@ void AttPosEKF::FuseMagnetometer()
             }
             // normalise the quaternion states
             float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
-            if (quatMag > 1e-12)
+            if (quatMag > 1e-12f)
             {
                 for (uint8_t j= 0; j<=3; j++)
                 {
@@ -1612,7 +1517,7 @@ void AttPosEKF::FuseAirspeed()
         SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
         SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
         SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
-        
+
         float H_TAS[n_states];
         for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f;
         H_TAS[4] = SH_TAS[2];
@@ -1661,7 +1566,7 @@ void AttPosEKF::FuseAirspeed()
         // Calculate the measurement innovation
         innovVtas = VtasPred - VtasMeas;
         // Check the innovation for consistency and don't fuse if > 5Sigma
-        if ((innovVtas*innovVtas*SK_TAS) < 25.0)
+        if ((innovVtas*innovVtas*SK_TAS) < 25.0f)
         {
             // correct the state vector
             for (uint8_t j=0; j <= 22; j++)
@@ -1758,7 +1663,7 @@ void AttPosEKF::FuseRangeFinder()
 
     // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data
     SH_RNG[4] = sin(rngFinderPitch);
-    cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cos(rngFinderPitch);
+    cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch);
     if (useRangeFinder && cosRngTilt > 0.87f)
     {
         // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset
@@ -1855,21 +1760,21 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
 
     int64_t bestTimeDelta = 200;
     unsigned bestStoreIndex = 0;
-    for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
+    for (unsigned storeIndexLocal = 0; storeIndexLocal < data_buffer_size; storeIndexLocal++)
     {
         // Work around a GCC compiler bug - we know 64bit support on ARM is
         // sketchy in GCC.
         uint64_t timeDelta;
 
-        if (msec > statetimeStamp[storeIndex]) {
-            timeDelta = msec - statetimeStamp[storeIndex];
+        if (msec > statetimeStamp[storeIndexLocal]) {
+            timeDelta = msec - statetimeStamp[storeIndexLocal];
         } else {
-            timeDelta = statetimeStamp[storeIndex] - msec;
+            timeDelta = statetimeStamp[storeIndexLocal] - msec;
         }
 
-        if (timeDelta < bestTimeDelta)
+        if (timeDelta < (uint64_t)bestTimeDelta)
         {
-            bestStoreIndex = storeIndex;
+            bestStoreIndex = storeIndexLocal;
             bestTimeDelta = timeDelta;
         }
     }
@@ -1926,7 +1831,7 @@ void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
     Tnb.y.z = 2*(q23 + q01);
 }
 
-void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
+void AttPosEKF::quat2Tbn(Mat3f &Tbn_ret, const float (&quat)[4])
 {
     // Calculate the body to nav cosine matrix
     float q00 = sq(quat[0]);
@@ -1940,15 +1845,15 @@ void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
     float q13 =  quat[1]*quat[3];
     float q23 =  quat[2]*quat[3];
 
-    Tbn.x.x = q00 + q11 - q22 - q33;
-    Tbn.y.y = q00 - q11 + q22 - q33;
-    Tbn.z.z = q00 - q11 - q22 + q33;
-    Tbn.x.y = 2*(q12 - q03);
-    Tbn.x.z = 2*(q13 + q02);
-    Tbn.y.x = 2*(q12 + q03);
-    Tbn.y.z = 2*(q23 - q01);
-    Tbn.z.x = 2*(q13 - q02);
-    Tbn.z.y = 2*(q23 + q01);
+    Tbn_ret.x.x = q00 + q11 - q22 - q33;
+    Tbn_ret.y.y = q00 - q11 + q22 - q33;
+    Tbn_ret.z.z = q00 - q11 - q22 + q33;
+    Tbn_ret.x.y = 2*(q12 - q03);
+    Tbn_ret.x.z = 2*(q13 + q02);
+    Tbn_ret.y.x = 2*(q12 + q03);
+    Tbn_ret.y.z = 2*(q23 - q01);
+    Tbn_ret.z.x = 2*(q13 - q02);
+    Tbn_ret.z.y = 2*(q23 + q01);
 }
 
 void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
@@ -1979,17 +1884,17 @@ void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd,
     velNED[2] = gpsVelD;
 }
 
-void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef)
+void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference)
 {
-    posNED[0] = earthRadius * (lat - latRef);
-    posNED[1] = earthRadius * cos(latRef) * (lon - lonRef);
-    posNED[2] = -(hgt - hgtRef);
+    posNED[0] = earthRadius * (lat - latReference);
+    posNED[1] = earthRadius * cos(latReference) * (lon - lonReference);
+    posNED[2] = -(hgt - hgtReference);
 }
 
-void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
+void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
 {
-    lat = latRef + posNED[0] * earthRadiusInv;
-    lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
+    lat = latRef + (double)posNED[0] * earthRadiusInv;
+    lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef);
     hgt = hgtRef - posNED[2];
 }
 
@@ -2042,10 +1947,10 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max)
     float ret;
     if (val > max) {
         ret = max;
-        ekf_debug("> max: %8.4f, val: %8.4f", max, val);
+        ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val);
     } else if (val < min) {
         ret = min;
-        ekf_debug("< min: %8.4f, val: %8.4f", min, val);
+        ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val);
     } else {
         ret = val;
     }
@@ -2194,10 +2099,71 @@ void AttPosEKF::ForceSymmetry()
         {
             P[i][j] = 0.5f * (P[i][j] + P[j][i]);
             P[j][i] = P[i][j];
+
+            if ((fabsf(P[i][j]) > EKF_COVARIANCE_DIVERGED) ||
+                (fabsf(P[j][i]) > EKF_COVARIANCE_DIVERGED)) {
+                current_ekf_state.covariancesExcessive = true;
+                current_ekf_state.error |= true;
+                InitializeDynamic(velNED, magDeclination);
+                return;
+            }
+
+            float symmetric = 0.5f * (P[i][j] + P[j][i]);
+            P[i][j] = symmetric;
+            P[j][i] = symmetric;
         }
     }
 }
 
+bool AttPosEKF::GyroOffsetsDiverged()
+{
+    // Detect divergence by looking for rapid changes of the gyro offset
+    Vector3f current_bias;
+    current_bias.x = states[10];
+    current_bias.y = states[11];
+    current_bias.z = states[12];
+
+    Vector3f delta = current_bias - lastGyroOffset;
+    float delta_len = delta.length();
+    float delta_len_scaled = 0.0f;
+
+    // Protect against division by zero
+    if (delta_len > 0.0f) {
+        float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f);
+        delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMU;
+    }
+
+    bool diverged = (delta_len_scaled > 1.0f);
+    lastGyroOffset = current_bias;
+    current_ekf_state.error |= diverged;
+    current_ekf_state.gyroOffsetsExcessive = diverged;
+
+    return diverged;
+}
+
+bool AttPosEKF::VelNEDDiverged()
+{
+    Vector3f current_vel;
+    current_vel.x = states[4];
+    current_vel.y = states[5];
+    current_vel.z = states[6];
+
+    Vector3f gps_vel;
+    gps_vel.x = velNED[0];
+    gps_vel.y = velNED[1];
+    gps_vel.z = velNED[2];
+
+    Vector3f delta = current_vel - gps_vel;
+    float delta_len = delta.length();
+
+    bool excessive = (delta_len > 20.0f);
+
+    current_ekf_state.error |= excessive;
+    current_ekf_state.velOffsetExcessive = excessive;
+
+    return excessive;
+}
+
 bool AttPosEKF::FilterHealthy()
 {
     if (!statesInitialised) {
@@ -2262,42 +2228,26 @@ void AttPosEKF::ResetVelocity(void)
     }
 }
 
-
-void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
-{
-    for (unsigned i = 0; i < n_states; i++)
-    {
-        err->states[i] = states[i];
-    }
-
-    err->velHealth = current_ekf_state.velHealth;
-    err->posHealth = current_ekf_state.posHealth;
-    err->hgtHealth = current_ekf_state.hgtHealth;
-    err->velTimeout = current_ekf_state.velTimeout;
-    err->posTimeout = current_ekf_state.posTimeout;
-    err->hgtTimeout = current_ekf_state.hgtTimeout;
-}
-
-bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
+bool AttPosEKF::StatesNaN() {
     bool err = false;
 
     // check all integrators
     if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) {
-        err_report->statesNaN = true;
+        current_ekf_state.angNaN = true;
         ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z);
         err = true;
         goto out;
     } // delta angles
 
     if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) {
-        err_report->statesNaN = true;
+        current_ekf_state.angNaN = true;
         ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
         err = true;
         goto out;
     } // delta angles
 
     if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) {
-        err_report->statesNaN = true;
+        current_ekf_state.summedDelVelNaN = true;
         ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z);
         err = true;
         goto out;
@@ -2308,7 +2258,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
         for (unsigned j = 0; j < n_states; j++) {
             if (!isfinite(KH[i][j])) {
 
-                err_report->covarianceNaN = true;
+                current_ekf_state.KHNaN = true;
                 err = true;
                 ekf_debug("KH NaN");
                 goto out;
@@ -2316,7 +2266,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
 
             if (!isfinite(KHP[i][j])) {
 
-                err_report->covarianceNaN = true;
+                current_ekf_state.KHPNaN = true;
                 err = true;
                 ekf_debug("KHP NaN");
                 goto out;
@@ -2324,7 +2274,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
 
             if (!isfinite(P[i][j])) {
 
-                err_report->covarianceNaN = true;
+                current_ekf_state.covarianceNaN = true;
                 err = true;
                 ekf_debug("P NaN");
             } // covariance matrix
@@ -2332,7 +2282,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
 
         if (!isfinite(Kfusion[i])) {
 
-            err_report->kalmanGainsNaN = true;
+            current_ekf_state.kalmanGainsNaN = true;
             ekf_debug("Kfusion NaN");
             err = true;
             goto out;
@@ -2340,7 +2290,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
 
         if (!isfinite(states[i])) {
 
-            err_report->statesNaN = true;
+            current_ekf_state.statesNaN = true;
             ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]);
             err = true;
             goto out;
@@ -2349,7 +2299,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
 
 out:
     if (err) {
-        FillErrorReport(err_report);
+        current_ekf_state.error |= true;
     }
 
     return err;
@@ -2365,47 +2315,105 @@ out:
  * updated, but before any of the fusion steps are
  * executed.
  */
-int AttPosEKF::CheckAndBound()
+int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
 {
 
     // Store the old filter state
     bool currStaticMode = staticMode;
 
+    // Limit reset rate to 5 Hz to allow the filter
+    // to settle
+    if (millis() - lastReset < 200) {
+        return 0;
+    }
+
+    if (ekfDiverged) {
+        ekfDiverged = false;
+    }
+
+    int ret = 0;
+
+    // Check if we're on ground - this also sets static mode.
+    OnGroundCheck();
+
     // Reset the filter if the states went NaN
-    if (StatesNaN(&last_ekf_error)) {
+    if (StatesNaN()) {
         ekf_debug("re-initializing dynamic");
 
-        InitializeDynamic(velNED, magDeclination);
+        // Reset and fill error report
+	    InitializeDynamic(velNED, magDeclination);
 
-        return 1;
+        ret = 1;
     }
 
     // Reset the filter if the IMU data is too old
     if (dtIMU > 0.3f) {
 
+        current_ekf_state.imuTimeout = true;
+
+        // Fill error report
+        GetFilterState(&last_ekf_error);
+
         ResetVelocity();
         ResetPosition();
         ResetHeight();
         ResetStoredStates();
 
+        // Timeout cleared with this reset
+        current_ekf_state.imuTimeout = false;
+
         // that's all we can do here, return
-        return 2;
+        ret = 2;
     }
 
-    // Check if we're on ground - this also sets static mode.
-    OnGroundCheck();
-
     // Check if we switched between states
     if (currStaticMode != staticMode) {
+        // Fill error report, but not setting error flag
+        GetFilterState(&last_ekf_error);
+
         ResetVelocity();
         ResetPosition();
         ResetHeight();
         ResetStoredStates();
 
-        return 3;
+        ret = 3;
+    }
+
+    // Reset the filter if gyro offsets are excessive
+    if (GyroOffsetsDiverged()) {
+
+        // Reset and fill error report
+        InitializeDynamic(velNED, magDeclination);
+
+        // that's all we can do here, return
+        ret = 4;
+    }
+
+    // Reset the filter if it diverges too far from GPS
+    if (VelNEDDiverged()) {
+
+        // Reset and fill error report
+        InitializeDynamic(velNED, magDeclination);
+
+        // that's all we can do here, return
+        ret = 5;
+    }
+
+    // The excessive covariance detection already
+    // reset the filter. Just need to report here.
+    if (last_ekf_error.covariancesExcessive) {
+        ret = 6;
+    }
+
+    if (ret) {
+        ekfDiverged = true;
+        lastReset = millis();
+
+        // This reads the last error and clears it
+        GetLastErrorState(last_error);
     }
 
-    return 0;
+    return ret;
 }
 
 void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat)
@@ -2456,6 +2464,30 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f
 
 void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
 {
+    if (current_ekf_state.error) {
+        GetFilterState(&last_ekf_error);
+    }
+
+    ZeroVariables();
+
+    // Reset error states
+    current_ekf_state.error = false;
+    current_ekf_state.angNaN = false;
+    current_ekf_state.summedDelVelNaN = false;
+    current_ekf_state.KHNaN = false;
+    current_ekf_state.KHPNaN = false;
+    current_ekf_state.PNaN = false;
+    current_ekf_state.covarianceNaN = false;
+    current_ekf_state.kalmanGainsNaN = false;
+    current_ekf_state.statesNaN = false;
+
+    current_ekf_state.velHealth = true;
+    //current_ekf_state.posHealth = ?;
+    //current_ekf_state.hgtHealth = ?;
+    
+    current_ekf_state.velTimeout = false;
+    //current_ekf_state.posTimeout = ?;
+    //current_ekf_state.hgtTimeout = ?;
 
     // Fill variables with valid data
     velNED[0] = initvelNED[0];
@@ -2494,7 +2526,11 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
     // write to state vector
     for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
     for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities
-    for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel
+    // positions:
+    states[7] = posNE[0];
+    states[8] = posNE[1];
+    states[9] = -hgtMea;
+    for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel
     states[16] = initMagNED.x; // Magnetic Field North
     states[17] = initMagNED.y; // Magnetic Field East
     states[18] = initMagNED.z; // Magnetic Field Down
@@ -2525,14 +2561,16 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do
     hgtRef = referenceHgt;
     refSet = true;
 
-    // we are at reference altitude, so measurement must be zero
-    hgtMea = 0.0f;
+    // we are at reference position, so measurement must be zero
+    posNE[0] = 0.0f;
+    posNE[1] = 0.0f;
+
+    // we are at an unknown, possibly non-zero altitude - so altitude
+    // is not reset (hgtMea)
 
     // the baro offset must be this difference now
     baroHgtOffset = baroHgt - referenceHgt;
 
-    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
-
     InitializeDynamic(initvelNED, declination);
 }
 
@@ -2540,27 +2578,8 @@ void AttPosEKF::ZeroVariables()
 {
 
     // Initialize on-init initialized variables
-    fusionModeGPS = 0;
-    covSkipCount = 0;
-    statesInitialised = false;
-    fuseVelData = false;
-    fusePosData = false;
-    fuseHgtData = false;
-    fuseMagData = false;
-    fuseVtasData = false;
-    onGround = true;
-    staticMode = true;
-    useAirspeed = true;
-    useCompass = true;
-    useRangeFinder = true;
-    numericalProtection = true;
-    refSet = false;
+
     storeIndex = 0;
-    gpsHgt = 0.0f;
-    baroHgt = 0.0f;
-    GPSstatus = 0;
-    VtasMeas = 0.0f;
-    magDeclination = 0.0f;
 
     // Do the data structure init
     for (unsigned i = 0; i < n_states; i++) {
@@ -2577,9 +2596,7 @@ void AttPosEKF::ZeroVariables()
     correctedDelAng.zero();
     summedDelAng.zero();
     summedDelVel.zero();
-
-    dAngIMU.zero();
-    dVelIMU.zero();
+    lastGyroOffset.zero();
 
     for (unsigned i = 0; i < data_buffer_size; i++) {
 
@@ -2598,12 +2615,27 @@ void AttPosEKF::ZeroVariables()
 
 }
 
-void AttPosEKF::GetFilterState(struct ekf_status_report *state)
+void AttPosEKF::GetFilterState(struct ekf_status_report *err)
 {
-    memcpy(state, &current_ekf_state, sizeof(*state));
+
+    // Copy states
+    for (unsigned i = 0; i < n_states; i++) {
+        current_ekf_state.states[i] = states[i];
+    }
+    current_ekf_state.n_states = n_states;
+
+    memcpy(err, &current_ekf_state, sizeof(*err));
+
+    // err->velHealth = current_ekf_state.velHealth;
+    // err->posHealth = current_ekf_state.posHealth;
+    // err->hgtHealth = current_ekf_state.hgtHealth;
+    // err->velTimeout = current_ekf_state.velTimeout;
+    // err->posTimeout = current_ekf_state.posTimeout;
+    // err->hgtTimeout = current_ekf_state.hgtTimeout;
 }
 
 void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
 {
     memcpy(last_error, &last_ekf_error, sizeof(*last_error));
+    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
 }
diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h
index e821089f2..7aad849f9 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h
@@ -1,76 +1,10 @@
-#include <math.h>
-#include <stdint.h>
-
 #pragma once
 
-#define GRAVITY_MSS 9.80665f
-#define deg2rad 0.017453292f
-#define rad2deg 57.295780f
-#define pi 3.141592657f
-#define earthRate 0.000072921f
-#define earthRadius 6378145.0f
-#define earthRadiusInv  1.5678540e-7f
-
-class Vector3f
-{
-private:
-public:
-    float x;
-    float y;
-    float z;
-
-    float length(void) const;
-    void zero(void);
-};
-
-class Mat3f
-{
-private:
-public:
-    Vector3f x;
-    Vector3f y;
-    Vector3f z;
-
-    Mat3f();
-
-    void identity();
-    Mat3f transpose(void) const;
-};
-
-Vector3f operator*(float sclIn1, Vector3f vecIn1);
-Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
-Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
-Vector3f operator*( Mat3f matIn, Vector3f vecIn);
-Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
-Vector3f operator*(Vector3f vecIn1, float sclIn1);
-
-void swap_var(float &d1, float &d2);
+#include "estimator_utilities.h"
 
 const unsigned int n_states = 23;
 const unsigned int data_buffer_size = 50;
 
-enum GPS_FIX {
-    GPS_FIX_NOFIX = 0,
-    GPS_FIX_2D = 2,
-    GPS_FIX_3D = 3
-};
-
-struct ekf_status_report {
-    bool velHealth;
-    bool posHealth;
-    bool hgtHealth;
-    bool velTimeout;
-    bool posTimeout;
-    bool hgtTimeout;
-    uint32_t velFailTime;
-    uint32_t posFailTime;
-    uint32_t hgtFailTime;
-    float states[n_states];
-    bool statesNaN;
-    bool covarianceNaN;
-    bool kalmanGainsNaN;
-};
-
 class AttPosEKF {
 
 public:
@@ -141,7 +75,7 @@ public:
         accelProcessNoise = 0.5f;
     }
 
-    struct {
+    struct mag_state_struct {
         unsigned obsIndex;
         float MagPred[3];
         float SH_MAG[9];
@@ -157,7 +91,12 @@ public:
         float magZbias;
         float R_MAG;
         Mat3f DCM;
-    } magstate;
+    };
+
+    struct mag_state_struct magstate;
+    struct mag_state_struct resetMagState;
+
+
 
 
     // Global variables
@@ -166,6 +105,7 @@ public:
     float P[n_states][n_states]; // covariance matrix
     float Kfusion[n_states]; // Kalman gains
     float states[n_states]; // state matrix
+    float resetStates[n_states];
     float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
     uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
 
@@ -183,6 +123,8 @@ public:
     float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
     Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
     Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
+    Vector3f lastGyroOffset;    // Last gyro offset
+    Vector3f delAngTotal;
 
     Mat3f Tbn; // transformation matrix from body to NED coordinates
     Mat3f Tnb; // transformation amtrix from NED to body coordinates
@@ -196,11 +138,11 @@ public:
     float varInnovVelPos[6]; // innovation variance output
 
     float velNED[3]; // North, East, Down velocity obs (m/s)
+    float accelGPSNED[3];   // Acceleration predicted by GPS in earth frame
     float posNE[2]; // North, East position obs (m)
     float hgtMea; //  measured height (m)
     float baroHgtOffset;        ///< the baro (weather) offset from normalized altitude
     float rngMea; // Ground distance
-    float posNED[3]; // North, East Down position (m)
 
     float innovMag[3]; // innovation output
     float varInnovMag[3]; // innovation variance output
@@ -243,6 +185,9 @@ public:
     bool useCompass;    ///< boolean true if magnetometer data is being used
     bool useRangeFinder;     ///< true when rangefinder is being used
 
+    bool ekfDiverged;
+    uint64_t lastReset;
+
     struct ekf_status_report current_ekf_state;
     struct ekf_status_report last_ekf_error;
 
@@ -299,9 +244,9 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]);
 
 static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
 
-static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
+void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
 
-static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
+static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
 
 static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
 
@@ -321,7 +266,7 @@ void ConstrainStates();
 
 void ForceSymmetry();
 
-int CheckAndBound();
+int CheckAndBound(struct ekf_status_report *last_error);
 
 void ResetPosition();
 
@@ -333,8 +278,7 @@ void GetFilterState(struct ekf_status_report *state);
 
 void GetLastErrorState(struct ekf_status_report *last_error);
 
-bool StatesNaN(struct ekf_status_report *err_report);
-void FillErrorReport(struct ekf_status_report *err);
+bool StatesNaN();
 
 void InitializeDynamic(float (&initvelNED)[3], float declination);
 
@@ -342,6 +286,10 @@ protected:
 
 bool FilterHealthy();
 
+bool GyroOffsetsDiverged();
+
+bool VelNEDDiverged();
+
 void ResetHeight(void);
 
 void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
new file mode 100644
index 000000000..b4767a0d3
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
@@ -0,0 +1,139 @@
+
+#include "estimator_utilities.h"
+
+// Define EKF_DEBUG here to enable the debug print calls
+// if the macro is not set, these will be completely
+// optimized out by the compiler.
+//#define EKF_DEBUG
+
+#ifdef EKF_DEBUG
+#include <stdio.h>
+
+static void
+ekf_debug_print(const char *fmt, va_list args)
+{
+    fprintf(stderr, "%s: ", "[ekf]");
+    vfprintf(stderr, fmt, args);
+
+    fprintf(stderr, "\n");
+}
+
+void
+ekf_debug(const char *fmt, ...)
+{
+    va_list args;
+
+    va_start(args, fmt);
+    ekf_debug_print(fmt, args);
+}
+
+#else
+
+void ekf_debug(const char *fmt, ...) { while(0){} }
+#endif
+
+float Vector3f::length(void) const
+{
+    return sqrt(x*x + y*y + z*z);
+}
+
+void Vector3f::zero(void)
+{
+    x = 0.0f;
+    y = 0.0f;
+    z = 0.0f;
+}
+
+Mat3f::Mat3f() {
+    identity();
+}
+
+void Mat3f::identity() {
+    x.x = 1.0f;
+    x.y = 0.0f;
+    x.z = 0.0f;
+
+    y.x = 0.0f;
+    y.y = 1.0f;
+    y.z = 0.0f;
+
+    z.x = 0.0f;
+    z.y = 0.0f;
+    z.z = 1.0f;
+}
+
+Mat3f Mat3f::transpose(void) const
+{
+    Mat3f ret = *this;
+    swap_var(ret.x.y, ret.y.x);
+    swap_var(ret.x.z, ret.z.x);
+    swap_var(ret.y.z, ret.z.y);
+    return ret;
+}
+
+// overload + operator to provide a vector addition
+Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x + vecIn2.x;
+    vecOut.y = vecIn1.y + vecIn2.y;
+    vecOut.z = vecIn1.z + vecIn2.z;
+    return vecOut;
+}
+
+// overload - operator to provide a vector subtraction
+Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x - vecIn2.x;
+    vecOut.y = vecIn1.y - vecIn2.y;
+    vecOut.z = vecIn1.z - vecIn2.z;
+    return vecOut;
+}
+
+// overload * operator to provide a matrix vector product
+Vector3f operator*( Mat3f matIn, Vector3f vecIn)
+{
+    Vector3f vecOut;
+    vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
+    vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
+    vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
+    return vecOut;
+}
+
+// overload % operator to provide a vector cross product
+Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
+    vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
+    vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
+    return vecOut;
+}
+
+// overload * operator to provide a vector scaler product
+Vector3f operator*(Vector3f vecIn1, float sclIn1)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x * sclIn1;
+    vecOut.y = vecIn1.y * sclIn1;
+    vecOut.z = vecIn1.z * sclIn1;
+    return vecOut;
+}
+
+// overload * operator to provide a vector scaler product
+Vector3f operator*(float sclIn1, Vector3f vecIn1)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x * sclIn1;
+    vecOut.y = vecIn1.y * sclIn1;
+    vecOut.z = vecIn1.z * sclIn1;
+    return vecOut;
+}
+
+void swap_var(float &d1, float &d2)
+{
+    float tmp = d1;
+    d1 = d2;
+    d2 = tmp;
+}
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h
new file mode 100644
index 000000000..d47568b62
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h
@@ -0,0 +1,82 @@
+#include <math.h>
+#include <stdint.h>
+
+#pragma once
+
+#define GRAVITY_MSS 9.80665f
+#define deg2rad 0.017453292f
+#define rad2deg 57.295780f
+#define pi 3.141592657f
+#define earthRate 0.000072921f
+#define earthRadius 6378145.0
+#define earthRadiusInv  1.5678540e-7
+
+class Vector3f
+{
+private:
+public:
+    float x;
+    float y;
+    float z;
+
+    float length(void) const;
+    void zero(void);
+};
+
+class Mat3f
+{
+private:
+public:
+    Vector3f x;
+    Vector3f y;
+    Vector3f z;
+
+    Mat3f();
+
+    void identity();
+    Mat3f transpose(void) const;
+};
+
+Vector3f operator*(float sclIn1, Vector3f vecIn1);
+Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
+Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
+Vector3f operator*( Mat3f matIn, Vector3f vecIn);
+Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
+Vector3f operator*(Vector3f vecIn1, float sclIn1);
+
+void swap_var(float &d1, float &d2);
+
+enum GPS_FIX {
+    GPS_FIX_NOFIX = 0,
+    GPS_FIX_2D = 2,
+    GPS_FIX_3D = 3
+};
+
+struct ekf_status_report {
+    bool error;
+    bool velHealth;
+    bool posHealth;
+    bool hgtHealth;
+    bool velTimeout;
+    bool posTimeout;
+    bool hgtTimeout;
+    bool imuTimeout;
+    uint32_t velFailTime;
+    uint32_t posFailTime;
+    uint32_t hgtFailTime;
+    float states[32];
+    unsigned n_states;
+    bool angNaN;
+    bool summedDelVelNaN;
+    bool KHNaN;
+    bool KHPNaN;
+    bool PNaN;
+    bool covarianceNaN;
+    bool kalmanGainsNaN;
+    bool statesNaN;
+    bool gyroOffsetsExcessive;
+    bool covariancesExcessive;
+    bool velOffsetExcessive;
+};
+
+void ekf_debug(const char *fmt, ...);
\ No newline at end of file
diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk
index 6fefec2c2..dc5220bf0 100644
--- a/src/modules/ekf_att_pos_estimator/module.mk
+++ b/src/modules/ekf_att_pos_estimator/module.mk
@@ -39,4 +39,5 @@ MODULE_COMMAND	= ekf_att_pos_estimator
 
 SRCS		= ekf_att_pos_estimator_main.cpp \
 		  ekf_att_pos_estimator_params.c \
-		  estimator.cpp
+		  estimator_23states.cpp \
+		  estimator_utilities.cpp
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 0e065211e..000c02e3d 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
@@ -43,8 +43,8 @@
  *    Proceedings of the AIAA Guidance, Navigation and Control
  *    Conference, Aug 2004. AIAA-2004-4900.
  *
- * Original implementation for total energy control class:
- *    Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl)
+ * Implementation for total energy control class:
+ *    Thomas Gubler
  *
  * More details and acknowledgements in the referenced library headers.
  *
@@ -88,9 +88,9 @@
 #include <mavlink/mavlink_log.h>
 #include <launchdetection/LaunchDetector.h>
 #include <ecl/l1/ecl_l1_pos_controller.h>
-#include <external_lgpl/tecs/tecs.h>
 #include <drivers/drv_range_finder.h>
 #include "landingslope.h"
+#include "mtecs/mTecs.h"
 
 
 /**
@@ -153,8 +153,6 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
-
 	/** manual control states */
 	float		_altctrl_hold_heading;		/**< heading the system should hold in altctrl mode */
 	double		_loiter_hold_lat;
@@ -200,7 +198,8 @@ private:
 	math::Matrix<3, 3> _R_nb;			///< current attitude
 
 	ECL_L1_Pos_Controller				_l1_control;
-	TECS						_tecs;
+	fwPosctrl::mTecs				_mTecs;
+	bool						_was_pos_control_mode;
 
 	struct {
 		float l1_period;
@@ -343,11 +342,11 @@ private:
 	/**
 	 * Control position.
 	 */
-	bool		control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed,
+	bool		control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed,
 					 const struct position_setpoint_triplet_s &_pos_sp_triplet);
 
 	float calculate_target_airspeed(float airspeed_demand);
-	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);
+	void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet);
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -368,6 +367,19 @@ private:
 	 * Reset landing state
 	 */
 	void reset_landing_state();
+
+	/*
+	 * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter)
+	 * XXX need to clean up/remove this function once mtecs fully replaces TECS
+	 */
+	void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
+			float pitch_min_rad, float pitch_max_rad,
+			float throttle_min, float throttle_max, float throttle_cruise,
+			bool climbout_mode, float climbout_pitch_min_rad,
+			float altitude,
+			const math::Vector<3> &ground_speed,
+			tecs_mode mode = TECS_MODE_NORMAL);
+
 };
 
 namespace l1_control
@@ -406,7 +418,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
 
 /* states */
-	_setpoint_valid(false),
 	_loiter_hold(false),
 	land_noreturn_horizontal(false),
 	land_noreturn_vertical(false),
@@ -431,6 +442,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_global_pos(),
 	_pos_sp_triplet(),
 	_sensor_combined(),
+	_mTecs(),
+	_was_pos_control_mode(false),
 	_range_finder()
 {
 	_nav_capabilities.turn_distance = 0.0f;
@@ -550,23 +563,6 @@ FixedwingPositionControl::parameters_update()
 	_l1_control.set_l1_period(_parameters.l1_period);
 	_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
 
-	_tecs.set_time_const(_parameters.time_const);
-	_tecs.set_min_sink_rate(_parameters.min_sink_rate);
-	_tecs.set_max_sink_rate(_parameters.max_sink_rate);
-	_tecs.set_throttle_damp(_parameters.throttle_damp);
-	_tecs.set_integrator_gain(_parameters.integrator_gain);
-	_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(_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 ||
 	    _parameters.airspeed_max < 5.0f ||
@@ -589,6 +585,9 @@ FixedwingPositionControl::parameters_update()
 	/* Update Launch Detector Parameters */
 	launchDetector.updateParams();
 
+	/* Update the mTecs */
+	_mTecs.updateParams();
+
 	return OK;
 }
 
@@ -635,9 +634,6 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 		}
 	}
 
-	/* update TECS state */
-	_tecs.enable_airspeed(_airspeed_valid);
-
 	return airspeed_updated;
 }
 
@@ -692,7 +688,6 @@ FixedwingPositionControl::vehicle_setpoint_poll()
 
 	if (pos_sp_triplet_updated) {
 		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
-		_setpoint_valid = true;
 	}
 }
 
@@ -734,15 +729,15 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
 }
 
 void
-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)
+FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 
-	if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
+	if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
 
 		/* 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;
+		float ground_speed_body = yaw_vector * ground_speed_2d;
 
 		/* 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;
@@ -801,12 +796,13 @@ float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt,
 }
 
 bool
-FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed,
+FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
 		const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 	bool setpoint = true;
 
-	calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet);
+	math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
+	calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
 
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
@@ -817,7 +813,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	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 = _pos_sp_triplet.current.alt - _global_pos.alt;
 
 	/* no throttle limit as default */
@@ -827,14 +822,21 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	// XXX this should only execute if auto AND safety off (actuators active),
 	// else integrators should be constantly reset.
-	if (_control_mode.flag_control_position_enabled) {
+	if (pos_sp_triplet.current.valid) {
+
+		if (!_was_pos_control_mode) {
+			/* reset integrators */
+			if (_mTecs.getEnabled()) {
+				_mTecs.resetIntegrators();
+				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+			}
+		}
+
+		_was_pos_control_mode = true;
 
 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
 
-		/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
-		_tecs.set_speed_weight(_parameters.speed_weight);
-
 		/* current waypoint (the one currently heading for) */
 		math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
@@ -863,31 +865,29 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		}
 
-		if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) {
+		if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) {
 			/* waypoint is a plain navigation waypoint */
-			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
+			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
 			_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(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
+						math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
+						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+						false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_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);
+						  pos_sp_triplet.current.loiter_direction, ground_speed_2d);
 			_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(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
+						math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
+						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+						false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
 
@@ -912,7 +912,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 //					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
 
-				_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
+				_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d);
 
 				/* 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));
@@ -922,7 +922,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			} else {
 
 				/* normal navigation */
-				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
 			}
 
 			_att_sp.roll_body = _l1_control.nav_roll();
@@ -946,7 +946,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 			/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
 			float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
-			float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance);
+			float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
 
 			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_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
@@ -981,11 +981,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					land_stayonground = true;
 				}
 
-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + flare_curve_alt_rel, 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));
+				tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel,
+						calculate_target_airspeed(airspeed_land), eas2tas,
+						flare_pitch_angle_rad, math::radians(15.0f),
+						0.0f, throttle_max, throttle_land,
+						false, flare_pitch_angle_rad,
+						_pos_sp_triplet.current.alt + relative_alt, ground_speed,
+						land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
 
 				if (!land_noreturn_vertical) {
 					mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
@@ -998,11 +1000,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 				 /* intersect glide slope:
 				  * minimize speed to approach speed
-				  * 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
+				  * if current position is higher than the slope follow the glide slope (sink to the
+				  * glide slope)
+				  * also if the system captures the slope it should stay
+				  * on the slope (bool land_onslope)
+				  * if current position is below the slope continue at previous wp altitude
+				  * until the intersection with slope
 				  * */
 				float altitude_desired_rel = relative_alt;
-				if (relative_alt > landing_slope_alt_rel_desired - 10.0f) {
+				if (relative_alt > landing_slope_alt_rel_desired || land_onslope) {
 					/* stay on slope */
 					altitude_desired_rel = landing_slope_alt_rel_desired;
 					if (!land_onslope) {
@@ -1011,14 +1017,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					}
 				} else {
 					/* continue horizontally */
-					altitude_desired_rel =  math::max(relative_alt, L_altitude_rel);
+					altitude_desired_rel =  _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt;
 				}
 
-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + altitude_desired_rel, 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(_pos_sp_triplet.current.alt + altitude_desired_rel,
+						calculate_target_airspeed(airspeed_approach), eas2tas,
+						math::radians(_parameters.pitch_limit_min),
+						math::radians(_parameters.pitch_limit_max),
+						_parameters.throttle_min,
+						_parameters.throttle_max,
+						_parameters.throttle_cruise,
+						false,
+						math::radians(_parameters.pitch_limit_min),
+						_pos_sp_triplet.current.alt + relative_alt,
+						ground_speed);
 			}
 
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
@@ -1051,7 +1063,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				}
 			}
 
-			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
+			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
 			_att_sp.roll_body = _l1_control.nav_roll();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
@@ -1062,22 +1074,36 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				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, _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));
+					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
+							calculate_target_airspeed(1.3f * _parameters.airspeed_min),
+							eas2tas,
+							math::radians(_parameters.pitch_limit_min),
+							math::radians(_parameters.pitch_limit_max),
+							_parameters.throttle_min, _parameters.throttle_max,
+							_parameters.throttle_cruise,
+							true,
+							math::max(math::radians(pos_sp_triplet.current.pitch_min),
+							math::radians(10.0f)),
+							_global_pos.alt,
+							ground_speed,
+							TECS_MODE_TAKEOFF);
 
 					/* 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, _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(_pos_sp_triplet.current.alt,
+							calculate_target_airspeed(_parameters.airspeed_trim),
+							eas2tas,
+								math::radians(_parameters.pitch_limit_min),
+								math::radians(_parameters.pitch_limit_max),
+								_parameters.throttle_min,
+								_parameters.throttle_max,
+								_parameters.throttle_cruise,
+								false,
+								math::radians(_parameters.pitch_limit_min),
+								_global_pos.alt,
+								ground_speed);
 				}
 
 			} else {
@@ -1111,19 +1137,21 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	} else if (0/* posctrl mode enabled */) {
 
+		_was_pos_control_mode = false;
+
 		/** POSCTRL FLIGHT **/
 
-		if (0/* switched from another mode to posctrl */) {
-			_altctrl_hold_heading = _att.yaw;
-		}
+	if (0/* switched from another mode to posctrl */) {
+		_altctrl_hold_heading = _att.yaw;
+	}
 
-		if (0/* posctrl on and manual control yaw non-zero */) {
-			_altctrl_hold_heading = _att.yaw + _manual.r;
-		}
+	if (0/* posctrl on and manual control yaw non-zero */) {
+		_altctrl_hold_heading = _att.yaw + _manual.r;
+	}
 
-		//XXX not used
+	//XXX not used
 
-		/* climb out control */
+	/* climb out control */
 //		bool climb_out = false;
 //
 //		/* user wants to climb out */
@@ -1131,25 +1159,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 //			climb_out = true;
 //		}
 
-		/* if in altctrl mode, set airspeed based on manual control */
+	/* if in altctrl mode, set airspeed based on manual control */
 
-		// XXX check if ground speed undershoot should be applied here
-		float altctrl_airspeed = _parameters.airspeed_min +
-					  (_parameters.airspeed_max - _parameters.airspeed_min) *
-					  _manual.z;
+	// XXX check if ground speed undershoot should be applied here
+	float altctrl_airspeed = _parameters.airspeed_min +
+					(_parameters.airspeed_max - _parameters.airspeed_min) *
+					_manual.z;
 
-		_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed);
+		_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
 		_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, _global_pos.alt + _manual.x * 2.0f,
-					    altctrl_airspeed,
-					    _airspeed.indicated_airspeed_m_s, eas2tas,
-					    false, _parameters.pitch_limit_min,
-					    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-					    _parameters.pitch_limit_min, _parameters.pitch_limit_max);
+
+		tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
+					math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
+					_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+					false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
 	} else if (0/* altctrl mode enabled */) {
 
+		_was_pos_control_mode = false;
+
 		/** ALTCTRL FLIGHT **/
 
 		if (0/* switched from another mode to altctrl */) {
@@ -1170,8 +1199,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* user switched off throttle */
 		if (_manual.z < 0.1f) {
 			throttle_max = 0.0f;
-			/* switch to pure pitch based altitude control, give up speed */
-			_tecs.set_speed_weight(0.0f);
 		}
 
 		/* climb out control */
@@ -1182,18 +1209,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			climb_out = true;
 		}
 
-		_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed);
+		_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
 		_att_sp.roll_body =	_manual.y;
 		_att_sp.yaw_body =	_manual.r;
-		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f,
-					    altctrl_airspeed,
-					    _airspeed.indicated_airspeed_m_s, eas2tas,
-					    climb_out, _parameters.pitch_limit_min,
-					    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-					    _parameters.pitch_limit_min, _parameters.pitch_limit_max);
+		tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
+					math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
+					_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+					climb_out, math::radians(_parameters.pitch_limit_min),
+					_global_pos.alt, ground_speed);
 
 	} else {
 
+		_was_pos_control_mode = false;
+
 		/** MANUAL FLIGHT **/
 
 		/* no flight mode applies, do not publish an attitude setpoint */
@@ -1210,9 +1238,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
 	}
 	else {
-		_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
+		_att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max);
 	}
-	_att_sp.pitch_body = _tecs.get_pitch_demand();
+	_att_sp.pitch_body = _mTecs.getPitchSetpoint();
 
 	if (_control_mode.flag_control_position_enabled) {
 		last_manual = false;
@@ -1326,7 +1354,7 @@ FixedwingPositionControl::task_main()
 			range_finder_poll();
 			// vehicle_baro_poll();
 
-			math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
+			math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e,  _global_pos.vel_d);
 			math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
 
 			/*
@@ -1388,6 +1416,30 @@ void FixedwingPositionControl::reset_landing_state()
 	land_onslope = false;
 }
 
+void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
+		float pitch_min_rad, float pitch_max_rad,
+		float throttle_min, float throttle_max, float throttle_cruise,
+		bool climbout_mode, float climbout_pitch_min_rad,
+		float altitude,
+		const math::Vector<3> &ground_speed,
+		tecs_mode mode)
+{
+	/* Using mtecs library: prepare arguments for mtecs call */
+	float flightPathAngle = 0.0f;
+	float ground_speed_length = ground_speed.length();
+	if (ground_speed_length > FLT_EPSILON) {
+		flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
+	}
+	fwPosctrl::LimitOverride limitOverride;
+	if (climbout_mode) {
+		limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
+	} else {
+		limitOverride.disablePitchMinOverride();
+	}
+	_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
+			limitOverride);
+}
+
 int
 FixedwingPositionControl::start()
 {
diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk
index cf419ec7e..af155fe08 100644
--- a/src/modules/fw_pos_control_l1/module.mk
+++ b/src/modules/fw_pos_control_l1/module.mk
@@ -39,4 +39,7 @@ MODULE_COMMAND	= fw_pos_control_l1
 
 SRCS		= fw_pos_control_l1_main.cpp \
 		  fw_pos_control_l1_params.c \
-		  landingslope.cpp
+		  landingslope.cpp \
+		  mtecs/mTecs.cpp \
+		  mtecs/limitoverride.cpp \
+		  mtecs/mTecs_params.c
diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp
new file mode 100644
index 000000000..58795edb6
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@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 limitoverride.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "limitoverride.h"
+
+namespace fwPosctrl {
+
+bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
+		BlockOutputLimiter &outputLimiterPitch)
+{
+	bool ret = false;
+
+	if (overrideThrottleMinEnabled)	{
+		outputLimiterThrottle.setMin(overrideThrottleMin);
+		ret = true;
+	}
+	if (overrideThrottleMaxEnabled)	{
+		outputLimiterThrottle.setMax(overrideThrottleMax);
+		ret = true;
+	}
+	if (overridePitchMinEnabled)	{
+		outputLimiterPitch.setMin(overridePitchMin);
+		ret = true;
+	}
+	if (overridePitchMaxEnabled)	{
+		outputLimiterPitch.setMax(overridePitchMax);
+		ret = true;
+	}
+
+	return ret;
+}
+
+} /* namespace fwPosctrl */
diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.h b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h
new file mode 100644
index 000000000..64c2e7bbd
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h
@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@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 limitoverride.h
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+
+#ifndef LIMITOVERRIDE_H_
+#define LIMITOVERRIDE_H_
+
+#include "mTecs_blocks.h"
+
+namespace fwPosctrl
+{
+
+/* A small class which provides helper functions to override control output limits which are usually set by
+* parameters in special cases
+*/
+class LimitOverride
+{
+public:
+	LimitOverride() :
+		overrideThrottleMinEnabled(false),
+		overrideThrottleMaxEnabled(false),
+		overridePitchMinEnabled(false),
+		overridePitchMaxEnabled(false)
+	{};
+
+	~LimitOverride() {};
+
+	/*
+	* Override the limits of the outputlimiter instances given by the arguments with the limits saved in
+	* this class (if enabled)
+	* @return true if the limit was applied
+	*/
+	bool applyOverride(BlockOutputLimiter &outputLimiterThrottle,
+			BlockOutputLimiter &outputLimiterPitch);
+
+	/* Functions to enable or disable the override */
+	void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled,
+			&overrideThrottleMin, value); }
+	void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); }
+	void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled,
+			&overrideThrottleMax, value); }
+	void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); }
+	void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled,
+			&overridePitchMin, value); }
+	void disablePitchMinOverride() { disable(&overridePitchMinEnabled); }
+	void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled,
+			&overridePitchMax, value); }
+	void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); }
+
+protected:
+	bool overrideThrottleMinEnabled;
+	float overrideThrottleMin;
+	bool overrideThrottleMaxEnabled;
+	float overrideThrottleMax;
+	bool overridePitchMinEnabled;
+	float overridePitchMin; //in degrees (replaces param values)
+	bool overridePitchMaxEnabled;
+	float overridePitchMax; //in degrees (replaces param values)
+
+	/* Enable a specific limit override */
+	void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; };
+
+	/* Disable a specific limit override */
+	void disable(bool *flag) { *flag = false; };
+};
+
+} /* namespace fwPosctrl */
+
+#endif /* LIMITOVERRIDE_H_ */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
new file mode 100644
index 000000000..fc0a2551c
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -0,0 +1,313 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@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 mTecs.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "mTecs.h"
+
+#include <lib/geo/geo.h>
+#include <stdio.h>
+
+namespace fwPosctrl {
+
+mTecs::mTecs() :
+	SuperBlock(NULL, "MT"),
+	/* Parameters */
+	_mTecsEnabled(this, "ENABLED"),
+	_airspeedMin(this, "FW_AIRSPD_MIN", false),
+	/* Publications */
+	_status(&getPublications(), ORB_ID(tecs_status)),
+	/* control blocks */
+	_controlTotalEnergy(this, "THR"),
+	_controlEnergyDistribution(this, "PIT", true),
+	_controlAltitude(this, "FPA", true),
+	_controlAirSpeed(this, "ACC"),
+	_flightPathAngleLowpass(this, "FPA_LP"),
+	_airspeedLowpass(this, "A_LP"),
+	_airspeedDerivative(this, "AD"),
+	_throttleSp(0.0f),
+	_pitchSp(0.0f),
+	_BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"),
+	_BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true),
+	_BlockOutputLimiterUnderspeedThrottle(this, "USP_THR"),
+	_BlockOutputLimiterUnderspeedPitch(this, "USP_PIT", true),
+	_BlockOutputLimiterLandThrottle(this, "LND_THR"),
+	_BlockOutputLimiterLandPitch(this, "LND_PIT", true),
+	timestampLastIteration(hrt_absolute_time()),
+	_firstIterationAfterReset(true),
+	_dtCalculated(false),
+	_counter(0),
+	_debug(false)
+{
+}
+
+mTecs::~mTecs()
+{
+}
+
+int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
+		float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
+{
+	/* check if all input arguments are numbers and abort if not so */
+	if (!isfinite(flightPathAngle) || !isfinite(altitude) ||
+			!isfinite(altitudeSp) || !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) {
+		return -1;
+	}
+
+	/* time measurement */
+	updateTimeMeasurement();
+
+	/* calculate flight path angle setpoint from altitude setpoint */
+	float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude);
+
+	/* Debug output */
+	if (_counter % 10 == 0) {
+		debug("***");
+		debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp);
+	}
+
+	/* Write part of the status message */
+	_status.altitudeSp = altitudeSp;
+	_status.altitude = altitude;
+
+
+	/* use flightpath angle setpoint for total energy control */
+	return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed,
+			airspeedSp, mode, limitOverride);
+}
+
+int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
+		float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
+{
+	/* check if all input arguments are numbers and abort if not so */
+	if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
+			!isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) {
+		return -1;
+	}
+
+	/* time measurement */
+	updateTimeMeasurement();
+
+	/* Filter airspeed */
+	float airspeedFiltered = _airspeedLowpass.update(airspeed);
+
+	/* calculate longitudinal acceleration setpoint from airspeed setpoint*/
+	float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeedFiltered);
+
+	/* Debug output */
+	if (_counter % 10 == 0) {
+		debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f,"
+				"accelerationLongitudinalSp%.4f",
+				(double)airspeedSp, (double)airspeed,
+				(double)airspeedFiltered, (double)accelerationLongitudinalSp);
+	}
+
+	/* Write part of the status message */
+	_status.airspeedSp = airspeedSp;
+	_status.airspeed = airspeed;
+	_status.airspeedFiltered = airspeedFiltered;
+
+
+	/* use longitudinal acceleration setpoint for total energy control */
+	return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeedFiltered,
+			accelerationLongitudinalSp, mode, limitOverride);
+}
+
+int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered,
+		float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride)
+{
+	/* check if all input arguments are numbers and abort if not so */
+	if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
+			!isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) {
+		return -1;
+	}
+	/* time measurement */
+	updateTimeMeasurement();
+
+	/* update parameters first */
+	updateParams();
+
+	/* Filter flightpathangle */
+	float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle);
+
+	/* calculate values (energies) */
+	float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered;
+	float airspeedDerivative = 0.0f;
+	if(_airspeedDerivative.getDt() > 0.0f) {
+		airspeedDerivative = _airspeedDerivative.update(airspeedFiltered);
+	}
+	float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G;
+	float airspeedDerivativeSp = accelerationLongitudinalSp;
+	float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G;
+	float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm;
+
+	float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm;
+	float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError;
+	float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp;
+	float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate;
+
+	float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm;
+	float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError;
+	float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp;
+	float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate;
+
+	/* Debug output */
+	if (_counter % 10 == 0) {
+		debug("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f",
+				(double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm);
+		debug("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f",
+				(double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2);
+	}
+
+	/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
+	if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
+		mode = TECS_MODE_UNDERSPEED;
+	}
+
+	/* Set special ouput limiters if we are not in TECS_MODE_NORMAL */
+	BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
+	BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
+	if (mode == TECS_MODE_TAKEOFF) {
+		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
+		outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
+	} else if (mode == TECS_MODE_LAND) {
+		// only limit pitch but do not limit throttle
+		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
+	} else if (mode == TECS_MODE_LAND_THROTTLELIM) {
+		outputLimiterThrottle = &_BlockOutputLimiterLandThrottle;
+		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
+	} else if (mode == TECS_MODE_UNDERSPEED) {
+		outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle;
+		outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
+	}
+
+	/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
+	 * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
+	 * is running) */
+	bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
+
+	/* Write part of the status message */
+	_status.flightPathAngleSp = flightPathAngleSp;
+	_status.flightPathAngle = flightPathAngle;
+	_status.flightPathAngleFiltered = flightPathAngleFiltered;
+	_status.airspeedDerivativeSp = airspeedDerivativeSp;
+	_status.airspeedDerivative = airspeedDerivative;
+	_status.totalEnergyRateSp = totalEnergyRateSp;
+	_status.totalEnergyRate = totalEnergyRate;
+	_status.energyDistributionRateSp = energyDistributionRateSp;
+	_status.energyDistributionRate = energyDistributionRate;
+	_status.mode = mode;
+
+	/** update control blocks **/
+	/* update total energy rate control block */
+	_throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError, outputLimiterThrottle);
+
+	/* update energy distribution rate control block */
+	_pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError, outputLimiterPitch);
+
+
+	if (_counter % 10 == 0) {
+		debug("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f",
+				(double)_throttleSp, (double)_pitchSp,
+				(double)flightPathAngleSp, (double)flightPathAngle,
+				(double)accelerationLongitudinalSp, (double)airspeedDerivative);
+	}
+
+	/* publish status messge */
+	_status.update();
+
+	/* clean up */
+	_firstIterationAfterReset = false;
+	_dtCalculated = false;
+
+	_counter++;
+
+	return 0;
+}
+
+void mTecs::resetIntegrators()
+{
+	_controlTotalEnergy.getIntegral().setY(0.0f);
+	_controlEnergyDistribution.getIntegral().setY(0.0f);
+	timestampLastIteration = hrt_absolute_time();
+	_firstIterationAfterReset = true;
+}
+
+void mTecs::resetDerivatives(float airspeed)
+{
+	_airspeedDerivative.setU(airspeed);
+}
+
+
+void mTecs::updateTimeMeasurement()
+{
+	if (!_dtCalculated) {
+		float deltaTSeconds = 0.0f;
+		if (!_firstIterationAfterReset) {
+			hrt_abstime timestampNow = hrt_absolute_time();
+			deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f;
+			timestampLastIteration = timestampNow;
+		}
+		setDt(deltaTSeconds);
+
+		_dtCalculated = true;
+	}
+}
+
+void mTecs::debug_print(const char *fmt, va_list args)
+{
+	fprintf(stderr, "%s: ", "[mtecs]");
+	vfprintf(stderr, fmt, args);
+
+	fprintf(stderr, "\n");
+}
+
+void mTecs::debug(const char *fmt, ...) {
+
+	if (!_debug) {
+		return;
+	}
+
+	va_list args;
+
+	va_start(args, fmt);
+	debug_print(fmt, args);
+}
+
+} /* namespace fwPosctrl */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
new file mode 100644
index 000000000..efa89a5d3
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -0,0 +1,155 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@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 mTecs.h
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+
+#ifndef MTECS_H_
+#define MTECS_H_
+
+#include "mTecs_blocks.h"
+#include "limitoverride.h"
+
+#include <controllib/block/BlockParam.hpp>
+#include <drivers/drv_hrt.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/tecs_status.h>
+
+namespace fwPosctrl
+{
+
+/* Main class of the mTecs */
+class mTecs : public control::SuperBlock
+{
+public:
+	mTecs();
+	virtual ~mTecs();
+
+	/*
+	 * Control in altitude setpoint and speed mode
+	 */
+	int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
+			float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
+
+	/*
+	 * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode
+	 */
+	int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
+			float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
+
+	/*
+	 * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case)
+	 */
+	int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered,
+			float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride);
+
+	/*
+	 * Reset all integrators
+	 */
+	void resetIntegrators();
+
+	/*
+	 * Reset all derivative calculations
+	 */
+	void resetDerivatives(float airspeed);
+
+	/* Accessors */
+	bool getEnabled() { return _mTecsEnabled.get() > 0; }
+	float getThrottleSetpoint() { return _throttleSp; }
+	float getPitchSetpoint() { return _pitchSp; }
+	float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
+
+protected:
+	/* parameters */
+	control::BlockParamInt _mTecsEnabled;		/**< 1 if mTecs is enabled */
+	control::BlockParamFloat _airspeedMin;		/**< minimal airspeed */
+
+	/* Publications */
+	uORB::Publication<tecs_status_s> _status;	/**< publish internal values for logging */
+
+	/* control blocks */
+	BlockFFPILimitedCustom _controlTotalEnergy;		/**< FFPI controller for total energy control: output
+								  is throttle */
+	BlockFFPILimitedCustom _controlEnergyDistribution;	/**< FFPI controller for energy distribution control:
+								  output is pitch */
+	BlockPDLimited	_controlAltitude;			/**< PD controller for altitude: output is the flight
+								  path angle setpoint */
+	BlockPDLimited	_controlAirSpeed;			/**< PD controller for airspeed: output is acceleration
+								  setpoint */
+
+	/* Other calculation Blocks */
+	control::BlockLowPass _flightPathAngleLowpass;	/**< low pass filter for the flight path angle */
+	control::BlockLowPass _airspeedLowpass;		/**< low pass filter for airspeed */
+	control::BlockDerivative _airspeedDerivative;	/**< airspeed derivative calulation */
+
+	/* Output setpoints */
+	float _throttleSp;				/**< Throttle Setpoint from 0 to 1 */
+	float _pitchSp;					/**< Pitch Setpoint from -pi to pi */
+
+	/* Output Limits in special modes */
+	BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle;	    /**< Throttle Limits during takeoff */
+	BlockOutputLimiter _BlockOutputLimiterTakeoffPitch;	    /**< Pitch Limit during takeoff */
+	BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle;   /**< Throttle Limits when underspeed is detected */
+	BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch;	    /**< Pitch Limit when underspeed is detected */
+	BlockOutputLimiter _BlockOutputLimiterLandThrottle;	    /**< Throttle Limits during landing (only in
+								      last phase)*/
+	BlockOutputLimiter _BlockOutputLimiterLandPitch;	    /**< Pitch Limit during landing */
+
+	/* Time measurements */
+	hrt_abstime timestampLastIteration;	/**< Saves the result of hrt_absolute_time() of the last iteration */
+
+	bool _firstIterationAfterReset;		/**< True during the first iteration after a reset */
+	bool _dtCalculated;			/**< True if dt has been calculated in this iteration */
+
+	int _counter;
+	bool _debug;				///< Set true to enable debug output
+
+
+	static void	debug_print(const char *fmt, va_list args);
+	void		debug(const char *fmt, ...);
+
+	/*
+	 * Measure and update the time step dt if this was not already done in the current iteration
+	 */
+	void updateTimeMeasurement();
+};
+
+} /* namespace fwPosctrl */
+
+#endif /* MTECS_H_ */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
new file mode 100644
index 000000000..e4e405227
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
@@ -0,0 +1,220 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@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 mTecs_blocks.h
+ *
+ * Custom blocks for the mTecs
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#pragma once
+
+#include <controllib/blocks.hpp>
+#include <systemlib/err.h>
+
+namespace fwPosctrl
+{
+
+using namespace control;
+
+/* An block which can be used to limit the output */
+class BlockOutputLimiter: public SuperBlock
+{
+public:
+// methods
+	BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
+		SuperBlock(parent, name),
+		_isAngularLimit(isAngularLimit),
+		_min(this, "MIN"),
+		_max(this, "MAX")
+	{};
+	virtual ~BlockOutputLimiter() {};
+	/*
+	 * Imposes the limits given by _min and _max on value
+	 *
+	 * @param value is changed to be on the interval _min to _max
+	 * @param difference if the value is changed this corresponds to the change of value * (-1)
+     * otherwise unchanged
+	 * @return: true if the limit is applied, false otherwise
+	 */
+	bool limit(float& value, float& difference) {
+		float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
+		float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
+		if (value < minimum) {
+			difference = value - minimum;
+			value = minimum;
+			return true;
+		} else if (value > maximum) {
+			difference = value - maximum;
+			value = maximum;
+			return true;
+		}
+		return false;
+	}
+//accessor:
+	bool isAngularLimit() {return _isAngularLimit ;}
+	float getMin() { return _min.get(); }
+	float getMax() { return _max.get(); }
+	void setMin(float value) { _min.set(value); }
+	void setMax(float value) { _max.set(value); }
+protected:
+//attributes
+	bool _isAngularLimit;
+	control::BlockParamFloat _min;
+	control::BlockParamFloat _max;
+};
+
+
+/* A combination of feed forward, P and I gain using the output limiter*/
+class BlockFFPILimited: public SuperBlock
+{
+public:
+// methods
+	BlockFFPILimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
+		SuperBlock(parent, name),
+		_outputLimiter(this, "", isAngularLimit),
+		_integral(this, "I"),
+		_kFF(this, "FF"),
+		_kP(this, "P"),
+		_kI(this, "I"),
+		_offset(this, "OFF")
+	{};
+	virtual ~BlockFFPILimited() {};
+	float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _outputLimiter); }
+// accessors
+	BlockIntegral &getIntegral() { return _integral; }
+	float getKFF() { return _kFF.get(); }
+	float getKP() { return _kP.get(); }
+	float getKI() { return _kI.get(); }
+	float getOffset() { return _offset.get(); }
+	BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
+protected:
+	BlockOutputLimiter _outputLimiter;
+
+	float calcUnlimitedOutput(float inputValue, float inputError) {return getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);}
+	float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) {
+		float difference = 0.0f;
+		float integralYPrevious = _integral.getY();
+		float output = calcUnlimitedOutput(inputValue, inputError);
+		if(outputLimiter.limit(output, difference) &&
+			(((difference < 0) && (getKI() * getIntegral().getY() < 0)) ||
+			((difference > 0) && (getKI() * getIntegral().getY() > 0)))) {
+				getIntegral().setY(integralYPrevious);
+		}
+		return output;
+	}
+private:
+	BlockIntegral _integral;
+	BlockParamFloat _kFF;
+	BlockParamFloat _kP;
+	BlockParamFloat _kI;
+	BlockParamFloat _offset;
+};
+
+/* A combination of feed forward, P and I gain using the output limiter with the option to provide a special output limiter (for example for takeoff)*/
+class BlockFFPILimitedCustom: public BlockFFPILimited
+{
+public:
+// methods
+	BlockFFPILimitedCustom(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
+		BlockFFPILimited(parent, name, isAngularLimit)
+		{};
+	virtual ~BlockFFPILimitedCustom() {};
+	float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) {
+		return calcLimitedOutput(inputValue, inputError, outputLimiter == NULL ? _outputLimiter : *outputLimiter);
+	}
+};
+
+/* A combination of P gain and output limiter */
+class BlockPLimited: public SuperBlock
+{
+public:
+// methods
+	BlockPLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
+		SuperBlock(parent, name),
+		_kP(this, "P"),
+		_outputLimiter(this, "", isAngularLimit)
+	{};
+	virtual ~BlockPLimited() {};
+	float update(float input) {
+		float difference = 0.0f;
+		float output = getKP() * input;
+		getOutputLimiter().limit(output, difference);
+		return output;
+	}
+// accessors
+	BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
+	float getKP() { return _kP.get(); }
+private:
+	control::BlockParamFloat _kP;
+	BlockOutputLimiter _outputLimiter;
+};
+
+/* A combination of P, D gains and output limiter */
+class BlockPDLimited: public SuperBlock
+{
+public:
+// methods
+	BlockPDLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
+		SuperBlock(parent, name),
+		_kP(this, "P"),
+		_kD(this, "D"),
+		_derivative(this, "D"),
+		_outputLimiter(this, "", isAngularLimit)
+	{};
+	virtual ~BlockPDLimited() {};
+	float update(float input) {
+		float difference = 0.0f;
+		float output = getKP() * input + (getDerivative().getDt() > 0.0f ? getKD() * getDerivative().update(input) : 0.0f);
+		getOutputLimiter().limit(output, difference);
+
+		return output;
+	}
+// accessors
+	float getKP() { return _kP.get(); }
+	float getKD() { return _kD.get(); }
+	BlockDerivative &getDerivative() { return _derivative; }
+	BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
+private:
+	control::BlockParamFloat _kP;
+	control::BlockParamFloat _kD;
+	BlockDerivative _derivative;
+	BlockOutputLimiter _outputLimiter;
+};
+
+}
+
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
new file mode 100644
index 000000000..5b9238780
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -0,0 +1,419 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@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 mTecs_params.c
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
+
+/*
+ * Controller parameters, accessible via MAVLink
+ */
+
+/**
+ * mTECS enabled
+ *
+ * Set to 1 to enable mTECS
+ *
+ * @min 0
+ * @max 1
+ * @group mTECS
+ */
+PARAM_DEFINE_INT32(MT_ENABLED, 1);
+
+/**
+ * Total Energy Rate Control Feedforward
+ * Maps the total energy rate setpoint to the throttle setpoint
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_FF, 0.7f);
+
+/**
+ * Total Energy Rate Control P
+ * Maps the total energy rate error to the throttle setpoint
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f);
+
+/**
+ * Total Energy Rate Control I
+ * Maps the integrated total energy rate to the throttle setpoint
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_I, 0.25f);
+
+/**
+ * Total Energy Rate Control Offset (Cruise throttle sp)
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f);
+
+/**
+ * Energy Distribution Rate Control Feedforward
+ * Maps the energy distribution rate setpoint to the pitch setpoint
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.4f);
+
+/**
+ * Energy Distribution Rate Control P
+ * Maps the energy distribution rate error to the pitch setpoint
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f);
+
+/**
+ * Energy Distribution Rate Control I
+ * Maps the integrated energy distribution rate error to the pitch setpoint
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f);
+
+
+/**
+ * Total Energy Distribution Offset (Cruise pitch sp)
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_OFF, 0.0f);
+
+/**
+ * Minimal Throttle Setpoint
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_MIN, 0.0f);
+
+/**
+ * Maximal Throttle Setpoint
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_MAX, 1.0f);
+
+/**
+ * Minimal Pitch Setpoint in Degrees
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
+
+/**
+ * Maximal Pitch Setpoint in Degrees
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
+
+/**
+ * Lowpass (cutoff freq.) for the flight path angle
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f);
+
+/**
+ * P gain for the altitude control
+ * Maps the altitude error to the flight path angle setpoint
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f);
+
+/**
+ * D gain for the altitude control
+ * Maps the change of altitude error to the flight path angle setpoint
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f);
+
+/**
+ * Lowpass for FPA error derivative calculation (see MT_FPA_D)
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f);
+
+
+/**
+ * Minimal flight path angle setpoint
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_MIN, -20.0f);
+
+/**
+ * Maximal flight path angle setpoint
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
+
+/**
+ * Lowpass (cutoff freq.) for airspeed
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f);
+
+/**
+ * P gain for the airspeed control
+ * Maps the airspeed error to the acceleration setpoint
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f);
+
+/**
+ * D gain for the airspeed control
+ * Maps the change of airspeed error to the acceleration setpoint
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f);
+
+/**
+ * Lowpass for ACC error derivative calculation (see MT_ACC_D)
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f);
+
+/**
+ * Minimal acceleration (air)
+ *
+ * @unit m/s^2
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f);
+
+/**
+ * Maximal acceleration (air)
+ *
+* @unit m/s^2
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
+
+/**
+ * Airspeed derivative calculation lowpass
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f);
+
+/**
+ * Minimal throttle during takeoff
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f);
+
+/**
+ * Maximal throttle during takeoff
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f);
+
+/**
+ * Minimal pitch during takeoff
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f);
+
+/**
+ * Maximal pitch during takeoff
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f);
+
+/**
+ * Minimal throttle in underspeed mode
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f);
+
+/**
+ * Maximal throttle in underspeed mode
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f);
+
+/**
+ * Minimal pitch in underspeed mode
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f);
+
+/**
+ * Maximal pitch in underspeed mode
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f);
+
+/**
+ * Minimal throttle in landing mode (only last phase of landing)
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f);
+
+/**
+ * Maximal throttle in landing mode (only last phase of landing)
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f);
+
+/**
+ * Minimal pitch in landing mode
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f);
+
+/**
+ * Maximal pitch in landing mode
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f);
+
+/**
+ * Integrator Limit for Total Energy Rate Control
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f);
+
+/**
+ * Integrator Limit for Energy Distribution Rate Control
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_I_MAX, 10.0f);
diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index 6dfd22fdf..7758faed7 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -181,16 +181,13 @@ int gpio_led_main(int argc, char *argv[])
 			} else {
 				gpio_led_started = true;
 				warnx("start, using pin: %s", pin_name);
+				exit(0);
 			}
-
-			exit(0);
-
-
 		} else if (!strcmp(argv[1], "stop")) {
 			if (gpio_led_started) {
 				gpio_led_started = false;
 				warnx("stop");
-
+				exit(0);
 			} else {
 				errx(1, "not running");
 			}
@@ -264,7 +261,7 @@ void gpio_led_cycle(FAR void *arg)
 		pattern = 0x2A;	// *_*_*_ fast blink (armed, error)
 
 	} else if (priv->status.arming_state == ARMING_STATE_ARMED) {
-		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) {
+		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) {
 			pattern = 0x3f;	// ****** solid (armed)
 
 		} else {
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
new file mode 100644
index 000000000..ca846a465
--- /dev/null
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -0,0 +1,415 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <crc32.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <fcntl.h>
+
+#include "mavlink_ftp.h"
+
+MavlinkFTP *MavlinkFTP::_server;
+
+MavlinkFTP *
+MavlinkFTP::getServer()
+{
+	// XXX this really cries out for some locking...
+	if (_server == nullptr) {
+		_server = new MavlinkFTP;
+	}
+	return _server;
+}
+
+MavlinkFTP::MavlinkFTP()
+{
+	// initialise the request freelist
+	dq_init(&_workFree);
+	sem_init(&_lock, 0, 1);
+    
+    // initialize session list
+    for (size_t i=0; i<kMaxSession; i++) {
+        _session_fds[i] = -1;
+    }
+
+	// drop work entries onto the free list
+	for (unsigned i = 0; i < kRequestQueueSize; i++) {
+		_qFree(&_workBufs[i]);
+	}
+
+}
+
+void
+MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
+{
+	// get a free request
+	auto req = _dqFree();
+
+	// if we couldn't get a request slot, just drop it
+	if (req != nullptr) {
+
+		// decode the request
+		if (req->decode(mavlink, msg)) {
+
+			// and queue it for the worker
+			work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
+		} else {
+			_qFree(req);
+		}
+	}
+}
+
+void
+MavlinkFTP::_workerTrampoline(void *arg)
+{
+	auto req = reinterpret_cast<Request *>(arg);
+	auto server = MavlinkFTP::getServer();
+
+	// call the server worker with the work item
+	server->_worker(req);
+}
+
+void
+MavlinkFTP::_worker(Request *req)
+{
+	auto hdr = req->header();
+	ErrorCode errorCode = kErrNone;
+	uint32_t messageCRC;
+
+	// basic sanity checks; must validate length before use
+	if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
+		errorCode = kErrNoRequest;
+		goto out;
+	}
+
+	// check request CRC to make sure this is one of ours
+	messageCRC = hdr->crc32;
+	hdr->crc32 = 0;
+	if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
+		errorCode = kErrNoRequest;
+		goto out;
+		printf("ftp: bad crc\n");
+	}
+
+	printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
+
+	switch (hdr->opcode) {
+	case kCmdNone:
+		break;
+
+	case kCmdTerminate:
+		errorCode = _workTerminate(req);
+		break;
+
+	case kCmdReset:
+		errorCode = _workReset();
+		break;
+
+	case kCmdList:
+		errorCode = _workList(req);
+		break;
+
+	case kCmdOpen:
+		errorCode = _workOpen(req, false);
+		break;
+
+	case kCmdCreate:
+		errorCode = _workOpen(req, true);
+		break;
+
+	case kCmdRead:
+		errorCode = _workRead(req);
+		break;
+
+	case kCmdWrite:
+		errorCode = _workWrite(req);
+		break;
+
+	case kCmdRemove:
+		errorCode = _workRemove(req);
+		break;
+
+	default:
+		errorCode = kErrNoRequest;
+		break;	
+	}
+
+out:
+	// handle success vs. error
+	if (errorCode == kErrNone) {
+		hdr->opcode = kRspAck;
+		printf("FTP: ack\n");
+	} else {
+		printf("FTP: nak %u\n", errorCode);
+		hdr->opcode = kRspNak;
+		hdr->size = 1;
+		hdr->data[0] = errorCode;
+	}
+
+	// respond to the request
+	_reply(req);
+
+	// free the request buffer back to the freelist
+	_qFree(req);
+}
+
+void
+MavlinkFTP::_reply(Request *req)
+{
+	auto hdr = req->header();
+
+	// message is assumed to be already constructed in the request buffer, so generate the CRC
+	hdr->crc32 = 0;
+	hdr->crc32 = crc32(req->rawData(), req->dataSize());
+
+	// then pack and send the reply back to the request source
+	req->reply();
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workList(Request *req)
+{
+	auto hdr = req->header();
+	DIR *dp = opendir(req->dataAsCString());
+
+	if (dp == nullptr) {
+		printf("FTP: can't open path '%s'\n", req->dataAsCString());
+		return kErrNotDir;
+	}
+
+	ErrorCode errorCode = kErrNone;
+	struct dirent entry, *result = nullptr;
+	unsigned offset = 0;
+
+	// move to the requested offset
+	seekdir(dp, hdr->offset);
+
+	for (;;) {
+		// read the directory entry
+		if (readdir_r(dp, &entry, &result)) {
+			errorCode = kErrIO;
+			break;
+		}
+
+		// no more entries?
+		if (result == nullptr) {
+			if (hdr->offset != 0 && offset == 0) {
+				// User is requesting subsequent dir entries but there were none. This means the user asked
+				// to seek past EOF.
+				errorCode = kErrEOF;
+			}
+			// Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that
+			break;
+		}
+
+		// name too big to fit?
+		if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) {
+			break;
+		}
+
+		// store the type marker
+		switch (entry.d_type) {
+		case DTYPE_FILE:
+			hdr->data[offset++] = kDirentFile;
+			break;
+		case DTYPE_DIRECTORY:
+			hdr->data[offset++] = kDirentDir;
+			break;
+		default:
+			hdr->data[offset++] = kDirentUnknown;
+			break;
+		}
+
+		// copy the name, which we know will fit
+		strcpy((char *)&hdr->data[offset], entry.d_name);
+		offset += strlen(entry.d_name) + 1;
+		printf("FTP: list %s\n", entry.d_name);
+	}
+
+	closedir(dp);
+	hdr->size = offset;
+
+	return errorCode;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workOpen(Request *req, bool create)
+{
+	auto hdr = req->header();
+    
+	int session_index = _findUnusedSession();
+	if (session_index < 0) {
+		return kErrNoSession;
+	}
+
+	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
+    
+	int fd = ::open(req->dataAsCString(), oflag);
+	if (fd < 0) {
+		return create ? kErrPerm : kErrNotFile;
+	}
+	_session_fds[session_index] = fd;
+
+	hdr->session = session_index;
+	hdr->size = 0;
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRead(Request *req)
+{
+	auto hdr = req->header();
+
+	int session_index = hdr->session;
+
+	if (!_validSession(session_index)) {
+		return kErrNoSession;
+	}
+
+	// Seek to the specified position
+	printf("Seek %d\n", hdr->offset);
+	if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
+		// Unable to see to the specified location
+		return kErrEOF;
+	}
+
+	int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength);
+	if (bytes_read < 0) {
+		// Negative return indicates error other than eof
+		return kErrIO;
+	}
+
+	printf("Read success %d\n", bytes_read);
+	hdr->size = bytes_read;
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workWrite(Request *req)
+{
+#if 0
+    // NYI: Coming soon
+	auto hdr = req->header();
+
+	// look up session
+	auto session = getSession(hdr->session);
+	if (session == nullptr) {
+		return kErrNoSession;
+	}
+
+	// append to file
+	int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
+
+	if (result < 0) {
+		// XXX might also be no space, I/O, etc.
+		return kErrNotAppend;
+	}
+
+	hdr->size = result;
+	return kErrNone;
+#else
+	return kErrPerm;
+#endif
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRemove(Request *req)
+{
+	auto hdr = req->header();
+
+	// for now, send error reply
+	return kErrPerm;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workTerminate(Request *req)
+{
+	auto hdr = req->header();
+    
+	if (!_validSession(hdr->session)) {
+		return kErrNoSession;
+	}
+    
+	::close(_session_fds[hdr->session]);
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workReset(void)
+{
+	for (size_t i=0; i<kMaxSession; i++) {
+		if (_session_fds[i] != -1) {
+			::close(_session_fds[i]);
+			_session_fds[i] = -1;
+		}
+	}
+
+	return kErrNone;
+}
+
+bool
+MavlinkFTP::_validSession(unsigned index)
+{
+	if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
+		return false;
+	}
+	return true;
+}
+
+int
+MavlinkFTP::_findUnusedSession(void)
+{
+	for (size_t i=0; i<kMaxSession; i++) {
+		if (_session_fds[i] == -1) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+char *
+MavlinkFTP::Request::dataAsCString()
+{
+	// guarantee nul termination
+	if (header()->size < kMaxDataLength) {
+		requestData()[header()->size] = '\0';
+	} else {
+		requestData()[kMaxDataLength - 1] = '\0';
+	}
+
+	// and return data
+	return (char *)&(header()->data[0]);
+}
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
new file mode 100644
index 000000000..e22e61553
--- /dev/null
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -0,0 +1,226 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+/**
+ * @file mavlink_ftp.h
+ *
+ * MAVLink remote file server.
+ *
+ * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
+ * a session ID and sequence number.
+ *
+ * A limited number of requests (currently 2) may be outstanding at a time.
+ * Additional messages will be discarded.
+ *
+ * Messages consist of a fixed header, followed by a data area.
+ *
+ */
+ 
+#include <dirent.h>
+#include <queue.h>
+
+#include <nuttx/wqueue.h>
+#include <systemlib/err.h>
+
+#include "mavlink_messages.h"
+
+class MavlinkFTP
+{
+public:
+	MavlinkFTP();
+
+	static MavlinkFTP	*getServer();
+
+	// static interface
+	void			handle_message(Mavlink* mavlink,
+					mavlink_message_t *msg);
+
+private:
+
+	static const unsigned	kRequestQueueSize = 2;
+
+	static MavlinkFTP	*_server;
+
+	struct RequestHeader
+	{
+		uint8_t		magic;
+		uint8_t		session;
+		uint8_t		opcode;
+		uint8_t		size;
+		uint32_t	crc32;
+		uint32_t	offset;
+		uint8_t		data[];
+	};
+
+	enum Opcode : uint8_t
+	{
+		kCmdNone,	// ignored, always acked
+		kCmdTerminate,	// releases sessionID, closes file
+		kCmdReset,	// terminates all sessions
+		kCmdList,	// list files in <path> from <offset>
+		kCmdOpen,	// opens <path> for reading, returns <session>
+		kCmdRead,	// reads <size> bytes from <offset> in <session>
+		kCmdCreate,	// creates <path> for writing, returns <session>
+		kCmdWrite,	// appends <size> bytes at <offset> in <session>
+		kCmdRemove,	// remove file (only if created by server?)
+
+		kRspAck,
+		kRspNak
+	};
+
+	enum ErrorCode : uint8_t
+	{
+		kErrNone,
+		kErrNoRequest,
+		kErrNoSession,
+		kErrSequence,
+		kErrNotDir,
+		kErrNotFile,
+		kErrEOF,
+		kErrNotAppend,
+		kErrTooBig,
+		kErrIO,
+		kErrPerm
+	};
+
+    int _findUnusedSession(void);
+    bool _validSession(unsigned index);
+
+    static const unsigned kMaxSession = 2;
+    int	_session_fds[kMaxSession];
+
+	class Request
+	{
+	public:
+		union {
+			dq_entry_t	entry;
+			work_s		work;
+		};
+
+		bool		decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
+			if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
+				_mavlink = mavlink;
+				mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
+				return true;
+			}
+			return false;
+		}
+
+		void		reply() {
+
+			// XXX the proper way would be an IOCTL / uORB call, rather than exploiting the
+			// flat memory architecture, as we're operating between threads here.
+			mavlink_message_t msg;
+			msg.checksum = 0;
+			unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
+				_mavlink->get_channel(), &msg, sequence(), rawData());
+
+            _mavlink->lockMessageBufferMutex();
+            bool fError = _mavlink->message_buffer_write(&msg, len);
+            _mavlink->unlockMessageBufferMutex();
+            
+            if (!fError) {
+				warnx("FTP TX ERR");
+			} else {
+				warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
+					_mavlink->get_system_id(),
+					_mavlink->get_component_id(),
+					_mavlink->get_channel(),
+					len,
+					msg.checksum);
+			}
+		}
+
+		uint8_t		*rawData() { return &_message.data[0]; }
+		RequestHeader 	*header()  { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
+		uint8_t         *requestData() { return &(header()->data[0]); }
+		unsigned	dataSize() { return header()->size + sizeof(RequestHeader); }
+		uint16_t	sequence() const { return _message.seqnr; }
+		mavlink_channel_t channel() { return _mavlink->get_channel(); }
+
+		char		*dataAsCString();
+
+	private:
+		Mavlink			*_mavlink;
+		mavlink_encapsulated_data_t _message;
+
+	};
+
+	static const uint8_t	kProtocolMagic = 'f';
+	static const char	kDirentFile = 'F';
+	static const char	kDirentDir = 'D';
+	static const char	kDirentUnknown = 'U';
+	static const uint8_t	kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
+
+	/// Request worker; runs on the low-priority work queue to service
+	/// remote requests.
+	///
+	static void		_workerTrampoline(void *arg);
+	void			_worker(Request *req);
+
+	/// Reply to a request (XXX should be a Request method)
+	///
+	void			_reply(Request *req);
+
+	ErrorCode		_workList(Request *req);
+	ErrorCode		_workOpen(Request *req, bool create);
+	ErrorCode		_workRead(Request *req);
+	ErrorCode		_workWrite(Request *req);
+	ErrorCode		_workRemove(Request *req);
+	ErrorCode		_workTerminate(Request *req);
+	ErrorCode		_workReset();
+
+	// work freelist
+	Request			_workBufs[kRequestQueueSize];
+	dq_queue_t		_workFree;
+	sem_t			_lock;
+
+	void			_qLock() { do {} while (sem_wait(&_lock) != 0); }
+	void			_qUnlock() { sem_post(&_lock); }
+
+	void			_qFree(Request *req) {
+		_qLock();
+		dq_addlast(&req->entry, &_workFree);
+		_qUnlock();
+	}
+
+	Request			*_dqFree() {
+		_qLock();
+		auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree));
+		_qUnlock();
+		return req;
+	}
+    
+};
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index a9b8323f3..9a5e31ef4 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -83,6 +83,10 @@
 #include "mavlink_rate_limiter.h"
 #include "mavlink_commands.h"
 
+#ifndef MAVLINK_CRC_EXTRA
+ #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
+#endif
+
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
@@ -114,6 +118,7 @@ static uint64_t last_write_try_times[6] = {0};
 void
 mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
 {
+
 	Mavlink *instance;
 
 	switch (channel) {
@@ -192,13 +197,14 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 
 			if (buf_free < desired) {
 				/* we don't want to send anything just in half, so return */
+				instance->count_txerr();
 				return;
 			}
 		}
 
 		ssize_t ret = write(uart, ch, desired);
 		if (ret != desired) {
-			warnx("TX FAIL");
+			instance->count_txerr();
 		} else {
 			last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
 		}
@@ -230,6 +236,7 @@ Mavlink::Mavlink() :
 	_verbose(false),
 	_forwarding_on(false),
 	_passing_on(false),
+	_ftp_on(false),
 	_uart_fd(-1),
 	_mavlink_param_queue_index(0),
 	_subscribe_to_stream(nullptr),
@@ -243,7 +250,8 @@ Mavlink::Mavlink() :
 	_param_use_hil_gps(0),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
+	_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
 {
 	_wpm = &_wpm_s;
 	mission.count = 0;
@@ -296,6 +304,7 @@ Mavlink::Mavlink() :
 Mavlink::~Mavlink()
 {
 	perf_free(_loop_perf);
+	perf_free(_txerr_perf);
 
 	if (_task_running) {
 		/* task wakes up every 10ms or so at the longest */
@@ -321,6 +330,12 @@ Mavlink::~Mavlink()
 }
 
 void
+Mavlink::count_txerr()
+{
+	perf_count(_txerr_perf);
+}
+
+void
 Mavlink::set_mode(enum MAVLINK_MODE mode)
 {
 	_mode = mode;
@@ -459,7 +474,7 @@ Mavlink::get_instance_id()
 	return _instance_id;
 }
 
-mavlink_channel_t
+const mavlink_channel_t
 Mavlink::get_channel()
 {
 	return _channel;
@@ -537,6 +552,16 @@ void Mavlink::mavlink_update_system(void)
 	_use_hil_gps = (bool)use_hil_gps;
 }
 
+int Mavlink::get_system_id()
+{
+	return mavlink_system.sysid;
+}
+
+int Mavlink::get_component_id()
+{
+	return mavlink_system.compid;
+}
+
 int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
 {
 	/* process baud rate */
@@ -899,7 +924,11 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
 	case MAV_CMD_NAV_TAKEOFF:
 		mission_item->pitch_min = mavlink_mission_item->param1;
 		break;
-
+	case MAV_CMD_DO_JUMP:
+		mission_item->do_jump_mission_index = mavlink_mission_item->param1;
+		mission_item->do_jump_current_count = 0;
+		mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
+		break;
 	default:
 		mission_item->acceptance_radius = mavlink_mission_item->param2;
 		mission_item->time_inside = mavlink_mission_item->param1;
@@ -915,6 +944,9 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
 	// mission_item->index = mavlink_mission_item->seq;
 	mission_item->origin = ORIGIN_MAVLINK;
 
+	/* reset DO_JUMP count */
+	mission_item->do_jump_current_count = 0;
+
 	return OK;
 }
 
@@ -932,6 +964,11 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
 		mavlink_mission_item->param1 = mission_item->pitch_min;
 		break;
 
+	case NAV_CMD_DO_JUMP:
+		mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
+		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
+		break;
+
 	default:
 		mavlink_mission_item->param2 = mission_item->acceptance_radius;
 		mavlink_mission_item->param1 = mission_item->time_inside;
@@ -1619,11 +1656,21 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
 int
 Mavlink::message_buffer_init(int size)
 {
+
 	_message_buffer.size = size;
 	_message_buffer.write_ptr = 0;
 	_message_buffer.read_ptr = 0;
 	_message_buffer.data = (char*)malloc(_message_buffer.size);
-	return (_message_buffer.data == 0) ? ERROR : OK;
+
+	int ret;
+	if (_message_buffer.data == 0) {
+		ret = ERROR;
+		_message_buffer.size = 0;
+	} else {
+		ret = OK;
+	}
+
+	return ret;
 }
 
 void
@@ -1751,7 +1798,7 @@ Mavlink::task_main(int argc, char *argv[])
 	 * set error flag instead */
 	bool err_flag = false;
 
-	while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) {
+	while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) {
 		switch (ch) {
 		case 'b':
 			_baudrate = strtoul(optarg, NULL, 10);
@@ -1807,6 +1854,10 @@ Mavlink::task_main(int argc, char *argv[])
 			_wait_to_transmit = true;
 			break;
 
+		case 'x':
+			_ftp_on = true;
+			break;
+
 		default:
 			err_flag = true;
 			break;
@@ -1872,9 +1923,12 @@ Mavlink::task_main(int argc, char *argv[])
 	mavlink_logbuffer_init(&_logbuffer, 5);
 
 	/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
-	if (_passing_on) {
-		/* initialize message buffer if multiplexing is on */
-		if (OK != message_buffer_init(300)) {
+	if (_passing_on || _ftp_on) {
+		/* initialize message buffer if multiplexing is on or its needed for FTP.
+		 * make space for two messages plus off-by-one space as we use the empty element
+		 * marker ring buffer approach.
+		 */
+		if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) {
 			errx(1, "can't allocate message buffer, exiting");
 		}
 
@@ -2037,32 +2091,50 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 		}
 
-		/* pass messages from other UARTs */
-		if (_passing_on) {
+		/* pass messages from other UARTs or FTP worker */
+		if (_passing_on || _ftp_on) {
 
 			bool is_part;
-			void *read_ptr;
+			uint8_t *read_ptr;
+            uint8_t *write_ptr;
 
-			/* guard get ptr by mutex */
 			pthread_mutex_lock(&_message_buffer_mutex);
-			int available = message_buffer_get_ptr(&read_ptr, &is_part);
+			int available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
 			pthread_mutex_unlock(&_message_buffer_mutex);
 
 			if (available > 0) {
-				/* write first part of buffer */
-				_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
-				message_buffer_mark_read(available);
+                // Reconstruct message from buffer
+
+				mavlink_message_t msg;
+                write_ptr = (uint8_t*)&msg;
+
+                // Pull a single message from the buffer
+                int read_count = available;
+                if (read_count > sizeof(mavlink_message_t)) {
+                    read_count = sizeof(mavlink_message_t);
+                }
+                
+                memcpy(write_ptr, read_ptr, read_count);
+                
+                // We hold the mutex until after we complete the second part of the buffer. If we don't
+                // we may end up breaking the empty slot overflow detection semantics when we mark the
+                // possibly partial read below.
+                pthread_mutex_lock(&_message_buffer_mutex);
+                
+				message_buffer_mark_read(read_count);
 
 				/* write second part of buffer if there is some */
-				if (is_part) {
-					/* guard get ptr by mutex */
-					pthread_mutex_lock(&_message_buffer_mutex);
-					available = message_buffer_get_ptr(&read_ptr, &is_part);
-					pthread_mutex_unlock(&_message_buffer_mutex);
-
-					_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
+				if (is_part && read_count < sizeof(mavlink_message_t)) {
+                    write_ptr += read_count;
+					available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
+                    read_count = sizeof(mavlink_message_t) - read_count;
+                    memcpy(write_ptr, read_ptr, read_count);
 					message_buffer_mark_read(available);
 				}
+                
+                pthread_mutex_unlock(&_message_buffer_mutex);
+
+                _mavlink_resend_uart(_channel, &msg);
 			}
 		}
 
@@ -2112,7 +2184,7 @@ Mavlink::task_main(int argc, char *argv[])
 	/* close mavlink logging device */
 	close(_mavlink_fd);
 
-	if (_passing_on) {
+	if (_passing_on || _ftp_on) {
 		message_buffer_destroy();
 		pthread_mutex_destroy(&_message_buffer_mutex);
 	}
@@ -2130,11 +2202,20 @@ int Mavlink::start_helper(int argc, char *argv[])
 	/* create the instance in task context */
 	Mavlink *instance = new Mavlink();
 
-	/* this will actually only return once MAVLink exits */
-	int res = instance->task_main(argc, argv);
+	int res;
 
-	/* delete instance on main thread end */
-	delete instance;
+	if (!instance) {
+
+		/* out of memory */
+		res = -ENOMEM;
+		warnx("OUT OF MEM");
+	} else {
+		/* this will actually only return once MAVLink exits */
+		res = instance->task_main(argc, argv);
+
+		/* delete instance on main thread end */
+		delete instance;
+	}
 
 	return res;
 }
@@ -2254,7 +2335,7 @@ Mavlink::stream(int argc, char *argv[])
 
 static void usage()
 {
-	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]");
+	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
 }
 
 int mavlink_main(int argc, char *argv[])
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 85a88442c..6777d56c3 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -123,27 +123,41 @@ public:
 	/**
 	 * Display the mavlink status.
 	 */
-	void		status();
+	void			status();
 
-	static int stream(int argc, char *argv[]);
+	static int		stream(int argc, char *argv[]);
 
-	static int	instance_count();
+	static int		instance_count();
 
-	static Mavlink *new_instance();
+	static Mavlink		*new_instance();
 
-	static Mavlink *get_instance(unsigned instance);
+	static Mavlink		*get_instance(unsigned instance);
 
-	static Mavlink *get_instance_for_device(const char *device_name);
+	static Mavlink		*get_instance_for_device(const char *device_name);
 
-	static int	destroy_all_instances();
+	static int		destroy_all_instances();
 
-	static bool	instance_exists(const char *device_name, Mavlink *self);
+	static bool		instance_exists(const char *device_name, Mavlink *self);
 
-	static void	forward_message(mavlink_message_t *msg, Mavlink *self);
+	static void		forward_message(mavlink_message_t *msg, Mavlink *self);
 
-	static int get_uart_fd(unsigned index);
+	static int		get_uart_fd(unsigned index);
 
-	int get_uart_fd();
+	int			get_uart_fd();
+
+	/**
+	 * Get the MAVLink system id.
+	 *
+	 * @return		The system ID of this vehicle
+	 */
+	int			get_system_id();
+
+	/**
+	 * Get the MAVLink component id.
+	 *
+	 * @return		The component ID of this vehicle
+	 */
+	int			get_component_id();
 
 	const char *_device_name;
 
@@ -153,30 +167,30 @@ public:
 		MAVLINK_MODE_CAMERA
 	};
 
-	void		set_mode(enum MAVLINK_MODE);
-	enum MAVLINK_MODE		get_mode() { return _mode; }
+	void			set_mode(enum MAVLINK_MODE);
+	enum MAVLINK_MODE	get_mode() { return _mode; }
 
-	bool		get_hil_enabled() { return _hil_enabled; }
+	bool			get_hil_enabled() { return _hil_enabled; }
 
-	bool		get_use_hil_gps() { return _use_hil_gps; }
+	bool			get_use_hil_gps() { return _use_hil_gps; }
 
-	bool		get_flow_control_enabled() { return _flow_control_enabled; }
+	bool			get_flow_control_enabled() { return _flow_control_enabled; }
 
-	bool		get_forwarding_on() { return _forwarding_on; }
+	bool			get_forwarding_on() { return _forwarding_on; }
 
 	/**
 	 * Handle waypoint related messages.
 	 */
-	void mavlink_wpm_message_handler(const mavlink_message_t *msg);
+	void			mavlink_wpm_message_handler(const mavlink_message_t *msg);
 
-	static int start_helper(int argc, char *argv[]);
+	static int		start_helper(int argc, char *argv[]);
 
 	/**
 	 * Handle parameter related messages.
 	 */
-	void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
+	void			mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
 
-	void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
+	void			get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
 
 	/**
 	 * Enable / disable Hardware in the Loop simulation mode.
@@ -186,90 +200,105 @@ public:
 	 *			requested change could not be made or was
 	 *			redundant.
 	 */
-	int		set_hil_enabled(bool hil_enabled);
+	int			set_hil_enabled(bool hil_enabled);
 
-	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
+	MavlinkOrbSubscription	*add_orb_subscription(const orb_id_t topic);
 
-	int get_instance_id();
+	int			get_instance_id();
 
 	/**
 	 * Enable / disable hardware flow control.
 	 *
 	 * @param enabled	True if hardware flow control should be enabled
 	 */
-	int enable_flow_control(bool enabled);
+	int			enable_flow_control(bool enabled);
 
-	mavlink_channel_t get_channel();
+	const mavlink_channel_t	get_channel();
+
+	void configure_stream_threadsafe(const char *stream_name, const float rate);
 
-	bool		_task_should_exit;		/**< if true, mavlink task should exit */
+	bool			_task_should_exit;	/**< if true, mavlink task should exit */
 
-	int get_mavlink_fd() { return _mavlink_fd; }
+	int			get_mavlink_fd() { return _mavlink_fd; }
+
+	MavlinkStream * get_streams() { return _streams; } const
 
 
 	/* Functions for waiting to start transmission until message received. */
-	void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
-	bool get_has_received_messages() { return _received_messages; }
-	void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
-	bool get_wait_to_transmit() { return _wait_to_transmit; }
-	bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
+	void			set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
+	bool			get_has_received_messages() { return _received_messages; }
+	void			set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
+	bool			get_wait_to_transmit() { return _wait_to_transmit; }
+	bool			should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
+
+	bool			message_buffer_write(void *ptr, int size);
+    
+    void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
+    void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
+
+	/**
+	 * Count a transmision error
+	 */
+	void count_txerr();
 
 protected:
-	Mavlink	*next;
+	Mavlink			*next;
 
 private:
-	int _instance_id;
+	int			_instance_id;
 
-	int		_mavlink_fd;
-	bool		_task_running;
+	int			_mavlink_fd;
+	bool			_task_running;
 
 	/* states */
-	bool		_hil_enabled;		/**< Hardware In the Loop mode */
-	bool		_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
-	bool		_is_usb_uart;		/**< Port is USB */
-	bool        _wait_to_transmit;  /**< Wait to transmit until received messages. */
-	bool        _received_messages; /**< Whether we've received valid mavlink messages. */
+	bool			_hil_enabled;		/**< Hardware In the Loop mode */
+	bool			_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
+	bool			_is_usb_uart;		/**< Port is USB */
+	bool        		_wait_to_transmit;  	/**< Wait to transmit until received messages. */
+	bool        		_received_messages;	/**< Whether we've received valid mavlink messages. */
 
-	unsigned	_main_loop_delay;		/**< mainloop delay, depends on data rate */
+	unsigned		_main_loop_delay;	/**< mainloop delay, depends on data rate */
 
-	MavlinkOrbSubscription *_subscriptions;
-	MavlinkStream *_streams;
+	MavlinkOrbSubscription	*_subscriptions;
+	MavlinkStream		*_streams;
 
-	orb_advert_t	_mission_pub;
-	struct mission_s mission;
-	MAVLINK_MODE _mode;
+	orb_advert_t		_mission_pub;
+	struct mission_s	mission;
+	MAVLINK_MODE		_mode;
 
-	uint8_t _mavlink_wpm_comp_id;
-	mavlink_channel_t _channel;
+	uint8_t			_mavlink_wpm_comp_id;
+	mavlink_channel_t	_channel;
 
 	struct mavlink_logbuffer _logbuffer;
-	unsigned int _total_counter;
+	unsigned int		_total_counter;
 
-	pthread_t _receive_thread;
+	pthread_t		_receive_thread;
 
 	/* Allocate storage space for waypoints */
-	mavlink_wpm_storage _wpm_s;
-	mavlink_wpm_storage *_wpm;
+	mavlink_wpm_storage	_wpm_s;
+	mavlink_wpm_storage	*_wpm;
 
-	bool _verbose;
-	bool _forwarding_on;
-	bool _passing_on;
-	int _uart_fd;
-	int _baudrate;
-	int _datarate;
+	bool			_verbose;
+	bool			_forwarding_on;
+	bool			_passing_on;
+	bool			_ftp_on;
+	int			_uart_fd;
+	int			_baudrate;
+	int			_datarate;
 
 	/**
 	 * If the queue index is not at 0, the queue sending
 	 * logic will send parameters from the current index
 	 * to len - 1, the end of the param list.
 	 */
-	unsigned int _mavlink_param_queue_index;
+	unsigned int		_mavlink_param_queue_index;
 
-	bool mavlink_link_termination_allowed;
+	bool			mavlink_link_termination_allowed;
 
-	char 	*_subscribe_to_stream;
-	float	_subscribe_to_stream_rate;
+	char 			*_subscribe_to_stream;
+	float			_subscribe_to_stream_rate;
 
-	bool		_flow_control_enabled;
+	bool			_flow_control_enabled;
 
 	struct mavlink_message_buffer {
 		int write_ptr;
@@ -277,11 +306,14 @@ private:
 		int size;
 		char *data;
 	};
+
 	mavlink_message_buffer	_message_buffer;
 
 	pthread_mutex_t		_message_buffer_mutex;
 
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t		_loop_perf;			/**< loop performance counter */
+	perf_counter_t		_txerr_perf;			/**< TX error counter */
+
 	bool			_param_initialized;
 	param_t			_param_system_id;
 	param_t			_param_component_id;
@@ -294,7 +326,7 @@ private:
 	 * @param param		The parameter id to send.
 	 * @return		zero on success, nonzero on failure.
 	 */
-	int mavlink_pm_send_param(param_t param);
+	int			mavlink_pm_send_param(param_t param);
 
 	/**
 	 * Send one parameter identified by index.
@@ -302,7 +334,7 @@ private:
 	 * @param index		The index of the parameter to send.
 	 * @return		zero on success, nonzero else.
 	 */
-	int mavlink_pm_send_param_for_index(uint16_t index);
+	int			mavlink_pm_send_param_for_index(uint16_t index);
 
 	/**
 	 * Send one parameter identified by name.
@@ -310,14 +342,14 @@ private:
 	 * @param name		The index of the parameter to send.
 	 * @return		zero on success, nonzero else.
 	 */
-	int mavlink_pm_send_param_for_name(const char *name);
+	int			mavlink_pm_send_param_for_name(const char *name);
 
 	/**
 	 * Send a queue of parameters, one parameter per function call.
 	 *
 	 * @return		zero on success, nonzero on failure
 	 */
-	int mavlink_pm_queued_send(void);
+	int			mavlink_pm_queued_send(void);
 
 	/**
 	 * Start sending the parameter queue.
@@ -327,12 +359,12 @@ private:
 	 * mavlink_pm_queued_send().
 	 * @see 		mavlink_pm_queued_send()
 	 */
-	void mavlink_pm_start_queued_send();
+	void			mavlink_pm_start_queued_send();
 
-	void mavlink_update_system();
+	void			mavlink_update_system();
 
-	void mavlink_waypoint_eventloop(uint64_t now);
-	void mavlink_wpm_send_waypoint_reached(uint16_t seq);
+	void			mavlink_waypoint_eventloop(uint64_t now);
+	void 			mavlink_wpm_send_waypoint_reached(uint16_t seq);
 	void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq);
 	void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq);
 	void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count);
@@ -349,7 +381,6 @@ private:
 	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
 
 	int configure_stream(const char *stream_name, const float rate);
-	void configure_stream_threadsafe(const char *stream_name, const float rate);
 
 	int message_buffer_init(int size);
 
@@ -359,8 +390,6 @@ private:
 
 	int message_buffer_is_empty();
 
-	bool message_buffer_write(void *ptr, int size);
-
 	int message_buffer_get_ptr(void **ptr, bool *is_part);
 
 	void message_buffer_mark_read(int n);
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index b295bf35f..e1ebc16cc 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -120,50 +120,77 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 	union px4_custom_mode custom_mode;
 	custom_mode.data = 0;
 
-	if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
-		/* use main state when navigator is not active */
-		if (status->main_state == MAIN_STATE_MANUAL) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+	switch (status->nav_state) {
+
+		case NAVIGATION_STATE_MANUAL:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+			                      | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+			break;
+
+		case NAVIGATION_STATE_ACRO:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
+			break;
 
-		} else if (status->main_state == MAIN_STATE_ALTCTL) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
+		case NAVIGATION_STATE_ALTCTL:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
+			break;
 
-		} else if (status->main_state == MAIN_STATE_POSCTL) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		case NAVIGATION_STATE_POSCTL:
+			*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_POSCTL;
+			break;
 
-		} else if (status->main_state == MAIN_STATE_AUTO) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		case NAVIGATION_STATE_AUTO_MISSION:
+			*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 if (status->main_state == MAIN_STATE_ACRO) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
-		}
-
-	} 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 (pos_sp_triplet->nav_state == NAV_STATE_READY) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+			break;
 
-		} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
+		case NAVIGATION_STATE_AUTO_LOITER:
+			*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_LOITER;
+			break;
 
-		} else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
-
-		} else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) {
+		case NAVIGATION_STATE_AUTO_RTL:
+			*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_RTL;
+			break;
 
-		} else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) {
+		case NAVIGATION_STATE_LAND:
+			*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_LAND;
-		}
+			break;
+
+		case NAVIGATION_STATE_AUTO_RTGS:
+			*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_RTGS;
+			break;
+
+		case NAVIGATION_STATE_TERMINATION:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+			break;
+
 	}
 
 	*mavlink_custom_mode = custom_mode.data;
@@ -205,6 +232,11 @@ public:
 		return "HEARTBEAT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HEARTBEAT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamHeartbeat();
@@ -227,8 +259,15 @@ protected:
 		struct position_setpoint_triplet_s pos_sp_triplet;
 
 		/* always send the heartbeat, independent of the update status of the topics */
-		(void)status_sub->update(&status);
-		(void)pos_sp_triplet_sub->update(&pos_sp_triplet);
+		if (!status_sub->update(&status)) {
+			/* if topic update failed fill it with defaults */
+			memset(&status, 0, sizeof(status));
+		}
+
+		if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+			/* if topic update failed fill it with defaults */
+			memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
+		}
 
 		uint8_t mavlink_state = 0;
 		uint8_t mavlink_base_mode = 0;
@@ -258,6 +297,11 @@ public:
 		return "SYS_STATUS";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_SYS_STATUS;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamSysStatus();
@@ -309,6 +353,11 @@ public:
 		return "HIGHRES_IMU";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HIGHRES_IMU;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamHighresIMU();
@@ -394,6 +443,11 @@ public:
 		return "ATTITUDE";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ATTITUDE;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamAttitude();
@@ -440,6 +494,11 @@ public:
 		return "ATTITUDE_QUATERNION";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamAttitudeQuaternion();
@@ -492,6 +551,11 @@ public:
 		return "VFR_HUD";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_VFR_HUD;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamVFRHUD();
@@ -575,6 +639,11 @@ public:
 		return "GPS_RAW_INT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GPS_RAW_INT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGPSRawInt();
@@ -605,8 +674,8 @@ protected:
 						     gps.lat,
 						     gps.lon,
 						     gps.alt,
-						     cm_uint16_from_m_float(gps.eph_m),
-						     cm_uint16_from_m_float(gps.epv_m),
+						     cm_uint16_from_m_float(gps.eph),
+						     cm_uint16_from_m_float(gps.epv),
 						     gps.vel_m_s * 100.0f,
 						     _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
 						     gps.satellites_visible);
@@ -628,6 +697,11 @@ public:
 		return "GLOBAL_POSITION_INT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGlobalPositionInt();
@@ -689,6 +763,11 @@ public:
 		return "LOCAL_POSITION_NED";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamLocalPositionNED();
@@ -740,6 +819,11 @@ public:
 		return "VICON_POSITION_ESTIMATE";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamViconPositionEstimate();
@@ -790,6 +874,11 @@ public:
 		return "GPS_GLOBAL_ORIGIN";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGPSGlobalOrigin();
@@ -830,6 +919,11 @@ public:
 		return MavlinkStreamServoOutputRaw<N>::get_name_static();
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
+	}
+
 	static const char *get_name_static()
 	{
 		switch (N) {
@@ -907,6 +1001,11 @@ public:
 		return "HIL_CONTROLS";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HIL_CONTROLS;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamHILControls();
@@ -1044,6 +1143,11 @@ public:
 		return "GLOBAL_POSITION_SETPOINT_INT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGlobalPositionSetpointInt();
@@ -1087,6 +1191,11 @@ public:
 		return "LOCAL_POSITION_SETPOINT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamLocalPositionSetpoint();
@@ -1135,6 +1244,11 @@ public:
 		return "ROLL_PITCH_YAW_THRUST_SETPOINT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamRollPitchYawThrustSetpoint();
@@ -1183,6 +1297,11 @@ public:
 		return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
@@ -1231,6 +1350,11 @@ public:
 		return "RC_CHANNELS_RAW";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_RC_CHANNELS_RAW;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamRCChannelsRaw();
@@ -1315,6 +1439,11 @@ public:
 		return "MANUAL_CONTROL";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_MANUAL_CONTROL;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamManualControl();
@@ -1364,6 +1493,11 @@ public:
 		return "OPTICAL_FLOW";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_OPTICAL_FLOW;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamOpticalFlow();
@@ -1412,6 +1546,11 @@ public:
 		return "ATTITUDE_CONTROLS";
 	}
 
+	uint8_t get_id()
+	{
+		return 0;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamAttitudeControls();
@@ -1470,6 +1609,11 @@ public:
 		return "NAMED_VALUE_FLOAT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamNamedValueFloat();
@@ -1518,6 +1662,11 @@ public:
 		return "CAMERA_CAPTURE";
 	}
 
+	uint8_t get_id()
+	{
+		return 0;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamCameraCapture();
@@ -1563,6 +1712,11 @@ public:
 		return "DISTANCE_SENSOR";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_DISTANCE_SENSOR;
+	}
+
 	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamDistanceSensor();
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 9c528adbe..bb977d277 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -106,12 +106,17 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
+	_telemetry_heartbeat_time(0),
+	_radio_status_available(false),
 	_hil_frames(0),
 	_old_timestamp(0),
 	_hil_local_proj_inited(0),
 	_hil_local_alt0(0.0)
 {
 	memset(&hil_local_pos, 0, sizeof(hil_local_pos));
+
+	// make sure the FTP server is started
+	(void)MavlinkFTP::getServer();
 }
 
 MavlinkReceiver::~MavlinkReceiver()
@@ -150,6 +155,18 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_manual_control(msg);
 		break;
 
+	case MAVLINK_MSG_ID_HEARTBEAT:
+		handle_message_heartbeat(msg);
+		break;
+
+	case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
+		handle_message_request_data_stream(msg);
+		break;
+
+	case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
+		MavlinkFTP::getServer()->handle_message(_mavlink, msg);
+		break;
+
 	default:
 		break;
 	}
@@ -411,6 +428,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 	memset(&tstatus, 0, sizeof(tstatus));
 
 	tstatus.timestamp = hrt_absolute_time();
+	tstatus.heartbeat_time = _telemetry_heartbeat_time;
 	tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
 	tstatus.rssi = rstatus.rssi;
 	tstatus.remote_rssi = rstatus.remrssi;
@@ -426,6 +444,9 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 	} else {
 		orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
 	}
+
+	/* this means that heartbeats alone won't be published to the radio status no more */
+	_radio_status_available = true;
 }
 
 void
@@ -452,6 +473,54 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 }
 
 void
+MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
+{
+	mavlink_heartbeat_t hb;
+	mavlink_msg_heartbeat_decode(msg, &hb);
+
+	/* ignore own heartbeats, accept only heartbeats from GCS */
+	if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
+		_telemetry_heartbeat_time = hrt_absolute_time();
+
+		/* if no radio status messages arrive, lets at least publish that heartbeats were received */
+		if (!_radio_status_available) {
+
+			struct telemetry_status_s tstatus;
+			memset(&tstatus, 0, sizeof(tstatus));
+
+			tstatus.timestamp = _telemetry_heartbeat_time;
+			tstatus.heartbeat_time = _telemetry_heartbeat_time;
+			tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+
+			if (_telemetry_status_pub < 0) {
+				_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
+
+			} else {
+				orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
+			}
+		}
+	}
+}
+
+void
+MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
+{
+	mavlink_request_data_stream_t req;
+	mavlink_msg_request_data_stream_decode(msg, &req);
+
+	if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid) {
+		float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f;
+
+		MavlinkStream *stream;
+		LL_FOREACH(_mavlink->get_streams(), stream) {
+			if (req.req_stream_id == stream->get_id()) {
+				_mavlink->configure_stream_threadsafe(stream->get_name(), rate);
+			}
+		}
+	}
+}
+
+void
 MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 {
 	mavlink_hil_sensor_t imu;
@@ -667,12 +736,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	hil_gps.lat = gps.lat;
 	hil_gps.lon = gps.lon;
 	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.eph = (float)gps.eph * 1e-2f; // from cm to m
+	hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m
 
 	hil_gps.timestamp_variance = timestamp;
 	hil_gps.s_variance_m_s = 5.0f;
-	hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
+	hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph;
 
 	hil_gps.timestamp_velocity = timestamp;
 	hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 9ab84b58a..040a07480 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -68,6 +68,8 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 
+#include "mavlink_ftp.h"
+
 class Mavlink;
 
 class MavlinkReceiver
@@ -112,6 +114,8 @@ private:
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
+	void handle_message_heartbeat(mavlink_message_t *msg);
+	void handle_message_request_data_stream(mavlink_message_t *msg);
 	void handle_message_hil_sensor(mavlink_message_t *msg);
 	void handle_message_hil_gps(mavlink_message_t *msg);
 	void handle_message_hil_state_quaternion(mavlink_message_t *msg);
@@ -138,6 +142,8 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
+	hrt_abstime _telemetry_heartbeat_time;
+	bool _radio_status_available;
 	int _hil_frames;
 	uint64_t _old_timestamp;
 	bool _hil_local_proj_inited;
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index a41ace48e..69809a386 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -67,6 +67,7 @@ public:
 	static const char *get_name_static();
 	virtual void subscribe(Mavlink *mavlink) = 0;
 	virtual const char *get_name() const = 0;
+	virtual uint8_t get_id() = 0;
 
 protected:
 	mavlink_channel_t _channel;
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index f532e26fe..a4d8bfbfb 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -43,7 +43,8 @@ SRCS		 += mavlink_main.cpp \
 			mavlink_messages.cpp \
 			mavlink_stream.cpp \
 			mavlink_rate_limiter.cpp \
-			mavlink_commands.cpp
+			mavlink_commands.cpp \
+			mavlink_ftp.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index bc8dbca50..266215308 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -78,7 +78,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
 {
 	double lat = vehicle->lat / 1e7d;
 	double lon = vehicle->lon / 1e7d;
-	float	alt = vehicle->alt;
+	//float	alt = vehicle->alt;
 
 	return inside(lat, lon, vehicle->alt);
 }
@@ -116,9 +116,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
 				}
 
 				// 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)) {
+				if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) &&
+							(lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) /
+							 (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) {
 							c = !c;
 				}
 
@@ -294,4 +294,5 @@ Geofence::loadFromFile(const char *filename)
 int Geofence::clearDm()
 {
 	dm_clear(DM_KEY_FENCE_POINTS);
+	return OK;
 }
diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
new file mode 100644
index 000000000..542483fb1
--- /dev/null
+++ b/src/modules/navigator/loiter.cpp
@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file loiter.cpp
+ *
+ * Helper class to loiter
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+
+#include "loiter.h"
+
+Loiter::Loiter(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+Loiter::~Loiter()
+{
+}
+
+bool
+Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	/* set loiter item, don't reuse an existing position setpoint */
+	return set_loiter_item(pos_sp_triplet);
+}
+
+void
+Loiter::on_inactive()
+{
+}
diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h
new file mode 100644
index 000000000..65ff5c31e
--- /dev/null
+++ b/src/modules/navigator/loiter.h
@@ -0,0 +1,74 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file loiter.h
+ *
+ * Helper class to loiter
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef NAVIGATOR_LOITER_H
+#define NAVIGATOR_LOITER_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Loiter : public MissionBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Loiter(Navigator *navigator, const char *name);
+
+	/**
+	 * Destructor
+	 */
+	~Loiter();
+
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	virtual void on_inactive();
+
+	/**
+	 * This function is called while the mode is active
+	 */
+	virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
+};
+
+#endif
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
new file mode 100644
index 000000000..72255103b
--- /dev/null
+++ b/src/modules/navigator/mission.cpp
@@ -0,0 +1,461 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file navigator_mission.cpp
+ *
+ * Helper class to access missions
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <sys/types.h>
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <drivers/drv_hrt.h>
+
+#include <dataman/dataman.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+
+#include "navigator.h"
+#include "mission.h"
+
+Mission::Mission(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_param_onboard_enabled(this, "ONBOARD_EN"),
+	_onboard_mission({0}),
+	_offboard_mission({0}),
+	_current_onboard_mission_index(-1),
+	_current_offboard_mission_index(-1),
+	_mission_result_pub(-1),
+	_mission_result({0}),
+	_mission_type(MISSION_TYPE_NONE)
+{
+	/* load initial params */
+	updateParams();
+	/* set initial mission items */
+	on_inactive();
+
+}
+
+Mission::~Mission()
+{
+}
+
+void
+Mission::on_inactive()
+{
+	_first_run = true;
+
+	/* check anyway if missions have changed so that feedback to groundstation is given */
+	bool onboard_updated;
+	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
+	if (onboard_updated) {
+		update_onboard_mission();
+	}
+
+	bool offboard_updated;
+	orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
+	if (offboard_updated) {
+		update_offboard_mission();
+	}
+}
+
+bool
+Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	bool updated = false;
+
+	/* check if anything has changed */
+	bool onboard_updated;
+	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
+	if (onboard_updated) {
+		update_onboard_mission();
+	}
+
+	bool offboard_updated;
+	orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
+	if (offboard_updated) {
+		update_offboard_mission();
+	}
+
+	/* reset mission items if needed */
+	if (onboard_updated || offboard_updated || _first_run) {
+		set_mission_items(pos_sp_triplet);
+		updated = true;
+		_first_run = false;
+	}
+
+	/* lets check if we reached the current mission item */
+	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
+		advance_mission();
+		set_mission_items(pos_sp_triplet);
+		updated = true;
+	}
+
+	return updated;
+}
+
+void
+Mission::update_onboard_mission()
+{
+	if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) {
+		/* accept the current index set by the onboard mission if it is within bounds */
+		if (_onboard_mission.current_index >=0
+		&& _onboard_mission.current_index < (int)_onboard_mission.count) {
+			_current_onboard_mission_index = _onboard_mission.current_index;
+		} else {
+			/* if less WPs available, reset to first WP */
+			if (_current_onboard_mission_index >= (int)_onboard_mission.count) {
+				_current_onboard_mission_index = 0;
+			/* if not initialized, set it to 0 */
+			} else if (_current_onboard_mission_index < 0) {
+				_current_onboard_mission_index = 0;
+			}
+			/* otherwise, just leave it */
+		}
+	} else {
+		_onboard_mission.count = 0;
+		_onboard_mission.current_index = 0;
+		_current_onboard_mission_index = 0;
+	}
+}
+
+void
+Mission::update_offboard_mission()
+{
+	if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
+
+		/* determine current index */
+		if (_offboard_mission.current_index >= 0
+		    && _offboard_mission.current_index < (int)_offboard_mission.count) {
+			_current_offboard_mission_index = _offboard_mission.current_index;
+		} else {
+			/* if less WPs available, reset to first WP */
+			if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
+				_current_offboard_mission_index = 0;
+			/* if not initialized, set it to 0 */
+			} else if (_current_offboard_mission_index < 0) {
+				_current_offboard_mission_index = 0;
+			}
+			/* otherwise, just leave it */
+		}
+
+		/* 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;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+
+		missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current,
+			                                      (size_t)_offboard_mission.count,
+							      _navigator->get_geofence(),
+							      _navigator->get_home_position()->alt);
+	} else {
+		_offboard_mission.count = 0;
+		_offboard_mission.current_index = 0;
+		_current_offboard_mission_index = 0;
+	}
+	report_current_offboard_mission_item();
+}
+
+
+void
+Mission::advance_mission()
+{
+	switch (_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;
+	}
+}
+
+void
+Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	set_previous_pos_setpoint(pos_sp_triplet);
+
+	/* try setting onboard mission item */
+	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
+		/* if mission type changed, notify */
+		if (_mission_type != MISSION_TYPE_ONBOARD) {
+			mavlink_log_info(_navigator->get_mavlink_fd(),
+					"#audio: onboard mission running");
+		}
+		_mission_type = MISSION_TYPE_ONBOARD;
+		_navigator->set_can_loiter_at_sp(false);
+
+	/* try setting offboard mission item */
+	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
+		/* if mission type changed, notify */
+		if (_mission_type != MISSION_TYPE_OFFBOARD) {
+			mavlink_log_info(_navigator->get_mavlink_fd(),
+					"#audio: offboard mission running");
+		}
+		_mission_type = MISSION_TYPE_OFFBOARD;
+		_navigator->set_can_loiter_at_sp(false);
+	} else {
+		if (_mission_type != MISSION_TYPE_NONE) {
+			mavlink_log_info(_navigator->get_mavlink_fd(),
+					"#audio: mission finished");
+		} else {
+			mavlink_log_info(_navigator->get_mavlink_fd(),
+					"#audio: no mission available");
+		}
+		_mission_type = MISSION_TYPE_NONE;
+        _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
+
+		set_loiter_item(pos_sp_triplet);
+		reset_mission_item_reached();
+		report_mission_finished();
+	}
+}
+
+bool
+Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
+{
+	/* make sure param is up to date */
+	updateParams();
+	if (_param_onboard_enabled.get() > 0 &&
+	    _current_onboard_mission_index >= 0&&
+	    _current_onboard_mission_index < (int)_onboard_mission.count) {
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index,
+					&new_mission_item)) {
+			/* convert the current mission item and set it valid */
+			mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
+			current_pos_sp->valid = true;
+
+			reset_mission_item_reached();
+
+			/* TODO: report this somehow */
+			memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
+			return true;
+		}
+	}
+	return false;
+}
+
+bool
+Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
+{
+	if (_current_offboard_mission_index >= 0 &&
+	    _current_offboard_mission_index < (int)_offboard_mission.count) {
+		dm_item_t dm_current;
+		if (_offboard_mission.dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) {
+			/* convert the current mission item and set it valid */
+			mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
+			current_pos_sp->valid = true;
+
+			reset_mission_item_reached();
+
+			report_current_offboard_mission_item();
+			memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
+			return true;
+		}
+	}
+	return false;
+}
+
+void
+Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp)
+{
+	int next_temp_mission_index = _onboard_mission.current_index + 1;
+
+	/* try if there is a next onboard mission */
+	if (_onboard_mission.current_index >= 0 &&
+	    next_temp_mission_index < (int)_onboard_mission.count) {
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) {
+			/* convert next mission item to position setpoint */
+			mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
+			next_pos_sp->valid = true;
+			return;
+		}
+	}
+
+	/* give up */
+	next_pos_sp->valid = false;
+	return;
+}
+
+void
+Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp)
+{
+	/* try if there is a next offboard mission */
+	int next_temp_mission_index = _offboard_mission.current_index + 1;
+	warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count);
+	if (_offboard_mission.current_index >= 0 &&
+	    next_temp_mission_index < (int)_offboard_mission.count) {
+		dm_item_t dm_current;
+		if (_offboard_mission.dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) {
+			/* convert next mission item to position setpoint */
+			mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
+			next_pos_sp->valid = true;
+			return;
+		}
+	}
+	/* give up */
+	next_pos_sp->valid = false;
+	return;
+}
+
+bool
+Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
+		           struct mission_item_s *new_mission_item)
+{
+	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
+	for (int i=0; i<10; i++) {
+		const ssize_t len = sizeof(struct mission_item_s);
+
+		/* read mission item from datamanager */
+		if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			mavlink_log_critical(_navigator->get_mavlink_fd(),
+			                     "#audio: ERROR waypoint could not be read");
+			return false;
+		}
+
+		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
+		if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
+
+			/* do DO_JUMP as many times as requested */
+			if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) {
+
+				/* only raise the repeat count if this is for the current mission item
+				* but not for the next mission item */
+				if (is_current) {
+					(new_mission_item->do_jump_current_count)++;
+					/* save repeat count */
+					if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET,
+						new_mission_item, len) != len) {
+						/* not supposed to happen unless the datamanager can't access the
+						 * dataman */
+						mavlink_log_critical(_navigator->get_mavlink_fd(),
+								"#audio: ERROR DO JUMP waypoint could not be written");
+						return false;
+					}
+				}
+				/* set new mission item index and repeat
+				* we don't have to validate here, if it's invalid, we should realize this later .*/
+				*mission_index = new_mission_item->do_jump_mission_index;
+			} else {
+				mavlink_log_info(_navigator->get_mavlink_fd(),
+						 "#audio: DO JUMP repetitions completed");
+				/* no more DO_JUMPS, therefore just try to continue with next mission item */
+				(*mission_index)++;
+			}
+
+		} else {
+			/* if it's not a DO_JUMP, then we were successful */
+			return true;
+		}
+	}
+
+	/* we have given up, we don't want to cycle forever */
+	mavlink_log_critical(_navigator->get_mavlink_fd(),
+			     "#audio: ERROR DO JUMP is cycling, giving up");
+	return false;
+}
+
+void
+Mission::report_mission_item_reached()
+{
+	if (_mission_type == MISSION_TYPE_OFFBOARD) {
+		_mission_result.mission_reached = true;
+		_mission_result.mission_index_reached = _current_offboard_mission_index;
+	}
+	publish_mission_result();
+}
+
+void
+Mission::report_current_offboard_mission_item()
+{
+	_mission_result.index_current_mission = _current_offboard_mission_index;
+	publish_mission_result();
+}
+
+void
+Mission::report_mission_finished()
+{
+	_mission_result.mission_finished = true;
+	publish_mission_result();
+}
+
+void
+Mission::publish_mission_result()
+{
+	/* lazily publish the mission result only once available */
+	if (_mission_result_pub > 0) {
+		/* publish mission result */
+		orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
+
+	} else {
+		/* advertise and publish */
+		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
+	}
+	/* reset reached bool */
+	_mission_result.mission_reached = false;
+	_mission_result.mission_finished = false;
+}
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
new file mode 100644
index 000000000..6e4761946
--- /dev/null
+++ b/src/modules/navigator/mission.h
@@ -0,0 +1,178 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file mission.h
+ *
+ * Navigator mode to access missions
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef NAVIGATOR_MISSION_H
+#define NAVIGATOR_MISSION_H
+
+#include <drivers/drv_hrt.h>
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <dataman/dataman.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+#include "mission_feasibility_checker.h"
+
+class Navigator;
+
+class Mission : public MissionBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Mission(Navigator *navigator, const char *name);
+
+	/**
+	 * Destructor
+	 */
+	virtual ~Mission();
+
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	virtual void on_inactive();
+
+	/**
+	 * This function is called while the mode is active
+	 */
+	virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
+
+private:
+	/**
+	 * Update onboard mission topic
+	 */
+	void update_onboard_mission();
+
+	/**
+	 * Update offboard mission topic
+	 */
+	void update_offboard_mission();
+
+	/**
+	 * Move on to next mission item or switch to loiter
+	 */
+	void advance_mission();
+
+	/**
+	 * Set new mission items
+	 */
+	void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
+
+	/**
+	 * Try to set the current position setpoint from an onboard mission item
+	 * @return true if mission item successfully set
+	 */
+	bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
+
+	/**
+	 * Try to set the current position setpoint from an offboard mission item
+	 * @return true if mission item successfully set
+	 */
+	bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
+
+	/**
+	 * Try to set the next position setpoint from an onboard mission item
+	 */
+	void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp);
+
+	/**
+	 * Try to set the next position setpoint from an offboard mission item
+	 */
+	void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp);
+
+	/**
+	 * Read a mission item from the dataman and watch out for DO_JUMPS
+	 * @return true if successful
+	 */
+	bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
+			       struct mission_item_s *new_mission_item);
+
+	/**
+	 * Report that a mission item has been reached
+	 */
+	void report_mission_item_reached();
+
+	/**
+	 * Rport the current mission item
+	 */
+	void report_current_offboard_mission_item();
+
+	/**
+	 * Report that the mission is finished if one exists or that none exists
+	 */
+	void report_mission_finished();
+
+	/**
+	 * Publish the mission result so commander and mavlink know what is going on
+	 */
+	void publish_mission_result();
+
+	control::BlockParamFloat _param_onboard_enabled;
+
+	struct mission_s _onboard_mission;
+	struct mission_s _offboard_mission;
+
+	int _current_onboard_mission_index;
+	int _current_offboard_mission_index;
+
+	orb_advert_t _mission_result_pub;
+	struct mission_result_s _mission_result;
+
+	enum {
+		MISSION_TYPE_NONE,
+		MISSION_TYPE_ONBOARD,
+		MISSION_TYPE_OFFBOARD
+	} _mission_type;
+
+	MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
+};
+
+#endif
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
new file mode 100644
index 000000000..9b8d3d9c7
--- /dev/null
+++ b/src/modules/navigator/mission_block.cpp
@@ -0,0 +1,242 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file mission_block.cpp
+ *
+ * Helper class to use mission items
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <sys/types.h>
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+
+#include "navigator.h"
+#include "mission_block.h"
+
+
+MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
+	NavigatorMode(navigator, name),
+	_waypoint_position_reached(false),
+	_waypoint_yaw_reached(false),
+	_time_first_inside_orbit(0),
+	_mission_item({0}),
+	_mission_item_valid(false)
+{
+}
+
+MissionBlock::~MissionBlock()
+{
+}
+
+bool
+MissionBlock::is_mission_item_reached()
+{
+	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
+		return _navigator->get_vstatus()->condition_landed;
+	}
+
+	/* TODO: count turns */
+	if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) {
+		return false;
+	}
+
+	hrt_abstime now = hrt_absolute_time();
+
+	if (!_waypoint_position_reached) {
+
+		float dist = -1.0f;
+		float dist_xy = -1.0f;
+		float dist_z = -1.0f;
+
+		float altitude_amsl = _mission_item.altitude_is_relative
+				      ? _mission_item.altitude + _navigator->get_home_position()->alt
+			              : _mission_item.altitude;
+
+		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
+				                          _navigator->get_global_position()->lat,
+							  _navigator->get_global_position()->lon,
+							  _navigator->get_global_position()->alt,
+				&dist_xy, &dist_z);
+
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) {
+			/* require only altitude for takeoff for multicopter */
+			if (_navigator->get_global_position()->alt >
+				altitude_amsl - _navigator->get_acceptance_radius()) {
+				_waypoint_position_reached = true;
+			}
+		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
+				_waypoint_position_reached = true;
+			}
+		} else {
+			/* for normal mission items used their acceptance radius */
+			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
+				_waypoint_position_reached = true;
+			}
+		}
+	}
+
+	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
+
+		/* TODO: removed takeoff, why? */
+		if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
+
+			/* check yaw if defined only for rotary wing except takeoff */
+			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
+
+			if (fabsf(yaw_err) < 0.2f) { /* TODO: 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, "#audio: waypoint reached, wait for %.1fs",
+			// 		(double)_mission_item.time_inside);
+			// }
+		}
+
+		/* check if the MAV was long enough inside the waypoint orbit */
+		if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
+			return true;
+		}
+	}
+	return false;
+}
+
+void
+MissionBlock::reset_mission_item_reached()
+{
+	_waypoint_position_reached = false;
+	_waypoint_yaw_reached = false;
+	_time_first_inside_orbit = 0;
+}
+
+void
+MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
+{
+	sp->valid = true;
+	sp->lat = item->lat;
+	sp->lon = item->lon;
+	sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->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;
+
+	switch (item->nav_cmd) {
+	case NAV_CMD_IDLE:
+		sp->type = SETPOINT_TYPE_IDLE;
+		break;
+
+	case NAV_CMD_TAKEOFF:
+		sp->type = SETPOINT_TYPE_TAKEOFF;
+		break;
+
+	case NAV_CMD_LAND:
+		sp->type = SETPOINT_TYPE_LAND;
+		break;
+
+	case NAV_CMD_LOITER_TIME_LIMIT:
+	case NAV_CMD_LOITER_TURN_COUNT:
+	case NAV_CMD_LOITER_UNLIMITED:
+		sp->type = SETPOINT_TYPE_LOITER;
+		break;
+
+	default:
+		sp->type = SETPOINT_TYPE_POSITION;
+		break;
+	}
+}
+
+void
+MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+    /* reuse current setpoint as previous setpoint */
+    if (pos_sp_triplet->current.valid) {
+        memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
+    }
+}
+
+bool
+MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+    /* don't change setpoint if 'can_loiter_at_sp' flag set */
+	if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
+		/* use current position */
+		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
+		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
+		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
+		pos_sp_triplet->current.yaw = NAN;	/* NAN means to use current yaw */
+
+	    _navigator->set_can_loiter_at_sp(true);
+	}
+
+    if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
+            || pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius()
+            || pos_sp_triplet->current.loiter_direction != 1
+            || pos_sp_triplet->previous.valid
+            || !pos_sp_triplet->current.valid
+            || pos_sp_triplet->next.valid) {
+        /* position setpoint triplet should be updated */
+        pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
+        pos_sp_triplet->current.loiter_radius = _navigator->get_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;
+        return true;
+    }
+
+    return false;
+}
diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/mission_block.h
index b0f88e016..f99002752 100644
--- a/src/modules/navigator/navigator_mission.h
+++ b/src/modules/navigator/mission_block.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 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
@@ -31,75 +31,76 @@
  *
  ****************************************************************************/
 /**
- * @file navigator_mission.h
- * Helper class to access missions
+ * @file mission_block.h
  *
- * @author Julian Oes <joes@student.ethz.ch>
+ * Helper class to use mission items
+ *
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#ifndef NAVIGATOR_MISSION_H
-#define NAVIGATOR_MISSION_H
+#ifndef NAVIGATOR_MISSION_BLOCK_H
+#define NAVIGATOR_MISSION_BLOCK_H
+
+#include <drivers/drv_hrt.h>
 
 #include <uORB/topics/mission.h>
-#include <uORB/topics/mission_result.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+
+#include "navigator_mode.h"
 
+class Navigator;
 
-class __EXPORT Mission
+class MissionBlock : public NavigatorMode
 {
 public:
 	/**
 	 * Constructor
 	 */
-	Mission();
+	MissionBlock(Navigator *navigator, const char *name);
 
 	/**
-	 * Destructor, also kills the sensors task.
+	 * Destructor
 	 */
-	~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		report_mission_item_reached();
-	void		report_current_offboard_mission_item();
-	void		publish_mission_result();
+	virtual ~MissionBlock();
 
-private:
-	bool		current_onboard_mission_available();
-	bool		current_offboard_mission_available();
-	bool		next_onboard_mission_available();
-	bool		next_offboard_mission_available();
+	/**
+	 * Check if mission item has been reached
+	 * @return true if successfully reached
+	 */
+	bool is_mission_item_reached();
+	/**
+	 * Reset all reached flags
+	 */
+	void reset_mission_item_reached();
 
-	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 */
+	/**
+	 * Convert a mission item to a position setpoint
+	 *
+	 * @param the mission item to convert
+	 * @param the position setpoint that needs to be set
+	 */
+	void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
 
-	bool		_onboard_mission_allowed;
+    /**
+     * Set previous position setpoint to current setpoint
+     */
+	void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet);
 
-	enum {
-		MISSION_TYPE_NONE,
-		MISSION_TYPE_ONBOARD,
-		MISSION_TYPE_OFFBOARD,
-	} 		_current_mission_type;
+	/**
+	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
+	 *
+	 * @param the position setpoint triplet to set
+	 * @return true if setpoint has changed
+	 */
+	bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet);
 
-	int		_mission_result_pub;
+	bool _waypoint_position_reached;
+	bool _waypoint_yaw_reached;
+	hrt_abstime _time_first_inside_orbit;
 
-	struct mission_result_s _mission_result;
+	mission_item_s _mission_item;
+	bool _mission_item_valid;
 };
 
 #endif
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index e1a6854b2..dd7f4c801 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -215,11 +215,12 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
 
 
 //	float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
+	return false;
 }
 
 void MissionFeasibilityChecker::updateNavigationCapabilities()
 {
-	int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
+	(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
 }
 
 void MissionFeasibilityChecker::init()
diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/mission_params.c
index 476f93414..8692328db 100644
--- a/src/modules/navigator/navigator_state.h
+++ b/src/modules/navigator/mission_params.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 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,24 +32,38 @@
  ****************************************************************************/
 
 /**
- * @file navigator_state.h
+ * @file mission_params.c
  *
- * Navigator state
+ * Parameters for mission.
  *
- * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
-#ifndef NAVIGATOR_STATE_H_
-#define NAVIGATOR_STATE_H_
+#include <nuttx/config.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;
+#include <systemlib/param/param.h>
 
-#endif /* NAVIGATOR_STATE_H_ */
+/*
+ * Mission parameters, accessible via MAVLink
+ */
+
+/**
+ * Take-off altitude
+ *
+ * Even if first waypoint has altitude less then MIS_TAKEOFF_ALT above home position, system will climb to
+ * MIS_TAKEOFF_ALT on takeoff, then go to waypoint.
+ *
+ * @unit meters
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
+
+
+/**
+ * Enable onboard mission
+ *
+ * @min 0
+ * @max 1
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0);
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 6ea9dec2b..a1e109030 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 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
@@ -39,7 +39,13 @@ MODULE_COMMAND	= navigator
 
 SRCS		= navigator_main.cpp \
 		  navigator_params.c \
-		  navigator_mission.cpp \
+		  navigator_mode.cpp \
+		  mission_block.cpp \
+		  mission.cpp \
+		  mission_params.c \
+		  loiter.cpp \
+		  rtl.cpp \
+		  rtl_params.c \
 		  mission_feasibility_checker.cpp \
 		  geofence.cpp \
 		  geofence_params.c
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
new file mode 100644
index 000000000..184ecd365
--- /dev/null
+++ b/src/modules/navigator/navigator.h
@@ -0,0 +1,219 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file navigator.h
+ * Helper class to access missions
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef NAVIGATOR_H
+#define NAVIGATOR_H
+
+#include <systemlib/perf_counter.h>
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/parameter_update.h>
+
+#include "navigator_mode.h"
+#include "mission.h"
+#include "loiter.h"
+#include "rtl.h"
+#include "geofence.h"
+
+/**
+ * Number of navigation modes that need on_active/on_inactive calls
+ * Currently: mission, loiter, and rtl
+ */
+#define NAVIGATOR_MODE_ARRAY_SIZE 3
+
+class Navigator : public control::SuperBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Navigator();
+
+	/**
+	 * Destructor, also kills the navigators task.
+	 */
+	~Navigator();
+
+	/**
+	 * 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);
+
+	/**
+	 * Setters
+	 */
+	void		set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
+
+	/**
+	 * Getters
+	 */
+	struct vehicle_status_s*	    get_vstatus() { return &_vstatus; }
+	struct vehicle_global_position_s*   get_global_position() { return &_global_pos; }
+	struct home_position_s*		    get_home_position() { return &_home_pos; }
+	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
+	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
+	Geofence&	get_geofence() { return _geofence; }
+	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
+	float		get_loiter_radius() { return _param_loiter_radius.get(); }
+	float		get_acceptance_radius() { return _param_acceptance_radius.get(); }
+	int		get_mavlink_fd() { return _mavlink_fd; }
+
+private:
+
+	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	int		_navigator_task;		/**< task handle for sensor task */
+
+	int		_mavlink_fd;			/**< the file descriptor to send messages over mavlink */
+
+	int		_global_pos_sub;		/**< global position subscription */
+	int		_home_pos_sub;			/**< home position subscription */
+	int		_vstatus_sub;			/**< vehicle status subscription */
+	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
+	int		_control_mode_sub;		/**< vehicle control mode subscription */
+	int		_onboard_mission_sub;		/**< onboard mission subscription */
+	int		_offboard_mission_sub;		/**< offboard mission subscription */
+	int		_param_update_sub;		/**< param update subscription */
+
+	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
+
+	vehicle_status_s				_vstatus;		/**< vehicle status */
+	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
+	vehicle_global_position_s			_global_pos;		/**< global vehicle position */
+	home_position_s					_home_pos;		/**< home position for RTL */
+	mission_item_s 					_mission_item;		/**< current mission item */
+	navigation_capabilities_s			_nav_caps;		/**< navigation capabilities */
+	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
+
+	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	Geofence	_geofence;			/**< class that handles the geofence */
+	bool		_geofence_violation_warning_sent; /**< prevents spaming to mavlink */
+
+	bool		_fence_valid;			/**< flag if fence is valid */
+	bool		_inside_fence;			/**< vehicle is inside fence */
+
+	NavigatorMode	*_navigation_mode;		/**< abstract pointer to current navigation mode class */
+	Mission		_mission;			/**< class that handles the missions */
+	Loiter		_loiter;			/**< class that handles loiter */
+	RTL 		_rtl;				/**< class that handles RTL */
+
+	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
+
+	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
+	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
+
+	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
+	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
+	/**
+	 * Retrieve global position
+	 */
+	void		global_position_update();
+
+	/**
+	 * Retrieve home position
+	 */
+	void		home_position_update();
+
+	/**
+	 * Retreive navigation capabilities
+	 */
+	void		navigation_capabilities_update();
+
+	/**
+	 * Retrieve vehicle status
+	 */
+	void		vehicle_status_update();
+
+	/**
+	 * Retrieve vehicle control mode
+	 */
+	void		vehicle_control_mode_update();
+
+	/**
+	 * Update parameters
+	 */
+	void		params_update();
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * Main task.
+	 */
+	void		task_main();
+
+	/**
+	 * Translate mission item to a position setpoint.
+	 */
+	void		mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
+
+	/**
+	 * Publish a new position setpoint triplet for position controllers
+	 */
+	void		publish_position_setpoint_triplet();
+};
+#endif
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 06e0c5904..546602741 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -32,18 +32,18 @@
  ****************************************************************************/
 /**
  * @file navigator_main.cpp
- * Implementation of the main navigation state machine.
  *
- * Handles missions, geo fencing and failsafe navigation behavior.
- * Published the mission item triplet for the position controller.
+ * Handles mission items, geo fencing and failsafe navigation behavior.
+ * Published the position setpoint 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 Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -54,42 +54,28 @@
 #include <poll.h>
 #include <time.h>
 #include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
 #include <drivers/device/device.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
+
 #include <uORB/uORB.h>
-#include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/home_position.h>
-#include <uORB/topics/position_setpoint_triplet.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 <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 <mathlib/mathlib.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;
+#include "navigator.h"
 
 /**
  * navigator app start / stop handling function
@@ -98,342 +84,53 @@ static const int ERROR = -1;
  */
 extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
 
-class Navigator : public StateTable
-{
-public:
-	/**
-	 * Constructor
-	 */
-	Navigator();
-
-	/**
-	 * Destructor, also kills the navigators task.
-	 */
-	~Navigator();
-
-	/**
-	* 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		_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		_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	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
-	orb_advert_t	_mission_result_pub;		/**< publish mission result topic */
-
-	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 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 */
-
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-
-	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		_mission_item_valid;		/**< current mission item valid */
-	bool		_global_pos_valid;		/**< track changes of global_position */
-	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;
-
-	uint64_t	_set_nav_state_timestamp;	/**< timestamp of last handled navigation state request */
-
-	bool		_pos_sp_triplet_updated;
-
-	const char *nav_states_str[NAV_STATE_MAX];
-
-	struct {
-		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 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
-	};
-
-	/**
-	 * 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.
-	 */
-	void		parameters_update();
-
-	/**
-	 * Retrieve global position
-	 */
-	void		global_position_update();
-
-	/**
-	 * Retrieve home position
-	 */
-	void		home_position_update();
-
-	/**
-	 * Retreive navigation capabilities
-	 */
-	void		navigation_capabilities_update();
-
-	/**
-	 * Retrieve offboard mission.
-	 */
-	void		offboard_mission_update(bool isrotaryWing);
-
-	/**
-	 * Retrieve onboard mission.
-	 */
-	void		onboard_mission_update();
-
-	/**
-	 * Retrieve vehicle status
-	 */
-	void		vehicle_status_update();
-
-	/**
-	 * Retrieve vehicle control mode
-	 */
-	void		vehicle_control_mode_update();
-
-	/**
-	 * Shim for calling task_main from task_create.
-	 */
-	static void	task_main_trampoline(int argc, char *argv[]);
-
-	/**
-	 * Main task.
-	 */
-	void		task_main();
-
-	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();
-
-	/**
-	 * Fork for state transitions
-	 */
-	void		request_loiter_or_ready();
-	void		request_mission_if_available();
-
-	/**
-	 * Guards offboard mission
-	 */
-	bool		offboard_mission_available(unsigned relative_index);
-
-	/**
-	 * Guards onboard mission
-	 */
-	bool		onboard_mission_available(unsigned relative_index);
-
-	/**
-	 * Reset all "reached" flags.
-	 */
-	void		reset_reached();
-
-	/**
-	 * 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
 {
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
 Navigator	*g_navigator;
 }
 
 Navigator::Navigator() :
-
-/* state machine transition table */
-	StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
-
+	SuperBlock(NULL, "NAV"),
 	_task_should_exit(false),
 	_navigator_task(-1),
 	_mavlink_fd(-1),
-
-/* subscriptions */
 	_global_pos_sub(-1),
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
-	_params_sub(-1),
-	_offboard_mission_sub(-1),
-	_onboard_mission_sub(-1),
 	_capabilities_sub(-1),
 	_control_mode_sub(-1),
-
-/* publications */
+	_onboard_mission_sub(-1),
+	_offboard_mission_sub(-1),
 	_pos_sp_triplet_pub(-1),
-
-/* performance counters */
+	_vstatus({}),
+	_control_mode({}),
+	_global_pos({}),
+	_home_pos({}),
+	_mission_item({}),
+	_nav_caps({}),
+	_pos_sp_triplet({}),
+	_mission_item_valid(false),
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
-
+	_geofence({}),
 	_geofence_violation_warning_sent(false),
 	_fence_valid(false),
 	_inside_fence(true),
-	_mission(),
-	_mission_item_valid(false),
-	_global_pos_valid(false),
-	_reset_loiter_pos(true),
-	_waypoint_position_reached(false),
-	_waypoint_yaw_reached(false),
-	_time_first_inside_orbit(0),
-	_need_takeoff(true),
-	_do_takeoff(false),
-	_set_nav_state_timestamp(0),
-	_pos_sp_triplet_updated(false),
-/* states */
-	_rtl_state(RTL_STATE_NONE)
+	_navigation_mode(nullptr),
+	_mission(this, "MIS"),
+	_loiter(this, "LOI"),
+	_rtl(this, "RTL"),
+	_update_triplet(false),
+	_param_loiter_radius(this, "LOITER_RAD"),
+	_param_acceptance_radius(this, "ACC_RAD")
 {
-	_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_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();
+	/* Create a list of our possible navigation types */
+	_navigation_mode_array[0] = &_mission;
+	_navigation_mode_array[1] = &_loiter;
+	_navigation_mode_array[2] = &_rtl;
+
+	updateParams();
 }
 
 Navigator::~Navigator()
@@ -462,27 +159,6 @@ Navigator::~Navigator()
 }
 
 void
-Navigator::parameters_update()
-{
-	/* 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::global_position_update()
 {
 	orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
@@ -500,56 +176,6 @@ Navigator::navigation_capabilities_update()
 	orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
 }
 
-
-void
-Navigator::offboard_mission_update(bool isrotaryWing)
-{
-	struct mission_s offboard_mission;
-
-	if (orb_copy(ORB_ID(offboard_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;
-
-		} else {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
-		}
-
-		missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence, _home_pos.alt);
-
-		_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
-
-		_mission.set_offboard_mission_count(offboard_mission.count);
-		_mission.set_current_offboard_mission_index(offboard_mission.current_index);
-
-	} else {
-		_mission.set_offboard_mission_count(0);
-		_mission.set_current_offboard_mission_index(0);
-	}
-
-	_mission.publish_mission_result();
-}
-
-void
-Navigator::onboard_mission_update()
-{
-	struct mission_s onboard_mission;
-
-	if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
-
-		_mission.set_onboard_mission_count(onboard_mission.count);
-		_mission.set_current_onboard_mission_index(onboard_mission.current_index);
-
-	} else {
-		_mission.set_onboard_mission_count(0);
-		_mission.set_current_onboard_mission_index(0);
-	}
-}
-
 void
 Navigator::vehicle_status_update()
 {
@@ -570,6 +196,13 @@ Navigator::vehicle_control_mode_update()
 }
 
 void
+Navigator::params_update()
+{
+	parameter_update_s param_update;
+	orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);
+}
+
+void
 Navigator::task_main_trampoline(int argc, char *argv[])
 {
 	navigator::g_navigator->task_main();
@@ -580,16 +213,12 @@ 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
-	 */
+	 * else clear geofence data in datamanager */
 	struct stat buffer;
 
 	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
@@ -603,68 +232,58 @@ Navigator::task_main()
 			warnx("Could not clear geofence");
 	}
 
-	/*
-	 * do subscriptions
-	 */
+	/* do subscriptions */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_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));
 	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
+	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
+	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
+	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
 
 	/* 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();
+	params_update();
 
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
 
-	unsigned prevState = NAV_STATE_NONE;
 	hrt_abstime mavlink_open_time = 0;
 	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[8];
+	struct pollfd fds[6];
 
 	/* Setup of loop */
-	fds[0].fd = _params_sub;
+	fds[0].fd = _global_pos_sub;
 	fds[0].events = POLLIN;
-	fds[1].fd = _global_pos_sub;
+	fds[1].fd = _home_pos_sub;
 	fds[1].events = POLLIN;
-	fds[2].fd = _home_pos_sub;
+	fds[2].fd = _capabilities_sub;
 	fds[2].events = POLLIN;
-	fds[3].fd = _capabilities_sub;
+	fds[3].fd = _vstatus_sub;
 	fds[3].events = POLLIN;
-	fds[4].fd = _offboard_mission_sub;
+	fds[4].fd = _control_mode_sub;
 	fds[4].events = POLLIN;
-	fds[5].fd = _onboard_mission_sub;
+	fds[5].fd = _param_update_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 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) {
+			/* timed out - periodic check for _task_should_exit, etc. */
 			continue;
-		}
 
-		/* this is undesirable but not much we can do - might want to flag unhappy status */
-		if (pret < 0) {
+		} else if (pret < 0) {
+			/* this is undesirable but not much we can do - might want to flag unhappy status */
 			warn("poll error %d, %d", pret, errno);
 			continue;
 		}
@@ -677,162 +296,103 @@ Navigator::task_main()
 			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 		}
 
+		/* parameters updated */
+		if (fds[5].revents & POLLIN) {
+			params_update();
+			updateParams();
+		}
+
 		/* vehicle control mode updated */
-		if (fds[7].revents & POLLIN) {
+		if (fds[4].revents & POLLIN) {
 			vehicle_control_mode_update();
 		}
 
 		/* vehicle status updated */
-		if (fds[6].revents & POLLIN) {
+		if (fds[3].revents & POLLIN) {
 			vehicle_status_update();
-
-			/* evaluate state requested by commander */
-			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
-				/* publish position setpoint triplet on each status update if navigator active */
-				_pos_sp_triplet_updated = true;
-
-				if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
-					/* commander requested new navigation mode, try to set it */
-					switch (_vstatus.set_nav_state) {
-					case NAV_STATE_NONE:
-						/* nothing to do */
-						break;
-
-					case NAV_STATE_LOITER:
-						request_loiter_or_ready();
-						break;
-
-					case NAV_STATE_MISSION:
-						request_mission_if_available();
-						break;
-
-					case NAV_STATE_RTL:
-						if (!(_rtl_state == RTL_STATE_DESCEND &&
-							  (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
-							_vstatus.condition_home_position_valid) {
-							dispatch(EVENT_RTL_REQUESTED);
-						}
-
-						break;
-
-					case NAV_STATE_LAND:
-						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) {
-						request_mission_if_available();
-					}
-				}
-
-				/* 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();
-					}
-				}
-
-			} else {
-				/* navigator shouldn't act */
-				dispatch(EVENT_NONE_REQUESTED);
-			}
-
-			_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
-		}
-
-		/* 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 */
 		}
 
 		/* navigation capabilities updated */
-		if (fds[3].revents & POLLIN) {
+		if (fds[2].revents & POLLIN) {
 			navigation_capabilities_update();
 		}
 
-		/* 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);
-		}
-
-		/* onboard mission updated */
-		if (fds[5].revents & POLLIN) {
-			onboard_mission_update();
-			// XXX check if mission really changed
-			dispatch(EVENT_MISSION_CHANGED);
-		}
-
 		/* home position updated */
-		if (fds[2].revents & POLLIN) {
+		if (fds[1].revents & POLLIN) {
 			home_position_update();
-			// XXX check if home position really changed
-			dispatch(EVENT_HOME_POSITION_CHANGED);
 		}
 
 		/* global position updated */
-		if (fds[1].revents & POLLIN) {
+		if (fds[0].revents & POLLIN) {
 			global_position_update();
 
-			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
-				/* publish position setpoint triplet on each position update if navigator active */
-				_pos_sp_triplet_updated = true;
-
-				if (myState == NAV_STATE_LAND && !_global_pos_valid) {
-					/* got global position when landing, update setpoint */
-					start_land();
-				}
-
-				/* 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();
-					}
-				}
-			}
-
 			/* 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)
 
 				/* 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;
 				}
-
 			} else {
 				/* Reset the _geofence_violation_warning_sent field */
 				_geofence_violation_warning_sent = false;
 			}
 		}
 
-		_global_pos_valid = _vstatus.condition_global_position_valid;
+		/* Do stuff according to navigation state set by commander */
+		switch (_vstatus.nav_state) {
+			case NAVIGATION_STATE_MANUAL:
+			case NAVIGATION_STATE_ACRO:
+			case NAVIGATION_STATE_ALTCTL:
+			case NAVIGATION_STATE_POSCTL:
+				_navigation_mode = nullptr;
+				_can_loiter_at_sp = false;
+				break;
+			case NAVIGATION_STATE_AUTO_MISSION:
+				_navigation_mode = &_mission;
+				break;
+			case NAVIGATION_STATE_AUTO_LOITER:
+				_navigation_mode = &_loiter;
+				break;
+			case NAVIGATION_STATE_AUTO_RTL:
+				_navigation_mode = &_rtl;
+				break;
+			case NAVIGATION_STATE_AUTO_RTGS:
+				_navigation_mode = &_rtl; /* TODO: change this to something else */
+				break;
+			case NAVIGATION_STATE_LAND:
+			case NAVIGATION_STATE_TERMINATION:
+			default:
+				_navigation_mode = nullptr;
+				_can_loiter_at_sp = false;
+				break;
+		}
 
-		/* publish position setpoint triplet if updated */
-		if (_pos_sp_triplet_updated) {
-			_pos_sp_triplet_updated = false;
-			publish_position_setpoint_triplet();
+		/* iterate through navigation modes and set active/inactive for each */
+		for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
+			if (_navigation_mode == _navigation_mode_array[i]) {
+				_update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet);
+			} else {
+				_navigation_mode_array[i]->on_inactive();
+			}
 		}
 
-		/* notify user about state changes */
-		if (myState != prevState) {
-			mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]);
-			prevState = myState;
+		/* if nothing is running, set position setpoint triplet invalid */
+		if (_navigation_mode == nullptr) {
+			_pos_sp_triplet.previous.valid = false;
+			_pos_sp_triplet.current.valid = false;
+			_pos_sp_triplet.next.valid = false;
+			_update_triplet = true;
+		}
+
+		if (_update_triplet) {
+			publish_position_setpoint_triplet();
+			_update_triplet = false;
 		}
 
 		perf_end(_loop_perf);
 	}
-
 	warnx("exiting.");
 
 	_navigator_task = -1;
@@ -863,19 +423,21 @@ Navigator::start()
 void
 Navigator::status()
 {
-	warnx("Global position: %svalid", _global_pos_valid ? "" : "in");
-
-	if (_global_pos_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));
-	}
+	/* TODO: add this again */
+	// warnx("Global position is %svalid", _global_pos_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");
+		/* TODO: needed? */
 //		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);
@@ -883,747 +445,19 @@ Navigator::status()
 	} 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 */		{NO_ACTION, NAV_STATE_READY},
-		/* 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()
-{
-	reset_reached();
-
-	_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()
-{
-	reset_reached();
-
-	_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()
-{
-	reset_reached();
-
-	_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 && !_vstatus.is_rotary_wing) {
-			_pos_sp_triplet.current.alt = min_alt_amsl;
-			mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
-
-		} else {
-			_pos_sp_triplet.current.alt = _global_pos.alt;
-			mavlink_log_info(_mavlink_fd, "#audio: 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()
-{
-	reset_reached();
-
-	/* 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) {
-		_mission.report_current_offboard_mission_item();
-
-		_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, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt));
-
-		} else {
-			if (onboard) {
-				mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index);
-
-			} else {
-				mavlink_log_info(_mavlink_fd, "#audio: 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()
-{
-	reset_reached();
-
-	/* 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()
-{
-	reset_reached();
-
-	/* 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_reached();
-
-	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_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, "#audio: RTL: climb to %.1fm above home", (double)(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
-			if (_pos_sp_triplet.previous.valid) {
-				/* if previous setpoint is valid then use it to calculate heading to home */
-				_mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);
-
-			} else {
-				/* else use current position */
-				_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
-			}
-			_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, "#audio: RTL: return at %.1fm above home", (double)(_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_LOITER_TIME_LIMIT;
-			_mission_item.acceptance_radius = _parameters.acceptance_radius;
-			_mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 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, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
-			break;
-		}
-
-	default: {
-			mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state);
-			start_loiter();
-			break;
-		}
-	}
-
-	_pos_sp_triplet_updated = true;
-}
-
-void
-Navigator::request_loiter_or_ready()
-{
-	/* XXX workaround: no landing detector for fixedwing yet */
-	if (_vstatus.condition_landed && _vstatus.is_rotary_wing) {
-		dispatch(EVENT_READY_REQUESTED);
-
-	} else {
-		dispatch(EVENT_LOITER_REQUESTED);
-	}
-}
-
-void
-Navigator::request_mission_if_available()
-{
-	if (_mission.current_mission_available()) {
-		dispatch(EVENT_MISSION_REQUESTED);
-
-	} else {
-		request_loiter_or_ready();
-	}
-}
-
-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;
-
-		if (_pos_sp_triplet.previous.valid) {
-			/* if previous setpoint is valid then use it to calculate heading to home */
-			sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon);
-
-		} else {
-			/* else use current position */
-			sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon);
-		}
-		sp->loiter_radius = _parameters.loiter_radius;
-		sp->loiter_direction = 1;
-		sp->pitch_min = 0.0f;
-
-	} 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_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;
-		}
-
-		if (_do_takeoff) {
-			/* require only altitude for takeoff */
-			if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) {
-				_waypoint_position_reached = true;
-			}
-
-		} else {
-			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 (dist >= 0.0f && dist <= acceptance_radius) {
-				_waypoint_position_reached = true;
-			}
-		}
-	}
-
-	if (_waypoint_position_reached && !_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.2f) { /* 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, "#audio: waypoint reached, wait for %.1fs", (double)_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) {
-			reset_reached();
-			return true;
-		}
-	}
-
-	return false;
-
-}
-
-void
-Navigator::reset_reached()
-{
-	_time_first_inside_orbit = 0;
-	_waypoint_position_reached = false;
-	_waypoint_yaw_reached = false;
-
-}
-
-void
-Navigator::on_mission_item_reached()
-{
-	if (myState == NAV_STATE_MISSION) {
-
-		_mission.report_mission_item_reached();
-
-		if (_do_takeoff) {
-			/* takeoff completed */
-			_do_takeoff = false;
-			mavlink_log_info(_mavlink_fd, "#audio: takeoff completed");
-
-		} else {
-			/* advance by one mission item */
-			_mission.move_to_next();
-		}
-
-		if (_mission.current_mission_available()) {
-			if (_mission_item.autocontinue) {
-				/* continue mission */
-				set_mission_item();
-
-			} else {
-				/* autocontinue disabled for this item */
-				request_loiter_or_ready();
-			}
-
-		} 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");
-			request_loiter_or_ready();
-		}
-
-	} 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);
-	}
-	_mission.publish_mission_result();
 }
 
 void
 Navigator::publish_position_setpoint_triplet()
 {
 	/* update navigation state */
-	_pos_sp_triplet.nav_state = static_cast<nav_state_t>(myState);
+	/* TODO: set nav_state */
 
 	/* 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);
 	}
 }
diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp
deleted file mode 100644
index 49fc62785..000000000
--- a/src/modules/navigator/navigator_mission.cpp
+++ /dev/null
@@ -1,319 +0,0 @@
-/****************************************************************************
- *
- *   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 navigator_mission.cpp
- * Helper class to access missions
- *
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#include <string.h>
-#include <stdlib.h>
-#include <dataman/dataman.h>
-#include <systemlib/err.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/mission_result.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_result_pub(-1)
-{
-	memset(&_mission_result, 0, sizeof(struct mission_result_s));
-}
-
-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) {
-		warnx("specifically set to %d", new_index);
-		_current_offboard_mission_index = (unsigned)new_index;
-	} else {
-
-		/* if less WPs available, reset to first WP */
-		if (_current_offboard_mission_index >= _offboard_mission_item_count) {
-			_current_offboard_mission_index = 0;
-		}
-	}
-	report_current_offboard_mission_item();
-}
-
-void
-Mission::set_current_onboard_mission_index(int new_index)
-{
-	if (new_index != -1) {
-		_current_onboard_mission_index = (unsigned)new_index;
-	} else {
-
-		/* if less WPs available, reset to first WP */
-		if (_current_onboard_mission_index >= _onboard_mission_item_count) {
-			_current_onboard_mission_index = 0;
-		}
-	}
-	// TODO: implement this for onboard missions as well
-	// report_current_mission_item();
-}
-
-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;
-	}
-}
-
-void
-Mission::report_mission_item_reached()
-{
-	if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
-		_mission_result.mission_reached = true;
-		_mission_result.mission_index_reached = _current_offboard_mission_index;
-	}
-}
-
-void
-Mission::report_current_offboard_mission_item()
-{
-	_mission_result.index_current_mission = _current_offboard_mission_index;
-}
-
-void
-Mission::publish_mission_result()
-{
-	/* lazily publish the mission result only once available */
-	if (_mission_result_pub > 0) {
-		/* publish mission result */
-		orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
-
-	} else {
-		/* advertise and publish */
-		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
-	}
-	/* reset reached bool */
-	_mission_result.mission_reached = false;
-}
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
new file mode 100644
index 000000000..25e767c2b
--- /dev/null
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file navigator_mode.cpp
+ *
+ * Helper class for different modes in navigator
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include "navigator_mode.h"
+
+NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
+	SuperBlock(NULL, name),
+	_navigator(navigator),
+	_first_run(true)
+{
+	/* load initial params */
+	updateParams();
+	/* set initial mission items */
+	on_inactive();
+}
+
+NavigatorMode::~NavigatorMode()
+{
+}
+
+void
+NavigatorMode::on_inactive()
+{
+	_first_run = true;
+}
+
+bool
+NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	pos_sp_triplet->current.valid = false;
+	_first_run = false;
+	return false;
+}
diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h
new file mode 100644
index 000000000..cbb53d91b
--- /dev/null
+++ b/src/modules/navigator/navigator_mode.h
@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file navigator_mode.h
+ *
+ * Helper class for different modes in navigator
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef NAVIGATOR_MODE_H
+#define NAVIGATOR_MODE_H
+
+#include <drivers/drv_hrt.h>
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <dataman/dataman.h>
+
+#include <uORB/topics/position_setpoint_triplet.h>
+
+class Navigator;
+
+class NavigatorMode : public control::SuperBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	NavigatorMode(Navigator *navigator, const char *name);
+
+	/**
+	 * Destructor
+	 */
+	virtual ~NavigatorMode();
+
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	virtual void on_inactive();
+
+	/**
+	 * This function is called while the mode is active
+	 *
+	 * @param position setpoint triplet to set
+	 * @return true if position setpoint triplet has been changed
+	 */
+	virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
+
+protected:
+	Navigator *_navigator;
+	bool _first_run;
+};
+
+#endif
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 5139283b6..084afe340 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 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
@@ -17,7 +17,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,
@@ -34,104 +34,33 @@
 /**
  * @file navigator_params.c
  *
- * Parameters defined by the navigator task.
+ * Parameters for navigator in general
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include <nuttx/config.h>
 
 #include <systemlib/param/param.h>
 
-/*
- * Navigator parameters, accessible via MAVLink
- */
-
-/**
- * Minimum altitude (fixed wing only)
- *
- * Minimum altitude above home for LOITER.
- *
- * @unit meters
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
-
-/**
- * Waypoint acceptance radius
- *
- * Default value of acceptance radius (if not specified in mission item).
- *
- * @unit meters
- * @min 0.0
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
-
 /**
- * Loiter radius (fixed wing only)
+ * Loiter radius (FW only)
  *
- * Default value of loiter radius (if not specified in mission item).
+ * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only).
  *
  * @unit meters
  * @min 0.0
- * @group Navigation
+ * @group Mission
  */
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
 
 /**
- * Enable onboard mission
- *
- * @group Navigation
- */
-PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
-
-/**
- * Take-off altitude
- *
- * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint.
- *
- * @unit meters
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
-
-/**
- * Landing altitude
- *
- * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed.
- *
- * @unit meters
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);
-
-/**
- * Return-To-Launch altitude
+ * Acceptance Radius
  *
- * Minimum altitude above home position for going home in RTL mode.
+ * Default acceptance radius, overridden by acceptance radius of waypoint if set.
  *
  * @unit meters
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);
-
-/**
- * Return-To-Launch delay
- *
- * Delay after descend before landing in RTL mode.
- * If set to -1 the system will not land but loiter at NAV_LAND_ALT.
- *
- * @unit seconds
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);
-
-/**
- * Enable parachute deployment
- *
- * @group Navigation
+ * @min 1.0
+ * @group Mission
  */
-PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0);
+PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
new file mode 100644
index 000000000..043f773a4
--- /dev/null
+++ b/src/modules/navigator/rtl.cpp
@@ -0,0 +1,320 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file navigator_rtl.cpp
+ * Helper class to access RTL
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+
+#include "navigator.h"
+#include "rtl.h"
+
+#define DELAY_SIGMA	0.01f
+
+RTL::RTL(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_rtl_state(RTL_STATE_NONE),
+	_param_return_alt(this, "RETURN_ALT"),
+	_param_descend_alt(this, "DESCEND_ALT"),
+	_param_land_delay(this, "LAND_DELAY")
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+RTL::~RTL()
+{
+}
+
+void
+RTL::on_inactive()
+{
+	_first_run = true;
+
+	/* reset RTL state only if setpoint moved */
+	if (!_navigator->get_can_loiter_at_sp()) {
+		_rtl_state = RTL_STATE_NONE;
+	}
+}
+
+bool
+RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	bool updated = false;
+
+	if (_first_run) {
+		_first_run = false;
+
+		/* decide where to enter the RTL procedure when we switch into it */
+		if (_rtl_state == RTL_STATE_NONE) {
+			/* for safety reasons don't go into RTL if landed */
+			if (_navigator->get_vstatus()->condition_landed) {
+				_rtl_state = RTL_STATE_LANDED;
+				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
+
+			/* if lower than return altitude, climb up first */
+			} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+				   + _param_return_alt.get()) {
+				_rtl_state = RTL_STATE_CLIMB;
+
+			/* otherwise go straight to return */
+			} else {
+				/* set altitude setpoint to current altitude */
+				_rtl_state = RTL_STATE_RETURN;
+				_mission_item.altitude_is_relative = false;
+				_mission_item.altitude = _navigator->get_global_position()->alt;
+			}
+		}
+
+		set_rtl_item(pos_sp_triplet);
+		updated = true;
+
+	} else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
+		advance_rtl();
+		set_rtl_item(pos_sp_triplet);
+		updated = true;
+	}
+
+	return updated;
+}
+
+void
+RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
+{
+	/* make sure we have the latest params */
+	updateParams();
+
+	set_previous_pos_setpoint(pos_sp_triplet);
+	_navigator->set_can_loiter_at_sp(false);
+
+	switch (_rtl_state) {
+	case RTL_STATE_CLIMB: {
+		float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
+
+		_mission_item.lat = _navigator->get_global_position()->lat;
+		_mission_item.lon = _navigator->get_global_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = climb_alt;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
+			(int)(climb_alt - _navigator->get_home_position()->alt));
+		break;
+	}
+
+	case RTL_STATE_RETURN: {
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		 // don't change altitude
+
+		 if (pos_sp_triplet->previous.valid) {
+		 	/* if previous setpoint is valid then use it to calculate heading to home */
+		 	_mission_item.yaw = get_bearing_to_next_waypoint(
+		 	        pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
+		 	        _mission_item.lat, _mission_item.lon);
+
+		 } else {
+		 	/* else use current position */
+		 	_mission_item.yaw = get_bearing_to_next_waypoint(
+		 	        _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+		 	        _mission_item.lat, _mission_item.lon);
+		 }
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
+			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
+		break;
+	}
+
+	case RTL_STATE_DESCEND: {
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = false;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
+			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
+		break;
+	}
+
+	case RTL_STATE_LOITER: {
+		bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
+
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = autoland;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+
+		if (autoland) {
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside);
+
+		} else {
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
+		}
+		break;
+	}
+
+	case RTL_STATE_LAND: {
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_LAND;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
+		break;
+	}
+
+	case RTL_STATE_LANDED: {
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_IDLE;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed");
+		break;
+	}
+
+	default:
+		break;
+	}
+
+	/* convert mission item to current position setpoint and make it valid */
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	reset_mission_item_reached();
+	pos_sp_triplet->current.valid = true;
+	pos_sp_triplet->next.valid = false;
+}
+
+void
+RTL::advance_rtl()
+{
+	switch (_rtl_state) {
+	case RTL_STATE_CLIMB:
+		_rtl_state = RTL_STATE_RETURN;
+		break;
+
+	case RTL_STATE_RETURN:
+		_rtl_state = RTL_STATE_DESCEND;
+		break;
+
+	case RTL_STATE_DESCEND:
+		/* only go to land if autoland is enabled */
+		if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
+			_rtl_state = RTL_STATE_LOITER;
+
+		} else {
+			_rtl_state = RTL_STATE_LAND;
+		}
+		break;
+
+	case RTL_STATE_LOITER:
+		_rtl_state = RTL_STATE_LAND;
+		break;
+
+	case RTL_STATE_LAND:
+		_rtl_state = RTL_STATE_LANDED;
+		break;
+
+	default:
+		break;
+	}
+}
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
new file mode 100644
index 000000000..b4b729e89
--- /dev/null
+++ b/src/modules/navigator/rtl.h
@@ -0,0 +1,110 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file navigator_rtl.h
+ * Helper class for RTL
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef NAVIGATOR_RTL_H
+#define NAVIGATOR_RTL_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class RTL : public MissionBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	RTL(Navigator *navigator, const char *name);
+
+	/**
+	 * Destructor
+	 */
+	~RTL();
+
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	void on_inactive();
+
+	/**
+	 * This function is called while the mode is active
+	 *
+	 * @param position setpoint triplet that needs to be set
+	 * @return true if updated
+	 */
+	bool on_active(position_setpoint_triplet_s *pos_sp_triplet);
+
+
+private:
+	/**
+	 * Set the RTL item
+	 */
+	void		set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet);
+
+	/**
+	 * Move to next RTL item
+	 */
+	void		advance_rtl();
+
+	enum RTLState {
+		RTL_STATE_NONE = 0,
+		RTL_STATE_CLIMB,
+		RTL_STATE_RETURN,
+		RTL_STATE_DESCEND,
+		RTL_STATE_LOITER,
+		RTL_STATE_LAND,
+		RTL_STATE_LANDED,
+	} _rtl_state;
+
+	control::BlockParamFloat _param_return_alt;
+	control::BlockParamFloat _param_descend_alt;
+	control::BlockParamFloat _param_land_delay;
+};
+
+#endif
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
new file mode 100644
index 000000000..bfe6ce7e1
--- /dev/null
+++ b/src/modules/navigator/rtl_params.c
@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rtl_params.c
+ *
+ * Parameters for RTL
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * RTL parameters, accessible via MAVLink
+ */
+
+/**
+ * Loiter radius after RTL (FW only)
+ *
+ * Default value of loiter radius after RTL (fixedwing only).
+ *
+ * @unit meters
+ * @min 0.0
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
+
+/**
+ * RTL altitude
+ *
+ * Altitude to fly back in RTL in meters
+ *
+ * @unit meters
+ * @min 0
+ * @max 1
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
+
+
+/**
+ * RTL loiter altitude
+ *
+ * Stay at this altitude above home position after RTL descending.
+ * Land (i.e. slowly descend) from this altitude if autolanding allowed.
+ *
+ * @unit meters
+ * @min 0
+ * @max 100
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
+
+/**
+ * RTL delay
+ *
+ * Delay after descend before landing in RTL mode.
+ * If set to -1 the system will not land but loiter at NAV_LAND_ALT.
+ *
+ * @unit seconds
+ * @min -1
+ * @max
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
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 f908d7a3b..62cee145e 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -619,13 +619,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				/* hysteresis for GPS quality */
 				if (gps_valid) {
-					if (gps.eph_m > max_eph_epv || gps.epv_m > max_eph_epv || gps.fix_type < 3) {
+					if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {
 						gps_valid = false;
 						mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
 					}
 
 				} else {
-					if (gps.eph_m < max_eph_epv * 0.7f && gps.epv_m < max_eph_epv * 0.7f && gps.fix_type >= 3) {
+					if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) {
 						gps_valid = true;
 						reset_est = true;
 						mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
@@ -705,8 +705,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						/* save rotation matrix at this moment */
 						memcpy(R_gps, R_buf[est_i], sizeof(R_gps));
 
-						w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m);
-						w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m);
+						w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
+						w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
 					}
 
 				} else {
@@ -859,7 +859,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
 
 		if (use_gps_z) {
-			epv = fminf(epv, gps.epv_m);
+			epv = fminf(epv, gps.epv);
 
 			inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
 		}
@@ -894,7 +894,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			}
 
 			if (use_gps_xy) {
-				eph = fminf(eph, gps.eph_m);
+				eph = fminf(eph, gps.eph);
 
 				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);
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 577cadfbb..e483b35e8 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -84,8 +84,10 @@
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/tecs_status.h>
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/servorail_status.h>
+#include <uORB/topics/wind_estimate.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -939,8 +941,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct telemetry_status_s telemetry;
 		struct range_finder_report range_finder;
 		struct estimator_status_report estimator_status;
+		struct tecs_status_s tecs_status;
 		struct system_power_s system_power;
 		struct servorail_status_s servorail_status;
+		struct wind_estimate_s wind_estimate;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -972,13 +976,16 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_BATT_s log_BATT;
 			struct log_DIST_s log_DIST;
 			struct log_TELE_s log_TELE;
-			struct log_ESTM_s log_ESTM;
+			struct log_EST0_s log_EST0;
+			struct log_EST1_s log_EST1;
 			struct log_PWR_s log_PWR;
 			struct log_VICN_s log_VICN;
 			struct log_GS0A_s log_GS0A;
 			struct log_GS0B_s log_GS0B;
 			struct log_GS1A_s log_GS1A;
 			struct log_GS1B_s log_GS1B;
+			struct log_TECS_s log_TECS;
+			struct log_WIND_s log_WIND;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1010,8 +1017,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int telemetry_sub;
 		int range_finder_sub;
 		int estimator_status_sub;
+		int tecs_status_sub;
 		int system_power_sub;
 		int servorail_status_sub;
+		int wind_sub;
 	} subs;
 
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -1037,8 +1046,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
 	subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
 	subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
+	subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
 	subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
 	subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
+	subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
+	/* we need to rate-limit wind, as we do not need the full update rate */
+	orb_set_interval(subs.wind_sub, 90);
 
 	thread_running = true;
 
@@ -1105,7 +1118,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			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.arming_state = (uint8_t) buf_status.arming_state;
-			log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state;
+			log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe;
 			log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
 			log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
 			log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
@@ -1126,8 +1139,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			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;
-			log_msg.body.log_GPS.eph = buf_gps_pos.eph_m;
-			log_msg.body.log_GPS.epv = buf_gps_pos.epv_m;
+			log_msg.body.log_GPS.eph = buf_gps_pos.eph;
+			log_msg.body.log_GPS.epv = buf_gps_pos.epv;
 			log_msg.body.log_GPS.lat = buf_gps_pos.lat;
 			log_msg.body.log_GPS.lon = buf_gps_pos.lon;
 			log_msg.body.log_GPS.alt = buf_gps_pos.alt * 0.001f;
@@ -1340,7 +1353,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- GLOBAL POSITION SETPOINT --- */
 		if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
 			log_msg.msg_type = LOG_GPSP_MSG;
-			log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
+			log_msg.body.log_GPSP.nav_state = 0;  /* TODO: Fix this */
 			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;
@@ -1477,15 +1490,52 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 		/* --- ESTIMATOR STATUS --- */
 		if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) {
-			log_msg.msg_type = LOG_ESTM_MSG;
-			unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s);
-			memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s));
-			memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy);
-			log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states;
-			log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan;
-			log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan;
-			log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan;
-			LOGBUFFER_WRITE_AND_COUNT(ESTM);
+			log_msg.msg_type = LOG_EST0_MSG;
+			unsigned maxcopy0 = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_EST0.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_EST0.s);
+			memset(&(log_msg.body.log_EST0.s), 0, sizeof(log_msg.body.log_EST0.s));
+			memcpy(&(log_msg.body.log_EST0.s), buf.estimator_status.states, maxcopy0);
+			log_msg.body.log_EST0.n_states = buf.estimator_status.n_states;
+			log_msg.body.log_EST0.nan_flags = buf.estimator_status.nan_flags;
+			log_msg.body.log_EST0.health_flags = buf.estimator_status.health_flags;
+			log_msg.body.log_EST0.timeout_flags = buf.estimator_status.timeout_flags;
+			LOGBUFFER_WRITE_AND_COUNT(EST0);
+
+			log_msg.msg_type = LOG_EST1_MSG;
+			unsigned maxcopy1 = ((sizeof(buf.estimator_status.states) - maxcopy0) < sizeof(log_msg.body.log_EST1.s)) ? (sizeof(buf.estimator_status.states) - maxcopy0) : sizeof(log_msg.body.log_EST1.s);
+			memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s));
+			memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1);
+			LOGBUFFER_WRITE_AND_COUNT(EST1);
+		}
+
+		/* --- TECS STATUS --- */
+		if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) {
+			log_msg.msg_type = LOG_TECS_MSG;
+			log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
+			log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
+			log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
+			log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
+			log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
+			log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
+			log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
+			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
+			log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
+			log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
+			log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
+			log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate;
+			log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp;
+			log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate;
+			log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode;
+			LOGBUFFER_WRITE_AND_COUNT(TECS);
+		}
+
+		/* --- WIND ESTIMATE --- */
+		if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) {
+			log_msg.msg_type = LOG_WIND_MSG;
+			log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north;
+			log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east;
+			log_msg.body.log_WIND.cov_x = buf.wind_estimate.covariance_north;
+			log_msg.body.log_WIND.cov_y = buf.wind_estimate.covariance_east;
+			LOGBUFFER_WRITE_AND_COUNT(WIND);
 		}
 
 		/* signal the other thread new data, but not yet unlock */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index f4d88f079..8c05e87c5 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -288,15 +288,7 @@ struct log_TELE_s {
 	uint8_t txbuf;
 };
 
-/* --- ESTM - ESTIMATOR STATUS --- */
-#define LOG_ESTM_MSG 23
-struct log_ESTM_s {
-	float s[10];
-	uint8_t n_states;
-	uint8_t states_nan;
-	uint8_t covariance_nan;
-	uint8_t kalman_gain_nan;
-};
+// ID 23 available
 
 /* --- PWR - ONBOARD POWER SYSTEM --- */
 #define LOG_PWR_MSG 24
@@ -346,6 +338,53 @@ struct log_GS1B_s {
 	uint8_t satellite_snr[16];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
 };
 
+/* --- TECS - TECS STATUS --- */
+#define LOG_TECS_MSG 30
+struct log_TECS_s {
+	float altitudeSp;
+	float altitude;
+	float flightPathAngleSp;
+	float flightPathAngle;
+	float flightPathAngleFiltered;
+	float airspeedSp;
+	float airspeed;
+	float airspeedFiltered;
+	float airspeedDerivativeSp;
+	float airspeedDerivative;
+
+	float totalEnergyRateSp;
+	float totalEnergyRate;
+	float energyDistributionRateSp;
+	float energyDistributionRate;
+
+	uint8_t mode;
+};
+
+/* --- WIND - WIND ESTIMATE --- */
+#define LOG_WIND_MSG 31
+struct log_WIND_s {
+	float x;
+	float y;
+	float cov_x;
+	float cov_y;
+};
+
+/* --- EST0 - ESTIMATOR STATUS --- */
+#define LOG_EST0_MSG 32
+struct log_EST0_s {
+	float s[12];
+	uint8_t n_states;
+	uint8_t nan_flags;
+	uint8_t health_flags;
+	uint8_t timeout_flags;
+};
+
+/* --- EST1 - ESTIMATOR STATUS --- */
+#define LOG_EST1_MSG 33
+struct log_EST1_s {
+	float s[16];
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -394,13 +433,16 @@ static const struct log_format_s log_formats[] = {
 	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"),
-	LOG_FORMAT(ESTM, "ffffffffffBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
+	LOG_FORMAT(EST0, "ffffffffffffBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"),
+	LOG_FORMAT(EST1, "ffffffffffffffff",	"s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
 	LOG_FORMAT(PWR, "fffBBBBB",		"Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
 	LOG_FORMAT(VICN, "ffffff",		"X,Y,Z,Roll,Pitch,Yaw"),
 	LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
+	LOG_FORMAT(TECS, "ffffffffffffffB",	"AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
+	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 999cf8bb3..c8a3ec8f0 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -243,6 +243,36 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
 PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
 
 /**
+ * Board rotation Y (Pitch) offset
+ *
+ * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user
+ * to fine tune the board offset in the event of misalignment.
+ *
+ * @group Sensor Calibration
+ */
+ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f);
+
+/**
+ * Board rotation X (Roll) offset
+ *
+ * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user
+ * to fine tune the board offset in the event of misalignment.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f);
+
+/**
+ * Board rotation Z (YAW) offset
+ *
+ * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user
+ * to fine tune the board offset in the event of misalignment.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
+
+/**
  * External magnetometer rotation
  *
  * This parameter defines the rotation of the external magnetometer relative
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index b268b1b36..16fcb4c26 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -229,7 +229,7 @@ private:
 	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 */
 	hrt_abstime _battery_current_timestamp;	/**< timestamp of last battery current reading */
 
@@ -252,6 +252,8 @@ private:
 
 		int board_rotation;
 		int external_mag_rotation;
+		
+		float board_offset[3];
 
 		int rc_map_roll;
 		int rc_map_pitch;
@@ -341,6 +343,8 @@ private:
 
 		param_t board_rotation;
 		param_t external_mag_rotation;
+		
+		param_t board_offset[3];
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -587,6 +591,11 @@ Sensors::Sensors() :
 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
 	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
+	
+	/* rotation offsets */
+	_parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
+	_parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
+	_parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -791,6 +800,18 @@ Sensors::parameters_update()
 
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
+	
+	param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0]));
+	param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
+	param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
+	
+	/** fine tune board offset on parameter update **/
+	math::Matrix<3, 3> board_rotation_offset; 
+	board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
+							 M_DEG_TO_RAD_F * _parameters.board_offset[1],
+							 M_DEG_TO_RAD_F * _parameters.board_offset[2]);
+	
+	_board_rotation = _board_rotation * board_rotation_offset;
 
 	return OK;
 }
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 6835ee4a2..668d9dfdf 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -75,7 +75,7 @@ __EXPORT extern void		perf_free(perf_counter_t handle);
 /**
  * Count a performance event.
  *
- * This call only affects counters that take single events; PC_COUNT etc.
+ * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc.
  *
  * @param handle		The handle returned from perf_alloc.
  */
diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h
index f2709d29f..e6011fdef 100644
--- a/src/modules/systemlib/state_table.h
+++ b/src/modules/systemlib/state_table.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 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
@@ -33,8 +33,9 @@
 
 /**
  * @file state_table.h
- * 
+ *
  * Finite-State-Machine helper class for state table
+ * @author: Julian Oes <julian@oes.ch>
  */
 
 #ifndef __SYSTEMLIB_STATE_TABLE_H
@@ -48,22 +49,28 @@ public:
 		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)
+	#define ACTION(_target) StateTable::Action(_target)
 
 	virtual ~StateTable() {}
-	
+
 	void dispatch(unsigned const sig) {
-		register Tran const *t = myTable + myState*myNsignals + sig;
-		(this->*(t->action))();
+		/* get transition using state table */
+		Tran const *t = myTable + myState*myNsignals + sig;
 
+		/* accept new state */
 		myState = t->nextState;
+
+		/*  */
+		(this->*(t->action))();
+	}
+	void doNothing() {
+		return;
 	}
-	void doNothing() {}
 protected:
 	unsigned myState;
 private:
@@ -72,4 +79,4 @@ private:
 	unsigned myNstates;
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp
index 5a5981617..cd0b30dd6 100644
--- a/src/modules/uORB/Publication.cpp
+++ b/src/modules/uORB/Publication.cpp
@@ -47,6 +47,7 @@
 #include "topics/vehicle_rates_setpoint.h"
 #include "topics/actuator_outputs.h"
 #include "topics/encoders.h"
+#include "topics/tecs_status.h"
 
 namespace uORB {
 
@@ -76,5 +77,6 @@ template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
 template class __EXPORT Publication<vehicle_rates_setpoint_s>;
 template class __EXPORT Publication<actuator_outputs_s>;
 template class __EXPORT Publication<encoders_s>;
+template class __EXPORT Publication<tecs_status_s>;
 
 }
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 90675bb2e..7c3bb0009 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -199,3 +199,9 @@ ORB_DEFINE(encoders, struct encoders_s);
 
 #include "topics/estimator_status.h"
 ORB_DEFINE(estimator_status, struct estimator_status_report);
+
+#include "topics/tecs_status.h"
+ORB_DEFINE(tecs_status, struct tecs_status_s);
+
+#include "topics/wind_estimate.h"
+ORB_DEFINE(wind_estimate, struct wind_estimate_s);
diff --git a/src/modules/uORB/topics/estimator_status.h b/src/modules/uORB/topics/estimator_status.h
index 5530cdb21..7f26b505b 100644
--- a/src/modules/uORB/topics/estimator_status.h
+++ b/src/modules/uORB/topics/estimator_status.h
@@ -64,9 +64,9 @@ struct estimator_status_report {
 	uint64_t timestamp;			/**< Timestamp in microseconds since boot */
 	float states[32];			/**< Internal filter states */
 	float n_states;				/**< Number of states effectively used */
-	bool states_nan;			/**< If set to true, one of the states is NaN */
-	bool covariance_nan;			/**< If set to true, the covariance matrix went NaN */
-	bool kalman_gain_nan;			/**< If set to true, the Kalman gain matrix went NaN */
+	uint8_t nan_flags;			/**< Bitmask to indicate NaN states */
+	uint8_t health_flags;			/**< Bitmask to indicate sensor health states (vel, pos, hgt) */
+	uint8_t timeout_flags;			/**< Bitmask to indicate timeout flags (vel, pos, hgt) */
 
 };
 
diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h
index 08d11abae..70071130d 100644
--- a/src/modules/uORB/topics/home_position.h
+++ b/src/modules/uORB/topics/home_position.h
@@ -59,10 +59,13 @@ struct home_position_s
 {
 	uint64_t timestamp;			/**< Timestamp (microseconds since system boot)	*/
 
-	//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				*/
+
+	float x;				/**< X coordinate in meters			*/
+	float y;				/**< Y coordinate in meters			*/
+	float z;				/**< Z coordinate in meters			*/
 };
 
 /**
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index ef4bc1def..d9dd61df1 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.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>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,9 @@
 /**
  * @file mission.h
  * Definition of a mission consisting of mission items.
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef TOPIC_MISSION_H_
@@ -50,6 +50,7 @@
 
 /* compatible to mavlink MAV_CMD */
 enum NAV_CMD {
+	NAV_CMD_IDLE=0,
 	NAV_CMD_WAYPOINT=16,
 	NAV_CMD_LOITER_UNLIMITED=17,
 	NAV_CMD_LOITER_TURN_COUNT=18,
@@ -58,7 +59,8 @@ enum NAV_CMD {
 	NAV_CMD_LAND=21,
 	NAV_CMD_TAKEOFF=22,
 	NAV_CMD_ROI=80,
-	NAV_CMD_PATHPLANNING=81
+	NAV_CMD_PATHPLANNING=81,
+	NAV_CMD_DO_JUMP=177
 };
 
 enum ORIGIN {
@@ -91,6 +93,9 @@ struct mission_item_s {
 	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		*/
+	int do_jump_mission_index;	/**< index where the do jump will go to                 */
+	unsigned do_jump_repeat_count;	/**< how many times do jump needs to be done            */
+	unsigned do_jump_current_count;	/**< count how many times the jump has been done	*/
 };
 
 struct mission_s
diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h
index 7c3921198..ad654a9ff 100644
--- a/src/modules/uORB/topics/mission_result.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -56,6 +56,7 @@ struct mission_result_s
 	bool mission_reached;		/**< true if mission has been reached			*/
 	unsigned mission_index_reached;	/**< index of the mission which has been reached	*/
 	unsigned index_current_mission; /**< index of the current mission			*/
+	bool mission_finished;		/**< true if mission has been completed			*/
 };
 
 /**
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 34aaa30dd..ce42035ba 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
  *   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>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,10 @@
 /**
  * @file mission_item_triplet.h
  * Definition of the global WGS84 position setpoint uORB topic.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef TOPIC_MISSION_ITEM_TRIPLET_H_
@@ -45,7 +46,6 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
-#include <navigator/navigator_state.h>
 
 /**
  * @addtogroup topics
@@ -54,11 +54,12 @@
 
 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) */
+	SETPOINT_TYPE_POSITION = 0,	/**< position setpoint */
+	SETPOINT_TYPE_VELOCITY,		/**< velocity setpoint */
+	SETPOINT_TYPE_LOITER,		/**< loiter setpoint */
+	SETPOINT_TYPE_TAKEOFF,		/**< takeoff setpoint */
+	SETPOINT_TYPE_LAND,		/**< land setpoint, altitude must be ignored, descend until landing */
+	SETPOINT_TYPE_IDLE,		/**< do nothing, switch off motors or keep at idle speed (MC) */
 };
 
 struct position_setpoint_s
@@ -84,8 +85,6 @@ 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 */
 };
 
 /**
diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h
new file mode 100644
index 000000000..c4d0c1874
--- /dev/null
+++ b/src/modules/uORB/topics/tecs_status.h
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 vehicle_global_position.h
+ * Definition of the global fused WGS84 position uORB topic.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ */
+
+#ifndef TECS_STATUS_T_H_
+#define TECS_STATUS_T_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+typedef enum {
+	TECS_MODE_NORMAL,
+	TECS_MODE_UNDERSPEED,
+	TECS_MODE_TAKEOFF,
+	TECS_MODE_LAND,
+	TECS_MODE_LAND_THROTTLELIM
+} tecs_mode;
+
+ /**
+ * Internal values of the (m)TECS fixed wing speed alnd altitude control system
+ */
+struct tecs_status_s {
+	uint64_t timestamp;		/**< timestamp, in microseconds since system start */
+
+	float altitudeSp;
+	float altitude;
+	float flightPathAngleSp;
+	float flightPathAngle;
+	float flightPathAngleFiltered;
+	float airspeedSp;
+	float airspeed;
+	float airspeedFiltered;
+	float airspeedDerivativeSp;
+	float airspeedDerivative;
+
+	float totalEnergyRateSp;
+	float totalEnergyRate;
+	float energyDistributionRateSp;
+	float energyDistributionRate;
+
+	tecs_mode mode;
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(tecs_status);
+
+#endif
diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h
index 76693c46e..e9e00d76c 100644
--- a/src/modules/uORB/topics/telemetry_status.h
+++ b/src/modules/uORB/topics/telemetry_status.h
@@ -57,6 +57,7 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
 
 struct telemetry_status_s {
 	uint64_t timestamp;
+	uint64_t heartbeat_time;	/**< Time of last received heartbeat from remote system */
 	enum TELEMETRY_STATUS_RADIO_TYPE type;	/**< type of the radio hardware     */
 	uint8_t rssi;				/**< local signal strength                      */
 	uint8_t remote_rssi;			/**< remote signal strength                     */
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index 4897ca737..e32529cb4 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -36,7 +36,7 @@
  * Definition of the global fused WGS84 position uORB topic.
  *
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
@@ -61,15 +61,14 @@
  * 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					   */
+	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start		*/
+	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 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.							   */
 	float eph;
 	float epv;
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 5924a324d..bbacb733a 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -65,8 +65,8 @@ struct vehicle_gps_position_s {
 	float c_variance_rad;				/**< course accuracy estimate rad */
 	uint8_t fix_type; 				/**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   */
 
-	float eph_m;					/**< GPS HDOP horizontal dilution of position in m */
-	float epv_m;					/**< GPS VDOP horizontal dilution of position in m */
+	float eph;					/**< GPS HDOP horizontal dilution of position in m */
+	float epv;					/**< GPS VDOP horizontal dilution of position in m */
 
 	unsigned noise_per_ms;				/**< */
 	unsigned jamming_indicator;			/**< */
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 85962883d..56590047f 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.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
@@ -45,6 +41,11 @@
  * All apps should write to subsystem_info:
  *
  *  (any app) --> subsystem_info (published) --> (commander app state machine)  --> vehicle_status --> (mavlink app)
+ *
+ * @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 <julian@oes.ch>
  */
 
 #ifndef VEHICLE_STATUS_H_
@@ -54,20 +55,22 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-#include <navigator/navigator_state.h>
-
 /**
  * @addtogroup topics @{
  */
 
-/* main state machine */
+/**
+ * Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
+ */
 typedef enum {
 	MAIN_STATE_MANUAL = 0,
 	MAIN_STATE_ALTCTL,
 	MAIN_STATE_POSCTL,
-	MAIN_STATE_AUTO,
+	MAIN_STATE_AUTO_MISSION,
+	MAIN_STATE_AUTO_LOITER,
+	MAIN_STATE_AUTO_RTL,
 	MAIN_STATE_ACRO,
-	MAIN_STATE_MAX
+	MAIN_STATE_MAX,
 } main_state_t;
 
 // If you change the order, add or remove arming_state_t states make sure to update the arrays
@@ -80,7 +83,7 @@ typedef enum {
 	ARMING_STATE_STANDBY_ERROR,
 	ARMING_STATE_REBOOT,
 	ARMING_STATE_IN_AIR_RESTORE,
-	ARMING_STATE_MAX
+	ARMING_STATE_MAX,
 } arming_state_t;
 
 typedef enum {
@@ -88,13 +91,23 @@ typedef enum {
 	HIL_STATE_ON
 } hil_state_t;
 
+/**
+ * Navigation state, i.e. "what should vehicle do".
+ */
 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;
+	NAVIGATION_STATE_MANUAL = 0,		/**< Manual mode */
+	NAVIGATION_STATE_ALTCTL,		/**< Altitude control mode */
+	NAVIGATION_STATE_POSCTL,		/**< Position control mode */
+	NAVIGATION_STATE_AUTO_MISSION,		/**< Auto mission mode */
+	NAVIGATION_STATE_AUTO_LOITER,		/**< Auto loiter mode */
+	NAVIGATION_STATE_AUTO_RTL,		/**< Auto return to launch mode */
+	NAVIGATION_STATE_AUTO_RTGS,		/**< Auto return to groundstation on data link loss */
+	NAVIGATION_STATE_ACRO,			/**< Acro mode */
+	NAVIGATION_STATE_LAND,			/**< Land mode */
+	NAVIGATION_STATE_DESCEND,			/**< Descend mode (no position control) */
+	NAVIGATION_STATE_TERMINATION,		/**< Termination mode */
+	NAVIGATION_STATE_MAX,
+} navigation_state_t;
 
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
@@ -154,12 +167,11 @@ struct vehicle_status_s {
 	uint16_t counter;   /**< incremented by the writing thread everytime new data is stored */
 	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 */
-	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 */
+	main_state_t main_state;		    	/**< main state machine */
+	navigation_state_t nav_state;		/**< set navigation state machine to specified value */
 	arming_state_t arming_state;			/**< current arming state */
-	hil_state_t hil_state;					/**< current hil state */
-	failsafe_state_t failsafe_state;		/**< current failsafe state */
+	hil_state_t hil_state;				/**< current hil state */
+	bool failsafe;					/**< true if system is in 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 */
@@ -186,6 +198,8 @@ struct vehicle_status_s {
 	bool rc_signal_lost;				/**< true if RC reception lost */
 	bool rc_input_blocked;				/**< set if RC input should be ignored */
 
+	bool data_link_lost;						/**< datalink to GCS lost */
+
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;
 	bool offboard_control_signal_weak;
diff --git a/src/modules/uORB/topics/wind_estimate.h b/src/modules/uORB/topics/wind_estimate.h
new file mode 100644
index 000000000..58333a64f
--- /dev/null
+++ b/src/modules/uORB/topics/wind_estimate.h
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file wind_estimate.h
+ *
+ * Wind estimate topic topic
+ *
+ */
+
+#ifndef TOPIC_WIND_ESTIMATE_H
+#define TOPIC_WIND_ESTIMATE_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/** Wind estimate */
+struct wind_estimate_s {
+
+	uint64_t	timestamp;		/**< Microseconds since system boot */
+	float		windspeed_north;	/**< Wind component in north / X direction */
+	float		windspeed_east;		/**< Wind component in east / Y direction */
+	float		covariance_north;	/**< Uncertainty - set to zero (no uncertainty) if not estimated */
+	float		covariance_east;	/**< Uncertainty - set to zero (no uncertainty) if not estimated */
+};
+
+/**
+ * @}
+ */
+
+ORB_DECLARE(wind_estimate);
+
+#endif
\ No newline at end of file
diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c
index a57eaafe7..fcc9b8366 100644
--- a/src/systemcmds/mtd/mtd.c
+++ b/src/systemcmds/mtd/mtd.c
@@ -193,8 +193,12 @@ ramtron_attach(void)
 		errx(1, "failed to initialize mtd driver");
 
 	int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000);
-        if (ret != OK)
-            warnx(1, "failed to set bus speed");
+	if (ret != OK) {
+		// FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried
+		// that but setting the bug speed does fail all the time. Which was then exiting and the board would
+		// not run correctly. So changed to warnx.
+		warnx("failed to set bus speed");
+	}
 
 	attached = true;
 }
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index df382e2c6..0b826b826 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -372,6 +372,7 @@ int test_mixer(int argc, char *argv[])
 	}
 
 	warnx("SUCCESS: No errors in mixer test");
+	return 0;
 }
 
 static int