Merge branch 'navigator_new' into navigator_new_vector, WIP

77 files changed, 5876 insertions, 1374 deletions

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index f551c1aa8..14bb820fa 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -113,6 +113,16 @@ then
 	#
 	commander start
 
+	#
+	# Start the datamanager
+	#
+	dataman start
+
+	#
+	# Start the Navigator
+	#
+	navigator start
+
 	if param compare SYS_AUTOSTART 1000
 	then
 		sh /etc/init.d/1000_rc_fw_easystar.hil		
diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk
index 5ecf93a3a..6da78b287 100644
--- a/makefiles/config_px4fmu-v1_backside.mk
+++ b/makefiles/config_px4fmu-v1_backside.mk
@@ -107,6 +107,7 @@ MODULES		+= modules/systemlib
 MODULES		+= modules/systemlib/mixer
 MODULES		+= modules/controllib
 MODULES		+= modules/uORB
+MODULES		+= modules/dataman
 
 #
 # Libraries
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index eaf8ba5b0..24c08b8bd 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -106,6 +106,7 @@ MODULES		+= modules/systemlib
 MODULES		+= modules/systemlib/mixer
 MODULES		+= modules/controllib
 MODULES		+= modules/uORB
+MODULES		+= modules/dataman
 
 #
 # Libraries
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index f9573b605..f63f143c6 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -104,6 +104,7 @@ MODULES		+= modules/systemlib
 MODULES		+= modules/systemlib/mixer
 MODULES		+= modules/controllib
 MODULES		+= modules/uORB
+MODULES		+= modules/dataman
 
 #
 # Libraries
diff --git a/makefiles/config_px4fmu-v2_logging.mk b/makefiles/config_px4fmu-v2_logging.mk
index ed90f6464..4e3cae5ee 100644
--- a/makefiles/config_px4fmu-v2_logging.mk
+++ b/makefiles/config_px4fmu-v2_logging.mk
@@ -115,6 +115,7 @@ MODULES		+= lib/ecl
 MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
 MODULES		+= lib/conversion
+MODULES		+= modules/dataman
 
 #
 # Demo apps
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index 66771faaa..03f1dfbe5 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 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
diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index bb8d45bea..90a744015 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -295,8 +295,8 @@ build_gps_response(uint8_t *buffer, size_t *size)
 			memset(&home, 0, sizeof(home));
 			orb_copy(ORB_ID(home_position), _home_sub, &home);
 
-			_home_lat = ((double)(home.lat))*1e-7d;
-			_home_lon = ((double)(home.lon))*1e-7d;
+			_home_lat = home.lat;
+			_home_lon = home.lon;
 			_home_position_set = true;
 		}
 
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 010272c6f..9812ea497 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -228,33 +228,36 @@ private:
 	device::Device		*_interface;
 
 	// XXX
-	unsigned		_hardware;			///< Hardware revision
-	unsigned		_max_actuators;		///<Maximum # of actuators supported by PX4IO
-	unsigned		_max_controls;		///<Maximum # of controls supported by PX4IO
-	unsigned		_max_rc_input;		///<Maximum receiver channels supported by PX4IO
-	unsigned		_max_relays;		///<Maximum relays supported by PX4IO
-	unsigned		_max_transfer;		///<Maximum number of I2C transfers supported by PX4IO
+	unsigned		_hardware;		///< Hardware revision
+	unsigned		_max_actuators;		///< Maximum # of actuators supported by PX4IO
+	unsigned		_max_controls;		///< Maximum # of controls supported by PX4IO
+	unsigned		_max_rc_input;		///< Maximum receiver channels supported by PX4IO
+	unsigned		_max_relays;		///< Maximum relays supported by PX4IO
+	unsigned		_max_transfer;		///< Maximum number of I2C transfers supported by PX4IO
 
 	unsigned 		_update_interval;	///< Subscription interval limiting send rate
 	bool			_rc_handling_disabled;	///< If set, IO does not evaluate, but only forward the RC values
 	unsigned		_rc_chan_count;		///< Internal copy of the last seen number of RC channels
 
-	volatile int		_task;			///<worker task id
-	volatile bool		_task_should_exit;	///<worker terminate flag
+	volatile int		_task;			///< worker task id
+	volatile bool		_task_should_exit;	///< worker terminate flag
 
-	int			_mavlink_fd;		///<mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
-	int			_thread_mavlink_fd;	///<mavlink file descriptor for thread.
+	int			_mavlink_fd;		///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
+	int			_thread_mavlink_fd;	///< mavlink file descriptor for thread.
 
 	perf_counter_t		_perf_update;		///<local performance counter for status updates
 	perf_counter_t		_perf_write;		///<local performance counter for PWM control writes
 	perf_counter_t		_perf_chan_count;	///<local performance counter for channel number changes
 
 	/* cached IO state */
-	uint16_t		_status;		///<Various IO status flags
-	uint16_t		_alarms;		///<Various IO alarms
+	uint16_t		_status;		///< Various IO status flags
+	uint16_t		_alarms;		///< Various IO alarms
 
 	/* subscribed topics */
-	int			_t_actuators;		///< actuator controls topic
+	int			_t_actuator_controls_0;	///< actuator controls group 0 topic
+	int			_t_actuator_controls_1;	///< actuator controls group 1 topic
+	int			_t_actuator_controls_2;	///< actuator controls group 2 topic
+	int			_t_actuator_controls_3;	///< actuator controls group 3 topic
 	int			_t_actuator_armed;	///< system armed control topic
 	int 			_t_vehicle_control_mode;///< vehicle control mode topic
 	int			_t_param;		///< parameter update topic
@@ -269,15 +272,15 @@ private:
 
 	actuator_outputs_s	_outputs;		///<mixed outputs
 
-	bool			_primary_pwm_device;	///<true if we are the default PWM output
+	bool			_primary_pwm_device;	///< true if we are the default PWM output
 
-	float			_battery_amp_per_volt;	///<current sensor amps/volt
-	float			_battery_amp_bias;	///<current sensor bias
-	float			_battery_mamphour_total;///<amp hours consumed so far
-	uint64_t		_battery_last_timestamp;///<last amp hour calculation timestamp
+	float			_battery_amp_per_volt;	///< current sensor amps/volt
+	float			_battery_amp_bias;	///< current sensor bias
+	float			_battery_mamphour_total;///< amp hours consumed so far
+	uint64_t		_battery_last_timestamp;///< last amp hour calculation timestamp
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-	bool			_dsm_vcc_ctl;		///<true if relay 1 controls DSM satellite RX power
+	bool			_dsm_vcc_ctl;		///< true if relay 1 controls DSM satellite RX power
 #endif
 
 	/**
@@ -291,9 +294,14 @@ private:
 	void			task_main();
 
 	/**
-	 * Send controls to IO
+	 * Send controls for one group to IO
 	 */
-	int			io_set_control_state();
+	int			io_set_control_state(unsigned group);
+
+	/**
+	 * Send all controls to IO
+	 */
+	int			io_set_control_groups();
 
 	/**
 	 * Update IO's arming-related state
@@ -467,7 +475,10 @@ PX4IO::PX4IO(device::Device *interface) :
 	_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
 	_status(0),
 	_alarms(0),
-	_t_actuators(-1),
+	_t_actuator_controls_0(-1),
+	_t_actuator_controls_1(-1),
+	_t_actuator_controls_2(-1),
+	_t_actuator_controls_3(-1),
 	_t_actuator_armed(-1),
 	_t_vehicle_control_mode(-1),
 	_t_param(-1),
@@ -769,15 +780,20 @@ PX4IO::task_main()
 	 * Subscribe to the appropriate PWM output topic based on whether we are the
 	 * primary PWM output or not.
 	 */
-	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
-				     ORB_ID(actuator_controls_1));
-	orb_set_interval(_t_actuators, 20);		/* default to 50Hz */
+	_t_actuator_controls_0 = orb_subscribe(ORB_ID(actuator_controls_0));
+	orb_set_interval(_t_actuator_controls_0, 20);		/* default to 50Hz */
+	_t_actuator_controls_1 = orb_subscribe(ORB_ID(actuator_controls_1));
+	orb_set_interval(_t_actuator_controls_1, 33);		/* default to 30Hz */
+	_t_actuator_controls_2 = orb_subscribe(ORB_ID(actuator_controls_2));
+	orb_set_interval(_t_actuator_controls_2, 33);		/* default to 30Hz */
+	_t_actuator_controls_3 = orb_subscribe(ORB_ID(actuator_controls_3));
+	orb_set_interval(_t_actuator_controls_3, 33);		/* default to 30Hz */
 	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
 	_t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_t_param = orb_subscribe(ORB_ID(parameter_update));
 	_t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
 
-	if ((_t_actuators < 0) ||
+	if ((_t_actuator_controls_0 < 0) ||
 	    (_t_actuator_armed < 0) ||
 	    (_t_vehicle_control_mode < 0) ||
 	    (_t_param < 0) ||
@@ -788,7 +804,7 @@ PX4IO::task_main()
 
 	/* poll descriptor */
 	pollfd fds[1];
-	fds[0].fd = _t_actuators;
+	fds[0].fd = _t_actuator_controls_0;
 	fds[0].events = POLLIN;
 
 	log("ready");
@@ -807,7 +823,11 @@ PX4IO::task_main()
 			if (_update_interval > 100)
 				_update_interval = 100;
 
-			orb_set_interval(_t_actuators, _update_interval);
+			orb_set_interval(_t_actuator_controls_0, _update_interval);
+			/*
+			 * NOT changing the rate of groups 1-3 here, because only attitude
+			 * really needs to run fast.
+			 */
 			_update_interval = 0;
 		}
 
@@ -827,7 +847,10 @@ PX4IO::task_main()
 
 		/* if we have new control data from the ORB, handle it */
 		if (fds[0].revents & POLLIN) {
-			io_set_control_state();
+
+			/* we're not nice to the lower-priority control groups and only check them
+			   when the primary group updated (which is now). */
+			io_set_control_groups();
 		}
 
 		if (now >= poll_last + IO_POLL_INTERVAL) {
@@ -870,7 +893,7 @@ PX4IO::task_main()
 				orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
 
 				// Check for a DSM pairing command
-				if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1 == 0.0f)) {
+				if (((int)cmd.command == VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) {
 					dsm_bind_ioctl((int)cmd.param2);
 				}
 			}
@@ -937,20 +960,74 @@ out:
 }
 
 int
-PX4IO::io_set_control_state()
+PX4IO::io_set_control_groups()
+{
+	bool attitude_ok = io_set_control_state(0);
+
+	/* send auxiliary control groups */
+	(void)io_set_control_state(1);
+	(void)io_set_control_state(2);
+	(void)io_set_control_state(3);
+
+	return attitude_ok;
+}
+
+int
+PX4IO::io_set_control_state(unsigned group)
 {
 	actuator_controls_s	controls;	///< actuator outputs
 	uint16_t 		regs[_max_actuators];
 
 	/* get controls */
-	orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
-		 ORB_ID(actuator_controls_1), _t_actuators, &controls);
+	bool changed;
+
+	switch (group) {
+		case 0:
+		{
+			orb_check(_t_actuator_controls_0, &changed);
+
+			if (changed) {
+				orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls);
+			}
+		}
+		break;
+		case 1:
+		{
+			orb_check(_t_actuator_controls_1, &changed);
+
+			if (changed) {
+				orb_copy(ORB_ID(actuator_controls_1), _t_actuator_controls_1, &controls);
+			}
+		}
+		break;
+		case 2:
+		{
+			orb_check(_t_actuator_controls_2, &changed);
+
+			if (changed) {
+				orb_copy(ORB_ID(actuator_controls_2), _t_actuator_controls_2, &controls);
+			}
+		}
+		break;
+		case 3:
+		{
+			orb_check(_t_actuator_controls_3, &changed);
+
+			if (changed) {
+				orb_copy(ORB_ID(actuator_controls_3), _t_actuator_controls_3, &controls);
+			}
+		}
+		break;
+	}
+
+	if (!changed)
+		return -1;
 
 	for (unsigned i = 0; i < _max_controls; i++)
 		regs[i] = FLOAT_TO_REG(controls.control[i]);
 
 	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
+	return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
 }
 
 
diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c
index 3125ce246..391e40ac1 100644
--- a/src/examples/flow_position_control/flow_position_control_main.c
+++ b/src/examples/flow_position_control/flow_position_control_main.c
@@ -61,7 +61,6 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/filtered_bottom_flow.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/perf_counter.h>
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 2eb58abd6..d7dbbebd4 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -45,39 +45,30 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+#include <systemlib/err.h>
 
 ECL_PitchController::ECL_PitchController() :
 	_last_run(0),
+	_tc(0.1f),
+	_k_p(0.0f),
+	_k_i(0.0f),
+	_k_d(0.0f),
+	_k_ff(0.0f),
+	_integrator_max(0.0f),
+	_max_rate_pos(0.0f),
+	_max_rate_neg(0.0f),
+	_roll_ff(0.0f),
 	_last_output(0.0f),
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_max_deflection_rad(math::radians(45.0f))
+	_bodyrate_setpoint(0.0f)
 {
 }
 
-float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
-				   bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
+float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
 {
-	/* get the usual dt estimate */
-	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
-	_last_run = ecl_absolute_time();
-	float dt = (float)dt_micros * 1e-6f;
-
-	/* lock integral for long intervals */
-	if (dt_micros > 500000)
-		lock_integrator = true;
 
-	float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_i_rate = _k_i * _tc;
-
-	/* input conditioning */
-	if (!isfinite(airspeed)) {
-		/* airspeed is NaN, +- INF or not available, pick center of band */
-		airspeed = 0.5f * (airspeed_min + airspeed_max);
-	} else if (airspeed < airspeed_min) {
-		airspeed = airspeed_min;
-	}
 
 	/* flying inverted (wings upside down) ? */
 	bool inverted = false;
@@ -105,29 +96,72 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 	if (inverted)
 		turn_offset = -turn_offset;
 
+	/* Calculate the error */
 	float pitch_error = pitch_setpoint - pitch;
-	/* rate setpoint from current error and time constant */
-	_rate_setpoint = pitch_error / _tc;
+
+	/*  Apply P controller: rate setpoint from current error and time constant */
+	_rate_setpoint =  pitch_error / _tc;
 
 	/* add turn offset */
 	_rate_setpoint += turn_offset;
 
-	_rate_error = _rate_setpoint - pitch_rate;
+	/* limit the rate */ //XXX: move to body angluar rates
+	if (_max_rate_pos > 0.01f && _max_rate_neg > 0.01f) {
+		if (_rate_setpoint > 0.0f) {
+			_rate_setpoint = (_rate_setpoint > _max_rate_pos) ? _max_rate_pos : _rate_setpoint;
+		} else {
+			_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
+		}
 
-	float ilimit_scaled = _integrator_max * scaler;
+	}
 
-	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+	return _rate_setpoint;
+}
 
-		float id = _rate_error * k_i_rate * dt * scaler;
+float ECL_PitchController::control_bodyrate(float roll, float pitch,
+		float pitch_rate, float yaw_rate,
+		float yaw_rate_setpoint,
+		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
+{
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
+
+	/* lock integral for long intervals */
+	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 */
+		airspeed = 0.5f * (airspeed_min + airspeed_max);
+	} else if (airspeed < airspeed_min) {
+		airspeed = airspeed_min;
+	}
+
+	/* Transform setpoint to body angular rates */
+	_bodyrate_setpoint = cosf(roll) * _rate_setpoint + cosf(pitch) * sinf(roll) * yaw_rate_setpoint; //jacobian
+
+	/* Transform estimation to body angular rates */
+	float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian
+
+	_rate_error = _bodyrate_setpoint - pitch_bodyrate;
+
+	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+		float id = _rate_error * dt;
 
 		/*
-		 * anti-windup: do not allow integrator to increase into the
-		 * wrong direction if actuator is at limit
+		 * anti-windup: do not allow integrator to increase if actuator is at limit
 		 */
-		if (_last_output < -_max_deflection_rad) {
+		if (_last_output < -1.0f) {
 			/* only allow motion to center: increase value */
 			id = math::max(id, 0.0f);
-		} else if (_last_output > _max_deflection_rad) {
+		} else if (_last_output > 1.0f) {
 			/* only allow motion to center: decrease value */
 			id = math::min(id, 0.0f);
 		}
@@ -136,11 +170,14 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 	}
 
 	/* integrator limit */
-	_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
-	/* store non-limited output */
-	_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler;
-
-	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+
+	/* Apply PI rate controller and store non-limited output */
+	_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
+//	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
+//	warnx("roll: _last_output %.4f", (double)_last_output);
+	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
 void ECL_PitchController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
index 1e6cec6a1..2ca0490fd 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
@@ -36,6 +36,7 @@
  * Definition of a simple orthogonal pitch PID controller.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  *
  * Acknowledgements:
  *
@@ -51,13 +52,18 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-class __EXPORT ECL_PitchController
+class __EXPORT ECL_PitchController //XXX: create controller superclass
 {
 public:
 	ECL_PitchController();
 
-	float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f,
-		      bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
+	float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed);
+
+
+	float control_bodyrate(float roll, float pitch,
+			float pitch_rate, float yaw_rate,
+			float yaw_rate_setpoint,
+			float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
 
 	void reset_integrator();
 
@@ -67,21 +73,30 @@ public:
 	void set_k_p(float k_p) {
 		_k_p = k_p;
 	}
+
 	void set_k_i(float k_i) {
 		_k_i = k_i;
 	}
 	void set_k_d(float k_d) {
 		_k_d = k_d;
 	}
+
+	void set_k_ff(float k_ff) {
+		_k_ff = k_ff;
+	}
+
 	void set_integrator_max(float max) {
 		_integrator_max = max;
 	}
+
 	void set_max_rate_pos(float max_rate_pos) {
 		_max_rate_pos = max_rate_pos;
 	}
+
 	void set_max_rate_neg(float max_rate_neg) {
 		_max_rate_neg = max_rate_neg;
 	}
+
 	void set_roll_ff(float roll_ff) {
 		_roll_ff = roll_ff;
 	}
@@ -94,6 +109,10 @@ public:
 		return _rate_setpoint;
 	}
 
+	float get_desired_bodyrate() {
+		return _bodyrate_setpoint;
+	}
+
 private:
 
 	uint64_t _last_run;
@@ -101,6 +120,7 @@ private:
 	float _k_p;
 	float _k_i;
 	float _k_d;
+	float _k_ff;
 	float _integrator_max;
 	float _max_rate_pos;
 	float _max_rate_neg;
@@ -109,7 +129,7 @@ private:
 	float _integrator;
 	float _rate_error;
 	float _rate_setpoint;
-	float _max_deflection_rad;
+	float _bodyrate_setpoint;
 };
 
 #endif // ECL_PITCH_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 0b1ffa7a4..bd6c9da71 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -45,21 +45,47 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+#include <systemlib/err.h>
 
 ECL_RollController::ECL_RollController() :
 	_last_run(0),
 	_tc(0.1f),
+	_k_p(0.0f),
+	_k_i(0.0f),
+	_k_d(0.0f),
+	_k_ff(0.0f),
+	_integrator_max(0.0f),
+	_max_rate(0.0f),
 	_last_output(0.0f),
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_max_deflection_rad(math::radians(45.0f))
+	_bodyrate_setpoint(0.0f)
 {
+}
+
+float ECL_RollController::control_attitude(float roll_setpoint, float roll)
+{
+
+	/* Calculate error */
+	float roll_error = roll_setpoint - roll;
+
+	/* Apply P controller */
+	_rate_setpoint = roll_error / _tc;
+
+	/* limit the rate */ //XXX: move to body angluar rates
+	if (_max_rate > 0.01f) {
+		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+	}
 
+	return _rate_setpoint;
 }
 
-float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate,
-				  float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
+float ECL_RollController::control_bodyrate(float pitch,
+		float roll_rate, float yaw_rate,
+		float yaw_rate_setpoint,
+		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
 {
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
@@ -70,10 +96,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_i_rate = _k_i * _tc;
+//	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)) {
 		/* airspeed is NaN, +- INF or not available, pick center of band */
 		airspeed = 0.5f * (airspeed_min + airspeed_max);
@@ -81,32 +108,27 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 		airspeed = airspeed_min;
 	}
 
-	float roll_error = roll_setpoint - roll;
-	_rate_setpoint = roll_error / _tc;
-
-	/* limit the rate */
-	if (_max_rate > 0.01f) {
-		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
-		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
-	}
 
-	_rate_error = _rate_setpoint - roll_rate;
+	/* Transform setpoint to body angular rates */
+	_bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian
 
+	/* Transform estimation to body angular rates */
+	float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian
 
-	float ilimit_scaled = _integrator_max * scaler;
+	/* Calculate body angular rate error */
+	_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
 
-	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
 
-		float id = _rate_error * k_i_rate * dt * scaler;
+		float id = _rate_error * dt;
 
 		/*
-		 * anti-windup: do not allow integrator to increase into the
-		 * wrong direction if actuator is at limit
+		 * anti-windup: do not allow integrator to increase if actuator is at limit
 		 */
-		if (_last_output < -_max_deflection_rad) {
+		if (_last_output < -1.0f) {
 			/* only allow motion to center: increase value */
 			id = math::max(id, 0.0f);
-		} else if (_last_output > _max_deflection_rad) {
+		} else if (_last_output > 1.0f) {
 			/* only allow motion to center: decrease value */
 			id = math::min(id, 0.0f);
 		}
@@ -115,11 +137,14 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	}
 
 	/* integrator limit */
-	_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
-	/* store non-limited output */
-	_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler;
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
+
+	/* Apply PI rate controller and store non-limited output */
+	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
 
-	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
 void ECL_RollController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
index 0d4ea9333..efc7b8944 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -36,6 +36,7 @@
  * Definition of a simple orthogonal roll PID controller.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  *
  * Acknowledgements:
  *
@@ -51,13 +52,17 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-class __EXPORT ECL_RollController
+class __EXPORT ECL_RollController //XXX: create controller superclass
 {
 public:
 	ECL_RollController();
 
-	float control(float roll_setpoint, float roll, float roll_rate,
-		      float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
+	float control_attitude(float roll_setpoint, float roll);
+
+	float control_bodyrate(float pitch,
+			float roll_rate, float yaw_rate,
+			float yaw_rate_setpoint,
+			float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
 
 	void reset_integrator();
 
@@ -66,18 +71,27 @@ public:
 			_tc = time_constant;
 		}
 	}
+
 	void set_k_p(float k_p) {
 		_k_p = k_p;
 	}
+
 	void set_k_i(float k_i) {
 		_k_i = k_i;
 	}
+
 	void set_k_d(float k_d) {
 		_k_d = k_d;
 	}
+
+	void set_k_ff(float k_ff) {
+		_k_ff = k_ff;
+	}
+
 	void set_integrator_max(float max) {
 		_integrator_max = max;
 	}
+
 	void set_max_rate(float max_rate) {
 		_max_rate = max_rate;
 	}
@@ -90,19 +104,24 @@ public:
 		return _rate_setpoint;
 	}
 
+	float get_desired_bodyrate() {
+		return _bodyrate_setpoint;
+	}
+
 private:
 	uint64_t _last_run;
 	float _tc;
 	float _k_p;
 	float _k_i;
 	float _k_d;
+	float _k_ff;
 	float _integrator_max;
 	float _max_rate;
 	float _last_output;
 	float _integrator;
 	float _rate_error;
 	float _rate_setpoint;
-	float _max_deflection_rad;
+	float _bodyrate_setpoint;
 };
 
 #endif // ECL_ROLL_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index b786acf24..7c366aaf2 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -44,29 +44,127 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+#include <systemlib/err.h>
 
 ECL_YawController::ECL_YawController() :
 	_last_run(0),
-	_last_error(0.0f),
+	_k_p(0.0f),
+	_k_i(0.0f),
+	_k_d(0.0f),
+	_k_ff(0.0f),
+	_integrator_max(0.0f),
+	_max_rate(0.0f),
+	_roll_ff(0.0f),
 	_last_output(0.0f),
-	_last_rate_hp_out(0.0f),
-	_last_rate_hp_in(0.0f),
-	_k_d_last(0.0f),
-	_integrator(0.0f)
+	_integrator(0.0f),
+	_rate_error(0.0f),
+	_rate_setpoint(0.0f),
+	_bodyrate_setpoint(0.0f),
+	_coordinated_min_speed(1.0f)
 {
+}
+
+float ECL_YawController::control_attitude(float roll, float pitch,
+		float speed_body_u, float speed_body_v, float speed_body_w,
+		float roll_rate_setpoint, float pitch_rate_setpoint)
+{
+//	static int counter = 0;
+	/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
+	_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
+			_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);
+		}
+
+//		if(counter % 20 == 0) {
+//			warnx("denumerator: %.4f, speed_body_u: %.4f, speed_body_w: %.4f, cosf(roll): %.4f, cosf(pitch): %.4f, sinf(pitch): %.4f", (double)denumerator, (double)speed_body_u, (double)speed_body_w, (double)cosf(roll), (double)cosf(pitch), (double)sinf(pitch));
+//		}
+	}
+
+	/* limit the rate */ //XXX: move to body angluar rates
+	if (_max_rate > 0.01f) {
+	_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+	_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+	}
+
 
+//	counter++;
+
+	if(!isfinite(_rate_setpoint)){
+		warnx("yaw rate sepoint not finite");
+		_rate_setpoint = 0.0f;
+	}
+
+	return _rate_setpoint;
 }
 
-float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator,
-				 float airspeed_min, float airspeed_max, float aspeed)
+float ECL_YawController::control_bodyrate(float roll, float pitch,
+		float pitch_rate, float yaw_rate,
+		float pitch_rate_setpoint,
+		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
 {
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
+
+	/* lock integral for long intervals */
+	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 */
+	airspeed = 0.5f * (airspeed_min + airspeed_max);
+	} else if (airspeed < airspeed_min) {
+	airspeed = airspeed_min;
+	}
+
+
+	/* Transform setpoint to body angular rates */
+	_bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian
+
+	/* Transform estimation to body angular rates */
+	float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian
+
+	/* Calculate body angular rate error */
+	_rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
+
+	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+	float id = _rate_error * dt;
+
+	/*
+	 * anti-windup: do not allow integrator to increase if actuator is at limit
+	 */
+	if (_last_output < -1.0f) {
+		/* only allow motion to center: increase value */
+		id = math::max(id, 0.0f);
+	} else if (_last_output > 1.0f) {
+		/* only allow motion to center: decrease value */
+		id = math::min(id, 0.0f);
+	}
+
+	_integrator += id;
+	}
+
+	/* integrator limit */
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+
+	/* Apply PI rate controller and store non-limited output */
+	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
 
-	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
 
-	return 0.0f;
+	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
 void ECL_YawController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index 66b227918..f15645fcf 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -35,6 +35,15 @@
  * @file ecl_yaw_controller.h
  * Definition of a simple orthogonal coordinated turn yaw PID controller.
  *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * Acknowledgements:
+ *
+ *   The control design is based on a design
+ *   by Paul Riseborough and Andrew Tridgell, 2013,
+ *   which in turn is based on initial work of
+ *   Jonathan Challinger, 2012.
  */
 #ifndef ECL_YAW_CONTROLLER_H
 #define ECL_YAW_CONTROLLER_H
@@ -42,47 +51,82 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-class __EXPORT ECL_YawController
+class __EXPORT ECL_YawController //XXX: create controller superclass
 {
 public:
 	ECL_YawController();
 
-	float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false,
-		      float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f));
+	float control_attitude(float roll, float pitch,
+			float speed_body_u, float speed_body_v, float speed_body_w,
+			float roll_rate_setpoint, float pitch_rate_setpoint);
+
+	float control_bodyrate(	float roll, float pitch,
+			float pitch_rate, float yaw_rate,
+			float pitch_rate_setpoint,
+			float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator);
 
 	void reset_integrator();
 
-	void set_k_side(float k_a) {
-		_k_side = k_a;
+	void set_k_p(float k_p) {
+			_k_p = k_p;
 	}
+
 	void set_k_i(float k_i) {
 		_k_i = k_i;
 	}
+
 	void set_k_d(float k_d) {
 		_k_d = k_d;
 	}
-	void set_k_roll_ff(float k_roll_ff) {
-		_k_roll_ff = k_roll_ff;
+
+	void set_k_ff(float k_ff) {
+		_k_ff = k_ff;
 	}
+
 	void set_integrator_max(float max) {
 		_integrator_max = max;
 	}
 
+	void set_max_rate(float max_rate) {
+		_max_rate = max_rate;
+	}
+
+	void set_k_roll_ff(float roll_ff) {
+		_roll_ff = roll_ff;
+	}
+
+	void set_coordinated_min_speed(float coordinated_min_speed) {
+		_coordinated_min_speed = coordinated_min_speed;
+	}
+
+
+	float get_rate_error() {
+		return _rate_error;
+	}
+
+	float get_desired_rate() {
+		return _rate_setpoint;
+	}
+
+	float get_desired_bodyrate() {
+		return _bodyrate_setpoint;
+	}
+
 private:
 	uint64_t _last_run;
-
-	float _k_side;
+	float _k_p;
 	float _k_i;
 	float _k_d;
-	float _k_roll_ff;
+	float _k_ff;
 	float _integrator_max;
-
-	float _last_error;
+	float _max_rate;
+	float  _roll_ff;
 	float _last_output;
-	float _last_rate_hp_out;
-	float _last_rate_hp_in;
-	float _k_d_last;
 	float _integrator;
+	float _rate_error;
+	float _rate_setpoint;
+	float _bodyrate_setpoint;
+	float _coordinated_min_speed;
 
 };
 
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index fb7ed486d..3b68a0a4e 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -70,7 +70,7 @@ float ECL_L1_Pos_Controller::target_bearing()
 float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
 {
 	/* following [2], switching on L1 distance */
-	return math::max(wp_radius, _L1_distance);
+	return math::min(wp_radius, _L1_distance);
 }
 
 bool ECL_L1_Pos_Controller::reached_loiter_target(void)
@@ -280,7 +280,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con
 	 */
 
 	// XXX check switch over
-	if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) |
+	if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) ||
 		(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) {
 		_lateral_accel = lateral_accel_sp_center;
 		_circle_mode = false;
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index d7f350e5c..0f28bccad 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -2,6 +2,7 @@
 
 #include "tecs.h"
 #include <ecl/ecl.h>
+#include <systemlib/err.h>
 
 using namespace math;
 
@@ -168,64 +169,88 @@ void TECS::_update_speed_demand(void)
 	// calculate velocity rate limits based on physical performance limits
 	// provision to use a different rate limit if bad descent or underspeed condition exists
 	// Use 50% of maximum energy rate to allow margin for total energy contgroller
-	float velRateMax;
-	float velRateMin;
-
-	if ((_badDescent) || (_underspeed)) {
-		velRateMax = 0.5f * _STEdot_max / _integ5_state;
-		velRateMin = 0.5f * _STEdot_min / _integ5_state;
-
-	} else {
-		velRateMax = 0.5f * _STEdot_max / _integ5_state;
-		velRateMin = 0.5f * _STEdot_min / _integ5_state;
-	}
-
-	// Apply rate limit
-	if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
-		_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
-		_TAS_rate_dem = velRateMax;
-
-	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
-		_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
-		_TAS_rate_dem = velRateMin;
-
-	} else {
-		_TAS_dem_adj = _TAS_dem;
-		_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
-	}
+//	float velRateMax;
+//	float velRateMin;
+//
+//	if ((_badDescent) || (_underspeed)) {
+//		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+//		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+//
+//	} else {
+//		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+//		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+//	}
+//
+//	// Apply rate limit
+//	if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
+//		_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
+//		_TAS_rate_dem = velRateMax;
+//
+//	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
+//		_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
+//		_TAS_rate_dem = velRateMin;
+//
+//	} else {
+//		_TAS_dem_adj = _TAS_dem;
+//
+//
+//	_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
+//	}
+
+	_TAS_dem_adj = _TAS_dem;
+	_TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now
 
 	// Constrain speed demand again to protect against bad values on initialisation.
 	_TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax);
-	_TAS_dem_last = _TAS_dem;
+//	_TAS_dem_last = _TAS_dem;
+
+//	warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f",
+//			(double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent, _underspeed, velRateMin);
+//	warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f,  _badDescent %u , _underspeed %u",
+//			(double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state,  _badDescent , _underspeed);
 }
 
-void TECS::_update_height_demand(float demand)
+void TECS::_update_height_demand(float demand, float state)
 {
-	// Apply 2 point moving average to demanded height
-	// This is required because height demand is only updated at 5Hz
-	_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
-	_hgt_dem_in_old = _hgt_dem;
-
-	// printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
-	// 	_maxClimbRate);
+//	// Apply 2 point moving average to demanded height
+//	// This is required because height demand is only updated at 5Hz
+//	_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
+//	_hgt_dem_in_old = _hgt_dem;
+//
+//	// printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
+//	// 	_maxClimbRate);
+//
+//	// Limit height rate of change
+//	if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
+//		_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
+//
+//	} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
+//		_hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
+//	}
+//
+//	_hgt_dem_prev = _hgt_dem;
+//
+//	// Apply first order lag to height demand
+//	_hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
+//	_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
+//	_hgt_dem_adj_last = _hgt_dem_adj;
+//
+//	// printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
+//	// 	_hgt_rate_dem);
+
+	_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
+	_hgt_dem_adj_last = _hgt_dem_adj;
 
+	_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p;
 	// Limit height rate of change
-	if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
-		_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
+	if (_hgt_rate_dem > _maxClimbRate) {
+		_hgt_rate_dem = _maxClimbRate;
 
-	} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
-		_hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
+	} else if (_hgt_rate_dem < -_maxSinkRate) {
+		_hgt_rate_dem = -_maxSinkRate;
 	}
 
-	_hgt_dem_prev = _hgt_dem;
-
-	// Apply first order lag to height demand
-	_hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
-	_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
-	_hgt_dem_adj_last = _hgt_dem_adj;
-
-	// printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
-	// 	_hgt_rate_dem);
+	//warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj);
 }
 
 void TECS::_detect_underspeed(void)
@@ -285,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 		// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
 		// drag increase during turns.
 		float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1)));
-		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
+		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f);
 
 		if (STEdot_dem >= 0) {
 			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
@@ -353,14 +378,18 @@ void TECS::_detect_bad_descent(void)
 	// 1) Underspeed protection not active
 	// 2) Specific total energy error > 0
 	// This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set
-	float STEdot = _SPEdot + _SKEdot;
-
-	if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
-		_badDescent = true;
-
-	} else {
-		_badDescent = false;
-	}
+//	float STEdot = _SPEdot + _SKEdot;
+//
+//	if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
+//
+//		warnx("bad descent detected: _STE_error %.1f, STEdot %.1f, _throttle_dem %.1f", _STE_error, STEdot, _throttle_dem);
+//		_badDescent = true;
+//
+//	} else {
+//		_badDescent = false;
+//	}
+
+	_badDescent = false;
 }
 
 void TECS::_update_pitch(void)
@@ -404,10 +433,18 @@ void TECS::_update_pitch(void)
 	// Apply max and min values for integrator state that will allow for no more than
 	// 5deg of saturation. This allows for some pitch variation due to gusts before the
 	// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
+	// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
+	// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
+	// integrator has to catch up before the nose can be raised to reduce speed during climbout.
 	float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
 	float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
+	if (_climbOutDem)
+	{
+		temp += _PITCHminf * gainInv;
+	}
 	_integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
 
+
 	// Calculate pitch demand from specific energy balance signals
 	_pitch_dem_unc = (temp + _integ7_state) / gainInv;
 
@@ -500,7 +537,7 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
 	_update_speed_demand();
 
 	// Calculate the height demand
-	_update_height_demand(hgt_dem);
+	_update_height_demand(hgt_dem, baro_altitude);
 
 	// Detect underspeed condition
 	_detect_underspeed();
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index 68832a039..d1ebacda1 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -180,6 +180,14 @@ public:
 		_indicated_airspeed_max = airspeed;
 	}
 
+	void set_heightrate_p(float heightrate_p) {
+		_heightrate_p = heightrate_p;
+	}
+
+	void set_speedrate_p(float speedrate_p) {
+		_speedrate_p = speedrate_p;
+	}
+
 private:
 	// Last time update_50Hz was called
 	uint64_t _update_50hz_last_usec;
@@ -203,6 +211,8 @@ private:
 	float _vertAccLim;
 	float _rollComp;
 	float _spdWeight;
+	float _heightrate_p;
+	float _speedrate_p;
 
 	// throttle demand in the range from 0.0 to 1.0
 	float _throttle_dem;
@@ -329,7 +339,7 @@ private:
 	void _update_speed_demand(void);
 
 	// Update the demanded height
-	void _update_height_demand(float demand);
+	void _update_height_demand(float demand, float state);
 
 	// Detect an underspeed condition
 	void _detect_underspeed(void);
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 43105fdba..f64bfb41a 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -387,6 +387,45 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
 	return return_value;
 }
 
+__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
+	                                          double lat_next, double lon_next, float alt_next,
+	                                          float *dist_xy, float *dist_z)
+{
+	double current_x_rad = lat_next / 180.0 * M_PI;
+	double current_y_rad = lon_next / 180.0 * M_PI;
+	double x_rad = lat_now / 180.0 * M_PI;
+	double y_rad = lon_now / 180.0 * M_PI;
+
+	double d_lat = x_rad - current_x_rad;
+	double d_lon = y_rad - current_y_rad;
+
+	double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad);
+	double c = 2 * atan2(sqrt(a), sqrt(1 - a));
+
+	float dxy = CONSTANTS_RADIUS_OF_EARTH * c;
+	float dz = alt_now - alt_next;
+
+	*dist_xy = fabsf(dxy);
+	*dist_z = fabsf(dz);
+
+	return sqrtf(dxy * dxy + dz * dz);
+}
+
+
+__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
+	                                           float x_next, float y_next, float z_next,
+	                                           float *dist_xy, float *dist_z)
+{
+	float dx = x_now - x_next;
+	float dy = y_now - y_next;
+	float dz = z_now - z_next;
+
+	*dist_xy = sqrtf(dx * dx + dy * dy);
+	*dist_z = fabsf(dz);
+
+	return sqrtf(dx * dx + dy * dy + dz * dz);
+}
+
 __EXPORT float _wrap_pi(float bearing)
 {
 	/* value is inf or NaN */
@@ -465,4 +504,26 @@ __EXPORT float _wrap_360(float bearing)
 	return bearing;
 }
 
+__EXPORT bool inside_geofence(const struct vehicle_global_position_s *vehicle, const struct fence_s *fence)
+{
+
+	/* Adaptation of algorithm originally presented as
+	 * PNPOLY - Point Inclusion in Polygon Test
+	 * W. Randolph Franklin (WRF) */
+
+	unsigned int i, j, vertices = fence->count;
+	bool c = false;
+	double lat = vehicle->lat / 1e7d;
+	double lon = vehicle->lon / 1e7d;
+
+	// skip vertex 0 (return point)
+	for (i = 0, j = vertices - 1; i < vertices; j = i++)
+		if (((fence->vertices[i].lon) >= lon != (fence->vertices[j].lon >= lon)) &&
+		    (lat <= (fence->vertices[j].lat - fence->vertices[i].lat) * (lon - fence->vertices[i].lon) /
+		     (fence->vertices[j].lon - fence->vertices[i].lon) + fence->vertices[i].lat))
+			c = !c;
+	return c;
+}
+
+
 
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 123ff80f1..5f92e14cf 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -47,6 +47,9 @@
 
 #pragma once
 
+#include "uORB/topics/fence.h"
+#include "uORB/topics/vehicle_global_position.h"
+
 __BEGIN_DECLS
 
 #include <stdbool.h>
@@ -121,9 +124,34 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
 __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
 		float radius, float arc_start_bearing, float arc_sweep);
 
+/*
+ * Calculate distance in global frame
+ */
+__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
+	                                          double lat_next, double lon_next, float alt_next,
+	                                          float *dist_xy, float *dist_z);
+
+/*
+ * Calculate distance in local frame (NED)
+ */
+__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
+	                                           float x_next, float y_next, float z_next,
+	                                           float *dist_xy, float *dist_z);
+
 __EXPORT float _wrap_180(float bearing);
 __EXPORT float _wrap_360(float bearing);
 __EXPORT float _wrap_pi(float bearing);
 __EXPORT float _wrap_2pi(float bearing);
 
+/**
+ * Return whether craft is inside geofence.
+ *
+ * Calculate whether point is inside arbitrary polygon
+ * @param craft pointer craft coordinates
+ * @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected
+ * @return true: craft is inside fence, false:craft is outside fence
+ */
+__EXPORT bool inside_geofence(const struct vehicle_global_position_s *craft, const struct fence_s *fence);
+
+
 __END_DECLS
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 0357542f0..28118b990 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -509,6 +509,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 		break;
 
+	/* Flight termination */
+	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
+
+			if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
+				transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON, control_mode);
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				/* reject parachute depoyment not armed */
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+
+		}
+		break;
+
 	default:
 		break;
 	}
@@ -1033,23 +1048,17 @@ int commander_thread_main(int argc, char *argv[])
 
 			if (!home_position_set && gps_position.fix_type >= 3 &&
 			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&	// XXX note that vdop is 0 for mtk
-			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) {
+			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
+			    && global_position.valid) {
 				/* copy position data to uORB home message, store it locally as well */
-				// TODO use global position estimate
-				home.lat = gps_position.lat;
-				home.lon = gps_position.lon;
-				home.alt = gps_position.alt;
 
-				home.eph_m = gps_position.eph_m;
-				home.epv_m = gps_position.epv_m;
 
-				home.s_variance_m_s = gps_position.s_variance_m_s;
-				home.p_variance_m = gps_position.p_variance_m;
+				home.lat = (double)global_position.lat / 1e7d;
+				home.lon = (double)global_position.lon / 1e7d;
+				home.altitude = (float)global_position.alt;
 
-				double home_lat_d = home.lat * 1e-7;
-				double home_lon_d = home.lon * 1e-7;
-				warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d);
-				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d);
+				warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude);
+				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude);
 
 				/* announce new home position */
 				if (home_pub > 0) {
@@ -1174,6 +1183,16 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
+		/*  Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
+		if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
+			transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON, &control_mode);
+			if (flighttermination_res == TRANSITION_CHANGED) {
+				tune_positive();
+			}
+		} else {
+			flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF, &control_mode);
+		}
+
 
 		/* handle commands last, as the system needs to be updated to handle them */
 		orb_check(cmd_sub, &updated);
@@ -1199,6 +1218,7 @@ int commander_thread_main(int argc, char *argv[])
 		bool arming_state_changed = check_arming_state_changed();
 		bool main_state_changed = check_main_state_changed();
 		bool navigation_state_changed = check_navigation_state_changed();
+		bool flighttermination_state_changed = check_flighttermination_state_changed();
 
 		hrt_abstime t1 = hrt_absolute_time();
 
@@ -1725,7 +1745,8 @@ void *commander_low_prio_loop(void *arg)
 		/* ignore commands the high-prio loop handles */
 		if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
 		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
-		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF)
+		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
+		    cmd.command == VEHICLE_CMD_DO_SET_SERVO)
 			continue;
 
 		/* only handle low-priority commands here */
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 490fc8fc6..ca3ec94b8 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -65,6 +65,7 @@ static const int ERROR = -1;
 static bool arming_state_changed = true;
 static bool main_state_changed = true;
 static bool navigation_state_changed = true;
+static bool flighttermination_state_changed = true;
 
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
@@ -239,8 +240,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 		case MAIN_STATE_SEATBELT:
 
 			/* need at minimum altitude estimate */
-			if (current_state->condition_local_altitude_valid ||
-				current_state->condition_global_position_valid) {
+			if (!current_state->is_rotary_wing ||
+				(current_state->condition_local_altitude_valid ||
+				current_state->condition_global_position_valid)) {
 				ret = TRANSITION_CHANGED;
 			}
 
@@ -450,6 +452,18 @@ check_navigation_state_changed()
 	}
 }
 
+bool
+check_flighttermination_state_changed()
+{
+	if (flighttermination_state_changed) {
+		flighttermination_state_changed = false;
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
 void
 set_navigation_state_changed()
 {
@@ -522,6 +536,44 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 }
 
 
+/**
+* Transition from one flightermination state to another
+*/
+transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode)
+{
+	transition_result_t ret = TRANSITION_DENIED;
+
+		/* only check transition if the new state is actually different from the current one */
+		if (new_flighttermination_state == status->flighttermination_state) {
+			ret = TRANSITION_NOT_CHANGED;
+
+		} else {
+
+			switch (new_flighttermination_state) {
+			case FLIGHTTERMINATION_STATE_ON:
+				ret = TRANSITION_CHANGED;
+				status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
+				warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
+				break;
+			case FLIGHTTERMINATION_STATE_OFF:
+				ret = TRANSITION_CHANGED;
+				status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
+				break;
+
+			default:
+				break;
+			}
+
+			if (ret == TRANSITION_CHANGED) {
+				flighttermination_state_changed = true;
+				control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
+			}
+		}
+
+		return ret;
+}
+
+
 
 // /*
 //  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 0bfdf36a8..e1ec9d4ad 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -70,8 +70,12 @@ bool check_main_state_changed();
 
 transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
 
+transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode);
+
 bool check_navigation_state_changed();
 
+bool check_flighttermination_state_changed();
+
 void set_navigation_state_changed();
 
 int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp
index 448a42a99..e213ac17f 100644
--- a/src/modules/controllib/uorb/blocks.cpp
+++ b/src/modules/controllib/uorb/blocks.cpp
@@ -54,26 +54,26 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {};
 
 void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
 				   vehicle_attitude_s &att,
-				   vehicle_global_position_setpoint_s &posCmd,
-				   vehicle_global_position_setpoint_s &lastPosCmd)
+				   mission_item_s &missionCmd,
+				   mission_item_s &lastMissionCmd)
 {
 
 	// heading to waypoint
 	float psiTrack = get_bearing_to_next_waypoint(
 				 (double)pos.lat / (double)1e7d,
 				 (double)pos.lon / (double)1e7d,
-				 (double)posCmd.lat / (double)1e7d,
-				 (double)posCmd.lon / (double)1e7d);
+				 missionCmd.lat,
+				 missionCmd.lon);
 
 	// cross track
 	struct crosstrack_error_s xtrackError;
 	get_distance_to_line(&xtrackError,
 			     (double)pos.lat / (double)1e7d,
 			     (double)pos.lon / (double)1e7d,
-			     (double)lastPosCmd.lat / (double)1e7d,
-			     (double)lastPosCmd.lon / (double)1e7d,
-			     (double)posCmd.lat / (double)1e7d,
-			     (double)posCmd.lon / (double)1e7d);
+			     lastMissionCmd.lat,
+			     lastMissionCmd.lon,
+			     missionCmd.lat,
+			     missionCmd.lon);
 
 	_psiCmd = _wrap_2pi(psiTrack -
 			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
@@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c
 	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
 	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
 	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
-	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
+	_missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20),
 	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
 	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
 	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
index 46dc1bec2..8cc0d77d4 100644
--- a/src/modules/controllib/uorb/blocks.hpp
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -43,7 +43,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
+#include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
@@ -82,8 +82,8 @@ public:
 	virtual ~BlockWaypointGuidance();
 	void update(vehicle_global_position_s &pos,
 		    vehicle_attitude_s &att,
-		    vehicle_global_position_setpoint_s &posCmd,
-		    vehicle_global_position_setpoint_s &lastPosCmd);
+		    mission_item_s &missionCmd,
+		    mission_item_s &lastMissionCmd);
 	float getPsiCmd() { return _psiCmd; }
 };
 
@@ -98,7 +98,7 @@ protected:
 	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
 	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
 	UOrbSubscription<vehicle_global_position_s> _pos;
-	UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
+	UOrbSubscription<mission_item_triplet_s> _missionCmd;
 	UOrbSubscription<manual_control_setpoint_s> _manual;
 	UOrbSubscription<vehicle_status_s> _status;
 	UOrbSubscription<parameter_update_s> _param_update;
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
new file mode 100644
index 000000000..dc2d6c312
--- /dev/null
+++ b/src/modules/dataman/dataman.c
@@ -0,0 +1,741 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	Lorenz Meier
+ *              Jean Cyr
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file dataman.c
+ * DATAMANAGER driver.
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <sys/ioctl.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <queue.h>
+
+#include "dataman.h"
+
+/**
+ * data manager app start / stop handling function
+ *
+ * @ingroup apps
+ */
+
+__EXPORT int dataman_main(int argc, char *argv[]);
+__EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen);
+__EXPORT ssize_t dm_write(dm_item_t  item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen);
+__EXPORT int dm_clear(dm_item_t item);
+__EXPORT int dm_restart(dm_reset_reason restart_type);
+
+/* Types of function calls supported by the worker task */
+typedef enum {
+	dm_write_func = 0,
+	dm_read_func,
+	dm_clear_func,
+	dm_restart_func,
+	dm_number_of_funcs
+} dm_function_t;
+
+/* Work task work item */
+typedef struct {
+	sq_entry_t link;	/**< list linkage */
+	sem_t wait_sem;
+	dm_function_t func;
+	ssize_t result;
+	union {
+		struct {
+			dm_item_t item;
+			unsigned char index;
+			dm_persitence_t persistence;
+			const void *buf;
+			size_t count;
+		} write_params;
+		struct {
+			dm_item_t item;
+			unsigned char index;
+			void *buf;
+			size_t count;
+		} read_params;
+		struct {
+			dm_item_t item;
+		} clear_params;
+		struct {
+			dm_reset_reason reason;
+		} restart_params;
+	};
+} work_q_item_t;
+
+/* Usage statistics */
+static unsigned g_func_counts[dm_number_of_funcs];
+
+/* table of maximum number of instances for each item type */
+static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
+	DM_KEY_SAFE_POINTS_MAX,
+	DM_KEY_FENCE_POINTS_MAX,
+	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
+	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
+	DM_KEY_WAYPOINTS_ONBOARD_MAX
+};
+
+/* Table of offset for index 0 of each item type */
+static unsigned int g_key_offsets[DM_KEY_NUM_KEYS];
+
+/* The data manager store file handle and file name */
+static int g_fd = -1, g_task_fd = -1;
+static const char *k_data_manager_device_path = "/fs/microsd/dataman";
+
+/* The data manager work queues */
+
+typedef struct {
+	sq_queue_t q;
+	sem_t mutex;	/* Mutual exclusion on work queue adds and deletes */
+	unsigned size;
+	unsigned max_size;
+} work_q_t;
+
+static work_q_t g_free_q;
+static work_q_t g_work_q;
+
+sem_t g_work_queued_sema;
+sem_t g_init_sema;
+
+static bool g_task_should_exit;		/**< if true, dataman task should exit */
+
+#define DM_SECTOR_HDR_SIZE 4
+static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE;
+
+static void init_q(work_q_t *q)
+{
+	sq_init(&(q->q));
+	sem_init(&(q->mutex), 1, 1);
+	q->size = q->max_size = 0;
+}
+
+static void destroy_q(work_q_t *q)
+{
+	sem_destroy(&(q->mutex));
+}
+
+static inline void
+lock_queue(work_q_t *q)
+{
+	sem_wait(&(q->mutex));
+}
+
+static inline void
+unlock_queue(work_q_t *q)
+{
+	sem_post(&(q->mutex));
+}
+
+static work_q_item_t *
+create_work_item(void)
+{
+	work_q_item_t *item;
+
+	lock_queue(&g_free_q);
+	if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q))))
+		g_free_q.size--;
+	unlock_queue(&g_free_q);
+
+	if (item == NULL)
+		item = (work_q_item_t *)malloc(sizeof(work_q_item_t));
+
+	if (item)
+		sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */
+
+	return item;
+}
+
+/* Work queue management functions */
+static void
+enqueue_work_item(work_q_item_t *item)
+{
+	/* put the work item on the work queue */
+	lock_queue(&g_work_q);
+	sq_addlast(&item->link, &(g_work_q.q));
+
+	if (++g_work_q.size > g_work_q.max_size)
+		g_work_q.max_size = g_work_q.size;
+
+	unlock_queue(&g_work_q);
+
+	/* tell the work thread that work is available */
+	sem_post(&g_work_queued_sema);
+}
+
+static void
+destroy_work_item(work_q_item_t *item)
+{
+	sem_destroy(&item->wait_sem);
+	lock_queue(&g_free_q);
+	sq_addfirst(&item->link, &(g_free_q.q));
+
+	if (++g_free_q.size > g_free_q.max_size)
+		g_free_q.max_size = g_free_q.size;
+
+	unlock_queue(&g_free_q);
+}
+
+static work_q_item_t *
+dequeue_work_item(void)
+{
+	work_q_item_t *work;
+	lock_queue(&g_work_q);
+
+	if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q)))
+		g_work_q.size--;
+
+	unlock_queue(&g_work_q);
+	return work;
+}
+
+/* Calculate the offset in file of specific item */
+static int
+calculate_offset(dm_item_t item, unsigned char index)
+{
+
+	/* Make sure the item type is valid */
+	if (item >= DM_KEY_NUM_KEYS)
+		return -1;
+
+	/* Make sure the index for this item type is valid */
+	if (index >= g_per_item_max_index[item])
+		return -1;
+
+	/* Calculate and return the item index based on type and index */
+	return g_key_offsets[item] + (index * k_sector_size);
+}
+
+/* Each data item is stored as follows
+ *
+ * byte 0: Length of user data item
+ * byte 1: Persistence of this data item
+ * byte DM_SECTOR_HDR_SIZE... : data item value
+ *
+ * The total size must not exceed k_sector_size
+ */
+
+/* write to the data manager file */
+static ssize_t
+_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
+{
+	unsigned char buffer[k_sector_size];
+	size_t len;
+	int offset;
+
+	/* Get the offset for this item */
+	offset = calculate_offset(item, index);
+
+	if (offset < 0) 
+		return -1;
+
+	/* Make sure caller has not given us more data than we can handle */
+	if (count > DM_MAX_DATA_SIZE) 
+		return -1;
+
+	/* Write out the data, prefixed with length and persistence level */
+	buffer[0] = count;
+	buffer[1] = persistence;
+	buffer[2] = 0;
+	buffer[3] = 0;
+	memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count);
+	count += DM_SECTOR_HDR_SIZE;
+
+	len = -1;
+
+	if (lseek(g_task_fd, offset, SEEK_SET) == offset)
+		if ((len = write(g_task_fd, buffer, count)) == count)
+			fsync(g_task_fd);
+
+	if (len != count)
+		return -1;
+
+	/* All is well... return the number of user data written */
+	return count - DM_SECTOR_HDR_SIZE;
+}
+
+/* Retrieve from the data manager file */
+static ssize_t
+_read(dm_item_t item, unsigned char index, void *buf, size_t count)
+{
+	unsigned char buffer[k_sector_size];
+	int len, offset;
+
+	/* Get the offset for this item */
+	offset = calculate_offset(item, index);
+
+	if (offset < 0)
+		return -1;
+
+	/* Make sure the caller hasn't asked for more data than we can handle */
+	if (count > DM_MAX_DATA_SIZE)
+		return -1;
+
+	/* Read the prefix and data */
+	len = -1;
+	if (lseek(g_task_fd, offset, SEEK_SET) == offset)
+		len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE);
+
+	/* Check for length issues */
+	if (len < 0)
+		return -1;
+
+	if (len == 0)
+		buffer[0] = 0;
+
+	if (buffer[0] > 0) {
+		if (buffer[0] > count)
+			return -1;
+
+		/* Looks good, copy it to the caller's buffer */
+		memcpy(buf, buffer + DM_SECTOR_HDR_SIZE, buffer[0]);
+	}
+
+	/* Return the number of bytes of caller data read */
+	return buffer[0];
+}
+
+static int
+_clear(dm_item_t item)
+{
+	int i, result = 0;
+
+	int offset = calculate_offset(item, 0);
+
+	if (offset < 0)
+		return -1;
+
+	for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) {
+		char buf[1];
+
+		if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+			result = -1;
+			break;
+		}
+
+		if (read(g_task_fd, buf, 1) < 1)
+			break;
+
+		if (buf[0]) {
+			if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+				result = -1;
+				break;
+			}
+
+			buf[0] = 0;
+
+			if (write(g_task_fd, buf, 1) != 1) {
+				result = -1;
+				break;
+			}
+		}
+
+		offset += k_sector_size;
+	}
+
+	fsync(g_task_fd);
+	return result;
+}
+
+/* Tell the data manager about the type of the last reset */
+static int
+_restart(dm_reset_reason reason)
+{
+	unsigned char buffer[2];
+	int offset, result = 0;
+
+	/* We need to scan the entire file and invalidate and data that should not persist after the last reset */
+
+	/* Loop through all of the data segments and delete those that are not persistent */
+	offset = 0;
+
+	while (1) {
+		size_t len;
+
+		/* Get data segment at current offset */
+		if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+			result = -1;
+			break;
+		}
+
+		len = read(g_task_fd, buffer, sizeof(buffer));
+
+		if (len == 0)
+			break;
+
+		/* check if segment contains data */
+		if (buffer[0]) {
+			int clear_entry = 0;
+
+			/* Whether data gets deleted depends on reset type and data segment's persistence setting */
+			if (reason == DM_INIT_REASON_POWER_ON) {
+				if (buffer[1] != DM_PERSIST_POWER_ON_RESET) {
+					clear_entry = 1;
+				}
+
+			} else {
+				if ((buffer[1] != DM_PERSIST_POWER_ON_RESET) && (buffer[1] != DM_PERSIST_IN_FLIGHT_RESET)) {
+					clear_entry = 1;
+				}
+			}
+
+			/* Set segment to unused if data does not persist */
+			if (clear_entry) {
+				if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+					result = -1;
+					break;
+				}
+
+				buffer[0] = 0;
+
+				len = write(g_task_fd, buffer, 1);
+
+				if (len != 1) {
+					result = -1;
+					break;
+				}
+			}
+		}
+
+		offset += k_sector_size;
+	}
+
+	fsync(g_task_fd);
+
+	/* tell the caller how it went */
+	return result;
+}
+
+/* write to the data manager file */
+__EXPORT ssize_t
+dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
+{
+	work_q_item_t *work;
+
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* Will return with queues locked */
+	if ((work = create_work_item()) == NULL) 
+		return -1; /* queues unlocked on failure */
+
+	work->func = dm_write_func;
+	work->write_params.item = item;
+	work->write_params.index = index;
+	work->write_params.persistence = persistence;
+	work->write_params.buf = buf;
+	work->write_params.count = count;
+	enqueue_work_item(work);
+
+	sem_wait(&work->wait_sem);
+	ssize_t result = work->result;
+	destroy_work_item(work);
+	return result;
+}
+
+/* Retrieve from the data manager file */
+__EXPORT ssize_t
+dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
+{
+	work_q_item_t *work;
+
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* Will return with queues locked */
+	if ((work = create_work_item()) == NULL)
+		return -1; /* queues unlocked on failure */
+
+	work->func = dm_read_func;
+	work->read_params.item = item;
+	work->read_params.index = index;
+	work->read_params.buf = buf;
+	work->read_params.count = count;
+	enqueue_work_item(work);
+
+	sem_wait(&work->wait_sem);
+	ssize_t result = work->result;
+	destroy_work_item(work);
+	return result;
+}
+
+__EXPORT int
+dm_clear(dm_item_t item)
+{
+	work_q_item_t *work;
+
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* Will return with queues locked */
+	if ((work = create_work_item()) == NULL)
+		return -1; /* queues unlocked on failure */
+
+	work->func = dm_clear_func;
+	work->clear_params.item = item;
+	enqueue_work_item(work);
+
+	sem_wait(&work->wait_sem);
+	int result = work->result;
+	destroy_work_item(work);
+	return result;
+}
+
+/* Tell the data manager about the type of the last reset */
+__EXPORT int
+dm_restart(dm_reset_reason reason)
+{
+	work_q_item_t *work;
+
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* Will return with queues locked */
+	if ((work = create_work_item()) == NULL)
+		return -1; /* queues unlocked on failure */
+
+	work->func = dm_restart_func;
+	work->restart_params.reason = reason;
+	enqueue_work_item(work);
+
+	sem_wait(&work->wait_sem);
+	int result = work->result;
+	destroy_work_item(work);
+	return result;
+}
+
+static int
+task_main(int argc, char *argv[])
+{
+	work_q_item_t *work;
+
+	/* inform about start */
+	warnx("Initializing..");
+
+	/* Initialize global variables */
+	g_key_offsets[0] = 0;
+
+	for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++)
+		g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * k_sector_size);
+
+	unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * k_sector_size);
+
+	for (unsigned i = 0; i < dm_number_of_funcs; i++)
+		g_func_counts[i] = 0;
+
+	g_task_should_exit = false;
+
+	init_q(&g_work_q);
+	init_q(&g_free_q);
+
+	sem_init(&g_work_queued_sema, 1, 0);
+
+	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
+	if (g_task_fd < 0) {
+		warnx("Could not open data manager file %s", k_data_manager_device_path);
+		sem_post(&g_init_sema);
+		return -1;
+	}
+	if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
+		close(g_task_fd);
+		warnx("Could not seek data manager file %s", k_data_manager_device_path);
+		sem_post(&g_init_sema);
+		return -1;
+	}
+	fsync(g_task_fd);
+
+	g_fd = g_task_fd;
+
+	warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset);
+
+	sem_post(&g_init_sema);
+
+	/* Start the endless loop, waiting for then processing work requests */
+	while (true) {
+
+		/* do we need to exit ??? */
+		if ((g_task_should_exit) && (g_fd >= 0)) {
+			/* Close the file handle to stop further queueing */
+			g_fd = -1;
+		}
+
+		if (!g_task_should_exit) {
+			/* wait for work */
+			sem_wait(&g_work_queued_sema);
+		}
+
+		/* Empty the work queue */
+		while ((work = dequeue_work_item())) {
+
+			switch (work->func) {
+			case dm_write_func:
+				g_func_counts[dm_write_func]++;
+				work->result =
+					_write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf, work->write_params.count);
+				break;
+
+			case dm_read_func:
+				g_func_counts[dm_read_func]++;
+				work->result =
+					_read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count);
+				break;
+
+			case dm_clear_func:
+				g_func_counts[dm_clear_func]++;
+				work->result = _clear(work->clear_params.item);
+				break;
+
+			case dm_restart_func:
+				g_func_counts[dm_restart_func]++;
+				work->result = _restart(work->restart_params.reason);
+				break;
+
+			default: /* should never happen */
+				work->result = -1;
+				break;
+			}
+
+			/* Inform the caller that work is done */
+			sem_post(&work->wait_sem);
+		}
+
+		/* time to go???? */
+		if ((g_task_should_exit) && (g_fd < 0))
+			break;
+	}
+
+	close(g_task_fd);
+	g_task_fd = -1;
+
+	/* Empty the work queue */
+	for (;;) {
+		if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
+			break;
+
+		free(work);
+	}
+
+	destroy_q(&g_work_q);
+	destroy_q(&g_free_q);
+	sem_destroy(&g_work_queued_sema);
+
+	return 0;
+}
+
+static int
+start(void)
+{
+	int task;
+
+	sem_init(&g_init_sema, 1, 0);
+
+	/* start the task */
+	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) {
+		warn("task start failed");
+		return -1;
+	}
+
+	/* wait for the thread to actuall initialize */
+	sem_wait(&g_init_sema);
+	sem_destroy(&g_init_sema);
+
+	return 0;
+}
+
+static void
+status(void)
+{
+	/* display usage statistics */
+	warnx("Writes   %d", g_func_counts[dm_write_func]);
+	warnx("Reads    %d", g_func_counts[dm_read_func]);
+	warnx("Clears   %d", g_func_counts[dm_clear_func]);
+	warnx("Restarts %d", g_func_counts[dm_restart_func]);
+	warnx("Max Q lengths work %d, free %d", g_work_q.max_size, g_free_q.max_size);
+}
+
+static void
+stop(void)
+{
+	/* Tell the worker task to shut down */
+	g_task_should_exit = true;
+	sem_post(&g_work_queued_sema);
+}
+
+static void
+usage(void)
+{
+	errx(1, "usage: dataman {start|stop|status}");
+}
+
+int
+dataman_main(int argc, char *argv[])
+{
+	if (argc < 2)
+		usage();
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (g_fd >= 0)
+			errx(1, "already running");
+
+		start();
+
+		if (g_fd < 0)
+			errx(1, "start failed");
+
+		exit(0);
+	}
+
+	if (g_fd < 0)
+		errx(1, "not running");
+
+	if (!strcmp(argv[1], "stop"))
+		stop();
+	else if (!strcmp(argv[1], "status"))
+		status();
+	else
+		usage();
+
+	exit(1);
+}
+
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
new file mode 100644
index 000000000..a70638ccc
--- /dev/null
+++ b/src/modules/dataman/dataman.h
@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file dataman.h
+ *
+ * DATAMANAGER driver.
+ */
+#ifndef _DATAMANAGER_H
+#define _DATAMANAGER_H
+
+#include <uORB/topics/mission.h>
+#include <uORB/topics/fence.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+	/* Types of items that the data manager can store */
+	typedef enum {
+		DM_KEY_SAFE_POINTS = 0,		/* Safe points coordinates, safe point 0 is home point */
+		DM_KEY_FENCE_POINTS,		/* Fence vertex coordinates */
+		DM_KEY_WAYPOINTS_OFFBOARD_0,	/* Mission way point coordinates sent over mavlink */
+		DM_KEY_WAYPOINTS_OFFBOARD_1,	/* (alernate between 0 and 1) */
+		DM_KEY_WAYPOINTS_ONBOARD,	/* Mission way point coordinates generated onboard */
+		DM_KEY_NUM_KEYS			/* Total number of item types defined */
+	} dm_item_t;
+
+	/* The maximum number of instances for each item type */
+	enum {
+		DM_KEY_SAFE_POINTS_MAX = 8,
+		DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
+		DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
+		DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
+		DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED
+	};
+
+	/* Data persistence levels */
+	typedef enum {
+		DM_PERSIST_POWER_ON_RESET = 0,	/* Data survives all resets */
+		DM_PERSIST_IN_FLIGHT_RESET,     /* Data survives in-flight resets only */
+		DM_PERSIST_VOLATILE             /* Data does not survive resets */
+	} dm_persitence_t;
+
+	/* The reason for the last reset */
+	typedef enum {
+		DM_INIT_REASON_POWER_ON = 0,	/* Data survives resets */
+		DM_INIT_REASON_IN_FLIGHT	/* Data survives in-flight resets only */
+	} dm_reset_reason;
+
+	/* Maximum size in bytes of a single item instance */
+	#define DM_MAX_DATA_SIZE 126
+
+	/* Retrieve from the data manager store */
+	__EXPORT ssize_t
+	dm_read(
+		dm_item_t item,			/* The item type to retrieve */
+		unsigned char index,		/* The index of the item */
+		void *buffer,			/* Pointer to caller data buffer */
+		size_t buflen			/* Length in bytes of data to retrieve */
+	);
+
+	/* write to the data manager store */
+	__EXPORT ssize_t
+	dm_write(
+		dm_item_t  item,		/* The item type to store */
+		unsigned char index,		/* The index of the item */
+		dm_persitence_t persistence,	/* The persistence level of this item */
+		const void *buffer,		/* Pointer to caller data buffer */
+		size_t buflen			/* Length in bytes of data to retrieve */
+	);
+
+	/* Retrieve from the data manager store */
+	__EXPORT int
+	dm_clear(
+		dm_item_t item			/* The item type to clear */
+		);
+
+	/* Tell the data manager about the type of the last reset */
+	__EXPORT int
+	dm_restart(
+		dm_reset_reason restart_type	/* The last reset type */
+	);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/modules/dataman/module.mk b/src/modules/dataman/module.mk
new file mode 100644
index 000000000..dce7a6235
--- /dev/null
+++ b/src/modules/dataman/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Main Navigation Controller
+#
+
+MODULE_COMMAND	= dataman
+
+SRCS		= dataman.c
+
+INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index 6dc19df41..108e9896d 100644
--- a/src/modules/fixedwing_backside/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -117,7 +117,7 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
 	_vCmd(this, "V_CMD"),
 	_crMax(this, "CR_MAX"),
 	_attPoll(),
-	_lastPosCmd(),
+	_lastMissionCmd(),
 	_timeStamp(0)
 {
 	_attPoll.fd = _att.getHandle();
@@ -141,8 +141,8 @@ void BlockMultiModeBacksideAutopilot::update()
 	setDt(dt);
 
 	// store old position command before update if new command sent
-	if (_posCmd.updated()) {
-		_lastPosCmd = _posCmd.getData();
+	if (_missionCmd.updated()) {
+		_lastMissionCmd = _missionCmd.getData();
 	}
 
 	// check for new updates
@@ -159,7 +159,7 @@ void BlockMultiModeBacksideAutopilot::update()
 	if (_status.main_state == MAIN_STATE_AUTO) {
 		// TODO use vehicle_control_mode here?
 		// update guidance
-		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
+		_guide.update(_pos, _att, _missionCmd.current, _lastMissionCmd.current);
 	}
 
 	// XXX handle STABILIZED (loiter on spot) as well
@@ -182,7 +182,7 @@ void BlockMultiModeBacksideAutopilot::update()
 		float vCmd = _vLimit.update(_vCmd.get());
 
 		// altitude hold
-		float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt);
+		float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt);
 
 		// heading hold
 		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp
index 567efeb35..b4dbc36b0 100644
--- a/src/modules/fixedwing_backside/fixedwing.hpp
+++ b/src/modules/fixedwing_backside/fixedwing.hpp
@@ -264,7 +264,7 @@ private:
 	BlockParamFloat _crMax;
 
 	struct pollfd _attPoll;
-	vehicle_global_position_set_triplet_s _lastPosCmd;
+	mission_item_triplet_s _lastMissionCmd;
 	enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
 	uint64_t _timeStamp;
 public:
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 00a0dcd61..26777f737 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -37,6 +37,7 @@
  * Implementation of a generic attitude controller based on classic orthogonal PIDs.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  *
  */
 
@@ -62,6 +63,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_global_position.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/pid/pid.h>
@@ -106,26 +108,30 @@ private:
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
 	int		_control_task;			/**< task handle for sensor task */
 
-	int		_att_sub;			/**< vehicle attitude subscription */
-	int		_accel_sub;			/**< accelerometer subscription */
+	int		_att_sub;				/**< vehicle attitude subscription */
+	int		_accel_sub;				/**< accelerometer subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
-	int		_vcontrol_mode_sub;			/**< vehicle status subscription */
-	int 		_params_sub;			/**< notification of parameter updates */
-	int 		_manual_sub;			/**< notification of manual control updates */
+	int		_vcontrol_mode_sub;		/**< vehicle status subscription */
+	int 	_params_sub;			/**< notification of parameter updates */
+	int 	_manual_sub;			/**< notification of manual control updates */
+	int		_global_pos_sub;		/**< global position subscription */
 
 	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint point */
 	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
+	orb_advert_t	_actuators_1_pub;		/**< actuator control group 1 setpoint (Airframe) */
 
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct accel_report				_accel;			/**< body frame accelerations */
+	struct vehicle_attitude_s				_att;			/**< vehicle attitude */
+	struct accel_report						_accel;			/**< body frame accelerations */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct airspeed_s				_airspeed;		/**< airspeed */
-	struct vehicle_control_mode_s			_vcontrol_mode;		/**< vehicle control mode */
+	struct airspeed_s						_airspeed;		/**< airspeed */
+	struct vehicle_control_mode_s			_vcontrol_mode;	/**< vehicle control mode */
 	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
+	struct actuator_controls_s			_actuators_airframe;		/**< actuator control inputs */
+	struct vehicle_global_position_s		_global_pos;	/**< global position */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -137,6 +143,7 @@ private:
 		float p_p;
 		float p_d;
 		float p_i;
+		float p_ff;
 		float p_rmax_pos;
 		float p_rmax_neg;
 		float p_integrator_max;
@@ -144,13 +151,17 @@ private:
 		float r_p;
 		float r_d;
 		float r_i;
+		float r_ff;
 		float r_integrator_max;
 		float r_rmax;
 		float y_p;
 		float y_i;
 		float y_d;
+		float y_ff;
 		float y_roll_feedforward;
 		float y_integrator_max;
+		float y_coordinated_min_speed;
+		float y_rmax;
 
 		float airspeed_min;
 		float airspeed_trim;
@@ -163,6 +174,7 @@ private:
 		param_t p_p;
 		param_t p_d;
 		param_t p_i;
+		param_t p_ff;
 		param_t p_rmax_pos;
 		param_t p_rmax_neg;
 		param_t p_integrator_max;
@@ -170,13 +182,17 @@ private:
 		param_t r_p;
 		param_t r_d;
 		param_t r_i;
+		param_t r_ff;
 		param_t r_integrator_max;
 		param_t r_rmax;
 		param_t y_p;
 		param_t y_i;
 		param_t y_d;
+		param_t y_ff;
 		param_t y_roll_feedforward;
 		param_t y_integrator_max;
+		param_t y_coordinated_min_speed;
+		param_t y_rmax;
 
 		param_t airspeed_min;
 		param_t airspeed_trim;
@@ -227,6 +243,11 @@ private:
 	void		vehicle_setpoint_poll();
 
 	/**
+	 * Check for global position updates.
+	 */
+	void		global_pos_poll();
+
+	/**
 	 * Shim for calling task_main from task_create.
 	 */
 	static void	task_main_trampoline(int argc, char *argv[]);
@@ -261,11 +282,13 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
 	_manual_sub(-1),
+	_global_pos_sub(-1),
 
 /* publications */
 	_rate_sp_pub(-1),
 	_actuators_0_pub(-1),
 	_attitude_sp_pub(-1),
+	_actuators_1_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@@ -273,31 +296,49 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_setpoint_valid(false),
 	_airspeed_valid(false)
 {
+	/* safely initialize structs */
+	_att = {0};
+	_accel = {0};
+	_att_sp = {0};
+	_manual = {0};
+	_airspeed = {0};
+	_vcontrol_mode = {0};
+	_actuators = {0};
+	_actuators_airframe = {0};
+	_global_pos = {0};
+
+
 	_parameter_handles.tconst = param_find("FW_ATT_TC");
-	_parameter_handles.p_p = param_find("FW_P_P");
-	_parameter_handles.p_d = param_find("FW_P_D");
-	_parameter_handles.p_i = param_find("FW_P_I");
+	_parameter_handles.p_p = param_find("FW_PR_P");
+	_parameter_handles.p_d = param_find("FW_PR_D");
+	_parameter_handles.p_i = param_find("FW_PR_I");
+	_parameter_handles.p_ff = param_find("FW_PR_FF");
 	_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
 	_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
-	_parameter_handles.p_integrator_max = param_find("FW_P_IMAX");
+	_parameter_handles.p_integrator_max = param_find("FW_PR_IMAX");
 	_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
 
-	_parameter_handles.r_p = param_find("FW_R_P");
-	_parameter_handles.r_d = param_find("FW_R_D");
-	_parameter_handles.r_i = param_find("FW_R_I");
-	_parameter_handles.r_integrator_max = param_find("FW_R_IMAX");
+	_parameter_handles.r_p = param_find("FW_RR_P");
+	_parameter_handles.r_d = param_find("FW_RR_D");
+	_parameter_handles.r_i = param_find("FW_RR_I");
+	_parameter_handles.r_ff = param_find("FW_RR_FF");
+	_parameter_handles.r_integrator_max = param_find("FW_RR_IMAX");
 	_parameter_handles.r_rmax = param_find("FW_R_RMAX");
 
-	_parameter_handles.y_p = param_find("FW_Y_P");
-	_parameter_handles.y_i = param_find("FW_Y_I");
-	_parameter_handles.y_d = param_find("FW_Y_D");
+	_parameter_handles.y_p = param_find("FW_YR_P");
+	_parameter_handles.y_i = param_find("FW_YR_I");
+	_parameter_handles.y_d = param_find("FW_YR_D");
+	_parameter_handles.y_ff = param_find("FW_YR_FF");
 	_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
-	_parameter_handles.y_integrator_max = param_find("FW_Y_IMAX");
+	_parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
+	_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
 
 	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
 	_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
 
+	_parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -335,6 +376,7 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.p_p, &(_parameters.p_p));
 	param_get(_parameter_handles.p_d, &(_parameters.p_d));
 	param_get(_parameter_handles.p_i, &(_parameters.p_i));
+	param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
 	param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
 	param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
 	param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
@@ -343,14 +385,19 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.r_p, &(_parameters.r_p));
 	param_get(_parameter_handles.r_d, &(_parameters.r_d));
 	param_get(_parameter_handles.r_i, &(_parameters.r_i));
+	param_get(_parameter_handles.r_ff, &(_parameters.r_ff));
+
 	param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
 	param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));
 
 	param_get(_parameter_handles.y_p, &(_parameters.y_p));
 	param_get(_parameter_handles.y_i, &(_parameters.y_i));
 	param_get(_parameter_handles.y_d, &(_parameters.y_d));
+	param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
 	param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward));
 	param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
+	param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
+	param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
 
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
@@ -358,28 +405,33 @@ FixedwingAttitudeControl::parameters_update()
 
 	/* pitch control parameters */
 	_pitch_ctrl.set_time_constant(_parameters.tconst);
-	_pitch_ctrl.set_k_p(math::radians(_parameters.p_p));
-	_pitch_ctrl.set_k_i(math::radians(_parameters.p_i));
-	_pitch_ctrl.set_k_d(math::radians(_parameters.p_d));
-	_pitch_ctrl.set_integrator_max(math::radians(_parameters.p_integrator_max));
+	_pitch_ctrl.set_k_p(_parameters.p_p);
+	_pitch_ctrl.set_k_i(_parameters.p_i);
+	_pitch_ctrl.set_k_d(_parameters.p_d);
+	_pitch_ctrl.set_k_ff(_parameters.p_ff);
+	_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
 	_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
 	_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
-	_pitch_ctrl.set_roll_ff(math::radians(_parameters.p_roll_feedforward));
+	_pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward);
 
 	/* roll control parameters */
 	_roll_ctrl.set_time_constant(_parameters.tconst);
-	_roll_ctrl.set_k_p(math::radians(_parameters.r_p));
-	_roll_ctrl.set_k_i(math::radians(_parameters.r_i));
-	_roll_ctrl.set_k_d(math::radians(_parameters.r_d));
-	_roll_ctrl.set_integrator_max(math::radians(_parameters.r_integrator_max));
+	_roll_ctrl.set_k_p(_parameters.r_p);
+	_roll_ctrl.set_k_i(_parameters.r_i);
+	_roll_ctrl.set_k_d(_parameters.r_d);
+	_roll_ctrl.set_k_ff(_parameters.r_ff);
+	_roll_ctrl.set_integrator_max(_parameters.r_integrator_max);
 	_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
 
 	/* yaw control parameters */
-	_yaw_ctrl.set_k_side(math::radians(_parameters.y_p));
-	_yaw_ctrl.set_k_i(math::radians(_parameters.y_i));
-	_yaw_ctrl.set_k_d(math::radians(_parameters.y_d));
-	_yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward));
-	_yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max));
+	_yaw_ctrl.set_k_p(_parameters.y_p);
+	_yaw_ctrl.set_k_i(_parameters.y_i);
+	_yaw_ctrl.set_k_d(_parameters.y_d);
+	_yaw_ctrl.set_k_ff(_parameters.y_ff);
+	_yaw_ctrl.set_k_roll_ff(_parameters.y_roll_feedforward);
+	_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
+	_yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed);
+	_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
 
 	return OK;
 }
@@ -421,6 +473,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll()
 
 	if (airspeed_updated) {
 		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+//		warnx("airspeed poll: ind: %.4f,  true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s);
 		return true;
 	}
 
@@ -453,6 +506,18 @@ FixedwingAttitudeControl::vehicle_setpoint_poll()
 }
 
 void
+FixedwingAttitudeControl::global_pos_poll()
+{
+	/* check if there is a new global position */
+	bool global_pos_updated;
+	orb_check(_global_pos_sub, &global_pos_updated);
+
+	if (global_pos_updated) {
+		orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
+	}
+}
+
+void
 FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
 {
 	att_control::g_control->task_main();
@@ -476,6 +541,7 @@ FixedwingAttitudeControl::task_main()
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vcontrol_mode_sub, 200);
@@ -551,6 +617,8 @@ FixedwingAttitudeControl::task_main()
 
 			vehicle_manual_poll();
 
+			global_pos_poll();
+
 			/* lock integrator until control is started */
 			bool lock_integrator;
 
@@ -561,22 +629,28 @@ FixedwingAttitudeControl::task_main()
 				lock_integrator = true;
 			}
 
+			/* Simple handling of failsafe: deploy parachute if failsafe is on */
+			if (_vcontrol_mode.flag_control_flighttermination_enabled) {
+				_actuators_airframe.control[1] = 1.0f;
+//				warnx("_actuators_airframe.control[1] = 1.0f;");
+			} else {
+				_actuators_airframe.control[1] = 0.0f;
+//				warnx("_actuators_airframe.control[1] = -1.0f;");
+			}
+
 			/* decide if in stabilized or full manual control */
 
 			if (_vcontrol_mode.flag_control_attitude_enabled) {
 
-				/* scale from radians to normalized -1 .. 1 range */
-				const float actuator_scaling = 1.0f / (M_PI_F / 4.0f);
-
 				/* scale around tuning airspeed */
 
 				float airspeed;
 
 				/* if airspeed is smaller than min, the sensor is not giving good readings */
 				if (!_airspeed_valid ||
-				    (_airspeed.indicated_airspeed_m_s < _parameters.airspeed_min) ||
+				    (_airspeed.indicated_airspeed_m_s < 0.1f * _parameters.airspeed_min) ||
 				    !isfinite(_airspeed.indicated_airspeed_m_s)) {
-					airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
+					airspeed = _parameters.airspeed_trim;
 
 				} else {
 					airspeed = _airspeed.indicated_airspeed_m_s;
@@ -585,7 +659,8 @@ FixedwingAttitudeControl::task_main()
 				float airspeed_scaling = _parameters.airspeed_trim / airspeed;
 				//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
 
-				float roll_sp, pitch_sp;
+				float roll_sp = 0.0f;
+				float pitch_sp = 0.0f;
 				float throttle_sp = 0.0f;
 
 				if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
@@ -635,46 +710,86 @@ FixedwingAttitudeControl::task_main()
 					}
 				}
 
-				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
+				/* Prepare speed_body_u and speed_body_w */
+				float speed_body_u = 0.0f;
+				float speed_body_v = 0.0f;
+				float speed_body_w = 0.0f;
+				if(_att.R_valid) 	{
+					speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz;
+					speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz;
+					speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz;
+				} else	{
+					warnx("Did not get a valid R\n");
+				}
 
-					float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
-									    airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-					_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
+				/* Run attitude controllers */
+				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
+					_roll_ctrl.control_attitude(roll_sp, _att.roll);
+					_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
+					_yaw_ctrl.control_attitude(_att.roll, _att.pitch,
+							speed_body_u, speed_body_v, speed_body_w,
+							_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
+
+					/* Run attitude RATE controllers which need the desired attitudes from above */
+					float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
+							_att.rollspeed, _att.yawspeed,
+							_yaw_ctrl.get_desired_rate(),
+							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f;
+					if (!isfinite(roll_u)) {
+						warnx("roll_u %.4f", roll_u);
+					}
 
-					float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
-									      lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-					_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
+					float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
+							_att.pitchspeed, _att.yawspeed,
+							_yaw_ctrl.get_desired_rate(),
+							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f;
+					if (!isfinite(pitch_u)) {
+						warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
+								pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body);
+					}
 
-					float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
-									  _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-					_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
+					float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
+							_att.pitchspeed, _att.yawspeed,
+							_pitch_ctrl.get_desired_rate(),
+							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f;
+					if (!isfinite(yaw_u)) {
+						warnx("yaw_u %.4f", yaw_u);
+					}
 
 					/* throttle passed through */
 					_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+					if (!isfinite(throttle_sp)) {
+						warnx("throttle_sp %.4f", throttle_sp);
+					}
+				} else {
+					warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp);
+				}
 
-					// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
-					// 			airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
-					// 			_actuators.control[2], _actuators.control[3]);
+				// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
+				// 			airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
+				// 			_actuators.control[2], _actuators.control[3]);
 
-					/*
-					 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
-					 * only once available
-					 */
-					vehicle_rates_setpoint_s rates_sp;
-					rates_sp.roll = _roll_ctrl.get_desired_rate();
-					rates_sp.pitch = _pitch_ctrl.get_desired_rate();
-					rates_sp.yaw = 0.0f; // XXX not yet implemented
+				/*
+				 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
+				 * only once available
+				 */
+				vehicle_rates_setpoint_s rates_sp;
+				rates_sp.roll = _roll_ctrl.get_desired_rate();
+				rates_sp.pitch = _pitch_ctrl.get_desired_rate();
+				rates_sp.yaw = _yaw_ctrl.get_desired_rate();
 
-					rates_sp.timestamp = hrt_absolute_time();
+				rates_sp.timestamp = hrt_absolute_time();
 
-					if (_rate_sp_pub > 0) {
-						/* publish the attitude setpoint */
-						orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
+				if (_rate_sp_pub > 0) {
+					/* publish the attitude setpoint */
+					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
 
-					} else {
-						/* advertise and publish */
-						_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
-					}
+				} else {
+					/* advertise and publish */
+					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
 				}
 
 			} else {
@@ -692,6 +807,7 @@ FixedwingAttitudeControl::task_main()
 
 			/* lazily publish the setpoint only once available */
 			_actuators.timestamp = hrt_absolute_time();
+			_actuators_airframe.timestamp = hrt_absolute_time();
 
 			if (_actuators_0_pub > 0) {
 				/* publish the attitude setpoint */
@@ -702,6 +818,19 @@ FixedwingAttitudeControl::task_main()
 				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
 			}
 
+			if (_actuators_1_pub > 0) {
+				/* publish the attitude setpoint */
+				orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe);
+//				warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
+//						(double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2],
+//						(double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5],
+//						(double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]);
+
+			} else {
+				/* advertise and publish */
+				_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
+			}
+
 		}
 
 		perf_end(_loop_perf);
diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index be76524da..0b37f48ba 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+f *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -44,11 +44,13 @@
 
 #include <systemlib/param/param.h>
 
+//XXX resolve unclear naming of paramters: FW_P_P --> FW_PR_P
+
 /*
  * Controller parameters, accessible via MAVLink
  *
  */
-
+//xxx: update descriptions
 // @DisplayName		Attitude Time Constant
 // @Description		This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
 // @Range		0.4 to 1.0 seconds, in tens of seconds
@@ -57,33 +59,33 @@ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
 // @DisplayName		Proportional gain.
 // @Description		This defines how much the elevator input will be commanded dependend on the current pitch error.
 // @Range		10 to 200, 1 increments
-PARAM_DEFINE_FLOAT(FW_P_P, 40.0f);
+PARAM_DEFINE_FLOAT(FW_PR_P, 0.3f);
 
 // @DisplayName		Damping gain.
 // @Description		This gain damps the airframe pitch rate. In particular relevant for flying wings.
 // @Range		0.0 to 10.0, 0.1 increments
-PARAM_DEFINE_FLOAT(FW_P_D, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PR_D, 0.0f); //xxx: remove
 
 // @DisplayName		Integrator gain.
 // @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
 // @Range		0 to 50.0
-PARAM_DEFINE_FLOAT(FW_P_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PR_I, 0.05f);
 
 // @DisplayName		Maximum positive / up pitch rate.
 // @Description		This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
 // @Range		0 to 90.0 degrees per seconds, in 1 increments
-PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
+PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f);
 
 // @DisplayName		Maximum negative / down pitch rate.
 // @Description		This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
 // @Range		0 to 90.0 degrees per seconds, in 1 increments
-PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
+PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
 
 // @DisplayName		Pitch Integrator Anti-Windup
 // @Description		This limits the range in degrees the integrator can wind up to.
 // @Range		0.0 to 45.0
 // @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f);
+PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
 
 // @DisplayName		Roll feedforward gain.
 // @Description		This compensates during turns and ensures the nose stays level.
@@ -97,27 +99,27 @@ PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f);
 // @Range		10.0 200.0
 // @Increment		10.0
 // @User		User
-PARAM_DEFINE_FLOAT(FW_R_P, 40.0f);
+PARAM_DEFINE_FLOAT(FW_RR_P, 0.5f);
 
 // @DisplayName		Damping Gain
 // @Description		Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence.
 // @Range		0.0 10.0
 // @Increment		1.0
 // @User		User
-PARAM_DEFINE_FLOAT(FW_R_D, 0.0f);
+PARAM_DEFINE_FLOAT(FW_RR_D, 0.0f); //xxx: remove
 
 // @DisplayName		Integrator Gain
 // @Description		This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors.
 // @Range		0.0 100.0
 // @Increment		5.0
 // @User		User
-PARAM_DEFINE_FLOAT(FW_R_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_RR_I, 0.05f);
 
 // @DisplayName		Roll Integrator Anti-Windup
 // @Description		This limits the range in degrees the integrator can wind up to.
 // @Range		0.0 to 45.0
 // @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f);
+PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
 
 // @DisplayName		Maximum Roll Rate
 // @Description		This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
@@ -126,11 +128,17 @@ PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f);
 PARAM_DEFINE_FLOAT(FW_R_RMAX, 60);
 
 
-PARAM_DEFINE_FLOAT(FW_Y_P, 0);
-PARAM_DEFINE_FLOAT(FW_Y_I, 0);
-PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f);
-PARAM_DEFINE_FLOAT(FW_Y_D, 0);
-PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 1);
+PARAM_DEFINE_FLOAT(FW_YR_P, 0.5);
+PARAM_DEFINE_FLOAT(FW_YR_I, 0.05);
+PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
+PARAM_DEFINE_FLOAT(FW_YR_D, 0); //xxx: remove
+PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0);
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f);
 PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f);
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f);
+PARAM_DEFINE_FLOAT(FW_Y_RMAX, 60);
+PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1.0f);
+
+PARAM_DEFINE_FLOAT(FW_RR_FF, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PR_FF, 0.0f);
+PARAM_DEFINE_FLOAT(FW_YR_FF, 0.0f);
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 5e32ac32f..40cb05b7d 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
@@ -67,7 +67,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
+#include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
@@ -83,9 +83,11 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
+#include <mavlink/mavlink_log.h>
 
 #include <ecl/l1/ecl_l1_pos_controller.h>
 #include <external_lgpl/tecs/tecs.h>
+#include "landingslope.h"
 
 /**
  * L1 control app start / stop handling function
@@ -115,12 +117,13 @@ public:
 	int		start();
 
 private:
+	int		_mavlink_fd;
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
 	int		_control_task;			/**< task handle for sensor task */
 
 	int		_global_pos_sub;
-	int		_global_set_triplet_sub;
+	int		_mission_item_triplet_sub;
 	int		_att_sub;			/**< vehicle attitude subscription */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
@@ -139,7 +142,7 @@ private:
 	struct airspeed_s				_airspeed;		/**< airspeed */
 	struct vehicle_control_mode_s				_control_mode;		/**< vehicle status */
 	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
-	struct vehicle_global_position_set_triplet_s	_global_triplet;	/**< triplet of global setpoints */
+	struct mission_item_triplet_s			_mission_item_triplet;		/**< triplet of mission items */
 	struct accel_report				_accel;			/**< body frame accelerations */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
@@ -160,7 +163,16 @@ private:
 
 	/* land states */
 	/* not in non-abort mode for landing yet */
-	bool land_noreturn;
+	bool land_noreturn_horizontal;
+	bool land_noreturn_vertical;
+	bool land_stayonground;
+	bool land_motor_lim;
+	bool land_onslope;
+
+	/* Landingslope object */
+	Landingslope landingslope;
+
+	float flare_curve_alt_last;
 	/* heading hold */
 	float target_bearing;
 
@@ -206,6 +218,16 @@ private:
 		float throttle_land_max;
 
 		float loiter_hold_radius;
+
+		float heightrate_p;
+		float speedrate_p;
+
+		float land_slope_angle;
+		float land_slope_length;
+		float land_H1_virt;
+		float land_flare_alt_relative;
+		float land_thrust_lim_horizontal_distance;
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -240,6 +262,16 @@ private:
 		param_t throttle_land_max;
 
 		param_t loiter_hold_radius;
+
+		param_t heightrate_p;
+		param_t speedrate_p;
+
+		param_t land_slope_angle;
+		param_t land_slope_length;
+		param_t land_H1_virt;
+		param_t land_flare_alt_relative;
+		param_t land_thrust_lim_horizontal_distance;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -280,13 +312,18 @@ private:
 	void		vehicle_setpoint_poll();
 
 	/**
+	 * Publish navigation capabilities
+	 */
+	void navigation_capabilities_publish();
+
+	/**
 	 * Control position.
 	 */
-	bool		control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed,
-					 const struct vehicle_global_position_set_triplet_s &global_triplet);
+	bool		control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed,
+					 const struct mission_item_triplet_s &_mission_item_triplet);
 
 	float calculate_target_airspeed(float airspeed_demand);
-	void calculate_gndspeed_undershoot();
+	void calculate_gndspeed_undershoot(const math::Vector2f &current_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet);
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -318,7 +355,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 /* subscriptions */
 	_global_pos_sub(-1),
-	_global_set_triplet_sub(-1),
+	_mission_item_triplet_sub(-1),
 	_att_sub(-1),
 	_airspeed_sub(-1),
 	_control_mode_sub(-1),
@@ -338,8 +375,28 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_airspeed_valid(false),
 	_groundspeed_undershoot(0.0f),
 	_global_pos_valid(false),
-	land_noreturn(false)
+	land_noreturn_horizontal(false),
+	land_noreturn_vertical(false),
+	land_stayonground(false),
+	land_motor_lim(false),
+	land_onslope(false),
+	flare_curve_alt_last(0.0f),
+	_mavlink_fd(-1)
 {
+	/* safely initialize structs */
+	vehicle_attitude_s			_att = {0};
+	vehicle_attitude_setpoint_s		_att_sp = {0};
+	navigation_capabilities_s		_nav_capabilities = {0};
+	manual_control_setpoint_s		_manual = {0};
+	airspeed_s				_airspeed = {0};
+	vehicle_control_mode_s			_control_mode = {0};
+	vehicle_global_position_s		_global_pos = {0};
+	mission_item_triplet_s			_mission_item_triplet = {0};
+	accel_report				_accel = {0};
+
+
+
+
 	_nav_capabilities.turn_distance = 0.0f;
 
 	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
@@ -358,6 +415,12 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
 	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
 
+	_parameter_handles.land_slope_angle = param_find("FW_LND_ANG");
+	_parameter_handles.land_slope_length = param_find("FW_LND_SLLR");
+	_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
+	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
+	_parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST");
+
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.min_sink_rate = 			param_find("FW_T_SINK_MIN");
 	_parameter_handles.max_sink_rate =			param_find("FW_T_SINK_MAX");
@@ -370,6 +433,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.roll_throttle_compensation = 	param_find("FW_T_RLL2THR");
 	_parameter_handles.speed_weight = 			param_find("FW_T_SPDWEIGHT");
 	_parameter_handles.pitch_damping = 			param_find("FW_T_PTCH_DAMP");
+	_parameter_handles.heightrate_p =			param_find("FW_T_HRATE_P");
+	_parameter_handles.speedrate_p =			param_find("FW_T_SRATE_P");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -435,6 +500,15 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
 	param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
 
+	param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
+	param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
+
+	param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
+	param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length));
+	param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
+	param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
+	param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance));
+
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
 	_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
@@ -447,12 +521,14 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
 	_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
 	_tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega);
-	_tecs.set_roll_throttle_compensation(math::radians(_parameters.roll_throttle_compensation));
+	_tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation);
 	_tecs.set_speed_weight(_parameters.speed_weight);
 	_tecs.set_pitch_damping(_parameters.pitch_damping);
 	_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
 	_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
 	_tecs.set_max_climb_rate(_parameters.max_climb_rate);
+	_tecs.set_heightrate_p(_parameters.heightrate_p);
+	_tecs.set_speedrate_p(_parameters.speedrate_p);
 
 	/* sanity check parameters */
 	if (_parameters.airspeed_max < _parameters.airspeed_min ||
@@ -464,6 +540,15 @@ FixedwingPositionControl::parameters_update()
 		return 1;
 	}
 
+	/* Update the landing slope */
+	landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt);
+
+	/* Update and publish the navigation capabilities */
+	_nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
+	_nav_capabilities.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement();
+	_nav_capabilities.landing_flare_length = landingslope.flare_length();
+	navigation_capabilities_publish();
+
 	return OK;
 }
 
@@ -548,11 +633,11 @@ void
 FixedwingPositionControl::vehicle_setpoint_poll()
 {
 	/* check if there is a new setpoint */
-	bool global_sp_updated;
-	orb_check(_global_set_triplet_sub, &global_sp_updated);
+	bool mission_item_triplet_updated;
+	orb_check(_mission_item_triplet_sub, &mission_item_triplet_updated);
 
-	if (global_sp_updated) {
-		orb_copy(ORB_ID(vehicle_global_position_set_triplet), _global_set_triplet_sub, &_global_triplet);
+	if (mission_item_triplet_updated) {
+		orb_copy(ORB_ID(mission_item_triplet), _mission_item_triplet_sub, &_mission_item_triplet);
 		_setpoint_valid = true;
 	}
 }
@@ -595,17 +680,29 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
 }
 
 void
-FixedwingPositionControl::calculate_gndspeed_undershoot()
+FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f &current_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet)
 {
 
 	if (_global_pos_valid) {
-		/* get ground speed vector */
-		math::Vector<2> ground_speed_vector(_global_pos.vx, _global_pos.vy);
 
-		/* rotate with current attitude */
-		math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
+		/* rotate ground speed vector with current attitude */
+		math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
 		yaw_vector.normalize();
-		float ground_speed_body = yaw_vector * ground_speed_vector;
+		float ground_speed_body = yaw_vector * ground_speed;
+
+		/* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/
+		float distance = 0.0f;
+		float delta_altitude = 0.0f;
+		if (mission_item_triplet.previous_valid) {
+			distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon);
+			delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude;
+		} else {
+			distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), mission_item_triplet.current.lat, mission_item_triplet.current.lon);
+			delta_altitude = mission_item_triplet.current.altitude -  _global_pos.alt;
+		}
+
+		float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance));
+
 
 		/*
 		 * Ground speed undershoot is the amount of ground velocity not reached
@@ -616,20 +713,29 @@ FixedwingPositionControl::calculate_gndspeed_undershoot()
 		 * not exceeded) travels towards a waypoint (and is not pushed more and more away
 		 * by wind). Not countering this would lead to a fly-away.
 		 */
-		_groundspeed_undershoot = math::max(_parameters.airspeed_min - ground_speed_body, 0.0f);
+		_groundspeed_undershoot = math::max(ground_speed_desired - ground_speed_body, 0.0f);
 
 	} else {
 		_groundspeed_undershoot = 0;
 	}
 }
 
+void FixedwingPositionControl::navigation_capabilities_publish()
+{
+	if (_nav_capabilities_pub > 0) {
+		orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
+	} else {
+		_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
+	}
+}
+
 bool
-FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed,
-		const struct vehicle_global_position_set_triplet_s &global_triplet)
+FixedwingPositionControl::control_position(const math::Vector2f &current_position, const math::Vector2f &ground_speed,
+		const struct mission_item_triplet_s &mission_item_triplet)
 {
 	bool setpoint = true;
 
-	calculate_gndspeed_undershoot();
+	calculate_gndspeed_undershoot(current_position, ground_speed, mission_item_triplet);
 
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
@@ -641,7 +747,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	math::Vector<3> accel_earth = _R_nb.transposed() * accel_body;
 
 	_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
-	float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
+	float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt;
 
 	/* no throttle limit as default */
 	float throttle_max = 1.0f;
@@ -658,252 +764,245 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
 		_tecs.set_speed_weight(_parameters.speed_weight);
 
-		/* execute navigation once we have a setpoint */
-		if (_setpoint_valid) {
+		/* current waypoint (the one currently heading for) */
+		math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
 
-			/* current waypoint (the one currently heading for) */
-			math::Vector<2> next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
+		/* current waypoint (the one currently heading for) */
+		math::Vector2f curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
 
-			/* previous waypoint */
-			math::Vector<2> prev_wp;
 
-			if (global_triplet.previous_valid) {
-				prev_wp(0) = global_triplet.previous.lat / 1e7f;
-				prev_wp(1) = global_triplet.previous.lon / 1e7f;
+		/* previous waypoint */
+		math::Vector2f prev_wp;
 
-			} else {
-				/*
-				 * No valid previous waypoint, go for the current wp.
-				 * This is automatically handled by the L1 library.
-				 */
-				prev_wp(0) = global_triplet.current.lat / 1e7f;
-				prev_wp(1) = global_triplet.current.lon / 1e7f;
+		if (mission_item_triplet.previous_valid) {
+			prev_wp.setX(mission_item_triplet.previous.lat);
+			prev_wp.setY(mission_item_triplet.previous.lon);
 
-			}
+		} else {
+			/*
+			 * No valid previous waypoint, go for the current wp.
+			 * This is automatically handled by the L1 library.
+			 */
+			prev_wp.setX(mission_item_triplet.current.lat);
+			prev_wp.setY(mission_item_triplet.current.lon);
 
-			// XXX add RTL switch
-			if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) {
+		}
 
-				math::Vector<2> rtl_pos(_launch_lat, _launch_lon);
+		if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
+			/* waypoint is a plain navigation waypoint */
+			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
+			_att_sp.roll_body = _l1_control.nav_roll();
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-				_l1_control.navigate_waypoints(rtl_pos, rtl_pos, current_position, ground_speed);
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+			_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
+						    _airspeed.indicated_airspeed_m_s, eas2tas,
+						    false, math::radians(_parameters.pitch_limit_min),
+						    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+						    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 
-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _launch_alt, calculate_target_airspeed(_parameters.airspeed_trim),
-							    _airspeed.indicated_airspeed_m_s, eas2tas,
-							    false, math::radians(_parameters.pitch_limit_min),
-							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+		} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+			   mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+			   mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
 
-				// XXX handle case when having arrived at home (loiter)
+			/* waypoint is a loiter waypoint */
+			_l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius,
+						  mission_item_triplet.current.loiter_direction, ground_speed);
+			_att_sp.roll_body = _l1_control.nav_roll();
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-			} else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
-				/* waypoint is a plain navigation waypoint */
-				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+			_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
+						    _airspeed.indicated_airspeed_m_s, eas2tas,
+						    false, math::radians(_parameters.pitch_limit_min),
+						    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+						    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 
-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
-							    _airspeed.indicated_airspeed_m_s, eas2tas,
-							    false, math::radians(_parameters.pitch_limit_min),
-							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+		} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
 
-			} else if (global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-				   global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-				   global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+			/* Horizontal landing control */
+			/* switch to heading hold for the last meters, continue heading hold after */
+			float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY());
+			//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
+			const float heading_hold_distance = 15.0f;
+			if (wp_distance < heading_hold_distance || land_noreturn_horizontal) {
 
-				/* waypoint is a loiter waypoint */
-				_l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius,
-							  global_triplet.current.loiter_direction, ground_speed);
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+				/* heading hold, along the line connecting this and the last waypoint */
+				
 
-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
-							    _airspeed.indicated_airspeed_m_s, eas2tas,
-							    false, math::radians(_parameters.pitch_limit_min),
-							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+				// if (mission_item_triplet.previous_valid) {
+				// 	target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
+				// } else {
 
-			} else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) {
+				if (!land_noreturn_horizontal) //set target_bearing in first occurrence
+					target_bearing = _att.yaw;
+				//}
 
-				/* switch to heading hold for the last meters, continue heading hold after */
+//					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
 
-				float wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), current_position(0), current_position(1));
-				//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
-				if (wp_distance < 15.0f || land_noreturn) {
+				_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
 
-					/* heading hold, along the line connecting this and the last waypoint */
-					
+				/* limit roll motion to prevent wings from touching the ground first */
+				_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
 
-					// if (global_triplet.previous_valid) {
-					// 	target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
-					// } else {
+				land_noreturn_horizontal = true;
 
-					if (!land_noreturn)
-						target_bearing = _att.yaw;
-					//}
-
-					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
+			} else {
 
-					_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
+				/* normal navigation */
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
+			}
 
-					if (altitude_error > -5.0f)
-						land_noreturn = true;
+			_att_sp.roll_body = _l1_control.nav_roll();
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-				} else {
 
-					/* normal navigation */
-					_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
-				}
+			/* Vertical landing control */
+			//xxx: using the tecs altitude controller for slope control for now
 
-				/* do not go down too early */
-				if (wp_distance > 50.0f) {
-					altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
-				}
+//				/* do not go down too early */
+//				if (wp_distance > 50.0f) {
+//					altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
+//				}
+			/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
+			// XXX this could make a great param
 
+			float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1)
+			float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
+			float airspeed_land = 1.3f * _parameters.airspeed_min;
+			float airspeed_approach = 1.3f * _parameters.airspeed_min;
 
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+			float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length;
+			float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
+			float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
 
-				/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
-				// XXX this could make a great param
 
-				float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
-				float land_pitch_min = math::radians(5.0f);
-				float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
-				float airspeed_land = _parameters.airspeed_min;
-				float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f;
 
-				if (altitude_error > -4.0f) {
+			if ( (_global_pos.alt < _mission_item_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
 
-					/* land with minimal speed */
+				/* land with minimal speed */
 
-					/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
-					_tecs.set_speed_weight(2.0f);
+//					/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
+//					_tecs.set_speed_weight(2.0f);
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    false, flare_angle_rad,
-								    0.0f, _parameters.throttle_max, throttle_land,
-								    math::radians(-10.0f), math::radians(15.0f));
+				/* kill the throttle if param requests it */
+				throttle_max = _parameters.throttle_max;
 
-					/* kill the throttle if param requests it */
+				 if (wp_distance < landingslope.motor_lim_horizontal_distance() || land_motor_lim) {
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
+					if (!land_motor_lim) {
+						land_motor_lim  = true;
+						mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle");
+					}
 
-					/* 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));
-
-				} else if (wp_distance < 60.0f && altitude_error > -20.0f) {
-
-					/* minimize speed to approach speed */
-
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    false, flare_angle_rad,
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
-
-				} else {
+				 }
 
-					/* normal cruise speed */
+				float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude);
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    false, math::radians(_parameters.pitch_limit_min),
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+				/* avoid climbout */
+				if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
+				{
+					flare_curve_alt = mission_item_triplet.current.altitude;
+					land_stayonground = true;
 				}
 
-			} else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
+				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land),
+												    _airspeed.indicated_airspeed_m_s, eas2tas,
+												    false, flare_pitch_angle_rad,
+												    0.0f, throttle_max, throttle_land,
+												    flare_pitch_angle_rad,  math::radians(15.0f));
 
-				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
+				if (!land_noreturn_vertical) {
+					mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
+					land_noreturn_vertical = true;
+				}
+				//warnx("Landing:  flare, _global_pos.alt  %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
 
-				/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
-				if (altitude_error > 10.0f) {
+				flare_curve_alt_last = flare_curve_alt;
 
-					/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)),
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+			} else if (wp_distance < L_wp_distance) {
 
-					/* 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));
+				/* minimize speed to approach speed, stay on landing slope */
+				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach),
+							    _airspeed.indicated_airspeed_m_s, eas2tas,
+							    false, flare_pitch_angle_rad,
+							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+				//warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length);
 
-				} else {
+				if (!land_onslope) {
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    false, math::radians(_parameters.pitch_limit_min),
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+					mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
+					land_onslope = true;
 				}
-			}
-
-			// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
-			//       (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
-			// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(),
-			//       (double)next_wp.getX(), (double)next_wp.getY(), (global_triplet.previous_valid) ? "valid" : "invalid");
-
-			// XXX at this point we always want no loiter hold if a
-			// mission is active
-			_loiter_hold = false;
 
-		} else if (_control_mode.flag_armed) {
-
-			/* hold position, but only if armed, climb 20m in case this is engaged on ground level */
+			} else {
 
-			// XXX rework with smarter state machine
+				 /* intersect glide slope:
+				 * if current position is higher or within 10m of slope follow the glide slope
+				 * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
+				 * */
+				float altitude_desired = _global_pos.alt;
+				if (_global_pos.alt > landing_slope_alt_desired - 10.0f) {
+					/* stay on slope */
+					altitude_desired = landing_slope_alt_desired;
+					//warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement);
+				} else {
+					/* continue horizontally */
+					altitude_desired =  math::max(_global_pos.alt, L_altitude);
+					//warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance);
+				}
 
-			if (!_loiter_hold) {
-				_loiter_hold_lat = _global_pos.lat / 1e7f;
-				_loiter_hold_lon = _global_pos.lon / 1e7f;
-				_loiter_hold_alt = _global_pos.alt + 25.0f;
-				_loiter_hold = true;
+				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach),
+							    _airspeed.indicated_airspeed_m_s, eas2tas,
+							    false, math::radians(_parameters.pitch_limit_min),
+							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 			}
 
-			altitude_error = _loiter_hold_alt - _global_pos.alt;
-
-			math::Vector<2> loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon);
+		} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
 
-			/* loiter around current position */
-			_l1_control.navigate_loiter(loiter_hold_pos, current_position, _parameters.loiter_hold_radius,
-						  1, ground_speed);
+			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
 			_att_sp.roll_body = _l1_control.nav_roll();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-			/* climb with full throttle if the altitude error is bigger than 5 meters */
-			bool climb_out = (altitude_error > 3);
+			/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
+			if (altitude_error > 15.0f) {
 
-			float min_pitch;
+				/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
+				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
+							    _airspeed.indicated_airspeed_m_s, eas2tas,
+							    true, math::max(math::radians(mission_item_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));
 
-			if (climb_out) {
-				min_pitch = math::radians(20.0f);
+				/* 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 {
-				min_pitch = math::radians(_parameters.pitch_limit_min);
-			}
 
-			_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _loiter_hold_alt, calculate_target_airspeed(_parameters.airspeed_trim),
-						    _airspeed.indicated_airspeed_m_s, eas2tas,
-						    climb_out, min_pitch,
-						    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-						    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
-
-			if (climb_out) {
-				/* limit roll motion to ensure enough lift */
-				_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
+				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
+							    _airspeed.indicated_airspeed_m_s, eas2tas,
+							    false, math::radians(_parameters.pitch_limit_min),
+							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 			}
 		}
 
+		// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
+		//       (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
+		// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(),
+		//       (double)next_wp.getX(), (double)next_wp.getY(), (mission_item_triplet.previous_valid) ? "valid" : "invalid");
+
+		// XXX at this point we always want no loiter hold if a
+		// mission is active
+		_loiter_hold = false;
+
 		/* reset land state */
-		if (global_triplet.current.nav_cmd != NAV_CMD_LAND) {
-			land_noreturn = false;
+		if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) {
+			land_noreturn_horizontal = false;
+			land_noreturn_vertical = false;
+			land_stayonground = false;
+			land_motor_lim = false;
+			land_onslope = false;
 		}
 
 		if (was_circle_mode && !_l1_control.circle_mode()) {
@@ -1018,7 +1117,7 @@ FixedwingPositionControl::task_main()
 	 * do subscriptions
 	 */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	_global_set_triplet_sub = orb_subscribe(ORB_ID(vehicle_global_position_set_triplet));
+	_mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
 	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -1080,6 +1179,11 @@ FixedwingPositionControl::task_main()
 		/* only run controller if position changed */
 		if (fds[1].revents & POLLIN) {
 
+			/* XXX Hack to get mavlink output going */
+			if (_mavlink_fd < 0) {
+				/* try to open the mavlink log device every once in a while */
+				_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+			}
 
 			static uint64_t last_run = 0;
 			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -1108,7 +1212,7 @@ FixedwingPositionControl::task_main()
 			 * Attempt to control position, on success (= sensors present and not in manual mode),
 			 * publish setpoint.
 			 */
-			if (control_position(current_position, ground_speed, _global_triplet)) {
+			if (control_position(current_position, ground_speed, _mission_item_triplet)) {
 				_att_sp.timestamp = hrt_absolute_time();
 
 				/* lazily publish the setpoint only once available */
@@ -1121,7 +1225,8 @@ FixedwingPositionControl::task_main()
 					_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
 				}
 
-				float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy);
+				/* XXX check if radius makes sense here */
+				float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.radius);
 
 				/* lazily publish navigation capabilities */
 				if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
@@ -1129,11 +1234,8 @@ FixedwingPositionControl::task_main()
 					/* set new turn distance */
 					_nav_capabilities.turn_distance = turn_distance;
 
-					if (_nav_capabilities_pub > 0) {
-						orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
-					} else {
-						_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
-					}
+					navigation_capabilities_publish();
+
 				}
 
 			}
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 3bb872405..10a0c00fc 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -111,3 +111,12 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
 
 
 PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
+
+PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
+PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
+
+PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f);
+PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
+PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
+PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
+PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f);
diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp
new file mode 100644
index 000000000..b139a6397
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/landingslope.cpp
@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file: landingslope.cpp
+ *
+ */
+
+#include "landingslope.h"
+
+#include <nuttx/config.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <math.h>
+#include <unistd.h>
+#include <mathlib/mathlib.h>
+
+void Landingslope::update(float landing_slope_angle_rad,
+		float flare_relative_alt,
+		float motor_lim_horizontal_distance,
+		float H1_virt)
+{
+
+	_landing_slope_angle_rad = landing_slope_angle_rad;
+	_flare_relative_alt = flare_relative_alt;
+	_motor_lim_horizontal_distance = motor_lim_horizontal_distance;
+	_H1_virt = H1_virt;
+
+	calculateSlopeValues();
+}
+
+void Landingslope::calculateSlopeValues()
+{
+	_H0 =  _flare_relative_alt + _H1_virt;
+	_d1 = _flare_relative_alt/tanf(_landing_slope_angle_rad);
+	_flare_constant = (_H0 * _d1)/_flare_relative_alt;
+	_flare_length = - logf(_H1_virt/_H0) * _flare_constant;
+	_horizontal_slope_displacement = (_flare_length - _d1);
+}
+
+float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude)
+{
+	return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
+}
+
+float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude)
+{
+	return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
+
+}
+
diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h
new file mode 100644
index 000000000..8ff431509
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/landingslope.h
@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file: landingslope.h
+ *
+ */
+
+#ifndef LANDINGSLOPE_H_
+#define LANDINGSLOPE_H_
+
+#include <math.h>
+#include <systemlib/err.h>
+
+class Landingslope
+{
+private:
+	//xxx: documentation of landing pending
+	float _landing_slope_angle_rad;
+	float _flare_relative_alt;
+	float _motor_lim_horizontal_distance;
+	float _H1_virt;
+	float _H0;
+	float _d1;
+	float _flare_constant;
+	float _flare_length;
+	float _horizontal_slope_displacement;
+
+	void calculateSlopeValues();
+
+public:
+	Landingslope() {}
+	~Landingslope() {}
+
+	float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude);
+
+	/**
+	 *
+	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 */
+	__EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
+	{
+		return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative
+	}
+
+	/**
+	 *
+	 * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude
+	 */
+	__EXPORT static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
+	{
+		return (slope_altitude - wp_landing_altitude)/tanf(landing_slope_angle_rad) + horizontal_slope_displacement;
+
+	}
+
+
+	float getFlareCurveAltitude(float wp_distance, float wp_altitude);
+
+	void update(float landing_slope_angle_rad,
+			float flare_relative_alt,
+			float motor_lim_horizontal_distance,
+			float H1_virt);
+
+
+	inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;}
+	inline float flare_relative_alt() {return _flare_relative_alt;}
+	inline float motor_lim_horizontal_distance() {return _motor_lim_horizontal_distance;}
+	inline float H1_virt() {return _H1_virt;}
+	inline float H0() {return _H0;}
+	inline float d1() {return _d1;}
+	inline float flare_constant() {return _flare_constant;}
+	inline float flare_length() {return _flare_length;}
+	inline float horizontal_slope_displacement() {return _horizontal_slope_displacement;}
+
+};
+
+
+#endif /* LANDINGSLOPE_H_ */
diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk
index b00b9aa5a..cf419ec7e 100644
--- a/src/modules/fw_pos_control_l1/module.mk
+++ b/src/modules/fw_pos_control_l1/module.mk
@@ -38,4 +38,5 @@
 MODULE_COMMAND	= fw_pos_control_l1
 
 SRCS		= fw_pos_control_l1_main.cpp \
-		  fw_pos_control_l1_params.c
+		  fw_pos_control_l1_params.c \
+		  landingslope.cpp
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 7c10e297b..2ec00a9bc 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -74,6 +74,8 @@
 #include "waypoints.h"
 #include "mavlink_parameters.h"
 
+#include <uORB/topics/mission_result.h>
+
 /* define MAVLink specific parameters */
 PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
 PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
@@ -644,6 +646,10 @@ int mavlink_thread_main(int argc, char *argv[])
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
 	}
 
+	int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
+	struct mission_result_s mission_result;
+	memset(&mission_result, 0, sizeof(mission_result));
+
 	thread_running = true;
 
 	/* arm counter to go off immediately */
@@ -690,6 +696,17 @@ int mavlink_thread_main(int argc, char *argv[])
 
 		lowspeed_counter++;
 
+		bool updated;
+		orb_check(mission_result_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
+
+			if (mission_result.mission_reached) {
+				mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index);
+			}
+		}
+
 		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
 
 		/* sleep quarter the time */
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index 124b3b2ae..318dcf08c 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -73,11 +73,15 @@
 #include "waypoints.h"
 #include "mavlink_parameters.h"
 
+
+
 static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
 static uint64_t loiter_start_time;
 
+#if 0
 static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
 	struct vehicle_global_position_setpoint_s *sp);
+#endif
 
 int
 mavlink_missionlib_send_message(mavlink_message_t *msg)
@@ -88,6 +92,8 @@ mavlink_missionlib_send_message(mavlink_message_t *msg)
 	return 0;
 }
 
+
+
 int
 mavlink_missionlib_send_gcs_string(const char *string)
 {
@@ -127,6 +133,7 @@ uint64_t mavlink_missionlib_get_system_timestamp()
 	return hrt_absolute_time();
 }
 
+#if 0
 /**
  * Set special vehicle setpoint fields based on current mission item.
  *
@@ -206,7 +213,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command)
 {
 	static orb_advert_t global_position_setpoint_pub = -1;
-	static orb_advert_t global_position_set_triplet_pub = -1;
+	// static orb_advert_t global_position_set_triplet_pub = -1;
 	static orb_advert_t local_position_setpoint_pub = -1;
 	static unsigned last_waypoint_index = -1;
 	char buf[50] = {0};
@@ -234,10 +241,10 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 
 
 		/* fill triplet: previous, current, next waypoint */
-		struct vehicle_global_position_set_triplet_s triplet;
+		// struct vehicle_global_position_set_triplet_s triplet;
 
 		/* current waypoint is same as sp */
-		memcpy(&(triplet.current), &sp, sizeof(sp));
+		// memcpy(&(triplet.current), &sp, sizeof(sp));
 
 		/*
 		 * Check if previous WP (in mission, not in execution order) 
@@ -291,48 +298,48 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 
 		/* populate last and next */
 
-		triplet.previous_valid = false;
-		triplet.next_valid = false;
-
-		if (last_setpoint_valid) {
-			triplet.previous_valid = true;
-			struct vehicle_global_position_setpoint_s sp;
-			sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f;
-			sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
-			sp.altitude = wpm->waypoints[last_setpoint_index].z;
-			sp.altitude_is_relative = false;
-			sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
-			set_special_fields(wpm->waypoints[last_setpoint_index].param1,
-				wpm->waypoints[last_setpoint_index].param2,
-				wpm->waypoints[last_setpoint_index].param3,
-				wpm->waypoints[last_setpoint_index].param4,
-				wpm->waypoints[last_setpoint_index].command, &sp);
-			memcpy(&(triplet.previous), &sp, sizeof(sp));
-		}
-
-		if (next_setpoint_valid) {
-			triplet.next_valid = true;
-			struct vehicle_global_position_setpoint_s sp;
-			sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f;
-			sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
-			sp.altitude = wpm->waypoints[next_setpoint_index].z;
-			sp.altitude_is_relative = false;
-			sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
-			set_special_fields(wpm->waypoints[next_setpoint_index].param1,
-				wpm->waypoints[next_setpoint_index].param2,
-				wpm->waypoints[next_setpoint_index].param3,
-				wpm->waypoints[next_setpoint_index].param4,
-				wpm->waypoints[next_setpoint_index].command, &sp);
-			memcpy(&(triplet.next), &sp, sizeof(sp));
-		}
+		// triplet.previous_valid = false;
+		// triplet.next_valid = false;
+
+		// if (last_setpoint_valid) {
+		// 	triplet.previous_valid = true;
+		// 	struct vehicle_global_position_setpoint_s sp;
+		// 	sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f;
+		// 	sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
+		// 	sp.altitude = wpm->waypoints[last_setpoint_index].z;
+		// 	sp.altitude_is_relative = false;
+		// 	sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
+		// 	set_special_fields(wpm->waypoints[last_setpoint_index].param1,
+		// 		wpm->waypoints[last_setpoint_index].param2,
+		// 		wpm->waypoints[last_setpoint_index].param3,
+		// 		wpm->waypoints[last_setpoint_index].param4,
+		// 		wpm->waypoints[last_setpoint_index].command, &sp);
+		// 	memcpy(&(triplet.previous), &sp, sizeof(sp));
+		// }
+
+		// if (next_setpoint_valid) {
+		// 	triplet.next_valid = true;
+		// 	struct vehicle_global_position_setpoint_s sp;
+		// 	sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f;
+		// 	sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
+		// 	sp.altitude = wpm->waypoints[next_setpoint_index].z;
+		// 	sp.altitude_is_relative = false;
+		// 	sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
+		// 	set_special_fields(wpm->waypoints[next_setpoint_index].param1,
+		// 		wpm->waypoints[next_setpoint_index].param2,
+		// 		wpm->waypoints[next_setpoint_index].param3,
+		// 		wpm->waypoints[next_setpoint_index].param4,
+		// 		wpm->waypoints[next_setpoint_index].command, &sp);
+		// 	memcpy(&(triplet.next), &sp, sizeof(sp));
+		// }
 
 		/* Initialize triplet publication if necessary */
-		if (global_position_set_triplet_pub < 0) {
-			global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet);
+		// if (global_position_set_triplet_pub < 0) {
+		// 	global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet);
 
-		} else {
-			orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
-		}
+		// } else {
+		// 	orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
+		// }
 
 		sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
 
@@ -388,3 +395,5 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 	printf("%s\n", buf);
 	//printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);
 }
+
+#endif
\ No newline at end of file
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index c37c35a63..17978615f 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -136,7 +136,7 @@ static const struct listener listeners[] = {
 	{l_input_rc,			&mavlink_subs.input_rc_sub,	0},
 	{l_global_position,		&mavlink_subs.global_pos_sub,	0},
 	{l_local_position,		&mavlink_subs.local_pos_sub,	0},
-	{l_global_position_setpoint,	&mavlink_subs.spg_sub,		0},
+	{l_global_position_setpoint,	&mavlink_subs.triplet_sub,	0},
 	{l_local_position_setpoint,	&mavlink_subs.spl_sub,		0},
 	{l_attitude_setpoint,		&mavlink_subs.spa_sub,		0},
 	{l_actuator_outputs,		&mavlink_subs.act_0_sub,	0},
@@ -402,23 +402,24 @@ l_local_position(const struct listener *l)
 void
 l_global_position_setpoint(const struct listener *l)
 {
-	struct vehicle_global_position_setpoint_s global_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp);
+	struct mission_item_triplet_s triplet;
+	orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet);
 
 	uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
+	
+	if (!triplet.current_valid)
+		return;
 
-	if (global_sp.altitude_is_relative)
+	if (triplet.current.altitude_is_relative)
 		coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
 
 	if (gcs_link)
 		mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
 				coordinate_frame,
-				global_sp.lat,
-				global_sp.lon,
-				global_sp.altitude * 1000.0f,
-				global_sp.yaw * M_RAD_TO_DEG_F * 100.0f);
+				(int32_t)(triplet.current.lat * 1e7d),
+				(int32_t)(triplet.current.lon * 1e7d),
+				(int32_t)(triplet.current.altitude * 1e3f),
+				(int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f));
 }
 
 void
@@ -499,8 +500,8 @@ l_actuator_outputs(const struct listener *l)
 						  act_outputs.output[6],
 						  act_outputs.output[7]);
 
-		/* only send in HIL mode */
-		if (mavlink_hil_enabled && armed.armed) {
+		/* only send in HIL mode and only send first group for HIL */
+		if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
 
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
@@ -656,7 +657,7 @@ l_home(const struct listener *l)
 
 	orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
 
-	mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt);
+	mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f);
 }
 
 void
@@ -770,9 +771,9 @@ uorb_receive_start(void)
 	orb_set_interval(mavlink_subs.local_pos_sub, 1000);	/* 1Hz active updates */
 
 	/* --- GLOBAL SETPOINT VALUE --- */
-	mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
-	orb_set_interval(mavlink_subs.spg_sub, 2000);	/* 0.5 Hz updates */
-
+	mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
+	orb_set_interval(mavlink_subs.triplet_sub, 2000);	/* 0.5 Hz updates */
+	
 	/* --- LOCAL SETPOINT VALUE --- */
 	mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
 	orb_set_interval(mavlink_subs.spl_sub, 2000);	/* 0.5 Hz updates */
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index 91773843b..7d24b8f93 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -50,9 +50,9 @@
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
+#include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -86,7 +86,7 @@ struct mavlink_subscriptions {
 	int local_pos_sub;
 	int spa_sub;
 	int spl_sub;
-	int spg_sub;
+	int triplet_sub;
 	int debug_key_value;
 	int input_rc_sub;
 	int optical_flow;
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 7e4a2688f..59db898b9 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
  *   Author: @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
  *           @author Lorenz Meier <lm@inf.ethz.ch>
  *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
@@ -49,19 +49,113 @@
 #include "missionlib.h"
 #include "waypoints.h"
 #include "util.h"
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
 
-#ifndef FM_PI
-#define FM_PI 3.1415926535897932384626433832795f
-#endif
+#include <geo/geo.h>
+#include <dataman/dataman.h>
 
 bool debug = false;
 bool verbose = false;
 
+orb_advert_t mission_pub = -1;
+struct mission_s mission;
 
-#define MAVLINK_WPM_NO_PRINTF
+//#define MAVLINK_WPM_NO_PRINTF
+#define MAVLINK_WPM_VERBOSE 0
 
 uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
 
+void publish_mission()
+{
+	/* Initialize mission publication if necessary */
+	if (mission_pub < 0) {
+		mission_pub = orb_advertise(ORB_ID(mission), &mission);
+
+	} else {
+		orb_publish(ORB_ID(mission), mission_pub, &mission);
+	}
+}
+
+int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
+{
+	/* only support global waypoints for now */
+	switch (mavlink_mission_item->frame) {
+		case MAV_FRAME_GLOBAL:
+			mission_item->lat = (double)mavlink_mission_item->x;
+			mission_item->lon = (double)mavlink_mission_item->y;
+			mission_item->altitude = mavlink_mission_item->z;
+			mission_item->altitude_is_relative = false;
+			break;
+
+		case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+			mission_item->lat = (double)mavlink_mission_item->x;
+			mission_item->lon = (double)mavlink_mission_item->y;
+			mission_item->altitude = mavlink_mission_item->z;
+			mission_item->altitude_is_relative = true;
+			break;
+
+		case MAV_FRAME_LOCAL_NED:
+		case MAV_FRAME_LOCAL_ENU:
+			return MAV_MISSION_UNSUPPORTED_FRAME;
+		case MAV_FRAME_MISSION:
+		default:
+			return MAV_MISSION_ERROR;
+	}
+
+	switch (mavlink_mission_item->command) {
+		case MAV_CMD_NAV_TAKEOFF:
+			mission_item->pitch_min = mavlink_mission_item->param2;
+			break;
+		default:
+			mission_item->radius = mavlink_mission_item->param2;
+			break;
+	}
+
+	mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F);
+	mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
+	mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
+	mission_item->nav_cmd = mavlink_mission_item->command;
+
+	mission_item->time_inside = mavlink_mission_item->param1 / 1e3f; /* from milliseconds to seconds */
+	mission_item->autocontinue = mavlink_mission_item->autocontinue;
+	mission_item->index = mavlink_mission_item->seq;
+	mission_item->origin = ORIGIN_MAVLINK;
+
+	return OK;
+}
+
+int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
+{
+	if (mission_item->altitude_is_relative) {
+		mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
+	} else {
+		mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+	}
+	
+	switch (mission_item->nav_cmd) {
+		case NAV_CMD_TAKEOFF:
+			mavlink_mission_item->param2 = mission_item->pitch_min;
+			break;
+		default:
+			mavlink_mission_item->param2 = mission_item->radius;
+			break;
+	}
+
+	mavlink_mission_item->x = (float)mission_item->lat;
+	mavlink_mission_item->y = (float)mission_item->lon;
+	mavlink_mission_item->z = mission_item->altitude;
+
+	mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F;
+	mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction;
+	mavlink_mission_item->command = mission_item->nav_cmd;
+	mavlink_mission_item->param1 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */
+	mavlink_mission_item->autocontinue = mission_item->autocontinue;
+	mavlink_mission_item->seq = mission_item->index;
+
+	return OK;
+}
+
 void mavlink_wpm_init(mavlink_wpm_storage *state)
 {
 	// Set all waypoints to zero
@@ -73,16 +167,17 @@ void mavlink_wpm_init(mavlink_wpm_storage *state)
 	state->current_partner_sysid = 0;
 	state->current_partner_compid = 0;
 	state->timestamp_lastaction = 0;
-	state->timestamp_last_send_setpoint = 0;
+	// state->timestamp_last_send_setpoint = 0;
 	state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
-	state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT;
-	state->idle = false;      				///< indicates if the system is following the waypoints or is waiting
-	state->current_active_wp_id = -1;		///< id of current waypoint
-	state->yaw_reached = false;						///< boolean for yaw attitude reached
-	state->pos_reached = false;						///< boolean for position reached
-	state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
-	state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
 
+	state->current_dataman_id = 0;
+	// state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT;
+	// state->idle = false;      				///< indicates if the system is following the waypoints or is waiting
+	// state->current_active_wp_id = -1;		///< id of current waypoint
+	// state->yaw_reached = false;						///< boolean for yaw attitude reached
+	// state->pos_reached = false;						///< boolean for position reached
+	// state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
+	// state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
 }
 
 /*
@@ -102,16 +197,15 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
 
 	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
 
-	if (MAVLINK_WPM_TEXT_FEEDBACK) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-		mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
-#else
+// 	if (MAVLINK_WPM_TEXT_FEEDBACK) {
+// #ifdef MAVLINK_WPM_NO_PRINTF
+// 		mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
+// #else
 
-		if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
+// 		if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
 
-#endif
-		mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
-	}
+// #endif
+// 	}
 }
 
 /*
@@ -161,7 +255,7 @@ void mavlink_wpm_send_setpoint(uint16_t seq)
 				cur->param2, cur->param3, cur->param4, cur->x,
 				cur->y, cur->z, cur->frame, cur->command);
 
-		wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp();
+		// wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp();
 
 	} else {
 		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n");
@@ -175,7 +269,7 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou
 
 	wpc.target_system = wpm->current_partner_sysid;
 	wpc.target_component = wpm->current_partner_compid;
-	wpc.count = count;
+	wpc.count = mission.count;
 
 	mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
 	mavlink_missionlib_send_message(&msg);
@@ -185,11 +279,12 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou
 	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
 }
 
-void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
+void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, mavlink_mission_item_t *wp)
 {
-	if (seq < wpm->size) {
+
+	// if (seq < wpm->size) {
 		mavlink_message_t msg;
-		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
+	// 	mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
 		wp->target_system = wpm->current_partner_sysid;
 		wp->target_component = wpm->current_partner_compid;
 		mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp);
@@ -199,9 +294,9 @@ void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
 
 		// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
 
-	} else {
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n");
-	}
+	// } else {
+	// 	if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n");
+	// }
 }
 
 void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
@@ -246,250 +341,191 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
 	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
 }
 
-/*
- * Calculate distance in global frame.
- *
- * The distance calculation is based on the WGS84 geoid (GPS)
- */
-float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z)
-{
-
-	if (seq < wpm->size) {
-		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
-
-		double current_x_rad = wp->x / 180.0 * M_PI;
-		double current_y_rad = wp->y / 180.0 * M_PI;
-		double x_rad = lat / 180.0 * M_PI;
-		double y_rad = lon / 180.0 * M_PI;
-
-		double d_lat = x_rad - current_x_rad;
-		double d_lon = y_rad - current_y_rad;
-
-		double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad);
-		double c = 2 * atan2(sqrt(a), sqrt(1 - a));
-
-		const double radius_earth = 6371000.0;
-
-		float dxy = radius_earth * c;
-		float dz = alt - wp->z;
-
-		*dist_xy = fabsf(dxy);
-		*dist_z = fabsf(dz);
-
-		return sqrtf(dxy * dxy + dz * dz);
-
-	} else {
-		return -1.0f;
-	}
-
-}
-
-/*
- * Calculate distance in local frame (NED)
- */
-float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z)
-{
-	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
-
-		float dx = (cur->x - x);
-		float dy = (cur->y - y);
-		float dz = (cur->z - z);
-
-		*dist_xy = sqrtf(dx * dx + dy * dy);
-		*dist_z = fabsf(dz);
-
-		return sqrtf(dx * dx + dy * dy + dz * dz);
-
-	} else {
-		return -1.0f;
-	}
-}
-
-void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance)
-{
-	static uint16_t counter;
-
-	if ((!global_pos->valid && !local_pos->xy_valid) ||
-		/* no waypoint */
-		wpm->size == 0) {
-		/* nothing to check here, return */
-		return;
-	}
-
-	if (wpm->current_active_wp_id < wpm->size) {
-
-		float orbit;
-		if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) {
-
-			orbit = wpm->waypoints[wpm->current_active_wp_id].param2;
-
-		} else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
-
-			orbit = wpm->waypoints[wpm->current_active_wp_id].param3;
-		} else {
-
-			// XXX set default orbit via param
-			orbit = 15.0f;
-		}
-
-		/* keep vertical orbit */
-		float vertical_switch_distance = orbit;
-
-		/* Take the larger turn distance - orbit or turn_distance */
-		if (orbit < turn_distance)
-			orbit = turn_distance;
-
-		int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
-		float dist = -1.0f;
-
-		float dist_xy = -1.0f;
-		float dist_z = -1.0f;
-
-		if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z);
-
-		} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z);
 
-		} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
-			dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z);
+// void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance)
+// {
+// 	static uint16_t counter;
 
-		} else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
-			/* Check if conditions of mission item are satisfied */
-			// XXX TODO
-		}
-
-		if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
-			wpm->pos_reached = true;
-		}
-
-		// check if required yaw reached
-		float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
-		float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
-		if (fabsf(yaw_err) < 0.05f) {
-			wpm->yaw_reached = true;
-		}
-	}
+// 	if ((!global_pos->valid && !local_pos->xy_valid) ||
+// 		/* no waypoint */
+// 		wpm->size == 0) {
+// 		/* nothing to check here, return */
+// 		return;
+// 	}
 
-	//check if the current waypoint was reached
-	if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) {
-		if (wpm->current_active_wp_id < wpm->size) {
-			mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
+// 	if (wpm->current_active_wp_id < wpm->size) {
 
-			if (wpm->timestamp_firstinside_orbit == 0) {
-				// Announce that last waypoint was reached
-				mavlink_wpm_send_waypoint_reached(cur_wp->seq);
-				wpm->timestamp_firstinside_orbit = now;
-			}
-
-			// check if the MAV was long enough inside the waypoint orbit
-			//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
+// 		float orbit;
+// 		if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) {
 
-			bool time_elapsed = false;
+// 			orbit = wpm->waypoints[wpm->current_active_wp_id].param2;
 
-			if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
-				time_elapsed = true;
-			} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
-				time_elapsed = true;
-			}
+// 		} else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
 
-			if (time_elapsed) {
+// 			orbit = wpm->waypoints[wpm->current_active_wp_id].param3;
+// 		} else {
 
-				/* safeguard against invalid missions with last wp autocontinue on */
-				if (wpm->current_active_wp_id == wpm->size - 1) {
-					/* stop handling missions here */
-					cur_wp->autocontinue = false;
-				}
+// 			// XXX set default orbit via param
+// 			orbit = 15.0f;
+// 		}
 
-				if (cur_wp->autocontinue) {
-
-					cur_wp->current = 0;
-
-					float navigation_lat = -1.0f;
-					float navigation_lon = -1.0f;
-					float navigation_alt = -1.0f;
-					int navigation_frame = -1;
-
-					/* initialize to current position in case we don't find a suitable navigation waypoint */
-					if (global_pos->valid) {
-						navigation_lat = global_pos->lat/1e7;
-						navigation_lon = global_pos->lon/1e7;
-						navigation_alt = global_pos->alt;
-						navigation_frame = MAV_FRAME_GLOBAL;
-					} else if (local_pos->xy_valid && local_pos->z_valid) {
-						navigation_lat = local_pos->x;
-						navigation_lon = local_pos->y;
-						navigation_alt = local_pos->z;
-						navigation_frame = MAV_FRAME_LOCAL_NED;
-					}
+// 		/* keep vertical orbit */
+// 		float vertical_switch_distance = orbit;
 
-					/* guard against missions without final land waypoint */
-					/* only accept supported navigation waypoints, skip unknown ones */
-					do {
-
-						/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
-						if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) {
-
-							/* this is a navigation waypoint */
-							navigation_frame = cur_wp->frame;
-							navigation_lat = cur_wp->x;
-							navigation_lon = cur_wp->y;
-							navigation_alt = cur_wp->z;
-						}
-
-						if (wpm->current_active_wp_id == wpm->size - 1) {
-
-							/* if we're not landing at the last nav waypoint, we're falling back to loiter */
-							if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) {
-								/* the last waypoint was reached, if auto continue is
-								 * activated AND it is NOT a land waypoint, keep the system loitering there.
-								 */
-								cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
-								cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
-								cur_wp->frame = navigation_frame;
-								cur_wp->x = navigation_lat;
-								cur_wp->y = navigation_lon;
-								cur_wp->z = navigation_alt;
-							}
-
-							/* we risk an endless loop for missions without navigation waypoints, abort. */
-							break;
-
-						} else {
-							if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
-								wpm->current_active_wp_id++;
-						}
+// 		/* Take the larger turn distance - orbit or turn_distance */
+// 		if (orbit < turn_distance)
+// 			orbit = turn_distance;
+
+// 		int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
+// 		float dist = -1.0f;
+
+// 		float dist_xy = -1.0f;
+// 		float dist_z = -1.0f;
+
+// 		if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
+// 			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z);
+
+// 		} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
+// 			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z);
+
+// 		} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
+// 			dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z);
+
+// 		} else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
+// 			/* Check if conditions of mission item are satisfied */
+// 			// XXX TODO
+// 		}
+
+// 		if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
+// 			wpm->pos_reached = true;
+// 		}
+
+// 		// check if required yaw reached
+// 		float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
+// 		float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
+// 		if (fabsf(yaw_err) < 0.05f) {
+// 			wpm->yaw_reached = true;
+// 		}
+// 	}
+
+// 	//check if the current waypoint was reached
+// 	if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) {
+// 		if (wpm->current_active_wp_id < wpm->size) {
+// 			mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
+
+// 			if (wpm->timestamp_firstinside_orbit == 0) {
+// 				// Announce that last waypoint was reached
+// 				mavlink_wpm_send_waypoint_reached(cur_wp->seq);
+// 				wpm->timestamp_firstinside_orbit = now;
+// 			}
+
+// 			// check if the MAV was long enough inside the waypoint orbit
+// 			//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
+
+// 			bool time_elapsed = false;
+
+// 			if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
+// 				time_elapsed = true;
+// 			} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
+// 				time_elapsed = true;
+// 			}
+
+// 			if (time_elapsed) {
+
+// 				/* safeguard against invalid missions with last wp autocontinue on */
+// 				if (wpm->current_active_wp_id == wpm->size - 1) {
+// 					/* stop handling missions here */
+// 					cur_wp->autocontinue = false;
+// 				}
+
+// 				if (cur_wp->autocontinue) {
+
+// 					cur_wp->current = 0;
+
+// 					float navigation_lat = -1.0f;
+// 					float navigation_lon = -1.0f;
+// 					float navigation_alt = -1.0f;
+// 					int navigation_frame = -1;
+
+// 					/* initialize to current position in case we don't find a suitable navigation waypoint */
+// 					if (global_pos->valid) {
+// 						navigation_lat = global_pos->lat/1e7;
+// 						navigation_lon = global_pos->lon/1e7;
+// 						navigation_alt = global_pos->alt;
+// 						navigation_frame = MAV_FRAME_GLOBAL;
+// 					} else if (local_pos->xy_valid && local_pos->z_valid) {
+// 						navigation_lat = local_pos->x;
+// 						navigation_lon = local_pos->y;
+// 						navigation_alt = local_pos->z;
+// 						navigation_frame = MAV_FRAME_LOCAL_NED;
+// 					}
+
+// 					/* guard against missions without final land waypoint */
+// 					/* only accept supported navigation waypoints, skip unknown ones */
+// 					do {
+
+// 						/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
+// 						if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) {
+
+// 							/* this is a navigation waypoint */
+// 							navigation_frame = cur_wp->frame;
+// 							navigation_lat = cur_wp->x;
+// 							navigation_lon = cur_wp->y;
+// 							navigation_alt = cur_wp->z;
+// 						}
+
+// 						if (wpm->current_active_wp_id == wpm->size - 1) {
+
+// 							/* if we're not landing at the last nav waypoint, we're falling back to loiter */
+// 							if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) {
+// 								/* the last waypoint was reached, if auto continue is
+// 								 * activated AND it is NOT a land waypoint, keep the system loitering there.
+// 								 */
+// 								cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
+// 								cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
+// 								cur_wp->frame = navigation_frame;
+// 								cur_wp->x = navigation_lat;
+// 								cur_wp->y = navigation_lon;
+// 								cur_wp->z = navigation_alt;
+// 							}
+
+// 							/* we risk an endless loop for missions without navigation waypoints, abort. */
+// 							break;
+
+// 						} else {
+// 							if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
+// 								wpm->current_active_wp_id++;
+// 						}
 			
-					} while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM));
-
-					// Fly to next waypoint
-					wpm->timestamp_firstinside_orbit = 0;
-					mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-					mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-					wpm->waypoints[wpm->current_active_wp_id].current = true;
-					wpm->pos_reached = false;
-					wpm->yaw_reached = false;
-					printf("Set new waypoint (%u)\n", wpm->current_active_wp_id);
-				}
-			}
-		}
-
-	} else {
-		wpm->timestamp_lastoutside_orbit = now;
-	}
-
-	counter++;
-}
+// 					} while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM));
+
+// 					// Fly to next waypoint
+// 					wpm->timestamp_firstinside_orbit = 0;
+// 					mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
+// 					mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
+// 					wpm->waypoints[wpm->current_active_wp_id].current = true;
+// 					wpm->pos_reached = false;
+// 					wpm->yaw_reached = false;
+// 					printf("Set new waypoint (%u)\n", wpm->current_active_wp_id);
+// 				}
+// 			}
+// 		}
+
+// 	} else {
+// 		wpm->timestamp_lastoutside_orbit = now;
+// 	}
+
+// 	counter++;
+// }
 
 
 int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap)
@@ -505,17 +541,17 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
 
 #endif
 		wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-		wpm->current_count = 0;
+		// wpm->current_count = 0;
 		wpm->current_partner_sysid = 0;
 		wpm->current_partner_compid = 0;
-		wpm->current_wp_id = -1;
+		// wpm->current_wp_id = -1;
 
-		if (wpm->size == 0) {
-			wpm->current_active_wp_id = -1;
-		}
+		// if (wpm->size == 0) {
+		// 	wpm->current_active_wp_id = -1;
+		// }
 	}
 
-	check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance);
+	// check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance);
 
 	return OK;
 }
@@ -537,7 +573,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 				if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
 					if (wpm->current_wp_id == wpm->size - 1) {
 
-						mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
+						// mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
 
 						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 						wpm->current_wp_id = 0;
@@ -561,25 +597,33 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
 					if (wpc.seq < wpm->size) {
 						// if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n");
-						wpm->current_active_wp_id = wpc.seq;
-						uint32_t i;
+						// wpm->current_active_wp_id = wpc.seq;
+						// uint32_t i;
 
-						for (i = 0; i < wpm->size; i++) {
-							if (i == wpm->current_active_wp_id) {
-								wpm->waypoints[i].current = true;
+						// for (i = 0; i < wpm->size; i++) {
+						// 	if (i == wpm->current_active_wp_id) {
+						// 		wpm->waypoints[i].current = true;
 
-							} else {
-								wpm->waypoints[i].current = false;
-							}
-						}
+						// 	} else {
+						// 		wpm->waypoints[i].current = false;
+						// 	}
+						// }
+
+						// mavlink_missionlib_send_gcs_string("NEW WP SET");
+
+						// wpm->yaw_reached = false;
+						// wpm->pos_reached = false;
+						
 
-						mavlink_missionlib_send_gcs_string("NEW WP SET");
+						mission.current_index = wpc.seq;
 
-						wpm->yaw_reached = false;
-						wpm->pos_reached = false;
-						mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-						mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-						wpm->timestamp_firstinside_orbit = 0;
+						publish_mission();
+						
+						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
+
+						//mavlink_wpm_send_waypoint_current(wpc.seq);
+						// mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
+						// wpm->timestamp_firstinside_orbit = 0;
 
 					} else {
 						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
@@ -671,7 +715,33 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 					wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
 					wpm->current_wp_id = wpr.seq;
-					mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpr.seq);
+
+					mavlink_mission_item_t wp;
+
+					struct mission_item_s mission_item;
+					ssize_t len = sizeof(struct mission_item_s);
+					
+					dm_item_t dm_current;
+
+					if (wpm->current_dataman_id == 0) {
+						dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+					} else {
+						dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+					}
+
+					if (dm_read(dm_current, wpr.seq, &mission_item, len) == len) {
+					
+						if (mission.current_index == wpr.seq) {
+							wp.current = true;
+						} else {
+							wp.current = false;
+						}
+	
+						map_mission_item_to_mavlink_mission_item(&mission_item, &wp);
+						mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp);
+					} else {
+						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
+					}
 
 				} else {
 					// if (verbose)
@@ -775,7 +845,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 							if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid);
 
-#endif
+#endif							
 						}
 
 						if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
@@ -794,6 +864,10 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 						wpm->current_partner_compid = msg->compid;
 						wpm->current_count = wpc.count;
 
+						if (wpc.count > NUM_MISSIONS_SUPPORTED) {
+							warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
+						}
+
 #ifdef MAVLINK_WPM_NO_PRINTF
 						mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY");
 #else
@@ -824,9 +898,9 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 //                            delete waypoints->back();
 //                            waypoints->pop_back();
 //                        }
-						wpm->current_active_wp_id = -1;
-						wpm->yaw_reached = false;
-						wpm->pos_reached = false;
+						// wpm->current_active_wp_id = -1;
+						// wpm->yaw_reached = false;
+						// wpm->pos_reached = false;
 						break;
 
 					} else {
@@ -886,7 +960,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 			mavlink_mission_item_t wp;
 			mavlink_msg_mission_item_decode(msg, &wp);
 
-			mavlink_missionlib_send_gcs_string("GOT WP");
+			// mavlink_missionlib_send_gcs_string("GOT WP");
 //			printf("sysid=%d, current_partner_sysid=%d\n", msg->sysid, wpm->current_partner_sysid);
 //			printf("compid=%d, current_partner_compid=%d\n", msg->compid, wpm->current_partner_compid);
 
@@ -900,7 +974,9 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 //                wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO
 
 				//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
-				if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && wp.seq < wpm->current_count)) {
+				if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) ||
+				    (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id &&
+				    wp.seq < wpm->current_count)) {
 					//mavlink_missionlib_send_gcs_string("DEBUG 2");
 
 //                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid);
@@ -908,9 +984,41 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 //                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid);
 //
 					wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
-					mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]);
-					memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
-//                    printf("WP seq: %d\n",wp.seq);
+					// mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]);
+					// memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
+
+					struct mission_item_s mission_item;
+
+					int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item);
+
+					if (ret != OK) {
+						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret);
+						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
+						break;
+					}
+
+					size_t len = sizeof(struct mission_item_s);
+
+					dm_item_t dm_next;
+
+					if (wpm->current_dataman_id == 0) {
+						dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1;
+						mission.dataman_id = 1;
+					} else {
+						dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0;
+						mission.dataman_id = 0;
+					}
+
+					if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
+						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
+						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
+						break;
+					}
+
+					if (wp.current) {
+						mission.current_index = wp.seq;
+					}
+
 					wpm->current_wp_id = wp.seq + 1;
 
 					// if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
@@ -918,46 +1026,59 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 //					printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count);
 					if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-						mavlink_missionlib_send_gcs_string("GOT ALL WPS");
+						// mavlink_missionlib_send_gcs_string("GOT ALL WPS");
 						// if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_count);
 
 						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0);
 
-						if (wpm->current_active_wp_id > wpm->rcv_size - 1) {
-							wpm->current_active_wp_id = wpm->rcv_size - 1;
-						}
+						// if (wpm->current_active_wp_id > wpm->rcv_size - 1) {
+						// 	wpm->current_active_wp_id = wpm->rcv_size - 1;
+						// }
 
-						// switch the waypoints list
-						// FIXME CHECK!!!
-						uint32_t i;
+						// bool copy_error = false;
+
+						// // switch the waypoints list
+						// // FIXME CHECK!!!
+						// uint32_t i;
+
+						// for (i = 0; i < wpm->current_count; ++i) {
+						// 	wpm->waypoints[i] = wpm->rcv_waypoints[i];
+						// 	if (map_mavlink_mission_item_to_mission_item(&wpm->rcv_waypoints[i], &mission.items[i]) != OK) {
+						// 		copy_error = true;
+						// 	}
+
+						// }
+						// TODO: update count?
 
-						for (i = 0; i < wpm->current_count; ++i) {
-							wpm->waypoints[i] = wpm->rcv_waypoints[i];
-						}
 
+						mission.count = wpm->current_count;
+						
+						publish_mission();
+
+						wpm->current_dataman_id = mission.dataman_id;
 						wpm->size = wpm->current_count;
 
 						//get the new current waypoint
 
-						for (i = 0; i < wpm->size; i++) {
-							if (wpm->waypoints[i].current == 1) {
-								wpm->current_active_wp_id = i;
-								//// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id);
-								wpm->yaw_reached = false;
-								wpm->pos_reached = false;
-								mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-								mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-								wpm->timestamp_firstinside_orbit = 0;
-								break;
-							}
-						}
-
-						if (i == wpm->size) {
-							wpm->current_active_wp_id = -1;
-							wpm->yaw_reached = false;
-							wpm->pos_reached = false;
-							wpm->timestamp_firstinside_orbit = 0;
-						}
+						// for (i = 0; i < wpm->size; i++) {
+						// 	if (wpm->waypoints[i].current == 1) {
+						// 		wpm->current_active_wp_id = i;
+						// 		//// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id);
+						// 		// wpm->yaw_reached = false;
+						// 		// wpm->pos_reached = false;
+						// 		mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
+						// 		// mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
+						// 		// wpm->timestamp_firstinside_orbit = 0;
+						// 		break;
+						// 	}
+						// }
+
+						// if (i == wpm->size) {
+						// 	wpm->current_active_wp_id = -1;
+						// 	wpm->yaw_reached = false;
+						// 	wpm->pos_reached = false;
+						// 	wpm->timestamp_firstinside_orbit = 0;
+						// }
 
 						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 
@@ -966,38 +1087,41 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 					}
 
 				} else {
-					if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-						//we're done receiving waypoints, answer with ack.
-						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0);
-						printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
-					}
-
-					// if (verbose)
-					{
-						if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) {
-//							 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state);
-							break;
 
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
-							if (!(wp.seq == 0)) {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq);
-							} else {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
-							}
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-							if (!(wp.seq == wpm->current_wp_id)) {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id);
-								mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
-
-							} else if (!(wp.seq < wpm->current_count)) {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq);
-							} else {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
-							}
-						} else {
-//							 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
-						}
-					}
+					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_INVALID_SEQUENCE);
+
+					// if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
+					// 	//we're done receiving waypoints, answer with ack.
+					// 	mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0);
+					// 	printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
+					// }
+
+// 					// if (verbose)
+// 					{
+// 						if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) {
+// //							 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state);
+// 							break;
+
+// 						} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
+// 							if (!(wp.seq == 0)) {
+// //								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq);
+// 							} else {
+// //								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
+// 							}
+// 						} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
+// 							if (!(wp.seq == wpm->current_wp_id)) {
+// //								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id);
+// 								mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
+
+// 							} else if (!(wp.seq < wpm->current_count)) {
+// //								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq);
+// 							} else {
+// //								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
+// 							}
+// 						} else {
+// //							 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
+// 						}
+// 					}
 				}
 			} else {
 				//we we're target but already communicating with someone else
@@ -1021,12 +1145,27 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 				// if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
 				// Delete all waypoints
 				wpm->size = 0;
-				wpm->current_active_wp_id = -1;
-				wpm->yaw_reached = false;
-				wpm->pos_reached = false;
+				// wpm->current_active_wp_id = -1;
+				// wpm->yaw_reached = false;
+				// wpm->pos_reached = false;
+
+				/* prepare mission topic */
+				mission.dataman_id = -1;
+				mission.count = 0;
+				mission.current_index = -1;
+
+				if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) {
+					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
+				} else {
+					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
+				}
+
+				publish_mission();
+
 
 			} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
 				// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state);
+				warnx("not cleared");
 			}
 
 			break;
diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
index d7d6b31dc..801bc0bcf 100644
--- a/src/modules/mavlink/waypoints.h
+++ b/src/modules/mavlink/waypoints.h
@@ -56,6 +56,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/mission.h>
 
 // FIXME XXX - TO BE MOVED TO XML
 enum MAVLINK_WPM_STATES {
@@ -81,7 +82,7 @@ enum MAVLINK_WPM_CODES {
 /* WAYPOINT MANAGER - MISSION LIB */
 
 #define MAVLINK_WPM_MAX_WP_COUNT 15
-#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE				  ///< Enable double buffer and in-flight updates
+// #define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE				  ///< Enable double buffer and in-flight updates
 #ifndef MAVLINK_WPM_TEXT_FEEDBACK
 #define MAVLINK_WPM_TEXT_FEEDBACK 0						  ///< Report back status information as text
 #endif
@@ -92,33 +93,38 @@ enum MAVLINK_WPM_CODES {
 
 struct mavlink_wpm_storage {
 	mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT];      ///< Currently active waypoints
-#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
-	mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT];  ///< Receive buffer for next waypoints
-#endif
+// #ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
+	// mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT];  ///< Receive buffer for next waypoints
+// #endif
 	uint16_t size;
 	uint16_t max_size;
 	uint16_t rcv_size;
 	enum MAVLINK_WPM_STATES current_state;
 	int16_t current_wp_id;							///< Waypoint in current transmission
-	int16_t current_active_wp_id;					///< Waypoint the system is currently heading towards
+	// int16_t current_active_wp_id;					///< Waypoint the system is currently heading towards
 	uint16_t current_count;
 	uint8_t current_partner_sysid;
 	uint8_t current_partner_compid;
 	uint64_t timestamp_lastaction;
-	uint64_t timestamp_last_send_setpoint;
-	uint64_t timestamp_firstinside_orbit;
-	uint64_t timestamp_lastoutside_orbit;
+	// uint64_t timestamp_last_send_setpoint;
+	// uint64_t timestamp_firstinside_orbit;
+	// uint64_t timestamp_lastoutside_orbit;
 	uint32_t timeout;
-	uint32_t delay_setpoint;
-	float accept_range_yaw;
-	float accept_range_distance;
-	bool yaw_reached;
-	bool pos_reached;
-	bool idle;
+	int current_dataman_id;
+	// uint32_t delay_setpoint;
+	// float accept_range_yaw;
+	// float accept_range_distance;
+	// bool yaw_reached;
+	// bool pos_reached;
+	// bool idle;
 };
 
 typedef struct mavlink_wpm_storage mavlink_wpm_storage;
 
+int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
+int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
+
+
 void mavlink_wpm_init(mavlink_wpm_storage *state);
 int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position,
 			       struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap);
diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h
index 1b49c9ce4..86bfa26f2 100644
--- a/src/modules/mavlink_onboard/orb_topics.h
+++ b/src/modules/mavlink_onboard/orb_topics.h
@@ -50,7 +50,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/optical_flow.h>
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
new file mode 100644
index 000000000..5af7e2a82
--- /dev/null
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -0,0 +1,685 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file multirotor_pos_control.c
+ *
+ * Multirotor position controller
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <termios.h>
+#include <time.h>
+#include <sys/prctl.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
+#include <uORB/topics/mission_item_triplet.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/pid/pid.h>
+#include <mavlink/mavlink_log.h>
+
+#include "multirotor_pos_control_params.h"
+#include "thrust_pid.h"
+
+
+static bool thread_should_exit = false;		/**< Deamon exit flag */
+static bool thread_running = false;		/**< Deamon status flag */
+static int deamon_task;				/**< Handle of deamon task / thread */
+
+__EXPORT int multirotor_pos_control_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of position controller.
+ */
+static int multirotor_pos_control_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static float scale_control(float ctl, float end, float dz);
+
+static float norm(float x, float y);
+
+static void usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n");
+	exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_spawn().
+ */
+int multirotor_pos_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		warnx("start");
+		thread_should_exit = false;
+		deamon_task = task_spawn_cmd("multirotor_pos_control",
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_MAX - 60,
+					     4096,
+					     multirotor_pos_control_thread_main,
+					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		warnx("stop");
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("app is running");
+
+		} else {
+			warnx("app not started");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+static float scale_control(float ctl, float end, float dz)
+{
+	if (ctl > dz) {
+		return (ctl - dz) / (end - dz);
+
+	} else if (ctl < -dz) {
+		return (ctl + dz) / (end - dz);
+
+	} else {
+		return 0.0f;
+	}
+}
+
+static float norm(float x, float y)
+{
+	return sqrtf(x * x + y * y);
+}
+
+static int multirotor_pos_control_thread_main(int argc, char *argv[])
+{
+	/* welcome user */
+	warnx("started");
+	static int mavlink_fd;
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd, "[mpc] started");
+
+	/* structures */
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+	struct vehicle_attitude_setpoint_s att_sp;
+	memset(&att_sp, 0, sizeof(att_sp));
+	struct manual_control_setpoint_s manual;
+	memset(&manual, 0, sizeof(manual));
+	struct vehicle_local_position_s local_pos;
+	memset(&local_pos, 0, sizeof(local_pos));
+	struct mission_item_triplet_s triplet;
+	memset(&triplet, 0, sizeof(triplet));
+	struct vehicle_global_velocity_setpoint_s global_vel_sp;
+	memset(&global_vel_sp, 0, sizeof(global_vel_sp));
+	struct vehicle_local_position_setpoint_s local_pos_sp;
+	memset(&local_pos_sp, 0, sizeof(local_pos_sp));
+
+	/* subscribe to attitude, motor setpoints and system state */
+	int param_sub = orb_subscribe(ORB_ID(parameter_update));
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	int mission_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
+
+	/* publish setpoint */
+	orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
+	orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
+	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+
+	bool reset_mission_sp = false;
+	bool global_pos_sp_valid = false;
+	bool reset_man_sp_z = true;
+	bool reset_man_sp_xy = true;
+	bool reset_int_z = true;
+	bool reset_int_z_manual = false;
+	bool reset_int_xy = true;
+	bool was_armed = false;
+	bool reset_auto_sp_xy = true;
+	bool reset_auto_sp_z = true;
+	bool reset_takeoff_sp = true;
+
+	hrt_abstime t_prev = 0;
+	const float alt_ctl_dz = 0.2f;
+	const float pos_ctl_dz = 0.05f;
+
+	float ref_alt = 0.0f;
+	hrt_abstime ref_alt_t = 0;
+	uint64_t local_ref_timestamp = 0;
+
+	PID_t xy_pos_pids[2];
+	PID_t xy_vel_pids[2];
+	PID_t z_pos_pid;
+	thrust_pid_t z_vel_pid;
+
+	thread_running = true;
+
+	struct multirotor_position_control_params params;
+	struct multirotor_position_control_param_handles params_h;
+	parameters_init(&params_h);
+	parameters_update(&params_h, &params);
+
+
+	for (int i = 0; i < 2; i++) {
+		pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
+		pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
+	}
+
+	pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
+	thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
+
+	while (!thread_should_exit) {
+
+		bool param_updated;
+		orb_check(param_sub, &param_updated);
+
+		if (param_updated) {
+			/* clear updated flag */
+			struct parameter_update_s ps;
+			orb_copy(ORB_ID(parameter_update), param_sub, &ps);
+			/* update params */
+			parameters_update(&params_h, &params);
+
+			for (int i = 0; i < 2; i++) {
+				pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
+				/* use integral_limit_out = tilt_max / 2 */
+				float i_limit;
+
+				if (params.xy_vel_i > 0.0f) {
+					i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
+
+				} else {
+					i_limit = 0.0f;	// not used
+				}
+
+				pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
+			}
+
+			pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
+			thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
+		}
+
+		bool updated;
+
+		orb_check(control_mode_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+		}
+
+		orb_check(mission_triplet_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(mission_item_triplet), mission_triplet_sub, &triplet);
+			global_pos_sp_valid = triplet.current_valid;
+			reset_mission_sp = true;
+		}
+
+		hrt_abstime t = hrt_absolute_time();
+		float dt;
+
+		if (t_prev != 0) {
+			dt = (t - t_prev) * 0.000001f;
+
+		} else {
+			dt = 0.0f;
+		}
+
+		if (control_mode.flag_armed && !was_armed) {
+			/* reset setpoints and integrals on arming */
+			reset_man_sp_z = true;
+			reset_man_sp_xy = true;
+			reset_auto_sp_z = true;
+			reset_auto_sp_xy = true;
+			reset_takeoff_sp = true;
+			reset_int_z = true;
+			reset_int_xy = true;
+		}
+
+		was_armed = control_mode.flag_armed;
+
+		t_prev = t;
+
+		if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) {
+			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
+			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
+
+			float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
+			float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
+			float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
+
+			if (control_mode.flag_control_manual_enabled) {
+				/* manual control */
+				/* check for reference point updates and correct setpoint */
+				if (local_pos.ref_timestamp != ref_alt_t) {
+					if (ref_alt_t != 0) {
+						/* home alt changed, don't follow large ground level changes in manual flight */
+						local_pos_sp.z += local_pos.ref_alt - ref_alt;
+					}
+
+					ref_alt_t = local_pos.ref_timestamp;
+					ref_alt = local_pos.ref_alt;
+					// TODO also correct XY setpoint
+				}
+
+				/* reset setpoints to current position if needed */
+				if (control_mode.flag_control_altitude_enabled) {
+					if (reset_man_sp_z) {
+						reset_man_sp_z = false;
+						local_pos_sp.z = local_pos.z;
+						mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
+					}
+
+					/* move altitude setpoint with throttle stick */
+					float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
+
+					if (z_sp_ctl != 0.0f) {
+						sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
+						local_pos_sp.z += sp_move_rate[2] * dt;
+
+						if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
+							local_pos_sp.z = local_pos.z + z_sp_offs_max;
+
+						} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
+							local_pos_sp.z = local_pos.z - z_sp_offs_max;
+						}
+					}
+				}
+
+				if (control_mode.flag_control_position_enabled) {
+					if (reset_man_sp_xy) {
+						reset_man_sp_xy = false;
+						local_pos_sp.x = local_pos.x;
+						local_pos_sp.y = local_pos.y;
+						pid_reset_integral(&xy_vel_pids[0]);
+						pid_reset_integral(&xy_vel_pids[1]);
+						mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
+					}
+
+					/* move position setpoint with roll/pitch stick */
+					float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
+					float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
+
+					if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
+						/* calculate direction and increment of control in NED frame */
+						float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
+						float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max;
+						sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
+						sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
+						local_pos_sp.x += sp_move_rate[0] * dt;
+						local_pos_sp.y += sp_move_rate[1] * dt;
+						/* limit maximum setpoint from position offset and preserve direction
+						 * fail safe, should not happen in normal operation */
+						float pos_vec_x = local_pos_sp.x - local_pos.x;
+						float pos_vec_y = local_pos_sp.y - local_pos.y;
+						float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
+
+						if (pos_vec_norm > 1.0f) {
+							local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
+							local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
+						}
+					}
+				}
+
+				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
+				local_pos_sp.yaw = att_sp.yaw_body;
+
+				/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
+				reset_auto_sp_xy = !control_mode.flag_control_position_enabled;
+				reset_auto_sp_z = !control_mode.flag_control_altitude_enabled;
+				reset_takeoff_sp = true;
+
+				/* force reprojection of global setpoint after manual mode */
+				reset_mission_sp = true;
+
+			} else if (control_mode.flag_control_auto_enabled) {
+				/* AUTO mode, use global setpoint */
+				if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
+					reset_auto_sp_xy = true;
+					reset_auto_sp_z = true;
+
+				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+					if (reset_takeoff_sp) {
+						reset_takeoff_sp = false;
+						local_pos_sp.x = local_pos.x;
+						local_pos_sp.y = local_pos.y;
+						local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
+						att_sp.yaw_body = att.yaw;
+						mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
+					}
+
+					reset_auto_sp_xy = false;
+					reset_auto_sp_z = true;
+
+				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
+					// TODO
+					reset_auto_sp_xy = true;
+					reset_auto_sp_z = true;
+
+				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
+					/* init local projection using local position ref */
+					if (local_pos.ref_timestamp != local_ref_timestamp) {
+						reset_mission_sp = true;
+						local_ref_timestamp = local_pos.ref_timestamp;
+						double lat_home = local_pos.ref_lat * 1e-7;
+						double lon_home = local_pos.ref_lon * 1e-7;
+						map_projection_init(lat_home, lon_home);
+						mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
+					}
+
+					if (reset_mission_sp) {
+						reset_mission_sp = false;
+						/* update global setpoint projection */
+
+						if (global_pos_sp_valid) {
+
+							/* project global setpoint to local setpoint */
+							map_projection_project(triplet.current.lat, triplet.current.lon, &(local_pos_sp.x), &(local_pos_sp.y));
+
+							if (triplet.current.altitude_is_relative) {
+								local_pos_sp.z = -triplet.current.altitude;
+
+							} else {
+								local_pos_sp.z = local_pos.ref_alt - triplet.current.lat;
+							}
+							/* update yaw setpoint only if value is valid */
+							if (isfinite(triplet.current.yaw) && fabsf(triplet.current.yaw) < M_TWOPI) {
+								att_sp.yaw_body = triplet.current.yaw;
+							}
+
+							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", triplet.current.lat, triplet.current.lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
+
+						} else {
+							if (reset_auto_sp_xy) {
+								reset_auto_sp_xy = false;
+								/* local position setpoint is invalid,
+								 * use current position as setpoint for loiter */
+								local_pos_sp.x = local_pos.x;
+								local_pos_sp.y = local_pos.y;
+								local_pos_sp.yaw = att.yaw;
+							}
+
+							if (reset_auto_sp_z) {
+								reset_auto_sp_z = false;
+								local_pos_sp.z = local_pos.z;
+							}
+
+							mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
+						}
+					}
+
+					reset_auto_sp_xy = true;
+					reset_auto_sp_z = true;
+				}
+
+				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) {
+					reset_takeoff_sp = true;
+				}
+
+				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
+					reset_mission_sp = true;
+				}
+
+				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
+				local_pos_sp.yaw = att_sp.yaw_body;
+
+				/* reset setpoints after AUTO mode */
+				reset_man_sp_xy = true;
+				reset_man_sp_z = true;
+
+			} else {
+				/* no control (failsafe), loiter or stay on ground */
+				if (local_pos.landed) {
+					/* landed: move setpoint down */
+					/* in air: hold altitude */
+					if (local_pos_sp.z < 5.0f) {
+						/* set altitude setpoint to 5m under ground,
+						 * don't set it too deep to avoid unexpected landing in case of false "landed" signal */
+						local_pos_sp.z = 5.0f;
+						mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
+					}
+
+					reset_man_sp_z = true;
+
+				} else {
+					/* in air: hold altitude */
+					if (reset_man_sp_z) {
+						reset_man_sp_z = false;
+						local_pos_sp.z = local_pos.z;
+						mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
+					}
+
+					reset_auto_sp_z = false;
+				}
+
+				if (control_mode.flag_control_position_enabled) {
+					if (reset_man_sp_xy) {
+						reset_man_sp_xy = false;
+						local_pos_sp.x = local_pos.x;
+						local_pos_sp.y = local_pos.y;
+						local_pos_sp.yaw = att.yaw;
+						att_sp.yaw_body = att.yaw;
+						mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
+					}
+
+					reset_auto_sp_xy = false;
+				}
+			}
+
+			/* publish local position setpoint */
+			orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+
+			/* run position & altitude controllers, calculate velocity setpoint */
+			if (control_mode.flag_control_altitude_enabled) {
+				global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
+
+			} else {
+				reset_man_sp_z = true;
+				global_vel_sp.vz = 0.0f;
+			}
+
+			if (control_mode.flag_control_position_enabled) {
+				/* calculate velocity set point in NED frame */
+				global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
+				global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];
+
+				/* limit horizontal speed */
+				float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
+
+				if (xy_vel_sp_norm > 1.0f) {
+					global_vel_sp.vx /= xy_vel_sp_norm;
+					global_vel_sp.vy /= xy_vel_sp_norm;
+				}
+
+			} else {
+				reset_man_sp_xy = true;
+				global_vel_sp.vx = 0.0f;
+				global_vel_sp.vy = 0.0f;
+			}
+
+			/* publish new velocity setpoint */
+			orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
+			// TODO subscribe to velocity setpoint if altitude/position control disabled
+
+			if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
+				/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
+				float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
+
+				if (control_mode.flag_control_climb_rate_enabled) {
+					if (reset_int_z) {
+						reset_int_z = false;
+						float i = params.thr_min;
+
+						if (reset_int_z_manual) {
+							i = manual.throttle;
+
+							if (i < params.thr_min) {
+								i = params.thr_min;
+
+							} else if (i > params.thr_max) {
+								i = params.thr_max;
+							}
+						}
+
+						thrust_pid_set_integral(&z_vel_pid, -i);
+						mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
+					}
+
+					thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
+					att_sp.thrust = -thrust_sp[2];
+
+				} else {
+					/* reset thrust integral when altitude control enabled */
+					reset_int_z = true;
+				}
+
+				if (control_mode.flag_control_velocity_enabled) {
+					/* calculate thrust set point in NED frame */
+					if (reset_int_xy) {
+						reset_int_xy = false;
+						pid_reset_integral(&xy_vel_pids[0]);
+						pid_reset_integral(&xy_vel_pids[1]);
+						mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
+					}
+
+					thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
+					thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
+
+					/* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
+					/* limit horizontal part of thrust */
+					float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
+					/* assuming that vertical component of thrust is g,
+					 * horizontal component = g * tan(alpha) */
+					float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
+
+					if (tilt > params.tilt_max) {
+						tilt = params.tilt_max;
+					}
+
+					/* convert direction to body frame */
+					thrust_xy_dir -= att.yaw;
+					/* calculate roll and pitch */
+					att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
+					att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
+
+				} else {
+					reset_int_xy = true;
+				}
+
+				att_sp.timestamp = hrt_absolute_time();
+
+				/* publish new attitude setpoint */
+				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+			}
+
+		} else {
+			/* position controller disabled, reset setpoints */
+			reset_man_sp_z = true;
+			reset_man_sp_xy = true;
+			reset_int_z = true;
+			reset_int_xy = true;
+			reset_mission_sp = true;
+			reset_auto_sp_xy = true;
+			reset_auto_sp_z = true;
+		}
+
+		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
+		reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;
+
+		/* run at approximately 50 Hz */
+		usleep(20000);
+	}
+
+	warnx("stopped");
+	mavlink_log_info(mavlink_fd, "[mpc] stopped");
+
+	thread_running = false;
+
+	fflush(stdout);
+	return 0;
+}
+
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
new file mode 100644
index 000000000..25b2636bb
--- /dev/null
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -0,0 +1,184 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file mission_feasibility_checker.cpp
+ * Provides checks if mission is feasible given the navigation capabilities
+ */
+
+#include "mission_feasibility_checker.h"
+
+#include <geo/geo.h>
+#include <math.h>
+#include <mathlib/mathlib.h>
+#include <mavlink/mavlink_log.h>
+#include <fw_pos_control_l1/landingslope.h>
+#include <systemlib/err.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <errno.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false)
+{
+	_nav_caps = {0};
+}
+
+
+bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems)
+{
+	/* Init if not done yet */
+	init();
+
+	/* Open mavlink fd */
+	if (_mavlink_fd < 0) {
+		/* try to open the mavlink log device every once in a while */
+		_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	}
+
+
+	if (isRotarywing)
+		return checkMissionFeasibleRotarywing(dm_current, nItems);
+	else
+		return checkMissionFeasibleFixedwing(dm_current, nItems);
+}
+
+bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems)
+{
+
+	return checkGeofence(dm_current, nItems);
+}
+
+bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems)
+{
+	/* Update fixed wing navigation capabilites */
+	updateNavigationCapabilities();
+//	warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
+
+	return (checkFixedWingLanding(dm_current, nItems) && checkGeofence(dm_current, nItems));
+}
+
+bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nItems)
+{
+	//xxx: check geofence
+	return true;
+}
+
+bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nItems)
+{
+	/* Go through all mission items and search for a landing waypoint
+	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
+
+
+	for (size_t i = 0; i < nItems; i++) {
+		static struct mission_item_s missionitem;
+		const ssize_t len = sizeof(struct mission_item_s);
+		if (dm_read(dm_current, i, &missionitem, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return false;
+		}
+
+		if (missionitem.nav_cmd == NAV_CMD_LAND) {
+			struct mission_item_s missionitem_previous;
+			if (i != 0) {
+				if (dm_read(dm_current, i-1, &missionitem_previous, len) != len) {
+					/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+					return false;
+				}
+
+				float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon);
+				float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
+				float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
+				float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
+//				warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f",
+//						wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req);
+//				warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f",
+//						_nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length);
+
+				if (wp_distance > _nav_caps.landing_flare_length) {
+					/* Last wp is before flare region */
+
+					if (delta_altitude < 0) {
+						if (missionitem_previous.altitude <= slope_alt_req) {
+							/* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */
+							return true;
+						} else {
+							/* Landing waypoint is above altitude of slope at the given waypoint distance */
+							mavlink_log_info(_mavlink_fd, "#audio: Landing: last waypoint too high/too close");
+							mavlink_log_info(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm",
+									(double)(slope_alt_req),
+									(double)(wp_distance_req - wp_distance));
+							return false;
+						}
+					} else {
+						/* Landing waypoint is above last waypoint */
+						mavlink_log_info(_mavlink_fd, "#audio: Landing waypoint above last nav waypoint");
+						return false;
+					}
+				} else {
+					/* Last wp is in flare region */
+					//xxx give recommendations
+					mavlink_log_info(_mavlink_fd, "#audio: Warning: Landing: last waypoint in flare region");
+					return false;
+				}
+			} else {
+				mavlink_log_info(_mavlink_fd, "#audio: Warning: starting with land waypoint");
+				return false;
+			}
+		}
+	}
+
+
+//	float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
+}
+
+void MissionFeasibilityChecker::updateNavigationCapabilities()
+{
+	int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
+}
+
+void MissionFeasibilityChecker::init()
+{
+	if (!_initDone) {
+
+		_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
+
+		_initDone = true;
+	}
+}
diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h
new file mode 100644
index 000000000..7d1cc2f8a
--- /dev/null
+++ b/src/modules/navigator/mission_feasibility_checker.h
@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file mission_feasibility_checker.h
+ * Provides checks if mission is feasible given the navigation capabilities
+ */
+#ifndef MISSION_FEASIBILITY_CHECKER_H_
+#define MISSION_FEASIBILITY_CHECKER_H_
+
+#include <unistd.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/navigation_capabilities.h>
+#include <dataman/dataman.h>
+
+
+class MissionFeasibilityChecker
+{
+private:
+	int		_mavlink_fd;
+
+	int _capabilities_sub;
+	struct navigation_capabilities_s _nav_caps;
+
+	bool _initDone;
+	void init();
+
+	/* Checks for all airframes */
+	bool checkGeofence(dm_item_t dm_current, size_t nItems);
+
+	/* Checks specific to fixedwing airframes */
+	bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems);
+	bool checkFixedWingLanding(dm_item_t dm_current, size_t nItems);
+	void updateNavigationCapabilities();
+
+	/* Checks specific to rotarywing airframes */
+	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems);
+public:
+
+	MissionFeasibilityChecker();
+	~MissionFeasibilityChecker() {}
+
+	/*
+	 * Returns true if mission is feasible and false otherwise
+	 */
+	bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems);
+
+};
+
+
+#endif /* MISSION_FEASIBILITY_CHECKER_H_ */
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 0404b06c7..6be4e87a0 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -38,4 +38,8 @@
 MODULE_COMMAND	= navigator
 
 SRCS		= navigator_main.cpp \
-		  navigator_params.c
+		  navigator_params.c \
+		  navigator_mission.cpp \
+		  mission_feasibility_checker.cpp
+
+INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 04307d96b..7be9229c9 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1,7 +1,9 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: 	Lorenz Meier
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Jean Cyr <jean.m.cyr@gmail.com>
+ *           @author Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,6 +38,7 @@
  * Implementation of the main navigation state machine.
  *
  * Handles missions, geo fencing and failsafe navigation behavior.
+ * Published the mission item triplet for the position controller.
  */
 
 #include <nuttx/config.h>
@@ -48,26 +51,39 @@
 #include <math.h>
 #include <poll.h>
 #include <time.h>
+#include <sys/ioctl.h>
+#include <drivers/device/device.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/airspeed.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/mission_item_triplet.h>
+#include <uORB/topics/mission_result.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission.h>
+#include <uORB/topics/fence.h>
+#include <uORB/topics/navigation_capabilities.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
-#include <geo/geo.h>
+#include <systemlib/state_table.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
+#include <geo/geo.h>
 #include <mathlib/mathlib.h>
+#include <dataman/dataman.h>
+#include <mavlink/mavlink_log.h>
+
+#include "navigator_mission.h"
+#include "mission_feasibility_checker.h"
+
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
 
 /**
  * navigator app start / stop handling function
@@ -76,7 +92,7 @@
  */
 extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
 
-class Navigator
+class Navigator : public StateTable
 {
 public:
 	/**
@@ -85,103 +101,148 @@ public:
 	Navigator();
 
 	/**
-	 * Destructor, also kills the sensors task.
+	 * Destructor, also kills the navigators task.
 	 */
 	~Navigator();
 
 	/**
-	 * Start the sensors task.
+	* Start the navigator task.
 	 *
 	 * @return		OK on success.
 	 */
 	int		start();
 
+	/**
+	 * Display the navigator status.
+	 */
+	void		status();
+
+	/**
+	 * Load fence parameters.
+	 */
+	bool		load_fence(unsigned vertices);
+
+	/**
+	 * Specify fence vertex position.
+	 */
+	void		fence_point(int argc, char *argv[]);
+
 private:
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
-	int		_navigator_task;			/**< task handle for sensor task */
+	int		_navigator_task;		/**< task handle for sensor task */
+
+	int		_mavlink_fd;
 
-	int		_global_pos_sub;
-	int		_att_sub;			/**< vehicle attitude subscription */
-	int		_attitude_sub;			/**< raw rc channels data subscription */
-	int		_airspeed_sub;			/**< airspeed subscription */
+	int		_global_pos_sub;		/**< global position subscription */
+	int		_home_pos_sub;			/**< home position subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
-	int 		_params_sub;			/**< notification of parameter updates */
-	int 		_manual_control_sub;		/**< notification of manual control updates */
-	int		_mission_sub;
+	int		_params_sub;			/**< notification of parameter updates */
+	int		_offboard_mission_sub;		/**< notification of offboard mission updates */
+	int 		_onboard_mission_sub;		/**< notification of onboard mission updates */
+	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
 
-	orb_advert_t	_triplet_pub;			/**< position setpoint */
+	orb_advert_t	_triplet_pub;			/**< publish position setpoint triplet */
+	orb_advert_t	_fence_pub;			/**< publish fence topic */ 
+	orb_advert_t	_mission_result_pub;		/**< publish mission result topic */
 
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
-	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct airspeed_s				_airspeed;		/**< airspeed */
 	struct vehicle_status_s				_vstatus;		/**< vehicle status */
 	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
-	struct vehicle_global_position_set_triplet_s	_global_triplet;	/**< triplet of global setpoints */
+	struct home_position_s				_home_pos;		/**< home position for RTL */
+	struct mission_item_triplet_s			_mission_item_triplet;	/**< triplet of mission items */
+	struct mission_result_s				_mission_result;	/**< mission result for commander/mavlink */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	
+	struct fence_s 					_fence;			/**< storage for fence vertices */
+	bool						_fence_valid;		/**< flag if fence is valid */
+        bool						_inside_fence;		/**< vehicle is inside fence */
 
-	unsigned	_mission_items_maxcount;				/**< maximum number of mission items supported */
-	struct	mission_item_s 				* _mission_items;	/**< storage for mission items */
-	bool		_mission_valid;						/**< flag if mission is valid */
+	struct navigation_capabilities_s		_nav_caps;
 
-	/** manual control states */
-	float		_seatbelt_hold_heading;		/**< heading the system should hold in seatbelt mode */
-	float		_loiter_hold_lat;
-	float		_loiter_hold_lon;
-	float		_loiter_hold_alt;
-	bool		_loiter_hold;
+	class 		Mission				_mission;
 
-	struct {
-		float throttle_cruise;
-	}		_parameters;			/**< local copies of interesting parameters */
+	bool		_waypoint_position_reached;
+	bool		_waypoint_yaw_reached;
+	uint64_t	_time_first_inside_orbit;
+
+	MissionFeasibilityChecker missionFeasiblityChecker;
 
 	struct {
-		param_t throttle_cruise;
+		float min_altitude;
+		float loiter_radius;
+		int onboard_mission_enabled;
+	}		_parameters;			/**< local copies of parameters */
 
-	}		_parameter_handles;		/**< handles for interesting parameters */
+	struct {
+		param_t min_altitude;
+		param_t loiter_radius;
+		param_t onboard_mission_enabled;
+
+	}		_parameter_handles;		/**< handles for parameters */
+
+	enum Event {
+		EVENT_NONE_REQUESTED,
+		EVENT_LOITER_REQUESTED,
+		EVENT_MISSION_REQUESTED,
+		EVENT_RTL_REQUESTED,
+		EVENT_MISSION_FINISHED,
+		EVENT_MISSION_CHANGED,
+		EVENT_HOME_POSITION_CHANGED,
+		MAX_EVENT
+	};
+
+	enum State {
+		STATE_INIT,
+		STATE_NONE,
+		STATE_LOITER,
+		STATE_MISSION,
+		STATE_MISSION_LOITER,
+		STATE_RTL,
+		STATE_RTL_LOITER,
+		MAX_STATE
+	};
 
+	/**
+	 * State machine transition table
+	 */
+	static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT];
 
 	/**
 	 * Update our local parameter cache.
 	 */
-	int		parameters_update();
+	void		parameters_update();
 
 	/**
-	 * Update control outputs
-	 *
+	 * Retrieve global position
 	 */
-	void		control_update();
+	void		global_position_update();
 
 	/**
-	 * Check for changes in vehicle status.
+	 * Retrieve home position
 	 */
-	void		vehicle_status_poll();
+	void		home_position_update();
 
 	/**
-	 * Check for position updates.
+	 * Retreive navigation capabilities
 	 */
-	void		vehicle_attitude_poll();
+	void		navigation_capabilities_update();
 
 	/**
-	 * Check for set triplet updates.
+	 * Retrieve offboard mission.
 	 */
-	void		mission_poll();
+	void		offboard_mission_update(bool isrotaryWing);
 
 	/**
-	 * Control throttle.
+	 * Retrieve onboard mission.
 	 */
-	float		control_throttle(float energy_error);
+	void		onboard_mission_update();
 
 	/**
-	 * Control pitch.
+	 * Retrieve vehicle status
 	 */
-	float		control_pitch(float altitude_error);
+	void		vehicle_status_update();
 
-	void calculate_airspeed_errors();
-	void calculate_gndspeed_undershoot();
-	void calculate_altitude_error();
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -192,6 +253,64 @@ private:
 	 * Main sensor collection task.
 	 */
 	void		task_main() __attribute__((noreturn));
+
+	void		publish_fence(unsigned vertices);
+
+	void		publish_safepoints(unsigned points);
+
+	bool		fence_valid(const struct fence_s &fence);
+
+	/**
+	 * Functions that are triggered when a new state is entered.
+	 */
+	void		start_none();
+	void		start_loiter();
+	void		start_mission();
+	void		start_mission_loiter();
+	void		start_rtl();
+	void		start_rtl_loiter();
+
+	/**
+	 * Guards offboard mission
+	 */
+	bool		offboard_mission_available(unsigned relative_index);
+
+	/**
+	 * Guards onboard mission
+	 */
+	bool		onboard_mission_available(unsigned relative_index);
+
+	/**
+	 * Check if current mission item has been reached.
+	 */
+	bool		mission_item_reached();
+
+	/**
+	 * Move to next waypoint
+	 */
+	void		advance_mission();
+
+	/**
+	 * Helper function to get a loiter item
+	 */
+	void		get_loiter_item(mission_item_s *new_loiter_position);
+
+	/**
+	 * Publish a new mission item triplet for position controller
+	 */
+	void		publish_mission_item_triplet();
+
+
+
+	/**
+	 * Compare two mission items if they are equivalent
+	 * Two mission items can be considered equivalent for the purpose of the navigator even if some fields differ.
+	 *
+	 * @return true if equivalent, false otherwise
+	 */
+	bool		cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b);
+
+	void		add_home_pos_to_rtl(struct mission_item_s *new_mission_item);
 };
 
 namespace navigator
@@ -206,39 +325,57 @@ static const int ERROR = -1;
 Navigator	*g_navigator;
 }
 
-Navigator::Navigator() :
+Navigator::Navigator() : 
+
+/* state machine transition table */
+	StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT),
 
 	_task_should_exit(false),
 	_navigator_task(-1),
+	_mavlink_fd(-1),
 
 /* subscriptions */
 	_global_pos_sub(-1),
-	_att_sub(-1),
-	_airspeed_sub(-1),
+	_home_pos_sub(-1),
 	_vstatus_sub(-1),
 	_params_sub(-1),
-	_manual_control_sub(-1),
+	_offboard_mission_sub(-1),
+	_onboard_mission_sub(-1),
+	_capabilities_sub(-1),
 
 /* publications */
 	_triplet_pub(-1),
+	_fence_pub(-1),
+	_mission_result_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
+
 /* states */
-	_mission_items_maxcount(20),
-	_mission_valid(false),
-	_loiter_hold(false)
+	_fence_valid(false),
+	_inside_fence(true),
+	_mission(),
+	_waypoint_position_reached(false),
+	_waypoint_yaw_reached(false),
+	_time_first_inside_orbit(0)
 {
-	_mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_items_maxcount);
-	if (!_mission_items) {
-		_mission_items_maxcount = 0;
-		warnx("no free RAM to allocate mission, rejecting any waypoints");
-	}
+	memset(&_fence, 0, sizeof(_fence));
 
-	_parameter_handles.throttle_cruise = param_find("NAV_DUMMY");
+	_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
+	_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
+	_parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN");
 
-	/* fetch initial parameter values */
-	parameters_update();
+	_mission_item_triplet.previous_valid = false;
+	_mission_item_triplet.current_valid = false;
+	_mission_item_triplet.next_valid = false;
+	memset(&_mission_item_triplet.previous, 0, sizeof(struct mission_item_s));
+	memset(&_mission_item_triplet.current, 0, sizeof(struct mission_item_s));
+	memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s));
+
+	memset(&_mission_result, 0, sizeof(struct mission_result_s));
+
+	/* Initialize state machine */
+	myState = STATE_INIT;
 }
 
 Navigator::~Navigator()
@@ -266,69 +403,87 @@ Navigator::~Navigator()
 	navigator::g_navigator = nullptr;
 }
 
-int
+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.throttle_cruise, &(_parameters.throttle_cruise));
+	param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude));
+	param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius));
+	param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled));
 
-	return OK;
+	_mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled);
 }
 
 void
-Navigator::vehicle_status_poll()
+Navigator::global_position_update()
 {
-	bool vstatus_updated;
-
-	/* Check HIL state if vehicle status has changed */
-	orb_check(_vstatus_sub, &vstatus_updated);
-
-	if (vstatus_updated) {
-
-		orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
-	}
+	/* load local copies */
+	orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
 }
 
 void
-Navigator::vehicle_attitude_poll()
+Navigator::home_position_update()
 {
-	/* check if there is a new position */
-	bool att_updated;
-	orb_check(_att_sub, &att_updated);
+	orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
+}
 
-	if (att_updated) {
-		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
-	}
+void
+Navigator::navigation_capabilities_update()
+{
+	orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
 }
 
+
 void
-Navigator::mission_poll()
+Navigator::offboard_mission_update(bool isrotaryWing)
 {
-	/* check if there is a new setpoint */
-	bool mission_updated;
-	orb_check(_mission_sub, &mission_updated);
+	struct mission_s offboard_mission;
+	if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
+
+		/* Check mission feasibility, for now do not handle the return value,
+		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
+		dm_item_t dm_current;
+		if (offboard_mission.dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+		missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count);
 
-	if (mission_updated) {
+		_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
+		_mission.set_current_offboard_mission_index(offboard_mission.current_index);
+		_mission.set_offboard_mission_count(offboard_mission.count);
 
-		struct mission_s mission;
-		orb_copy(ORB_ID(mission), _mission_sub, &mission);
+	} else {
+		_mission.set_current_offboard_mission_index(0);
+		_mission.set_offboard_mission_count(0);
+	}
+}
 
-		// XXX this is not optimal yet, but a first prototype /
-		// test implementation
+void
+Navigator::onboard_mission_update()
+{
+	struct mission_s onboard_mission;
+	if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
 
-		if (mission.count <= _mission_items_maxcount) {
-			/*
-			 * Perform an atomic copy & state update
-			 */
-			irqstate_t flags = irqsave();
+		_mission.set_current_onboard_mission_index(onboard_mission.current_index);
+		_mission.set_onboard_mission_count(onboard_mission.count);
 
-			memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s));
-			_mission_valid = true;
+	} else {
+		_mission.set_current_onboard_mission_index(0);
+		_mission.set_onboard_mission_count(0);
+	}
+}
 
-			irqrestore(flags);
-		} else {
-			warnx("mission larger than storage space");
-		}
+void
+Navigator::vehicle_status_update()
+{
+	/* try to load initial states */
+	if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {
+		_vstatus.arming_state = ARMING_STATE_STANDBY; /* in case the commander is not be running */
 	}
 }
 
@@ -346,41 +501,64 @@ Navigator::task_main()
 	warnx("Initializing..");
 	fflush(stdout);
 
+	_fence_valid = load_fence(GEOFENCE_MAX_VERTICES);
+
 	/*
 	 * do subscriptions
 	 */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	_mission_sub = orb_subscribe(ORB_ID(mission));
-	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	_offboard_mission_sub = orb_subscribe(ORB_ID(mission));
+	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
+	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
-	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
+
+	
+	/* copy all topics first time */
+	vehicle_status_update();
+	parameters_update();
+	global_position_update();
+	home_position_update();
+	navigation_capabilities_update();
+	offboard_mission_update(_vstatus.is_rotary_wing);
+	onboard_mission_update();
+
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vstatus_sub, 200);
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
 
-	parameters_update();
-
 	/* wakeup source(s) */
-	struct pollfd fds[2];
+	struct pollfd fds[7];
 
 	/* Setup of loop */
 	fds[0].fd = _params_sub;
 	fds[0].events = POLLIN;
 	fds[1].fd = _global_pos_sub;
 	fds[1].events = POLLIN;
+	fds[2].fd = _home_pos_sub;
+	fds[2].events = POLLIN;
+	fds[3].fd = _capabilities_sub;
+	fds[3].events = POLLIN;
+	fds[4].fd = _offboard_mission_sub;
+	fds[4].events = POLLIN;
+	fds[5].fd = _onboard_mission_sub;
+	fds[5].events = POLLIN;
+	fds[6].fd = _vstatus_sub;
+	fds[6].events = POLLIN;
+
 
 	while (!_task_should_exit) {
 
-		/* wait for up to 500ms for data */
+		/* wait for up to 100ms for data */
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit, etc. */
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -390,187 +568,721 @@ Navigator::task_main()
 
 		perf_begin(_loop_perf);
 
-		/* check vehicle status for changes to publication state */
-		vehicle_status_poll();
+		/* only update vehicle status if it changed */
+		if (fds[6].revents & POLLIN) {
+			
+			vehicle_status_update();
+
+			/* Evaluate state machine from commander and set the navigator mode accordingly */
+			if (_vstatus.main_state == MAIN_STATE_AUTO) {
+
+				switch (_vstatus.navigation_state) {
+					case NAVIGATION_STATE_DIRECT:
+					case NAVIGATION_STATE_STABILIZE:
+					case NAVIGATION_STATE_ALTHOLD:
+					case NAVIGATION_STATE_VECTOR:
+						
+						/* don't publish a mission item triplet */
+						dispatch(EVENT_NONE_REQUESTED);
+						break;
+
+					case NAVIGATION_STATE_AUTO_READY:
+					case NAVIGATION_STATE_AUTO_TAKEOFF:
+					case NAVIGATION_STATE_AUTO_MISSION:
+
+						/* try mission if none is available, fallback to loiter instead */
+						if (_mission.current_mission_available()) {
+							dispatch(EVENT_MISSION_REQUESTED);
+						} else {
+							dispatch(EVENT_LOITER_REQUESTED);
+						}
+						break;
 
-		/* only update parameters if they changed */
+					case NAVIGATION_STATE_AUTO_LOITER:
+
+						dispatch(EVENT_LOITER_REQUESTED);
+						break;
+
+
+					case NAVIGATION_STATE_AUTO_RTL:
+						
+						dispatch(EVENT_RTL_REQUESTED);
+						break;
+
+					case NAVIGATION_STATE_AUTO_LAND:
+
+						/* TODO add this */
+						
+						break;
+
+					default:
+						warnx("ERROR: Navigation state not supported");
+						break;
+				}
+
+			} else {
+				/* not in AUTO */
+				dispatch(EVENT_NONE_REQUESTED);
+			}
+
+			/* XXX Hack to get mavlink output going, try opening the fd with 5Hz */
+			if (_mavlink_fd < 0) {
+				/* try to open the mavlink log device every once in a while */
+				_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+			}
+		}
+
+
+		/* only update parameters if it changed */
 		if (fds[0].revents & POLLIN) {
-			/* read from param to clear updated flag */
-			struct parameter_update_s update;
-			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
 
-			/* update parameters from storage */
 			parameters_update();
+			/* note that these new parameters won't be in effect until a mission triplet is published again */
+		}
+
+		/* only update craft capabilities if they have changed */
+		if (fds[3].revents & POLLIN) {
+			navigation_capabilities_update();
+		}
+
+		if (fds[4].revents & POLLIN) {
+			offboard_mission_update(_vstatus.is_rotary_wing);
+			// XXX check if mission really changed 
+			dispatch(EVENT_MISSION_CHANGED);
+		}
+
+		if (fds[5].revents & POLLIN) {
+			onboard_mission_update();
+			// XXX check if mission really changed
+			dispatch(EVENT_MISSION_CHANGED);
+		}
+
+		if (fds[2].revents & POLLIN) {
+			home_position_update();
+			// XXX check if home position really changed
+			dispatch(EVENT_HOME_POSITION_CHANGED);
 		}
 
 		/* only run controller if position changed */
 		if (fds[1].revents & POLLIN) {
+			global_position_update();
+			/* only check if waypoint has been reached in Mission or RTL mode */
+			if (mission_item_reached()) {
 
+				if (_vstatus.main_state == MAIN_STATE_AUTO &&
+				    (_vstatus.navigation_state == NAVIGATION_STATE_AUTO_READY ||
+				     _vstatus.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF ||
+				     _vstatus.navigation_state == NAVIGATION_STATE_AUTO_MISSION)) {
 
-			static uint64_t last_run = 0;
-			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-			last_run = hrt_absolute_time();
+					/* advance by one mission item */
+					_mission.move_to_next();
 
-			/* guard against too large deltaT's */
-			if (deltaT > 1.0f)
-				deltaT = 0.01f;
+					/* if no more mission items available send this to state machine and start loiter at the last WP */
+					if (_mission.current_mission_available()) {
+						advance_mission();
+					} else {
+						dispatch(EVENT_MISSION_FINISHED);
+					}
+				} else {
+					dispatch(EVENT_MISSION_FINISHED);
+				}
+			}
+		}
+		perf_end(_loop_perf);
+	}
 
-			/* load local copies */
-			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
+	warnx("exiting.");
 
-			vehicle_attitude_poll();
+	_navigator_task = -1;
+	_exit(0);
+}
 
-			mission_poll();
+int
+Navigator::start()
+{
+	ASSERT(_navigator_task == -1);
 
-			math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy);
-			// Current waypoint
-			math::Vector<2> next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
-			// Global position
-			math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
+	/* start the task */
+	_navigator_task = task_spawn_cmd("navigator",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 5,
+					 2048,
+					 (main_t)&Navigator::task_main_trampoline,
+					 nullptr);
 
-			/* AUTONOMOUS FLIGHT */
+	if (_navigator_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
 
-			if (1 /* autonomous flight */) {
+	return OK;
+}
 
-				/* execute navigation once we have a setpoint */
-				if (_mission_valid) {
+void
+Navigator::status() 
+{
+	warnx("Global position is %svalid", _global_pos.valid ? "" : "in");
+	if (_global_pos.valid) {
+		warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d);
+		warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
+			(double)_global_pos.alt, (double)_global_pos.relative_alt);
+		warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f",
+			(double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz);
+		warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
+	}
+	if (_fence_valid) {
+		warnx("Geofence is valid");
+		warnx("Vertex longitude latitude");
+		for (unsigned i = 0; i < _fence.count; i++)
+		warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
+	} else {
+		warnx("Geofence not set");
+	}
 
-					// Next waypoint
-					math::Vector<2> prev_wp;
+	switch (myState) {
+		case STATE_INIT:
+			warnx("State: Init");
+			break;
+		case STATE_NONE:
+			warnx("State: None");
+			break;
+		case STATE_LOITER:
+			warnx("State: Loiter");
+			break;
+		case STATE_MISSION:
+			warnx("State: Mission");
+			break;
+		case STATE_MISSION_LOITER:
+			warnx("State: Loiter after Mission");
+			break;
+		case STATE_RTL:
+			warnx("State: RTL");
+			break;
+		case STATE_RTL_LOITER:
+			warnx("State: Loiter after RTL");
+			break;
+		default:
+			warnx("State: Unknown");
+			break;
+	}
+}
 
-					if (_global_triplet.previous_valid) {
-						prev_wp(0) = _global_triplet.previous.lat / 1e7f;
-						prev_wp(1) = _global_triplet.previous.lon / 1e7f;
+void
+Navigator::publish_fence(unsigned vertices)
+{
+	if (_fence_pub == -1)
+		_fence_pub = orb_advertise(ORB_ID(fence), &vertices);
+	else
+		orb_publish(ORB_ID(fence), _fence_pub, &vertices);
+}
 
-					} else {
-						/*
-						 * No valid next waypoint, go for heading hold.
-						 * This is automatically handled by the L1 library.
-						 */
-						prev_wp(0) = _global_triplet.current.lat / 1e7f;
-						prev_wp(1) = _global_triplet.current.lon / 1e7f;
+bool
+Navigator::fence_valid(const struct fence_s &fence)
+{
+	// NULL fence is valid
+	if (fence.count == 0) {
+		return true;
+	}
 
-					}
+	// Otherwise
+	if ((fence.count < 4) || (fence.count > GEOFENCE_MAX_VERTICES)) {
+		warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1);
+		return false;
+	}
 
+	return true;
+}
 
+bool
+Navigator::load_fence(unsigned vertices)
+{
+	struct fence_s temp_fence;
 
-					/********   MAIN NAVIGATION STATE MACHINE   ********/
+	unsigned i;
+	for (i = 0; i < vertices; i++) {
+		if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
+			break;
+		}
+	}
 
-					// XXX to be put in its own class
+	temp_fence.count = i;
 
-					if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
-						/* waypoint is a plain navigation waypoint */
-						
+	if (fence_valid(temp_fence))
+		memcpy(&_fence, &temp_fence, sizeof(_fence));
+	else
+		warnx("Invalid fence file, ignored!");
 
-					} else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-						_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-						_global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+	return _fence.count != 0;
+}
 
-						/* waypoint is a loiter waypoint */
-						
-					}
+void
+Navigator::fence_point(int argc, char *argv[])
+{
+	int ix, last;
+	double lon, lat;
+	struct fence_vertex_s vertex;
+	char *end;
+
+	if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) {
+		dm_clear(DM_KEY_FENCE_POINTS);
+		publish_fence(0);
+		return;
+	}
 
-					// XXX at this point we always want no loiter hold if a
-					// mission is active
-					_loiter_hold = false;
+	if (argc < 3)
+		errx(1, "Specify: -clear | sequence latitude longitude [-publish]");
 
-				} else {
+	ix = atoi(argv[0]);
+	if (ix >= DM_KEY_FENCE_POINTS_MAX)
+		errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX);
 
-					if (!_loiter_hold) {
-						_loiter_hold_lat = _global_pos.lat / 1e7f;
-						_loiter_hold_lon = _global_pos.lon / 1e7f;
-						_loiter_hold_alt = _global_pos.alt;
-						_loiter_hold = true;
-					}
+	lat = strtod(argv[1], &end);
+	lon = strtod(argv[2], &end);
 
-					//_parameters.loiter_hold_radius
-				}
+	last = 0;
+	if ((argc > 3) && (strcmp(argv[3], "-publish") == 0))
+		last = 1;
 
-			} else if (0/* seatbelt mode enabled */) {
+	vertex.lat = (float)lat;
+	vertex.lon = (float)lon;
 
-				/** SEATBELT FLIGHT **/
-				continue;
+	if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) {
+		if (last)
+			publish_fence((unsigned)ix + 1);
+		return;
+	}
 
-			} else {
+	errx(1, "can't store fence point");
+}
 
-				/** MANUAL FLIGHT **/
 
-				/* no flight mode applies, do not publish an attitude setpoint */
-				continue;
-			}
 
-			/********   MAIN NAVIGATION STATE MACHINE   ********/
+StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = {
+	{	
+							/* STATE_INIT */
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), STATE_NONE},
+		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), STATE_RTL},
+		/* EVENT_MISSION_FINISHED */		{NO_ACTION, STATE_INIT},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, STATE_INIT},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, STATE_INIT},
+	},
+	{	
+							/* STATE_NONE */
+		/* EVENT_NONE_REQUESTED */		{NO_ACTION, STATE_NONE},
+		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), STATE_RTL},
+		/* EVENT_MISSION_FINISHED */		{NO_ACTION, STATE_NONE},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, STATE_NONE},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, STATE_NONE},
+	},
+	{	
+							/* STATE_LOITER */
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), STATE_NONE},
+		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), STATE_RTL},
+		/* EVENT_MISSION_FINISHED */		{NO_ACTION, STATE_LOITER},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, STATE_LOITER},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, STATE_LOITER},
+	},
+	{	
+							/* STATE_MISSION */
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), STATE_NONE},
+		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), STATE_RTL},
+		/* EVENT_MISSION_FINISHED */		{ACTION(&Navigator::start_mission_loiter), STATE_MISSION_LOITER},
+		/* EVENT_MISSION_CHANGED */		{ACTION(&Navigator::start_mission), STATE_MISSION},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, STATE_MISSION},
+	},
+	{	
+							/* STATE_MISSION_LOITER */
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), STATE_NONE},
+		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, STATE_MISSION_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, STATE_MISSION_LOITER},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), STATE_RTL},
+		/* EVENT_MISSION_FINISHED */		{NO_ACTION, STATE_MISSION_LOITER},
+		/* EVENT_MISSION_CHANGED */		{ACTION(&Navigator::start_mission), STATE_MISSION},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, STATE_MISSION_LOITER},
+	},
+	{	
+							/* STATE_RTL */
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), STATE_NONE},
+		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{NO_ACTION, STATE_RTL},
+		/* EVENT_MISSION_FINISHED */		{ACTION(&Navigator::start_rtl_loiter), STATE_RTL_LOITER},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, STATE_RTL},
+		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_loiter), STATE_RTL},
+	},
+	{	
+							/* STATE_RTL_LOITER */
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), STATE_NONE},
+		/* EVENT_LOITER_REQUESTED */		{NO_ACTION,  STATE_RTL_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{NO_ACTION, STATE_RTL_LOITER},
+		/* EVENT_MISSION_FINISHED */		{NO_ACTION, STATE_RTL_LOITER},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, STATE_RTL_LOITER},
+		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_loiter), STATE_LOITER},
+	},
+};
 
-			if (_global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-				// XXX define launch position and return
+void
+Navigator::start_none()
+{
+	_mission_item_triplet.previous_valid = false;
+	_mission_item_triplet.current_valid = false;
+	_mission_item_triplet.next_valid = false;
 
-			} else if (_global_triplet.current.nav_cmd == NAV_CMD_LAND) {
-				// XXX flared descent on final approach
+	publish_mission_item_triplet();
+}
 
-			} else if (_global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
+void
+Navigator::start_loiter()
+{
+	_mission_item_triplet.previous_valid = false;
+	_mission_item_triplet.current_valid = true;
+	_mission_item_triplet.next_valid = false;
 
-				/* apply minimum pitch if altitude has not yet been reached */
-				if (_global_pos.alt < _global_triplet.current.altitude) {
-					_att_sp.pitch_body = math::max(_att_sp.pitch_body, _global_triplet.current.param1);
-				}
-			}
+	_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
+	_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
+	_mission_item_triplet.current.yaw = 0.0f;
 
-			/* lazily publish the setpoint only once available */
-			if (_triplet_pub > 0) {
-				/* publish the attitude setpoint */
-				orb_publish(ORB_ID(vehicle_global_position_set_triplet), _triplet_pub, &_global_triplet);
+	get_loiter_item(&_mission_item_triplet.current);
 
-			} else {
-				/* advertise and publish */
-				_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &_global_triplet);
-			}
+	float global_min_alt = _parameters.min_altitude + _home_pos.altitude;
+
+	/* Use current altitude if above min altitude set by parameter */
+	if (_global_pos.alt < global_min_alt) {
+		_mission_item_triplet.current.altitude = global_min_alt;
+		mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt));
+	} else {
+		_mission_item_triplet.current.altitude = _global_pos.alt;
+		mavlink_log_info(_mavlink_fd, "[navigator] loiter here");
+	}
+
+	publish_mission_item_triplet();
+}
+
+
+void
+Navigator::start_mission()
+{
+	/* leave previous mission item as isas is */
+
+	int ret;
+	bool onboard;
+	unsigned index;
 
+	ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index);
+
+	if (ret == OK) {
+
+		add_home_pos_to_rtl(&_mission_item_triplet.current);
+		_mission_item_triplet.current_valid = true;
+
+		if (onboard) {
+			mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
+		} else {
+			mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index);
 		}
+	} else {
+		/* since a mission is not started without WPs available, this is not supposed to happen */
+		_mission_item_triplet.current_valid = false;
+		warnx("ERROR: current WP can't be set");
+	}
 
-		perf_end(_loop_perf);
+	ret = _mission.get_next_mission_item(&_mission_item_triplet.next);
+
+	if (ret == OK) {
+
+		add_home_pos_to_rtl(&_mission_item_triplet.next);
+		_mission_item_triplet.next_valid = true;
+	} else {
+		/* this will fail for the last WP */
+		_mission_item_triplet.next_valid = false;
 	}
 
-	warnx("exiting.\n");
+	publish_mission_item_triplet();
+}
 
-	_navigator_task = -1;
-	_exit(0);
+
+
+void
+Navigator::advance_mission()
+{
+	/* copy current mission to previous item */
+	memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
+	_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
+
+	int ret;
+	bool onboard;
+	unsigned index;
+
+	ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index);
+
+	if (ret == OK) {
+
+		add_home_pos_to_rtl(&_mission_item_triplet.current);
+		_mission_item_triplet.current_valid = true;
+
+		if (onboard) {
+			mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
+		} else {
+			mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index);
+		}
+	} else {
+		/* since a mission is not advanced without WPs available, this is not supposed to happen */
+		_mission_item_triplet.current_valid = false;
+		warnx("ERROR: current WP can't be set");
+	}
+
+	ret = _mission.get_next_mission_item(&_mission_item_triplet.next);
+
+	if (ret == OK) {
+
+		add_home_pos_to_rtl(&_mission_item_triplet.next);
+
+		_mission_item_triplet.next_valid = true;
+	} else {
+		/* this will fail for the last WP */
+		_mission_item_triplet.next_valid = false;
+	}
+
+	publish_mission_item_triplet();
 }
 
-int
-Navigator::start()
+void
+Navigator::start_mission_loiter()
 {
-	ASSERT(_navigator_task == -1);
+	/* make sure the current WP is valid */
+	if (!_mission_item_triplet.current_valid) {
+		warnx("ERROR: cannot switch to offboard mission loiter");
+	}
 
-	/* start the task */
-	_navigator_task = task_spawn_cmd("navigator",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       2048,
-				       (main_t)&Navigator::task_main_trampoline,
-				       nullptr);
+	get_loiter_item(&_mission_item_triplet.current);
 
-	if (_navigator_task < 0) {
-		warn("task start failed");
-		return -errno;
+	publish_mission_item_triplet();
+
+	mavlink_log_info(_mavlink_fd, "[navigator] loiter at last WP");
+}
+
+void
+Navigator::start_rtl()
+{
+
+	/* discard all mission item and insert RTL item */
+	_mission_item_triplet.previous_valid = false;
+	_mission_item_triplet.current_valid = true;
+	_mission_item_triplet.next_valid = false;
+
+	_mission_item_triplet.current.lat = _home_pos.lat;
+	_mission_item_triplet.current.lon = _home_pos.lon;
+	_mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude;
+	_mission_item_triplet.current.yaw = 0.0f;
+	_mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
+	_mission_item_triplet.current.loiter_direction = 1;
+	_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
+	_mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number
+	_mission_item_triplet.current.autocontinue = false;
+	_mission_item_triplet.current_valid = true;
+
+	publish_mission_item_triplet();
+
+	mavlink_log_info(_mavlink_fd, "[navigator] return to launch");
+}	
+
+
+void
+Navigator::start_rtl_loiter()
+{
+	_mission_item_triplet.previous_valid = false;
+	_mission_item_triplet.current_valid = true;
+	_mission_item_triplet.next_valid = false;
+
+	_mission_item_triplet.current.lat = _home_pos.lat;
+	_mission_item_triplet.current.lon = _home_pos.lon;
+	_mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude;
+	
+	get_loiter_item(&_mission_item_triplet.current);
+
+	publish_mission_item_triplet();
+
+	mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL");
+}
+
+bool
+Navigator::mission_item_reached()
+{
+	/* only check if there is actually a mission item to check */
+	if (!_mission_item_triplet.current_valid) {
+		return false;
 	}
 
-	return OK;
+	/* don't try to reach the landing mission, just stay in that mode, XXX maybe add another state for this */
+	if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
+		return false;
+	}
+
+	/* XXX TODO count turns */
+	if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+	     _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+	     _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
+	    _mission_item_triplet.current.loiter_radius > 0.01f) {
+
+		return false;
+	}	
+
+	uint64_t now = hrt_absolute_time();
+	float orbit;
+
+	if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.radius > 0.01f) {
+
+		orbit = _mission_item_triplet.current.radius;
+
+	} else {
+
+		// XXX set default orbit via param
+		orbit = 15.0f;
+	}
+
+	/* keep vertical orbit */
+	float vertical_switch_distance = orbit;
+
+
+	// TODO add frame
+	// int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
+	
+	float dist = -1.0f;
+	float dist_xy = -1.0f;
+	float dist_z = -1.0f;
+
+	// if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
+	dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude,
+		                              (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt,
+		                              &dist_xy, &dist_z);
+
+	// warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude);
+	// warnx("2 lat: %2.2f, lon: %2.2f, alt: %2.2f", (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt);
+
+	// warnx("Dist: %4.4f", dist);
+
+	// } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
+	// 	dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z);
+
+	// } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
+	// 	dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z);
+
+	// } else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
+	// 	/* Check if conditions of mission item are satisfied */
+	// 	// XXX TODO
+	// }
+
+	if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
+		_waypoint_position_reached = true;
+	}
+
+	/* check if required yaw reached */
+	float yaw_sp = _wrap_pi(_mission_item_triplet.current.yaw);
+	float yaw_err = _wrap_pi(yaw_sp - _global_pos.yaw);
+
+	if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
+		_waypoint_yaw_reached = true;
+	}
+
+	/* check if the current waypoint was reached */
+	if (_waypoint_position_reached /* && _waypoint_yaw_reached */) { /* XXX what about yaw? */
+		
+		if (_time_first_inside_orbit == 0) {
+			/* XXX announcment? */
+			_time_first_inside_orbit = now;
+		}
+		
+		/* check if the MAV was long enough inside the waypoint orbit */
+		if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6)
+		     || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
+
+			_time_first_inside_orbit = 0;
+			_waypoint_yaw_reached = false;
+			_waypoint_position_reached = false;
+			return true;
+		}
+	}
+	return false;
+
+}
+
+void
+Navigator::get_loiter_item(struct mission_item_s *new_loiter_position)
+{
+	new_loiter_position->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+	new_loiter_position->loiter_direction = 1;
+	new_loiter_position->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
+	new_loiter_position->radius = 50.0f; // TODO: get rid of magic number
+	new_loiter_position->autocontinue = false;
+}
+
+void
+Navigator::publish_mission_item_triplet()
+{
+	/* lazily publish the mission triplet only once available */
+	if (_triplet_pub > 0) {
+		/* publish the mission triplet */
+		orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet);
+
+	} else {
+		/* advertise and publish */
+		_triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet);
+	}
+}
+
+
+bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) {
+	if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON &&
+	    fabsf(a.lat - b.lat) < FLT_EPSILON &&
+	    fabsf(a.lon - b.lon) < FLT_EPSILON &&
+	    fabsf(a.altitude - b.altitude) < FLT_EPSILON &&
+	    fabsf(a.yaw - b.yaw) < FLT_EPSILON &&
+	    fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON &&
+	    fabsf(a.loiter_direction - b.loiter_direction) < FLT_EPSILON &&
+	    fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON &&
+	    fabsf(a.radius - b.radius) < FLT_EPSILON &&
+	    fabsf(a.time_inside - b.time_inside) < FLT_EPSILON &&
+	    fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON &&
+	    fabsf(a.index - b.index) < FLT_EPSILON) {
+		return true;
+	} else {
+		warnx("a.index %d, b.index %d", a.index, b.index);
+		return false;
+	}
+}
+
+
+static void usage()
+{
+	errx(1, "usage: navigator {start|stop|status|fence}");
 }
 
 int navigator_main(int argc, char *argv[])
 {
-	if (argc < 1)
-		errx(1, "usage: navigator {start|stop|status}");
+	if (argc < 2) {
+		usage();
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (navigator::g_navigator != nullptr)
+		if (navigator::g_navigator != nullptr) {
 			errx(1, "already running");
+		}
 
 		navigator::g_navigator = new Navigator;
 
-		if (navigator::g_navigator == nullptr)
+		if (navigator::g_navigator == nullptr) {
 			errx(1, "alloc failed");
+		}
 
 		if (OK != navigator::g_navigator->start()) {
 			delete navigator::g_navigator;
@@ -578,27 +1290,38 @@ int navigator_main(int argc, char *argv[])
 			err(1, "start failed");
 		}
 
-		exit(0);
+		return 0;
 	}
 
-	if (!strcmp(argv[1], "stop")) {
-		if (navigator::g_navigator == nullptr)
-			errx(1, "not running");
+	if (navigator::g_navigator == nullptr)
+		errx(1, "not running");
 
+	if (!strcmp(argv[1], "stop")) {
 		delete navigator::g_navigator;
 		navigator::g_navigator = nullptr;
-		exit(0);
-	}
 
-	if (!strcmp(argv[1], "status")) {
-		if (navigator::g_navigator) {
-			errx(0, "running");
+	} else if (!strcmp(argv[1], "status")) {
+		navigator::g_navigator->status();
 
-		} else {
-			errx(1, "not running");
-		}
+	} else if (!strcmp(argv[1], "fence")) {
+		navigator::g_navigator->fence_point(argc - 2, argv + 2);
+
+	} else {
+		usage();
 	}
 
-	warnx("unrecognized command");
-	return 1;
+	return 0;
+}
+
+void
+Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item)
+{
+	if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
+		/* if it is a RTL waypoint, append the home position */
+		new_mission_item->lat = _home_pos.lat;
+		new_mission_item->lon = _home_pos.lon;
+		new_mission_item->altitude = _home_pos.altitude + _parameters.min_altitude;
+		new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
+		new_mission_item->radius = 50.0f; // TODO: get rid of magic number
+	}
 }
diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp
new file mode 100644
index 000000000..6576aae70
--- /dev/null
+++ b/src/modules/navigator/navigator_mission.cpp
@@ -0,0 +1,257 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file navigator_mission.cpp
+ * Helper class to access missions
+ */
+
+// #include <stdio.h>
+// #include <stdlib.h>
+// #include <string.h>
+// #include <unistd.h>
+
+#include <stdlib.h>
+#include <dataman/dataman.h>
+#include "navigator_mission.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+
+Mission::Mission() : 
+	
+	_offboard_dataman_id(-1),
+	_current_offboard_mission_index(0),
+	_current_onboard_mission_index(0),
+	_offboard_mission_item_count(0),
+	_onboard_mission_item_count(0),
+	_onboard_mission_allowed(false),
+	_current_mission_type(MISSION_TYPE_NONE)
+{}
+
+Mission::~Mission()
+{
+	
+}
+
+void
+Mission::set_offboard_dataman_id(int new_id)
+{
+	_offboard_dataman_id = new_id;
+}
+
+void
+Mission::set_current_offboard_mission_index(int new_index)
+{
+	if (new_index != -1) {
+		_current_offboard_mission_index = (unsigned)new_index;
+	}
+}
+
+void
+Mission::set_current_onboard_mission_index(int new_index)
+{
+	if (new_index != -1) {
+		_current_onboard_mission_index = (unsigned)new_index;
+	}
+}
+
+void
+Mission::set_offboard_mission_count(unsigned new_count)
+{
+	_offboard_mission_item_count = new_count;
+}
+
+void
+Mission::set_onboard_mission_count(unsigned new_count)
+{
+	_onboard_mission_item_count = new_count;
+}
+
+void
+Mission::set_onboard_mission_allowed(bool allowed)
+{
+	_onboard_mission_allowed = allowed;
+}
+
+bool
+Mission::current_mission_available()
+{
+	return (current_onboard_mission_available() || current_offboard_mission_available());
+
+}
+
+bool
+Mission::next_mission_available()
+{
+	return (next_onboard_mission_available() || next_offboard_mission_available());
+}
+
+int
+Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index)
+{
+	/* try onboard mission first */
+	if (current_onboard_mission_available()) {
+		
+		const ssize_t len = sizeof(struct mission_item_s);
+		if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return ERROR;
+		}
+		_current_mission_type = MISSION_TYPE_ONBOARD;
+		*onboard = true;
+		*index = _current_onboard_mission_index;
+
+	/* otherwise fallback to offboard */
+	} else if (current_offboard_mission_available()) {
+
+		dm_item_t dm_current;
+
+		if (_offboard_dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+
+		const ssize_t len = sizeof(struct mission_item_s);
+		if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			_current_mission_type = MISSION_TYPE_NONE;
+			return ERROR;
+		}
+		_current_mission_type = MISSION_TYPE_OFFBOARD;
+		*onboard = false;
+		*index = _current_offboard_mission_index;
+
+	} else {
+		/* happens when no more mission items can be added as a next item */
+		_current_mission_type = MISSION_TYPE_NONE;
+		return ERROR;
+	}
+
+	return OK;
+}
+
+int
+Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
+{
+	/* try onboard mission first */
+	if (next_onboard_mission_available()) {
+		
+		const ssize_t len = sizeof(struct mission_item_s);
+		if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return ERROR;
+		}
+
+	/* otherwise fallback to offboard */
+	} else if (next_offboard_mission_available()) {
+
+		dm_item_t dm_current;
+
+		if (_offboard_dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+
+		const ssize_t len = sizeof(struct mission_item_s);
+		if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return ERROR;
+		}
+
+	} else {
+		/* happens when no more mission items can be added as a next item */
+		return ERROR;
+	}
+
+	return OK;
+}
+
+
+bool
+Mission::current_onboard_mission_available()
+{
+	return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed;
+}
+
+bool
+Mission::current_offboard_mission_available()
+{
+	return _offboard_mission_item_count > _current_offboard_mission_index;
+}
+
+bool
+Mission::next_onboard_mission_available()
+{
+	unsigned next = 0;
+
+	if (_current_mission_type != MISSION_TYPE_ONBOARD) {
+		next = 1;
+	}
+
+	return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed;
+}
+
+bool
+Mission::next_offboard_mission_available()
+{
+	unsigned next = 0;
+
+	if (_current_mission_type != MISSION_TYPE_OFFBOARD) {
+		next = 1;
+	}
+
+	return _offboard_mission_item_count > (_current_offboard_mission_index + next);
+}
+
+void
+Mission::move_to_next()
+{
+	switch (_current_mission_type) {
+		case MISSION_TYPE_ONBOARD:
+			_current_onboard_mission_index++;
+			break;
+		case MISSION_TYPE_OFFBOARD:
+			_current_offboard_mission_index++;
+			break;
+		case MISSION_TYPE_NONE:
+		default:
+			break;
+	}
+}
\ No newline at end of file
diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h
new file mode 100644
index 000000000..15d4e86bf
--- /dev/null
+++ b/src/modules/navigator/navigator_mission.h
@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file navigator_mission.h
+ * Helper class to access missions
+ */
+
+#ifndef NAVIGATOR_MISSION_H
+#define NAVIGATOR_MISSION_H
+
+#include <uORB/topics/mission.h>
+
+
+class __EXPORT Mission
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Mission();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~Mission();
+
+	void		set_offboard_dataman_id(int new_id);
+	void		set_current_offboard_mission_index(int new_index);
+	void		set_current_onboard_mission_index(int new_index);
+	void		set_offboard_mission_count(unsigned new_count);
+	void		set_onboard_mission_count(unsigned new_count);
+
+	void		set_onboard_mission_allowed(bool allowed);
+
+	bool		current_mission_available();
+	bool		next_mission_available();
+
+	int		get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index);
+	int		get_next_mission_item(struct mission_item_s *mission_item);
+
+	void		move_to_next();
+
+	void		add_home_pos(struct mission_item_s *new_mission_item);
+
+private:
+	bool		current_onboard_mission_available();
+	bool		current_offboard_mission_available();
+	bool		next_onboard_mission_available();
+	bool		next_offboard_mission_available();
+
+	int 		_offboard_dataman_id;
+	unsigned	_current_offboard_mission_index;
+	unsigned	_current_onboard_mission_index;
+	unsigned	_offboard_mission_item_count;		/** number of offboard mission items available */
+	unsigned	_onboard_mission_item_count;		/** number of onboard mission items available */
+
+	bool		_onboard_mission_allowed;
+
+	enum {
+		MISSION_TYPE_NONE,
+		MISSION_TYPE_ONBOARD,
+		MISSION_TYPE_OFFBOARD,
+	} 		_current_mission_type;
+};
+
+#endif
\ No newline at end of file
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 06df9a452..b9d887379 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -1,7 +1,8 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Author: @autho Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,6 +39,7 @@
  * Parameters defined by the navigator task.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #include <nuttx/config.h>
@@ -49,5 +51,6 @@
  *
  */
 
-PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f);
-
+PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
+PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f);
+PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
\ No newline at end of file
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 d09c8ca00..52d8c9fe3 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -828,8 +828,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (local_pos.xy_global) {
 				double est_lat, est_lon;
 				map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
-				global_pos.lat = (int32_t)(est_lat * 1e7);
-				global_pos.lon = (int32_t)(est_lon * 1e7);
+				global_pos.lat = (int32_t)(est_lat * 1e7d);
+				global_pos.lon = (int32_t)(est_lon * 1e7d);
 				global_pos.time_gps_usec = gps.time_gps_usec;
 			}
 
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index ed29c8339..541eed0e1 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -66,7 +66,7 @@ controls_init(void)
 	sbus_init("/dev/ttyS2");
 
 	/* default to a 1:1 input map, all enabled */
-	for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
+	for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) {
 		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
 
 		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS]    = 0;
@@ -113,7 +113,7 @@ controls_tick() {
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
-	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */);
+	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
 	if (sbus_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
 	}
@@ -136,8 +136,8 @@ controls_tick() {
 	perf_end(c_gather_ppm);
 
 	/* limit number of channels to allowable data size */
-	if (r_raw_rc_count > PX4IO_INPUT_CHANNELS)
-		r_raw_rc_count = PX4IO_INPUT_CHANNELS;
+	if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
+		r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
 
 	/*
 	 * In some cases we may have received a frame, but input has still
@@ -210,14 +210,16 @@ controls_tick() {
 
 				/* and update the scaled/mapped version */
 				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-				ASSERT(mapped < PX4IO_CONTROL_CHANNELS);
+				if (mapped < PX4IO_CONTROL_CHANNELS) {
 
-				/* invert channel if pitch - pulling the lever down means pitching up by convention */
-				if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
-					scaled = -scaled;
+					/* invert channel if pitch - pulling the lever down means pitching up by convention */
+					if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
+						scaled = -scaled;
+
+					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+					assigned_channels |= (1 << mapped);
 
-				r_rc_values[mapped] = SIGNED_TO_REG(scaled);
-				assigned_channels |= (1 << mapped);
+				}
 			}
 		}
 
@@ -334,8 +336,8 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
 
 		/* PPM data exists, copy it */
 		*num_values = ppm_decoded_channels;
-		if (*num_values > PX4IO_CONTROL_CHANNELS)
-			*num_values = PX4IO_CONTROL_CHANNELS;
+		if (*num_values > PX4IO_RC_INPUT_CHANNELS)
+			*num_values = PX4IO_RC_INPUT_CHANNELS;
 
 		for (unsigned i = 0; i < *num_values; i++)
 			values[i] = ppm_buffer[i];
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index fd3b72015..4d306d6d0 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -355,7 +355,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 			continue;
 
 		/* ignore channels out of range */
-		if (channel >= PX4IO_INPUT_CHANNELS)
+		if (channel >= PX4IO_RC_INPUT_CHANNELS)
 			continue;
 
 		/* update the decoded channel count */
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 35ef5fcf6..e55ef784a 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -77,7 +77,8 @@ enum mixer_source {
 	MIX_NONE,
 	MIX_FMU,
 	MIX_OVERRIDE,
-	MIX_FAILSAFE
+	MIX_FAILSAFE,
+	MIX_OVERRIDE_FMU_OK
 };
 static mixer_source source;
 
@@ -135,10 +136,19 @@ mixer_tick(void)
 		if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
-		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
+		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
+		     !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
 
 		 	/* if allowed, mix from RC inputs directly */
 			source = MIX_OVERRIDE;
+		} else 	if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
+		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
+
+			/* if allowed, mix from RC inputs directly up to available rc channels */
+			source = MIX_OVERRIDE_FMU_OK;
 		}
 	}
 
@@ -240,24 +250,35 @@ mixer_callback(uintptr_t handle,
 	       uint8_t control_index,
 	       float &control)
 {
-	if (control_group != 0)
+	if (control_group > 3)
 		return -1;
 
 	switch (source) {
 	case MIX_FMU:
-		if (control_index < PX4IO_CONTROL_CHANNELS) {
-			control = REG_TO_FLOAT(r_page_controls[control_index]);
+		if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) {
+			control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
 			break;
 		}
 		return -1;
 
 	case MIX_OVERRIDE:
-		if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) {
+		if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
 			control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
 			break;
 		}
 		return -1;
 
+	case MIX_OVERRIDE_FMU_OK:
+		/* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */
+		if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
+			control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
+			break;
+		} else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
+			control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
+			break;
+		}
+		return -1;
+
 	case MIX_FAILSAFE:
 	case MIX_NONE:
 		control = 0.0f;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 500e0ed4b..e5bef6eb3 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 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
@@ -63,7 +63,7 @@
  * readable pages to be densely packed. Page numbers do not need to be
  * packed.
  *
- * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise, 
+ * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, 
  * [2] denotes definitions specific to the PX4IOv2 board.
  */
 
@@ -76,6 +76,9 @@
 
 #define PX4IO_PROTOCOL_VERSION		4
 
+/* maximum allowable sizes on this protocol version */
+#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT	8	/**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */
+
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG		0
 #define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* PX4IO_PROTOCOL_VERSION */
@@ -87,6 +90,7 @@
 #define PX4IO_P_CONFIG_RC_INPUT_COUNT		6	/* hardcoded max R/C input count supported */
 #define PX4IO_P_CONFIG_ADC_INPUT_COUNT		7	/* hardcoded max ADC inputs */
 #define PX4IO_P_CONFIG_RELAY_COUNT		8	/* hardcoded # of relay outputs */
+#define PX4IO_P_CONFIG_CONTROL_GROUP_COUNT	8	/**< hardcoded # of control groups*/
 
 /* dynamic status page */
 #define PX4IO_PAGE_STATUS		1
@@ -186,7 +190,7 @@ enum {							/* DSM bind states */
 	dsm_bind_reinit_uart
 };
  					     /*	8 */
-#define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
+#define PX4IO_P_SETUP_SET_DEBUG			9		/* debug level for IO board */
 
 #define PX4IO_P_SETUP_REBOOT_BL		       10	/* reboot IO into bootloader */
 #define PX4IO_REBOOT_BL_MAGIC               14662       /* required argument for reboot (random) */
@@ -194,41 +198,51 @@ enum {							/* DSM bind states */
 #define PX4IO_P_SETUP_CRC		       11	/* get CRC of IO firmware */
 
 /* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS		51		/* 0..CONFIG_CONTROL_COUNT */
+#define PX4IO_PAGE_CONTROLS			51		/**< actuator control groups, one after the other, 8 wide */
+#define PX4IO_P_CONTROLS_GROUP_0		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+#define PX4IO_P_CONTROLS_GROUP_1		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+#define PX4IO_P_CONTROLS_GROUP_2		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+#define PX4IO_P_CONTROLS_GROUP_3		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+
+#define PX4IO_P_CONTROLS_GROUP_VALID		64
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0	(1 << 0) /* group 0 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1	(1 << 1) /* group 1 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2	(1 << 2) /* group 2 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3	(1 << 3) /* group 3 is valid / received */
 
 /* raw text load to the mixer parser - ignores offset */
-#define PX4IO_PAGE_MIXERLOAD		52
+#define PX4IO_PAGE_MIXERLOAD			52
 
 /* R/C channel config */
-#define PX4IO_PAGE_RC_CONFIG		53		/* R/C input configuration */
-#define PX4IO_P_RC_CONFIG_MIN			0	/* lowest input value */
-#define PX4IO_P_RC_CONFIG_CENTER		1	/* center input value */
-#define PX4IO_P_RC_CONFIG_MAX			2	/* highest input value */
-#define PX4IO_P_RC_CONFIG_DEADZONE		3	/* band around center that is ignored */
-#define PX4IO_P_RC_CONFIG_ASSIGNMENT		4	/* mapped input value */
-#define PX4IO_P_RC_CONFIG_OPTIONS		5	/* channel options bitmask */
+#define PX4IO_PAGE_RC_CONFIG			53		/**< R/C input configuration */
+#define PX4IO_P_RC_CONFIG_MIN			0		/**< lowest input value */
+#define PX4IO_P_RC_CONFIG_CENTER		1		/**< center input value */
+#define PX4IO_P_RC_CONFIG_MAX			2		/**< highest input value */
+#define PX4IO_P_RC_CONFIG_DEADZONE		3		/**< band around center that is ignored */
+#define PX4IO_P_RC_CONFIG_ASSIGNMENT		4		/**< mapped input value */
+#define PX4IO_P_RC_CONFIG_OPTIONS		5		/**< channel options bitmask */
 #define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED	(1 << 0)
 #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE	(1 << 1)
-#define PX4IO_P_RC_CONFIG_STRIDE		6	/* spacing between channel config data */
+#define PX4IO_P_RC_CONFIG_STRIDE		6		/**< spacing between channel config data */
 
 /* PWM output - overrides mixer */
-#define PX4IO_PAGE_DIRECT_PWM		54		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DIRECT_PWM			54		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* PWM failsafe values - zero disables the output */
-#define PX4IO_PAGE_FAILSAFE_PWM		55		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_FAILSAFE_PWM			55		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* Debug and test page - not used in normal operation */
-#define PX4IO_PAGE_TEST			127
-#define PX4IO_P_TEST_LED			0	/* set the amber LED on/off */
+#define PX4IO_PAGE_TEST				127
+#define PX4IO_P_TEST_LED			0		/**< set the amber LED on/off */
 
 /* PWM minimum values for certain ESCs */
-#define PX4IO_PAGE_CONTROL_MIN_PWM		106	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_CONTROL_MIN_PWM		106		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* PWM maximum values for certain ESCs */
-#define PX4IO_PAGE_CONTROL_MAX_PWM		107	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_CONTROL_MAX_PWM		107		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* PWM disarmed values that are active, even when SAFETY_SAFE */
-#define PX4IO_PAGE_DISARMED_PWM		108	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DISARMED_PWM		108			/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /**
  * As-needed mixer data upload.
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index b9c9e0232..dea04a663 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -53,7 +53,9 @@
  */
 #define PX4IO_SERVO_COUNT		8
 #define PX4IO_CONTROL_CHANNELS		8
-#define PX4IO_INPUT_CHANNELS		8 // XXX this should be 18 channels
+#define PX4IO_CONTROL_GROUPS		2
+#define PX4IO_RC_INPUT_CHANNELS		18
+#define PX4IO_RC_MAPPED_CONTROL_CHANNELS		8 /**< This is the maximum number of channels mapped/used */
 
 /*
  * Debug logging
@@ -169,6 +171,8 @@ extern pwm_limit_t pwm_limit;
 
 #define BUTTON_SAFETY		stm32_gpioread(GPIO_BTN_SAFETY)
 
+#define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel)
+
 /*
  * Mixer
  */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 6aa3a5cd2..916b893c4 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 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
@@ -70,7 +70,7 @@ static const uint16_t	r_page_config[] = {
 	[PX4IO_P_CONFIG_MAX_TRANSFER]		= 64,	/* XXX hardcoded magic number */
 	[PX4IO_P_CONFIG_CONTROL_COUNT]		= PX4IO_CONTROL_CHANNELS,
 	[PX4IO_P_CONFIG_ACTUATOR_COUNT]		= PX4IO_SERVO_COUNT,
-	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= PX4IO_CONTROL_CHANNELS,
+	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= PX4IO_RC_INPUT_CHANNELS,
 	[PX4IO_P_CONFIG_ADC_INPUT_COUNT]	= PX4IO_ADC_CHANNEL_COUNT,
 	[PX4IO_P_CONFIG_RELAY_COUNT]		= PX4IO_RELAY_CHANNELS,
 };
@@ -89,9 +89,7 @@ uint16_t		r_page_status[] = {
 	[PX4IO_P_STATUS_IBATT]			= 0,
 	[PX4IO_P_STATUS_VSERVO]			= 0,
 	[PX4IO_P_STATUS_VRSSI]			= 0,
-	[PX4IO_P_STATUS_PRSSI]			= 0,
-	[PX4IO_P_STATUS_NRSSI]			= 0,
-	[PX4IO_P_STATUS_RC_DATA]		= 0
+	[PX4IO_P_STATUS_PRSSI]			= 0
 };
 
 /**
@@ -116,7 +114,7 @@ uint16_t		r_page_servos[PX4IO_SERVO_COUNT];
 uint16_t		r_page_raw_rc_input[] =
 {
 	[PX4IO_P_RAW_RC_COUNT]			= 0,
-	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon
+	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
 };
 
 /**
@@ -126,7 +124,7 @@ uint16_t		r_page_raw_rc_input[] =
  */
 uint16_t		r_page_rc_input[] = {
 	[PX4IO_P_RC_VALID]			= 0,
-	[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
+	[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0
 };
 
 /**
@@ -178,7 +176,7 @@ volatile uint16_t	r_page_setup[] =
  *
  * Control values from the FMU.
  */
-volatile uint16_t	r_page_controls[PX4IO_CONTROL_CHANNELS];
+volatile uint16_t	r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS];
 
 /*
  * PAGE 102 does not have a buffer.
@@ -189,7 +187,7 @@ volatile uint16_t	r_page_controls[PX4IO_CONTROL_CHANNELS];
  *
  * R/C channel input configuration.
  */
-uint16_t		r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
+uint16_t		r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
 
 /* valid options */
 #define PX4IO_P_RC_CONFIG_OPTIONS_VALID	(PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
@@ -241,7 +239,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 	case PX4IO_PAGE_CONTROLS:
 
 		/* copy channel data */
-		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+		while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
 
 			/* XXX range-check value? */
 			r_page_controls[offset] = *values;
@@ -554,7 +552,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
 		uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
 
-		if (channel >= PX4IO_CONTROL_CHANNELS)
+		if (channel >= PX4IO_RC_INPUT_CHANNELS)
 			return -1;
 
 		/* disable the channel until we have a chance to sanity-check it */
@@ -602,7 +600,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
 					count++;
 				}
-				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) {
+				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
 					count++;
 				}
 
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 3dcfe7f5b..11ccd7356 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -252,7 +252,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
 	}
 
 	/* decode switch channels if data fields are wide enough */
-	if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) {
+	if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) {
 		chancount = 18;
 
 		/* channel 17 (index 16) */
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index da7586de0..c6648de05 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -72,7 +72,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
@@ -693,7 +693,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_local_position_s local_pos;
 		struct vehicle_local_position_setpoint_s local_pos_sp;
 		struct vehicle_global_position_s global_pos;
-		struct vehicle_global_position_setpoint_s global_pos_sp;
+		struct mission_item_triplet_s triplet;
 		struct vehicle_gps_position_s gps_pos;
 		struct vehicle_vicon_position_s vicon_pos;
 		struct optical_flow_s flow;
@@ -718,7 +718,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int local_pos_sub;
 		int local_pos_sp_sub;
 		int global_pos_sub;
-		int global_pos_sp_sub;
+		int triplet_sub;
 		int gps_pos_sub;
 		int vicon_pos_sub;
 		int flow_sub;
@@ -841,8 +841,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fdsc_count++;
 
 	/* --- GLOBAL POSITION SETPOINT--- */
-	subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
-	fds[fdsc_count].fd = subs.global_pos_sp_sub;
+	subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
+	fds[fdsc_count].fd = subs.triplet_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
@@ -1165,20 +1165,21 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 			/* --- GLOBAL POSITION SETPOINT --- */
 			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp);
+				orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet);
 				log_msg.msg_type = LOG_GPSP_MSG;
-				log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative;
-				log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat;
-				log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon;
-				log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude;
-				log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw;
-				log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius;
-				log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction;
-				log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd;
-				log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1;
-				log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2;
-				log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3;
-				log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4;
+				log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative;
+				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.altitude = buf.triplet.current.altitude;
+				log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
+				log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd;				
+				log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
+				log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
+				log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
+				log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
+				log_msg.body.log_GPSP.radius = buf.triplet.current.radius;
+				log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside;
+				log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
 				LOGBUFFER_WRITE_AND_COUNT(GPSP);
 			}
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 6bfc8606e..ff58d23f6 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -214,13 +214,12 @@ struct log_GPSP_s {
 	int32_t lon;
 	float altitude;
 	float yaw;
+	uint8_t nav_cmd;
 	float loiter_radius;
 	int8_t loiter_direction;
-	uint8_t nav_cmd;
-	float param1;
-	float param2;
-	float param3;
-	float param4;
+	float radius;
+	float time_inside;
+	float pitch_min;
 };
 
 /* --- ESC - ESC STATE --- */
@@ -298,7 +297,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
-	LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
+	LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitRad,LoitDir,Rad,TimeIn,pitMin"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
 	LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 78d4b410a..c54128cb5 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -190,6 +190,24 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC15_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f);
 
+PARAM_DEFINE_FLOAT(RC16_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC16_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC16_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC16_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC16_DZ, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC17_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC17_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC17_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC17_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC17_DZ, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC18_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC18_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC18_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC18_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f);
+
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
 #endif
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 9e2eeafa4..66491500e 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -165,7 +165,7 @@ public:
 	int		start();
 
 private:
-	static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX;	/**< maximum number of r/c channels we handle */
+	static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS;	/**< maximum number of r/c channels we handle */
 
 	hrt_abstime	_rc_last_valid;		/**< last time we got a valid RC signal */
 
@@ -600,7 +600,7 @@ Sensors::parameters_update()
 	float tmpRevFactor = 0.0f;
 
 	/* rc values */
-	for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
+	for (unsigned int i = 0; i < _rc_max_chan_count; i++) {
 
 		param_get(_parameter_handles.min[i], &(_parameters.min[i]));
 		param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c
index b4350cc24..21e15ec56 100644
--- a/src/modules/systemlib/rc_check.c
+++ b/src/modules/systemlib/rc_check.c
@@ -45,7 +45,7 @@
 #include <systemlib/rc_check.h>
 #include <systemlib/param/param.h>
 #include <mavlink/mavlink_log.h>
-#include <uORB/topics/rc_channels.h>
+#include <drivers/drv_rc_input.h>
 
 int rc_calibration_check(int mavlink_fd) {
 
@@ -66,7 +66,7 @@ int rc_calibration_check(int mavlink_fd) {
 
 	int channel_fail_count = 0;
 
-	for (int i = 0; i < RC_CHANNELS_MAX; i++) {
+	for (int i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
 		/* should the channel be enabled? */
 		uint8_t count = 0;
 
diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h
new file mode 100644
index 000000000..f2709d29f
--- /dev/null
+++ b/src/modules/systemlib/state_table.h
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 state_table.h
+ * 
+ * Finite-State-Machine helper class for state table
+ */
+
+#ifndef __SYSTEMLIB_STATE_TABLE_H
+#define __SYSTEMLIB_STATE_TABLE_H
+
+class StateTable
+{
+public:
+	typedef void (StateTable::*Action)();
+	struct Tran {
+		Action action;
+		unsigned nextState;
+	};
+	
+	StateTable(Tran const *table, unsigned nStates, unsigned nSignals)
+	: myTable(table), myNsignals(nSignals), myNstates(nStates) {}
+	
+	#define NO_ACTION &StateTable::doNothing
+	#define ACTION(_target) static_cast<StateTable::Action>(_target)
+
+	virtual ~StateTable() {}
+	
+	void dispatch(unsigned const sig) {
+		register Tran const *t = myTable + myState*myNsignals + sig;
+		(this->*(t->action))();
+
+		myState = t->nextState;
+	}
+	void doNothing() {}
+protected:
+	unsigned myState;
+private:
+	Tran const *myTable;
+	unsigned myNsignals;
+	unsigned myNstates;
+};
+
+#endif
\ No newline at end of file
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index c6a252b55..79a820c06 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -117,17 +117,21 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi
 #include "topics/vehicle_bodyframe_speed_setpoint.h"
 ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s);
 
-#include "topics/vehicle_global_position_setpoint.h"
-ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s);
-
-#include "topics/vehicle_global_position_set_triplet.h"
-ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s);
+#include "topics/mission_item_triplet.h"
+ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s);
 
 #include "topics/vehicle_global_velocity_setpoint.h"
 ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
 
 #include "topics/mission.h"
 ORB_DEFINE(mission, struct mission_s);
+ORB_DEFINE(onboard_mission, struct mission_s);
+
+#include "topics/mission_result.h"
+ORB_DEFINE(mission_result, struct mission_result_s);
+
+#include "topics/fence.h"
+ORB_DEFINE(fence, unsigned);
 
 #include "topics/vehicle_attitude_setpoint.h"
 ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h
new file mode 100644
index 000000000..6f16c51cf
--- /dev/null
+++ b/src/modules/uORB/topics/fence.h
@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Jean Cyr <jean.m.cyr@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 fence.h
+ * Definition of geofence.
+ */
+
+#ifndef TOPIC_FENCE_H_
+#define TOPIC_FENCE_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+#define GEOFENCE_MAX_VERTICES 16
+
+/**
+ * This is the position of a geofence vertex
+ *
+ */
+struct fence_vertex_s {
+	// Worst case float precision gives us 2 meter resolution at the equator
+	float lat;			/**< latitude in degrees */
+	float lon;			/**< longitude in degrees */
+};
+
+/**
+ * This is the position of a geofence
+ *
+ */
+struct fence_s { 
+	unsigned count;     /**< number of actual vertices */
+	struct fence_vertex_s vertices[GEOFENCE_MAX_VERTICES];
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(fence);
+
+#endif
diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h
index 7e1b82a0f..1cf37e29c 100644
--- a/src/modules/uORB/topics/home_position.h
+++ b/src/modules/uORB/topics/home_position.h
@@ -2,6 +2,7 @@
  *
  *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,9 +35,10 @@
 
 /**
  * @file home_position.h
- * Definition of the GPS home position uORB topic.
+ * Definition of the home position uORB topic.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #ifndef TOPIC_HOME_POSITION_H_
@@ -55,16 +57,12 @@
  */
 struct home_position_s
 {
-	uint64_t timestamp;             /**< Timestamp (microseconds since system boot)   */
-	uint64_t time_gps_usec;         /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module   */
-	
-	int32_t lat;                    /**< Latitude in 1E7 degrees */
-	int32_t lon;                    /**< Longitude in 1E7 degrees */
-	int32_t alt;                    /**< Altitude in 1E3 meters (millimeters) above MSL */
-	float eph_m;                   /**< GPS HDOP horizontal dilution of position in m */
-	float epv_m;                   /**< GPS VDOP horizontal dilution of position in m */
-	float s_variance_m_s;               /**< speed accuracy estimate m/s */
-	float p_variance_m;               /**< position accuracy estimate m */
+	uint64_t timestamp;			/**< Timestamp (microseconds since system boot)	*/
+
+	bool altitude_is_relative;
+	double lat;				/**< Latitude in degrees 			*/
+	double lon;				/**< Longitude in degrees			*/
+	float altitude;				/**< Altitude in meters				*/
 };
 
 /**
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index 978a3383a..dcdb234fa 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -35,8 +35,8 @@
  ****************************************************************************/
 
 /**
- * @file mission_item.h
- * Definition of one mission item.
+ * @file mission.h
+ * Definition of a mission consisting of mission items.
  */
 
 #ifndef TOPIC_MISSION_H_
@@ -46,14 +46,24 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
+#define NUM_MISSIONS_SUPPORTED 256
+
+/* compatible to mavlink MAV_CMD */
 enum NAV_CMD {
-	NAV_CMD_WAYPOINT = 0,
-	NAV_CMD_LOITER_TURN_COUNT,
-	NAV_CMD_LOITER_TIME_LIMIT,
-	NAV_CMD_LOITER_UNLIMITED,
-	NAV_CMD_RETURN_TO_LAUNCH,
-	NAV_CMD_LAND,
-	NAV_CMD_TAKEOFF
+	NAV_CMD_WAYPOINT=16,
+	NAV_CMD_LOITER_UNLIMITED=17,
+	NAV_CMD_LOITER_TURN_COUNT=18,
+	NAV_CMD_LOITER_TIME_LIMIT=19,
+	NAV_CMD_RETURN_TO_LAUNCH=20,
+	NAV_CMD_LAND=21,
+	NAV_CMD_TAKEOFF=22,
+	NAV_CMD_ROI=80,
+	NAV_CMD_PATHPLANNING=81
+};
+
+enum ORIGIN {
+	ORIGIN_MAVLINK = 0,
+	ORIGIN_ONBOARD
 };
 
 /**
@@ -70,23 +80,26 @@ enum NAV_CMD {
 struct mission_item_s
 {
 	bool altitude_is_relative;	/**< true if altitude is relative from start point	*/
-	double lat;			/**< latitude in degrees * 1E7				*/
-	double lon;			/**< longitude in degrees * 1E7				*/
+	double lat;			/**< latitude in degrees				*/
+	double lon;			/**< longitude in degrees				*/
 	float altitude;			/**< altitude in meters					*/
 	float yaw;			/**< in radians NED -PI..+PI 				*/
 	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
-	uint8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
-	enum NAV_CMD nav_cmd;		/**< true if loitering is enabled			*/
-	float param1;
-	float param2;
-	float param3;
-	float param4;
+	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
+	enum NAV_CMD nav_cmd;		/**< navigation command					*/
+	float radius;			/**< radius in which the mission is accepted as reached in meters */
+	float time_inside;		/**< time that the MAV should stay inside the radius before advancing in seconds */
+	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
+	bool autocontinue;		/**< true if next waypoint should follow after this one */
+	int index;			/**< index matching the mavlink waypoint                */
+	enum ORIGIN origin;		/**< where the waypoint has been generated		*/
 };
 
 struct mission_s
 {
-	struct mission_item_s *items;
-	unsigned count;
+	int dataman_id;			/**< default -1, there are two offboard storage places in the dataman: 0 or 1 */
+	unsigned count;			/**< count of the missions stored in the datamanager */
+	int current_index;		/**< default -1, start at the one changed latest */
 };
 
 /**
@@ -95,5 +108,6 @@ struct mission_s
 
 /* register this as object request broker structure */
 ORB_DECLARE(mission);
+ORB_DECLARE(onboard_mission);
 
 #endif
diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/mission_item_triplet.h
index 8516b263f..b35eae607 100644
--- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h
+++ b/src/modules/uORB/topics/mission_item_triplet.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
  *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
  *           @author Julian Oes <joes@student.ethz.ch>
  *           @author Lorenz Meier <lm@inf.ethz.ch>
@@ -35,18 +35,18 @@
  ****************************************************************************/
 
 /**
- * @file vehicle_global_position_setpoint.h
+ * @file mission_item_triplet.h
  * Definition of the global WGS84 position setpoint uORB topic.
  */
 
-#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_
-#define TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_
+#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_
+#define TOPIC_MISSION_ITEM_TRIPLET_H_
 
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
 
-#include "vehicle_global_position_setpoint.h"
+#include "mission.h"
 
 /**
  * @addtogroup topics
@@ -58,14 +58,19 @@
  *
  * This are the three next waypoints (or just the next two or one).
  */
-struct vehicle_global_position_set_triplet_s
+struct mission_item_triplet_s
 {
-	bool previous_valid;					/**< flag indicating previous position is valid */
-	bool next_valid;					/**< flag indicating next position is valid */
+	bool previous_valid;
+	bool current_valid;				/**< flag indicating previous mission item is valid */
+	bool next_valid;					/**< flag indicating next mission item is valid */
 
-	struct vehicle_global_position_setpoint_s previous;
-	struct vehicle_global_position_setpoint_s current;
-	struct vehicle_global_position_setpoint_s next;
+	struct mission_item_s previous;
+	struct mission_item_s current;
+	struct mission_item_s next;
+
+	int previous_index;
+	int current_index;
+	int next_index;
 };
 
 /**
@@ -73,6 +78,6 @@ struct vehicle_global_position_set_triplet_s
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(vehicle_global_position_set_triplet);
+ORB_DECLARE(mission_item_triplet);
 
 #endif
diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/mission_result.h
index a56434d3b..c99706b97 100644
--- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -35,45 +35,26 @@
  ****************************************************************************/
 
 /**
- * @file vehicle_global_position_setpoint.h
- * Definition of the global WGS84 position setpoint uORB topic.
+ * @file mission_result.h
+ * Mission results that navigator needs to pass on to commander and mavlink.
  */
 
-#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_
-#define TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_
+#ifndef TOPIC_MISSION_RESULT_H
+#define TOPIC_MISSION_RESULT_H
 
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
-#include "mission.h"
 
 /**
  * @addtogroup topics
  * @{
  */
 
-/**
- * Global position setpoint in WGS84 coordinates.
- *
- * This is the position the MAV is heading towards. If it of type loiter,
- * the MAV is circling around it with the given loiter radius in meters.
- */
-struct vehicle_global_position_setpoint_s
+struct mission_result_s
 {
-	bool altitude_is_relative;	/**< true if altitude is relative from start point	*/
-	int32_t lat;			/**< latitude in degrees * 1E7				*/
-	int32_t lon;			/**< longitude in degrees * 1E7				*/
-	float altitude;			/**< altitude in meters					*/
-	float yaw;			/**< in radians NED -PI..+PI 				*/
-	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
-	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
-	enum NAV_CMD nav_cmd;		/**< true if loitering is enabled			*/
-	float param1;
-	float param2;
-	float param3;
-	float param4;
-	float turn_distance_xy;		/**< The distance on the plane which will mark this as reached */
-	float turn_distance_z;		/**< The distance in Z direction which will mark this as reached */
+	bool mission_reached;		/**< true if mission has been reached			*/
+	unsigned mission_index;		/**< index of the mission which has been reached	*/
 };
 
 /**
@@ -81,6 +62,6 @@ struct vehicle_global_position_setpoint_s
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(vehicle_global_position_setpoint);
+ORB_DECLARE(mission_result);
 
 #endif
diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h
index 6a3e811e3..391756f99 100644
--- a/src/modules/uORB/topics/navigation_capabilities.h
+++ b/src/modules/uORB/topics/navigation_capabilities.h
@@ -53,6 +53,11 @@
  */
 struct navigation_capabilities_s {
     float turn_distance;    /**< the optimal distance to a waypoint to switch to the next */
+
+    /* Landing parameters: see fw_pos_control_l1/landingslope.h */
+    float landing_horizontal_slope_displacement;
+    float landing_slope_angle_rad;
+    float landing_flare_length;
 };
 
 /**
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 5a8580143..086b2ef15 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Nils Wenzler <wenzlern@student.ethz.ch>
- *           @author Ivan Ovinnikov <oivan@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012, 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
@@ -47,13 +44,13 @@
 
 /**
  * The number of RC channel inputs supported.
- * Current (Q1/2013) radios support up to 18 channels,
+ * Current (Q4/2013) radios support up to 18 channels,
  * leaving at a sane value of 15.
  * This number can be greater then number of RC channels,
  * because single RC channel can be mapped to multiple
  * functions, e.g. for various mode switches.
  */
-#define RC_CHANNELS_MAX   15
+#define RC_CHANNELS_MAPPED_MAX   15
 
 /** 
  * This defines the mapping of the RC functions.
@@ -91,7 +88,7 @@ struct rc_channels_s {
   uint64_t timestamp_last_valid;      /**< timestamp of last valid RC signal. */
   struct {
     float scaled;                     /**< Scaled to -1..1 (throttle: 0..1) */
-  } chan[RC_CHANNELS_MAX];
+  } chan[RC_CHANNELS_MAPPED_MAX];
   uint8_t chan_count;                 /**< number of valid channels */
 
   /*String array to store the names of the functions*/
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 38fb74c9b..e26fb97c8 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -78,6 +78,7 @@ struct vehicle_control_mode_s
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
 	bool flag_control_climb_rate_enabled;		/**< true if climb rate is controlled */
+	bool flag_control_flighttermination_enabled;   /**< true if flighttermination is enabled */
 
 	bool flag_control_auto_enabled;		// TEMP
 	uint8_t auto_state;	// TEMP navigation state for AUTO modes
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 1c184d3a7..1c3763924 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -95,6 +95,12 @@ typedef enum {
 	HIL_STATE_ON
 } hil_state_t;
 
+
+typedef enum {
+	FLIGHTTERMINATION_STATE_OFF = 0,
+	FLIGHTTERMINATION_STATE_ON
+} flighttermination_state_t;
+
 typedef enum {
 	MODE_SWITCH_MANUAL = 0,
 	MODE_SWITCH_ASSISTED,
@@ -229,6 +235,8 @@ struct vehicle_status_s
 	uint16_t errors_count2;
 	uint16_t errors_count3;
 	uint16_t errors_count4;
+
+	flighttermination_state_t flighttermination_state;
 };
 
 /**
diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c
new file mode 100644
index 000000000..5b121e34e
--- /dev/null
+++ b/src/systemcmds/tests/test_dataman.c
@@ -0,0 +1,182 @@
+/****************************************************************************
+ * px4/sensors/test_dataman.c
+ *
+ *  Copyright (C) 2012 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_led.h>
+#include <systemlib/systemlib.h>
+#include <drivers/drv_hrt.h>
+#include <semaphore.h>
+
+
+#include "tests.h"
+
+#include "dataman/dataman.h"
+
+static sem_t *sems;
+
+static int
+task_main(int argc, char *argv[])
+{
+	char buffer[DM_MAX_DATA_SIZE];
+	hrt_abstime wstart, wend, rstart, rend;
+
+	warnx("Starting dataman test task %s", argv[1]);
+	/* try to read an invalid item */
+	int my_id = atoi(argv[1]);
+	/* try to read an invalid item */
+	if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) {
+		warnx("%d read an invalid item failed", my_id);
+		goto fail;
+	}
+	/* try to read an invalid index */
+	if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) {
+		warnx("%d read an invalid index failed", my_id);
+		goto fail;
+	}
+	srand(hrt_absolute_time() ^ my_id);
+	unsigned hit = 0, miss = 0;
+	wstart = hrt_absolute_time();
+	for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
+		memset(buffer, my_id, sizeof(buffer));
+		buffer[1] = i;
+		unsigned hash = i ^ my_id;
+		unsigned len = (hash & 63) + 2;
+
+		int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len);
+		warnx("ret: %d", ret);
+		if (ret != len) {
+			warnx("%d write failed, index %d, length %d", my_id, hash, len);
+			goto fail;
+		}
+		usleep(rand() & ((64 * 1024) - 1));
+	}
+	rstart = hrt_absolute_time();
+	wend = rstart;
+
+	for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
+		unsigned hash = i ^ my_id;
+		unsigned len2, len = (hash & 63) + 2;
+		if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD, hash, buffer, sizeof(buffer))) < 2) {
+			warnx("%d read failed length test, index %d", my_id, hash);
+			goto fail;
+		}
+		if (buffer[0] == my_id) {
+			hit++;
+			if (len2 != len) {
+				warnx("%d read failed length test, index %d, wanted %d, got %d", my_id, hash, len, len2);
+				goto fail;
+			}
+			if (buffer[1] != i) {
+				warnx("%d data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]);
+				goto fail;
+			}
+		}
+		else
+			miss++;
+	}
+	rend = hrt_absolute_time();
+	warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.",
+		my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000);
+	sem_post(sems + my_id);
+	return 0;
+fail:
+	warnx("Test %d fail, buffer %02x %02x %02x %02x %02x %02x",
+		my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);
+	sem_post(sems + my_id);
+	return -1;
+}
+
+int test_dataman(int argc, char *argv[])
+{
+	int i, num_tasks = 4;
+	char buffer[DM_MAX_DATA_SIZE];
+
+	if (argc > 1)
+		num_tasks = atoi(argv[1]);
+	
+	sems = (sem_t *)malloc(num_tasks * sizeof(sem_t));
+	warnx("Running %d tasks", num_tasks);
+	for (i = 0; i < num_tasks; i++) {
+		int task;
+		char a[16];
+		sprintf(a, "%d", i);
+		const char *av[2];
+		av[0] = a;
+		av[1] = 0;
+		sem_init(sems + i, 1, 0);
+		/* start the task */
+		if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) {
+			warn("task start failed");
+		}
+	}
+	for (i = 0; i < num_tasks; i++) {
+		sem_wait(sems + i);
+		sem_destroy(sems + i);
+	}
+	free(sems);
+	dm_restart(DM_INIT_REASON_IN_FLIGHT);
+	for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
+		if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0)
+			break;
+	}
+	if (i >= NUM_MISSIONS_SUPPORTED) {
+		warnx("Restart in-flight failed");
+		return -1;
+
+	}
+	dm_restart(DM_INIT_REASON_POWER_ON);
+	for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
+		if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) {
+			warnx("Restart power-on failed");
+			return -1;
+		}
+	}
+	return 0;
+}