Merge commit '95bdc1a9bd364ce95abe06b097579cc8a9162e33' into navigator_new_vector

35 files changed, 1256 insertions, 1938 deletions

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 9812ea497..cbdd5acc4 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -850,7 +850,7 @@ PX4IO::task_main()
 
 			/* 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();
+			(void)io_set_control_groups();
 		}
 
 		if (now >= poll_last + IO_POLL_INTERVAL) {
@@ -962,14 +962,14 @@ out:
 int
 PX4IO::io_set_control_groups()
 {
-	bool attitude_ok = io_set_control_state(0);
+	int ret = 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;
+	return ret;
 }
 
 int
@@ -1095,8 +1095,10 @@ PX4IO::io_set_rc_config()
 	 * assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical
 	 * controls.
 	 */
+
+	/* fill the mapping with an error condition triggering value */
 	for (unsigned i = 0; i < _max_rc_input; i++)
-		input_map[i] = -1;
+		input_map[i] = UINT8_MAX;
 
 	/*
 	 * NOTE: The indices for mapped channels are 1-based
@@ -1128,12 +1130,6 @@ PX4IO::io_set_rc_config()
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 4;
 
-	ichan = 5;
-
-	for (unsigned i = 0; i < _max_rc_input; i++)
-		if (input_map[i] == -1)
-			input_map[i] = ichan++;
-
 	/*
 	 * Iterate all possible RC inputs.
 	 */
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index 5bfbe04b8..b7c9b89a4 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.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
@@ -168,7 +168,7 @@
 # error HRT_TIMER_CLOCK must be greater than 1MHz
 #endif
 
-/*
+/**
  * Minimum/maximum deadlines.
  *
  * These are suitable for use with a 16-bit timer/counter clocked
@@ -276,7 +276,7 @@ static void		hrt_call_invoke(void);
  * Specific registers and bits used by PPM sub-functions
  */
 #ifdef HRT_PPM_CHANNEL
-/* 
+/*
  * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it.
  *
  * Note that we assume that M3 means STM32F1 (since we don't really care about the F2).
@@ -336,17 +336,18 @@ static void		hrt_call_invoke(void);
 /*
  * PPM decoder tuning parameters
  */
-# define PPM_MAX_PULSE_WIDTH	700		/* maximum width of a valid pulse */
-# define PPM_MIN_CHANNEL_VALUE	800		/* shortest valid channel signal */
-# define PPM_MAX_CHANNEL_VALUE	2200		/* longest valid channel signal */
-# define PPM_MIN_START		2400		/* shortest valid start gap (only 2nd part of pulse) */
+# define PPM_MIN_PULSE_WIDTH	200		/**< minimum width of a valid first pulse */
+# define PPM_MAX_PULSE_WIDTH	600		/**< maximum width of a valid first pulse */
+# define PPM_MIN_CHANNEL_VALUE	800		/**< shortest valid channel signal */
+# define PPM_MAX_CHANNEL_VALUE	2200		/**< longest valid channel signal */
+# define PPM_MIN_START		2300		/**< shortest valid start gap (only 2nd part of pulse) */
 
 /* decoded PPM buffer */
 #define PPM_MIN_CHANNELS	5
 #define PPM_MAX_CHANNELS	20
 
-/* Number of same-sized frames required to 'lock' */
-#define PPM_CHANNEL_LOCK	4		/* should be less than the input timeout */
+/** Number of same-sized frames required to 'lock' */
+#define PPM_CHANNEL_LOCK	4		/**< should be less than the input timeout */
 
 __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
 __EXPORT uint16_t ppm_frame_length = 0;
@@ -363,12 +364,12 @@ unsigned ppm_pulse_next;
 
 static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
 
-/* PPM decoder state machine */
+/** PPM decoder state machine */
 struct {
-	uint16_t	last_edge;	/* last capture time */
-	uint16_t	last_mark;	/* last significant edge */
-	uint16_t	frame_start;	/* the frame width */
-	unsigned	next_channel;	/* next channel index */
+	uint16_t	last_edge;	/**< last capture time */
+	uint16_t	last_mark;	/**< last significant edge */
+	uint16_t	frame_start;	/**< the frame width */
+	unsigned	next_channel;	/**< next channel index */
 	enum {
 		UNSYNCH = 0,
 		ARM,
@@ -390,7 +391,7 @@ static void	hrt_ppm_decode(uint32_t status);
 # define CCER_PPM	0
 #endif /* HRT_PPM_CHANNEL */
 
-/*
+/**
  * Initialise the timer we are going to use.
  *
  * We expect that we'll own one of the reduced-function STM32 general
@@ -436,7 +437,7 @@ hrt_tim_init(void)
 }
 
 #ifdef HRT_PPM_CHANNEL
-/*
+/**
  * Handle the PPM decoder state machine.
  */
 static void
@@ -518,8 +519,8 @@ hrt_ppm_decode(uint32_t status)
 	case ARM:
 
 		/* we expect a pulse giving us the first mark */
-		if (width > PPM_MAX_PULSE_WIDTH)
-			goto error;		/* pulse was too long */
+		if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too short or too long */
 
 		/* record the mark timing, expect an inactive edge */
 		ppm.last_mark = ppm.last_edge;
@@ -533,8 +534,8 @@ hrt_ppm_decode(uint32_t status)
 	case INACTIVE:
 
 		/* we expect a short pulse */
-		if (width > PPM_MAX_PULSE_WIDTH)
-			goto error;		/* pulse was too long */
+		if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too short or too long */
 
 		/* this edge is not interesting, but now we are ready for the next mark */
 		ppm.phase = ACTIVE;
@@ -576,7 +577,7 @@ error:
 }
 #endif /* HRT_PPM_CHANNEL */
 
-/*
+/**
  * Handle the compare interupt by calling the callout dispatcher
  * and then re-scheduling the next deadline.
  */
@@ -605,6 +606,7 @@ hrt_tim_isr(int irq, void *context)
 
 		hrt_ppm_decode(status);
 	}
+
 #endif
 
 	/* was this a timer tick? */
@@ -623,7 +625,7 @@ hrt_tim_isr(int irq, void *context)
 	return OK;
 }
 
-/*
+/**
  * Fetch a never-wrapping absolute time value in microseconds from
  * some arbitrary epoch shortly after system start.
  */
@@ -670,7 +672,7 @@ hrt_absolute_time(void)
 	return abstime;
 }
 
-/*
+/**
  * Convert a timespec to absolute time
  */
 hrt_abstime
@@ -684,7 +686,7 @@ ts_to_abstime(struct timespec *ts)
 	return result;
 }
 
-/*
+/**
  * Convert absolute time to a timespec.
  */
 void
@@ -695,7 +697,7 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
 	ts->tv_nsec = abstime * 1000;
 }
 
-/*
+/**
  * Compare a time value with the current time.
  */
 hrt_abstime
@@ -710,7 +712,7 @@ hrt_elapsed_time(const volatile hrt_abstime *then)
 	return delta;
 }
 
-/*
+/**
  * Store the absolute time in an interrupt-safe fashion
  */
 hrt_abstime
@@ -725,7 +727,7 @@ hrt_store_absolute_time(volatile hrt_abstime *now)
 	return ts;
 }
 
-/*
+/**
  * Initalise the high-resolution timing module.
  */
 void
@@ -740,7 +742,7 @@ hrt_init(void)
 #endif
 }
 
-/*
+/**
  * Call callout(arg) after interval has elapsed.
  */
 void
@@ -753,7 +755,7 @@ hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, v
 			  arg);
 }
 
-/*
+/**
  * Call callout(arg) at calltime.
  */
 void
@@ -762,7 +764,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v
 	hrt_call_internal(entry, calltime, 0, callout, arg);
 }
 
-/*
+/**
  * Call callout(arg) every period.
  */
 void
@@ -781,13 +783,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 	irqstate_t flags = irqsave();
 
 	/* if the entry is currently queued, remove it */
-        /* note that we are using a potentially uninitialised
-           entry->link here, but it is safe as sq_rem() doesn't
-           dereference the passed node unless it is found in the
-           list. So we potentially waste a bit of time searching the
-           queue for the uninitialised entry->link but we don't do
-           anything actually unsafe.
-        */
+	/* note that we are using a potentially uninitialised
+	   entry->link here, but it is safe as sq_rem() doesn't
+	   dereference the passed node unless it is found in the
+	   list. So we potentially waste a bit of time searching the
+	   queue for the uninitialised entry->link but we don't do
+	   anything actually unsafe.
+	*/
 	if (entry->deadline != 0)
 		sq_rem(&entry->link, &callout_queue);
 
@@ -801,7 +803,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 	irqrestore(flags);
 }
 
-/*
+/**
  * If this returns true, the call has been invoked and removed from the callout list.
  *
  * Always returns false for repeating callouts.
@@ -812,7 +814,7 @@ hrt_called(struct hrt_call *entry)
 	return (entry->deadline == 0);
 }
 
-/*
+/**
  * Remove the entry from the callout list.
  */
 void
@@ -895,17 +897,18 @@ hrt_call_invoke(void)
 		/* if the callout has a non-zero period, it has to be re-entered */
 		if (call->period != 0) {
 			// re-check call->deadline to allow for
-			// callouts to re-schedule themselves 
+			// callouts to re-schedule themselves
 			// using hrt_call_delay()
 			if (call->deadline <= now) {
 				call->deadline = deadline + call->period;
 			}
+
 			hrt_call_enter(call);
 		}
 	}
 }
 
-/*
+/**
  * Reschedule the next timer interrupt.
  *
  * This routine must be called with interrupts disabled.
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index d7dbbebd4..b66d1dba5 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -52,7 +52,6 @@ ECL_PitchController::ECL_PitchController() :
 	_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),
@@ -91,6 +90,7 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl
 	}
 
 	/* calculate the offset in the rate resulting from rolling  */
+	//xxx needs explanation and conversion to body angular rates or should be removed
 	float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
 				tanf(roll) * sinf(roll)) * _roll_ff;
 	if (inverted)
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
index 2ca0490fd..30a82a86a 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
@@ -77,9 +77,6 @@ public:
 	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;
@@ -119,7 +116,6 @@ private:
 	float _tc;
 	float _k_p;
 	float _k_i;
-	float _k_d;
 	float _k_ff;
 	float _integrator_max;
 	float _max_rate_pos;
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index bd6c9da71..9e7d35f68 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -52,7 +52,6 @@ ECL_RollController::ECL_RollController() :
 	_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),
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
index efc7b8944..92c64b95f 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -80,10 +80,6 @@ public:
 		_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;
 	}
@@ -113,7 +109,6 @@ private:
 	float _tc;
 	float _k_p;
 	float _k_i;
-	float _k_d;
 	float _k_ff;
 	float _integrator_max;
 	float _max_rate;
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index 7c366aaf2..5e2200727 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -50,11 +50,9 @@ ECL_YawController::ECL_YawController() :
 	_last_run(0),
 	_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),
 	_integrator(0.0f),
 	_rate_error(0.0f),
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index f15645fcf..03f3202d0 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -75,10 +75,6 @@ public:
 		_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;
 	}
@@ -91,10 +87,6 @@ public:
 		_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;
 	}
@@ -116,7 +108,6 @@ private:
 	uint64_t _last_run;
 	float _k_p;
 	float _k_i;
-	float _k_d;
 	float _k_ff;
 	float _integrator_max;
 	float _max_rate;
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index a6a40b4a1..999446a47 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -52,11 +52,13 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
 PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
 
 /* gyro measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f);
-/* accelerometer measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
+/* accel measurement noise */
+PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
+/* mag measurement noise */
+PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
+/* offset estimation - UNUSED */
+PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
 
 /* offsets in roll, pitch and yaw of sensor plane and body */
 PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
@@ -75,10 +77,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->q3 	=	param_find("EKF_ATT_V3_Q3");
 	h->q4 	=	param_find("EKF_ATT_V3_Q4");
 
-	h->r0 	=	param_find("EKF_ATT_V3_R0");
-	h->r1 	=	param_find("EKF_ATT_V3_R1");
-	h->r2 	=	param_find("EKF_ATT_V3_R2");
-	h->r3 	=	param_find("EKF_ATT_V3_R3");
+	h->r0 	=	param_find("EKF_ATT_V4_R0");
+	h->r1 	=	param_find("EKF_ATT_V4_R1");
+	h->r2 	=	param_find("EKF_ATT_V4_R2");
+	h->r3 	=	param_find("EKF_ATT_V4_R3");
 
 	h->roll_off  =	param_find("ATT_ROLL_OFF3");
 	h->pitch_off =	param_find("ATT_PITCH_OFF3");
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 28118b990..199bfb0da 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -69,7 +69,6 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_armed.h>
@@ -194,7 +193,7 @@ void usage(const char *reason);
 /**
  * React to commands that are sent e.g. from the mavlink module.
  */
-void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
+void handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
 
 /**
  * Mainloop of commander.
@@ -215,8 +214,6 @@ void print_reject_arm(const char *msg);
 
 void print_status();
 
-transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
-
 /**
  * Loop that runs at a lower rate and priority for calibration and parameter tasks.
  */
@@ -346,13 +343,12 @@ void print_status()
 	warnx("arming: %s", armed_str);
 }
 
-static orb_advert_t control_mode_pub;
 static orb_advert_t status_pub;
 
-void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
+void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
 {
 	/* result of the command */
-	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+	enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
 
 	/* only handle high-priority commands here */
 
@@ -365,12 +361,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 			/* set HIL state */
 			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
-			int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
+			int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
 
 			/* if HIL got enabled, reset battery status state */
-			if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
+			if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
 				/* reset the arming mode to disarmed */
-				arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
+				arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
 
 				if (arming_res != TRANSITION_DENIED) {
 					mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
@@ -386,12 +382,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			arming_res = TRANSITION_NOT_CHANGED;
 
 			if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
-				if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) {
+				if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) {
 					print_reject_arm("NOT ARMING: Press safety switch first.");
 					arming_res = TRANSITION_DENIED;
 
 				} else {
-					arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
+					arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
 				}
 
 				if (arming_res == TRANSITION_CHANGED) {
@@ -401,7 +397,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			} else {
 				if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
 					arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-					arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed);
+					arming_res = arming_state_transition(status, safety, new_arming_state, armed);
 
 					if (arming_res == TRANSITION_CHANGED) {
 						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
@@ -462,29 +458,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			break;
 		}
 
-	case VEHICLE_CMD_NAV_TAKEOFF: {
-			if (armed->armed) {
-				transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
-
-				if (nav_res == TRANSITION_CHANGED) {
-					mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
-				}
-
-				if (nav_res != TRANSITION_DENIED) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-				}
-
-			} else {
-				/* reject TAKEOFF not armed */
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-			}
-
-			break;
-		}
-
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
 			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
@@ -494,7 +467,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 					arming_res = TRANSITION_DENIED;
 
 				} else {
-					arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
+					arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
 				}
 
 				if (arming_res == TRANSITION_CHANGED) {
@@ -513,7 +486,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	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);
+				transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON);
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
@@ -524,29 +497,22 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 		break;
 
+	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+	case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
+	case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
+	case VEHICLE_CMD_PREFLIGHT_STORAGE:
+		/* ignore commands that handled in low prio loop */
+		break;
+
 	default:
+		/* warn about unsupported commands */
+		answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
 		break;
 	}
 
-	/* supported command handling stop */
-	if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
-		tune_positive();
-
-	} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
-		/* we do not care in the high prio loop about commands we don't know */
-	} else {
-		tune_negative();
-
-		if (result == VEHICLE_CMD_RESULT_DENIED) {
-			mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command);
-
-		} else if (result == VEHICLE_CMD_RESULT_FAILED) {
-			mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command);
-
-		} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
-			mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
-
-		}
+	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+		/* already warned about unsupported commands in "default" case */
+		answer_command(*cmd, result);
 	}
 
 	/* send any requested ACKs */
@@ -564,9 +530,6 @@ static struct actuator_armed_s armed;
 
 static struct safety_s safety;
 
-/* flags for control apps */
-struct vehicle_control_mode_s control_mode;
-
 int commander_thread_main(int argc, char *argv[])
 {
 	/* not yet initialized */
@@ -609,11 +572,9 @@ int commander_thread_main(int argc, char *argv[])
 	/* Initialize armed with all false */
 	memset(&armed, 0, sizeof(armed));
 
-	/* Initialize all flags to false */
-	memset(&control_mode, 0, sizeof(control_mode));
-
 	status.main_state = MAIN_STATE_MANUAL;
-	status.navigation_state = NAVIGATION_STATE_DIRECT;
+	status.set_nav_state = NAV_STATE_INIT;
+	status.set_nav_state_timestamp = 0;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
 
@@ -625,9 +586,6 @@ int commander_thread_main(int argc, char *argv[])
 	status.rc_signal_lost = true;
 	status.offboard_control_signal_lost = true;
 
-	/* allow manual override initially */
-	control_mode.flag_external_manual_override_ok = true;
-
 	/* set battery warning flag */
 	status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
 	status.condition_battery_voltage_valid = false;
@@ -635,9 +593,6 @@ int commander_thread_main(int argc, char *argv[])
 	// XXX for now just set sensors as initialized
 	status.condition_system_sensors_initialized = true;
 
-	// XXX just disable offboard control for now
-	control_mode.flag_control_offboard_enabled = false;
-
 	/* advertise to ORB */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
 	/* publish current state machine */
@@ -649,8 +604,6 @@ int commander_thread_main(int argc, char *argv[])
 
 	armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
 
-	control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
-
 	/* home position */
 	orb_advert_t home_pub = -1;
 	struct home_position_s home;
@@ -803,11 +756,9 @@ int commander_thread_main(int argc, char *argv[])
 				    status.system_type == VEHICLE_TYPE_QUADROTOR ||
 				    status.system_type == VEHICLE_TYPE_HEXAROTOR ||
 				    status.system_type == VEHICLE_TYPE_OCTOROTOR) {
-					control_mode.flag_external_manual_override_ok = false;
 					status.is_rotary_wing = true;
 
 				} else {
-					control_mode.flag_external_manual_override_ok = true;
 					status.is_rotary_wing = false;
 				}
 
@@ -859,7 +810,7 @@ int commander_thread_main(int argc, char *argv[])
 			// XXX this would be the right approach to do it, but do we *WANT* this?
 			// /* disarm if safety is now on and still armed */
 			// if (safety.safety_switch_available && !safety.safety_off) {
-			// 	(void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+			// 	(void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 			// }
 		}
 
@@ -990,10 +941,10 @@ int commander_thread_main(int argc, char *argv[])
 				battery_tune_played = false;
 
 				if (armed.armed) {
-					arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
+					arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
 
 				} else {
-					arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
+					arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
 				}
 
 				status_changed = true;
@@ -1012,7 +963,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
 			// XXX check for sensors
-			arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 
 		} else {
 			// XXX: Add emergency stuff if sensors are lost
@@ -1102,15 +1053,13 @@ int commander_thread_main(int argc, char *argv[])
 				 * do it only for rotary wings */
 				if (status.is_rotary_wing &&
 				    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
-				    (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY ||
-				     (status.condition_landed && (
-					      status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
-					      status.navigation_state == NAVIGATION_STATE_VECTOR
-				      ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+				    (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
+				    sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+
 					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 						/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
 						arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-						res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed);
+						res = arming_state_transition(&status, &safety, new_arming_state, &armed);
 						stick_off_counter = 0;
 
 					} else {
@@ -1132,7 +1081,7 @@ int commander_thread_main(int argc, char *argv[])
 							print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 
 						} else {
-							res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
+							res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
 						}
 
 						stick_on_counter = 0;
@@ -1185,12 +1134,12 @@ 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);
+			transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
 			if (flighttermination_res == TRANSITION_CHANGED) {
 				tune_positive();
 			}
 		} else {
-			flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF, &control_mode);
+			flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF);
 		}
 
 
@@ -1202,32 +1151,18 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
 
 			/* handle it */
-			handle_command(&status, &safety, &control_mode, &cmd, &armed);
-		}
-
-		/* evaluate the navigation state machine */
-		transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
-
-		if (res == TRANSITION_DENIED) {
-			/* DENIED here indicates bug in the commander */
-			warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
-			mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
+			handle_command(&status, &safety, &cmd, &armed);
 		}
 
 		/* check which state machines for changes, clear "changed" flag */
 		bool arming_state_changed = check_arming_state_changed();
 		bool main_state_changed = check_main_state_changed();
-		bool navigation_state_changed = check_navigation_state_changed();
 		bool flighttermination_state_changed = check_flighttermination_state_changed();
 
 		hrt_abstime t1 = hrt_absolute_time();
 
-		if (navigation_state_changed || arming_state_changed) {
-			control_mode.flag_armed = armed.armed;	// copy armed state to vehicle_control_mode topic
-		}
-
-		if (arming_state_changed || main_state_changed || navigation_state_changed) {
-			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
+		if (arming_state_changed || main_state_changed) {
+			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state);
 			status_changed = true;
 		}
 
@@ -1235,8 +1170,6 @@ int commander_thread_main(int argc, char *argv[])
 		if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
 			status.timestamp = t1;
 			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
-			control_mode.timestamp = t1;
-			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
 			armed.timestamp = t1;
 			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		}
@@ -1549,133 +1482,6 @@ print_reject_arm(const char *msg)
 	}
 }
 
-transition_result_t
-check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
-{
-	transition_result_t res = TRANSITION_DENIED;
-
-	if (status->main_state == MAIN_STATE_AUTO) {
-		if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
-			// TODO AUTO_LAND handling
-			if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
-				/* don't switch to other states until takeoff not completed */
-				// XXX: only respect the condition_landed when the local position is actually valid
-				if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
-					return TRANSITION_NOT_CHANGED;
-				}
-			}
-
-			if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF &&
-			    status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
-			    status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
-			    status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
-				/* possibly on ground, switch to TAKEOFF if needed */
-				if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
-					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
-					return res;
-				}
-			}
-
-			/* switch to AUTO mode */
-			if (status->rc_signal_found_once && !status->rc_signal_lost) {
-				/* act depending on switches when manual control enabled */
-				if (status->return_switch == RETURN_SWITCH_RETURN) {
-					/* RTL */
-					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
-
-				} else {
-					if (status->mission_switch == MISSION_SWITCH_MISSION) {
-						/* MISSION */
-						res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
-
-					} else {
-						/* LOITER */
-						res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
-					}
-				}
-
-			} else {
-				/* switch to MISSION when no RC control and first time in some AUTO mode  */
-				if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
-				    status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
-				    status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
-				    status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
-					res = TRANSITION_NOT_CHANGED;
-
-				} else {
-					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
-				}
-			}
-
-		} else {
-			/* disarmed, always switch to AUTO_READY */
-			res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
-		}
-
-	} else {
-		/* manual control modes */
-		if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) {
-			/* switch to failsafe mode */
-			bool manual_control_old = control_mode->flag_control_manual_enabled;
-
-			if (!status->condition_landed && status->condition_local_position_valid) {
-				/* in air: try to hold position if possible */
-				res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
-
-			} else {
-				/* landed: don't try to hold position but land (if taking off) */
-				res = TRANSITION_DENIED;
-			}
-
-			if (res == TRANSITION_DENIED) {
-				res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
-			}
-
-			control_mode->flag_control_manual_enabled = false;
-
-			if (res == TRANSITION_NOT_CHANGED && manual_control_old) {
-				/* mark navigation state as changed to force immediate flag publishing */
-				set_navigation_state_changed();
-				res = TRANSITION_CHANGED;
-			}
-
-			if (res == TRANSITION_CHANGED) {
-				if (control_mode->flag_control_position_enabled) {
-					mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD");
-
-				} else {
-					if (status->condition_landed) {
-						mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)");
-
-					} else {
-						mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD");
-					}
-				}
-			}
-
-		} else {
-			switch (status->main_state) {
-			case MAIN_STATE_MANUAL:
-				res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
-				break;
-
-			case MAIN_STATE_SEATBELT:
-				res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
-				break;
-
-			case MAIN_STATE_EASY:
-				res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
-				break;
-
-			default:
-				break;
-			}
-		}
-	}
-
-	return res;
-}
-
 void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
 {
 	switch (result) {
@@ -1784,7 +1590,7 @@ void *commander_low_prio_loop(void *arg)
 				/* try to go to INIT/PREFLIGHT arming state */
 
 				// XXX disable interrupts in arming_state_transition
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) {
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
 				}
@@ -1824,7 +1630,7 @@ void *commander_low_prio_loop(void *arg)
 				else
 					tune_negative();
 
-				arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 
 				break;
 			}
@@ -1876,7 +1682,7 @@ void *commander_low_prio_loop(void *arg)
 			}
 
 		default:
-			answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
+			/* don't answer on unsupported commands, it will be done in main loop */
 			break;
 		}
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index ca3ec94b8..731e0e3ff 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -46,7 +46,6 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
@@ -64,12 +63,10 @@ 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,
-	const struct vehicle_control_mode_s *control_mode,
 	arming_state_t new_arming_state, struct actuator_armed_s *armed)
 {
 	/*
@@ -86,7 +83,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 	} else {
 
 		/* enforce lockdown in HIL */
-		if (control_mode->flag_system_hil_enabled) {
+		if (status->hil_state == HIL_STATE_ON) {
 			armed->lockdown = true;
 		} else {
 			armed->lockdown = false;
@@ -109,7 +106,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 			/* allow coming from INIT and disarming from ARMED */
 			if (status->arming_state == ARMING_STATE_INIT
 			    || status->arming_state == ARMING_STATE_ARMED
-			    || control_mode->flag_system_hil_enabled) {
+			    || status->hil_state == HIL_STATE_ON) {
 
 				/* sensors need to be initialized for STANDBY state */
 				if (status->condition_system_sensors_initialized) {
@@ -126,7 +123,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 			/* allow arming from STANDBY and IN-AIR-RESTORE */
 			if ((status->arming_state == ARMING_STATE_STANDBY
 			     || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
-			    && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */
+			    && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */
 				ret = TRANSITION_CHANGED;
 				armed->armed = true;
 				armed->ready_to_arm = true;
@@ -289,169 +286,6 @@ check_main_state_changed()
 	}
 }
 
-transition_result_t
-navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
-{
-	transition_result_t ret = TRANSITION_DENIED;
-
-	/* only check transition if the new state is actually different from the current one */
-	if (new_navigation_state == status->navigation_state) {
-		ret = TRANSITION_NOT_CHANGED;
-
-	} else {
-
-		switch (new_navigation_state) {
-		case NAVIGATION_STATE_DIRECT:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = false;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = false;
-			control_mode->flag_control_climb_rate_enabled = false;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_STABILIZE:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = false;
-			control_mode->flag_control_climb_rate_enabled = false;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_ALTHOLD:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_VECTOR:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_AUTO_READY:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = false;
-			control_mode->flag_control_attitude_enabled = false;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = false;
-			control_mode->flag_control_climb_rate_enabled = false;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
-
-		case NAVIGATION_STATE_AUTO_TAKEOFF:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
-
-		case NAVIGATION_STATE_AUTO_LOITER:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_AUTO_MISSION:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
-
-		case NAVIGATION_STATE_AUTO_RTL:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
-
-		case NAVIGATION_STATE_AUTO_LAND:
-
-			/* deny transitions from landed state */
-			if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
-				ret = TRANSITION_CHANGED;
-				control_mode->flag_control_rates_enabled = true;
-				control_mode->flag_control_attitude_enabled = true;
-				control_mode->flag_control_velocity_enabled = true;
-				control_mode->flag_control_position_enabled = true;
-				control_mode->flag_control_altitude_enabled = true;
-				control_mode->flag_control_climb_rate_enabled = true;
-				control_mode->flag_control_manual_enabled = false;
-				control_mode->flag_control_auto_enabled = true;
-			}
-
-			break;
-
-		default:
-			break;
-		}
-
-		if (ret == TRANSITION_CHANGED) {
-			status->navigation_state = new_navigation_state;
-			control_mode->auto_state = status->navigation_state;
-			navigation_state_changed = true;
-		}
-	}
-
-	return ret;
-}
-
-bool
-check_navigation_state_changed()
-{
-	if (navigation_state_changed) {
-		navigation_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
 bool
 check_flighttermination_state_changed()
 {
@@ -464,16 +298,10 @@ check_flighttermination_state_changed()
 	}
 }
 
-void
-set_navigation_state_changed()
-{
-	navigation_state_changed = true;
-}
-
 /**
 * Transition from one hil state to another
 */
-int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd)
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
 	bool valid_transition = false;
 	int ret = ERROR;
@@ -502,7 +330,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 			    || current_status->arming_state == ARMING_STATE_STANDBY
 			    || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
 
-				current_control_mode->flag_system_hil_enabled = true;
 				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
 				valid_transition = true;
 			}
@@ -521,9 +348,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 		current_status->timestamp = hrt_absolute_time();
 		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
 
-		current_control_mode->timestamp = hrt_absolute_time();
-		orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
-
 		// XXX also set lockdown here
 
 		ret = OK;
@@ -539,7 +363,7 @@ 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 flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
@@ -566,7 +390,8 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s *
 
 			if (ret == TRANSITION_CHANGED) {
 				flighttermination_state_changed = true;
-				control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
+				// TODO
+				//control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
 			}
 		}
 
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index e1ec9d4ad..e569fb4f3 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -48,7 +48,6 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/safety.h>
-#include <uORB/topics/vehicle_control_mode.h>
 
 typedef enum {
 	TRANSITION_DENIED = -1,
@@ -58,7 +57,7 @@ typedef enum {
 } transition_result_t;
 
 transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed);
+		arming_state_t new_arming_state, struct actuator_armed_s *armed);
 
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
@@ -68,9 +67,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
 
 bool check_main_state_changed();
 
-transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
-
-transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_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);
 
 bool check_navigation_state_changed();
 
@@ -78,6 +75,6 @@ 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);
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
 
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 26777f737..df9d325b3 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -310,7 +310,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
 	_parameter_handles.tconst = param_find("FW_ATT_TC");
 	_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");
@@ -319,7 +318,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
 
 	_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");
@@ -327,9 +325,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
 	_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_YR_IMAX");
 	_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
 
@@ -374,7 +370,6 @@ FixedwingAttitudeControl::parameters_update()
 
 	param_get(_parameter_handles.tconst, &(_parameters.tconst));
 	param_get(_parameter_handles.p_p, &(_parameters.p_p));
-	param_get(_parameter_handles.p_d, &(_parameters.p_d));
 	param_get(_parameter_handles.p_i, &(_parameters.p_i));
 	param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
 	param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
@@ -383,7 +378,6 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward));
 
 	param_get(_parameter_handles.r_p, &(_parameters.r_p));
-	param_get(_parameter_handles.r_d, &(_parameters.r_d));
 	param_get(_parameter_handles.r_i, &(_parameters.r_i));
 	param_get(_parameter_handles.r_ff, &(_parameters.r_ff));
 
@@ -392,9 +386,7 @@ FixedwingAttitudeControl::parameters_update()
 
 	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));
@@ -407,7 +399,6 @@ FixedwingAttitudeControl::parameters_update()
 	_pitch_ctrl.set_time_constant(_parameters.tconst);
 	_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));
@@ -418,7 +409,6 @@ FixedwingAttitudeControl::parameters_update()
 	_roll_ctrl.set_time_constant(_parameters.tconst);
 	_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));
@@ -426,9 +416,7 @@ FixedwingAttitudeControl::parameters_update()
 	/* yaw control parameters */
 	_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));
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 0b37f48ba..1c615094c 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -38,107 +38,141 @@ f *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  * Parameters defined by the fixed-wing attitude control task
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
 
 #include <systemlib/param/param.h>
 
-//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.
+// @Description		This defines the latency between a step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
 // @Range		0.4 to 1.0 seconds, in tens of seconds
 PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
 
-// @DisplayName		Proportional gain.
-// @Description		This defines how much the elevator input will be commanded dependend on the current pitch error.
+// @DisplayName		Pitch rate proportional gain.
+// @Description		This defines how much the elevator input will be commanded depending on the current body angular rate error.
 // @Range		10 to 200, 1 increments
-PARAM_DEFINE_FLOAT(FW_PR_P, 0.3f);
+PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f);
 
-// @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_PR_D, 0.0f); //xxx: remove
-
-// @DisplayName		Integrator gain.
+// @DisplayName		Pitch rate integrator gain.
 // @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
 // @Range		0 to 50.0
-PARAM_DEFINE_FLOAT(FW_PR_I, 0.05f);
+PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f);
 
 // @DisplayName		Maximum positive / up pitch rate.
 // @Description		This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
 // @Range		0 to 90.0 degrees per seconds, in 1 increments
-PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f);
+PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.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, 60.0f);
+PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
 
-// @DisplayName		Pitch Integrator Anti-Windup
-// @Description		This limits the range in degrees the integrator can wind up to.
-// @Range		0.0 to 45.0
-// @Increment		1.0
+// @DisplayName		Pitch rate integrator limit
+// @Description		The portion of the integrator part in the control surface deflection is limited to this value
+// @Range		0.0 to 1
+// @Increment		0.1
 PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
 
-// @DisplayName		Roll feedforward gain.
+// @DisplayName		Roll to Pitch feedforward gain.
 // @Description		This compensates during turns and ensures the nose stays level.
 // @Range		0.5 2.0
 // @Increment		0.05
 // @User		User
-PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f);
+PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...)
 
-// @DisplayName		Proportional Gain.
-// @Description		This gain controls the roll angle to roll actuator output.
+// @DisplayName		Roll rate proportional Gain.
+// @Description		This defines how much the aileron input will be commanded depending on the current body angular rate error.
 // @Range		10.0 200.0
 // @Increment		10.0
 // @User		User
-PARAM_DEFINE_FLOAT(FW_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_RR_D, 0.0f); //xxx: remove
+PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
 
-// @DisplayName		Integrator Gain
-// @Description		This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors.
+// @DisplayName		Roll rate integrator Gain
+// @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
 // @Range		0.0 100.0
 // @Increment		5.0
 // @User		User
-PARAM_DEFINE_FLOAT(FW_RR_I, 0.05f);
+PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f);
 
 // @DisplayName		Roll Integrator Anti-Windup
-// @Description		This limits the range in degrees the integrator can wind up to.
-// @Range		0.0 to 45.0
-// @Increment		1.0
+// @Description		The portion of the integrator part in the control surface deflection is limited to this value.
+// @Range		0.0 to 1.0
+// @Increment		0.1
 PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
 
 // @DisplayName		Maximum Roll Rate
 // @Description		This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
 // @Range		0 to 90.0 degrees per seconds
 // @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_R_RMAX, 60);
+PARAM_DEFINE_FLOAT(FW_R_RMAX, 0);
 
+// @DisplayName		Yaw rate proportional gain.
+// @Description		This defines how much the rudder input will be commanded depending on the current body angular rate error.
+// @Range		10 to 200, 1 increments
+PARAM_DEFINE_FLOAT(FW_YR_P, 0.05);
+
+// @DisplayName		Yaw rate integrator gain.
+// @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
+// @Range		0 to 50.0
+PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f);
 
-PARAM_DEFINE_FLOAT(FW_YR_P, 0.5);
-PARAM_DEFINE_FLOAT(FW_YR_I, 0.05);
+// @DisplayName		Yaw rate integrator limit
+// @Description		The portion of the integrator part in the control surface deflection is limited to this value
+// @Range		0.0 to 1
+// @Increment		0.1
 PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
-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);
+
+// @DisplayName		Maximum Yaw Rate
+// @Description		This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
+// @Range		0 to 90.0 degrees per seconds
+// @Increment		1.0
+PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0);
+
+// @DisplayName		Roll rate feed forward
+// @Description		Direct feed forward from rate setpoint to control surface output
+// @Range		0 to 10
+// @Increment		0.1
+PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f);
+
+// @DisplayName		Pitch rate feed forward
+// @Description		Direct feed forward from rate setpoint to control surface output
+// @Range		0 to 10
+// @Increment		0.1
+PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f);
+
+// @DisplayName		Yaw rate feed forward
+// @Description		Direct feed forward from rate setpoint to control surface output
+// @Range		0 to 10
+// @Increment		0.1
+PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
+
+// @DisplayName		Minimal speed for yaw coordination
+// @Description		For airspeeds above this value the yaw rate is calculated for a coordinated turn. Set to a very high value to disable.
+// @Range		0 to 90.0 degrees per seconds
+// @Increment		1.0
+PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f);
+
+/* Airspeed parameters: the following parameters about airspeed are used by the attitude and the positon controller */
+
+// @DisplayName		Minimum Airspeed
+// @Description		If the airspeed falls below this value the TECS controller will try to increase airspeed more aggressively
+// @Range		0.0 to 30
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f);
+
+// @DisplayName		Trim Airspeed
+// @Description		The TECS controller tries to fly at this airspeed
+// @Range		0.0 to 30
+PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
+
+// @DisplayName		Maximum Airspeed
+// @Description		If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively
+// @Range		0.0 to 30
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 349c10118..5699fa1bb 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -49,6 +49,7 @@
  * More details and acknowledgements in the referenced library headers.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
@@ -227,6 +228,7 @@ private:
 		float land_H1_virt;
 		float land_flare_alt_relative;
 		float land_thrust_lim_horizontal_distance;
+		float land_heading_hold_horizontal_distance;
 
 	}		_parameters;			/**< local copies of interesting parameters */
 
@@ -271,6 +273,7 @@ private:
 		param_t land_H1_virt;
 		param_t land_flare_alt_relative;
 		param_t land_thrust_lim_horizontal_distance;
+		param_t land_heading_hold_horizontal_distance;
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -420,6 +423,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_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.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
 
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.min_sink_rate = 			param_find("FW_T_SINK_MIN");
@@ -508,6 +512,7 @@ FixedwingPositionControl::parameters_update()
 	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));
+	param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
 
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
@@ -822,19 +827,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			/* switch to heading hold for the last meters, continue heading hold after */
 			float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
 			//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
-			const float heading_hold_distance = 15.0f;
-			if (wp_distance < heading_hold_distance || land_noreturn_horizontal) {
+			if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
 
 				/* heading hold, along the line connecting this and the last waypoint */
-				
 
-				// if (mission_item_triplet.previous_valid) {
-				// 	target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), next_wp(0), next_wp(1));
-				// } else {
-
-				if (!land_noreturn_horizontal) //set target_bearing in first occurrence
-					target_bearing = _att.yaw;
-				//}
+				if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
+					if (mission_item_triplet.previous_valid) {
+						target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY());
+					} else {
+						target_bearing = _att.yaw;
+					}
+					mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
+				}
 
 //					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
 
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 10a0c00fc..9f46b5170 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
@@ -120,3 +120,4 @@ 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);
+PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 2ec00a9bc..4c38cf35a 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -68,7 +68,6 @@
 
 #include "waypoints.h"
 #include "orb_topics.h"
-#include "missionlib.h"
 #include "mavlink_hil.h"
 #include "util.h"
 #include "waypoints.h"
@@ -221,6 +220,8 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 	} else if (v_status.main_state == MAIN_STATE_AUTO) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		// TODO use control_mode topic
+		/*
 		if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
 		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
@@ -234,6 +235,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
 		}
+		*/
 	}
 	*mavlink_custom_mode = custom_mode.data;
 
@@ -707,25 +709,25 @@ int mavlink_thread_main(int argc, char *argv[])
 			}
 		}
 
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
+		mavlink_waypoint_eventloop(hrt_absolute_time());
 
 		/* sleep quarter the time */
 		usleep(25000);
 
 		/* check if waypoint has been reached against the last positions */
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
+		mavlink_waypoint_eventloop(hrt_absolute_time());
 
 		/* sleep quarter the time */
 		usleep(25000);
 
 		/* send parameters at 20 Hz (if queued for sending) */
 		mavlink_pm_queued_send();
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
+		mavlink_waypoint_eventloop(hrt_absolute_time());
 
 		/* sleep quarter the time */
 		usleep(25000);
 
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
+		mavlink_waypoint_eventloop(hrt_absolute_time());
 
 		if (baudrate > 57600) {
 			mavlink_pm_queued_send();
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index f8694ef28..4f9c718d2 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -79,7 +79,6 @@ __BEGIN_DECLS
 #include "mavlink_bridge_header.h"
 #include "waypoints.h"
 #include "orb_topics.h"
-#include "missionlib.h"
 #include "mavlink_hil.h"
 #include "mavlink_parameters.h"
 #include "util.h"
@@ -846,7 +845,7 @@ receive_thread(void *arg)
 					handle_message(&msg);
 
 					/* handle packet with waypoint component */
-					mavlink_wpm_message_handler(&msg, &global_pos, &local_pos);
+					mavlink_wpm_message_handler(&msg);
 
 					/* handle packet with parameter component */
 					mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
deleted file mode 100644
index 318dcf08c..000000000
--- a/src/modules/mavlink/missionlib.c
+++ /dev/null
@@ -1,399 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file missionlib.h
- * MAVLink missionlib components
- */
-
-// XXX trim includes
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <math.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <mqueue.h>
-#include <string.h>
-#include "mavlink_bridge_header.h"
-#include <drivers/drv_hrt.h>
-#include <time.h>
-#include <float.h>
-#include <unistd.h>
-#include <nuttx/sched.h>
-#include <sys/prctl.h>
-#include <termios.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <poll.h>
-
-#include <systemlib/err.h>
-#include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <mavlink/mavlink_log.h>
-
-#include "geo/geo.h"
-#include "waypoints.h"
-#include "orb_topics.h"
-#include "missionlib.h"
-#include "mavlink_hil.h"
-#include "util.h"
-#include "waypoints.h"
-#include "mavlink_parameters.h"
-
-
-
-static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
-static uint64_t loiter_start_time;
-
-#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)
-{
-	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
-
-	mavlink_send_uart_bytes(chan, missionlib_msg_buf, len);
-	return 0;
-}
-
-
-
-int
-mavlink_missionlib_send_gcs_string(const char *string)
-{
-	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
-	mavlink_statustext_t statustext;
-	int i = 0;
-
-	while (i < len - 1) {
-		statustext.text[i] = string[i];
-
-		if (string[i] == '\0')
-			break;
-
-		i++;
-	}
-
-	if (i > 1) {
-		/* Enforce null termination */
-		statustext.text[i] = '\0';
-		mavlink_message_t msg;
-
-		mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
-		return mavlink_missionlib_send_message(&msg);
-
-	} else {
-		return 1;
-	}
-}
-
-/**
- * Get system time since boot in microseconds
- *
- * @return the system time since boot in microseconds
- */
-uint64_t mavlink_missionlib_get_system_timestamp()
-{
-	return hrt_absolute_time();
-}
-
-#if 0
-/**
- * Set special vehicle setpoint fields based on current mission item.
- *
- * @return true if the mission item could be interpreted
- * successfully, it return false on failure.
- */
-bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
-	struct vehicle_global_position_setpoint_s *sp)
-{
-	switch (command) {
-		case MAV_CMD_NAV_LOITER_UNLIM:
-			sp->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
-			break;
-		case MAV_CMD_NAV_LOITER_TIME:
-			sp->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
-			loiter_start_time = hrt_absolute_time();
-			break;
-		// case MAV_CMD_NAV_LOITER_TURNS:
-		// 	sp->nav_cmd = NAV_CMD_LOITER_TURN_COUNT;
-		// 	break;
-		case MAV_CMD_NAV_WAYPOINT:
-			sp->nav_cmd = NAV_CMD_WAYPOINT;
-			break;
-		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-			sp->nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
-			break;
-		case MAV_CMD_NAV_LAND:
-			sp->nav_cmd = NAV_CMD_LAND;
-			break;
-		case MAV_CMD_NAV_TAKEOFF:
-			sp->nav_cmd = NAV_CMD_TAKEOFF;
-			break;
-		default:
-			/* abort */
-			return false;
-	}
-
-	sp->loiter_radius = param3;
-	sp->loiter_direction = (param3 >= 0) ? 1 : -1;
-
-	sp->param1 = param1;
-	sp->param2 = param2;
-	sp->param3 = param3;
-	sp->param4 = param4;
-
-
-	/* define the turn distance */
-	float orbit = 15.0f;
-
-	if (command == (int)MAV_CMD_NAV_WAYPOINT) {
-
-		orbit = param2;
-
-	} else if (command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-			command == (int)MAV_CMD_NAV_LOITER_TIME ||
-			command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
-
-		orbit = param3;
-	} else {
-
-		// XXX set default orbit via param
-		// 15 initialized above
-	}
-
-	sp->turn_distance_xy = orbit;
-	sp->turn_distance_z = orbit;
-}
-
-/**
- * This callback is executed each time a waypoint changes.
- *
- * It publishes the vehicle_global_position_setpoint_s or the
- * vehicle_local_position_setpoint_s topic, depending on the type of waypoint
- */
-void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
-		float param2, float param3, float param4, float param5_lat_x,
-		float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command)
-{
-	static orb_advert_t global_position_setpoint_pub = -1;
-	// static orb_advert_t global_position_set_triplet_pub = -1;
-	static orb_advert_t local_position_setpoint_pub = -1;
-	static unsigned last_waypoint_index = -1;
-	char buf[50] = {0};
-
-	// XXX include check if WP is supported, jump to next if not
-
-	/* Update controller setpoints */
-	if (frame == (int)MAV_FRAME_GLOBAL) {
-		/* global, absolute waypoint */
-		struct vehicle_global_position_setpoint_s sp;
-		sp.lat = param5_lat_x * 1e7f;
-		sp.lon = param6_lon_y * 1e7f;
-		sp.altitude = param7_alt_z;
-		sp.altitude_is_relative = false;
-		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
-		set_special_fields(param1, param2, param3, param4, command, &sp);
-
-		/* Initialize setpoint publication if necessary */
-		if (global_position_setpoint_pub < 0) {
-			global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
-		}
-
-
-		/* fill triplet: previous, current, next waypoint */
-		// struct vehicle_global_position_set_triplet_s triplet;
-
-		/* current waypoint is same as sp */
-		// memcpy(&(triplet.current), &sp, sizeof(sp));
-
-		/*
-		 * Check if previous WP (in mission, not in execution order) 
-		 * is available and identify correct index
-		 */
-		int last_setpoint_index = -1;
-		bool last_setpoint_valid = false;
-
-		if (index > 0) {
-			last_setpoint_index = index - 1;
-		}
-
-		while (last_setpoint_index >= 0) {
-
-			if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL &&
-				(wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
-				wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
-				last_setpoint_valid = true;
-				break;
-			}
-
-			last_setpoint_index--;
-		}
-
-		/*
-		 * Check if next WP (in mission, not in execution order) 
-		 * is available and identify correct index
-		 */
-		int next_setpoint_index = -1;
-		bool next_setpoint_valid = false;
-
-		/* next waypoint */
-		if (wpm->size > 1) {
-			next_setpoint_index = index + 1;
-		}
-
-		while (next_setpoint_index < wpm->size) {
-
-			if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
-					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
-				next_setpoint_valid = true;
-				break;
-			}
-
-			next_setpoint_index++;
-		}
-
-		/* populate last and next */
-
-		// triplet.previous_valid = false;
-		// triplet.next_valid = false;
-
-		// if (last_setpoint_valid) {
-		// 	triplet.previous_valid = true;
-		// 	struct vehicle_global_position_setpoint_s sp;
-		// 	sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f;
-		// 	sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
-		// 	sp.altitude = wpm->waypoints[last_setpoint_index].z;
-		// 	sp.altitude_is_relative = false;
-		// 	sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
-		// 	set_special_fields(wpm->waypoints[last_setpoint_index].param1,
-		// 		wpm->waypoints[last_setpoint_index].param2,
-		// 		wpm->waypoints[last_setpoint_index].param3,
-		// 		wpm->waypoints[last_setpoint_index].param4,
-		// 		wpm->waypoints[last_setpoint_index].command, &sp);
-		// 	memcpy(&(triplet.previous), &sp, sizeof(sp));
-		// }
-
-		// if (next_setpoint_valid) {
-		// 	triplet.next_valid = true;
-		// 	struct vehicle_global_position_setpoint_s sp;
-		// 	sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f;
-		// 	sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
-		// 	sp.altitude = wpm->waypoints[next_setpoint_index].z;
-		// 	sp.altitude_is_relative = false;
-		// 	sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
-		// 	set_special_fields(wpm->waypoints[next_setpoint_index].param1,
-		// 		wpm->waypoints[next_setpoint_index].param2,
-		// 		wpm->waypoints[next_setpoint_index].param3,
-		// 		wpm->waypoints[next_setpoint_index].param4,
-		// 		wpm->waypoints[next_setpoint_index].command, &sp);
-		// 	memcpy(&(triplet.next), &sp, sizeof(sp));
-		// }
-
-		/* Initialize triplet publication if necessary */
-		// if (global_position_set_triplet_pub < 0) {
-		// 	global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet);
-
-		// } else {
-		// 	orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
-		// }
-
-		sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
-
-	} else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-		/* global, relative alt (in relation to HOME) waypoint */
-		struct vehicle_global_position_setpoint_s sp;
-		sp.lat = param5_lat_x * 1e7f;
-		sp.lon = param6_lon_y * 1e7f;
-		sp.altitude = param7_alt_z;
-		sp.altitude_is_relative = true;
-		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
-		set_special_fields(param1, param2, param3, param4, command, &sp);
-
-		/* Initialize publication if necessary */
-		if (global_position_setpoint_pub < 0) {
-			global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
-		}
-
-
-
-		sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
-
-	} else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) {
-		/* local, absolute waypoint */
-		struct vehicle_local_position_setpoint_s sp;
-		sp.x = param5_lat_x;
-		sp.y = param6_lon_y;
-		sp.z = param7_alt_z;
-		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
-
-		/* Initialize publication if necessary */
-		if (local_position_setpoint_pub < 0) {
-			local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp);
-		}
-
-		sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
-	} else {
-		warnx("non-navigation WP, ignoring");
-		mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring.");
-		return;
-	}
-
-	/* only set this for known waypoint types (non-navigation types would have returned earlier) */
-	last_waypoint_index = index;
-
-	mavlink_missionlib_send_gcs_string(buf);
-	printf("%s\n", buf);
-	//printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);
-}
-
-#endif
\ No newline at end of file
diff --git a/src/modules/mavlink/missionlib.h b/src/modules/mavlink/missionlib.h
deleted file mode 100644
index c7d8f90c5..000000000
--- a/src/modules/mavlink/missionlib.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file missionlib.h
- * MAVLink mission helper library
- */
-
-#pragma once
-
-#include "mavlink_bridge_header.h"
-
-//extern void	mavlink_wpm_send_message(mavlink_message_t *msg);
-//extern void	mavlink_wpm_send_gcs_string(const char *string);
-//extern uint64_t	mavlink_wpm_get_system_timestamp(void);
-extern int	mavlink_missionlib_send_message(mavlink_message_t *msg);
-extern int	mavlink_missionlib_send_gcs_string(const char *string);
-extern uint64_t	mavlink_missionlib_get_system_timestamp(void);
-extern void	mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
-		float param2, float param3, float param4, float param5_lat_x,
-		float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index 5d3d6a73c..89a097c24 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -37,7 +37,6 @@
 
 MODULE_COMMAND	 = mavlink
 SRCS		 += mavlink.c \
-			   missionlib.c \
 			   mavlink_parameters.c \
 			   mavlink_receiver.cpp \
 			   orb_listener.c \
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 17978615f..28478a803 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -60,7 +60,6 @@
 
 #include "waypoints.h"
 #include "orb_topics.h"
-#include "missionlib.h"
 #include "mavlink_hil.h"
 #include "util.h"
 
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index 7d24b8f93..9000728cb 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -45,6 +45,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/offboard_control_setpoint.h>
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 59db898b9..2ff11e813 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -44,28 +44,63 @@
 #include <sys/prctl.h>
 #include <unistd.h>
 #include <stdio.h>
-
 #include "mavlink_bridge_header.h"
-#include "missionlib.h"
 #include "waypoints.h"
 #include "util.h"
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
-
 #include <geo/geo.h>
 #include <dataman/dataman.h>
+#include <drivers/drv_hrt.h>
+#include <systemlib/err.h>
 
-bool debug = false;
-bool verbose = false;
+bool verbose = true;
 
 orb_advert_t mission_pub = -1;
 struct mission_s mission;
 
-//#define MAVLINK_WPM_NO_PRINTF
-#define MAVLINK_WPM_VERBOSE 0
-
 uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
 
+void
+mavlink_missionlib_send_message(mavlink_message_t *msg)
+{
+	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
+
+	mavlink_send_uart_bytes(chan, missionlib_msg_buf, len);
+}
+
+
+
+int
+mavlink_missionlib_send_gcs_string(const char *string)
+{
+	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
+	mavlink_statustext_t statustext;
+	int i = 0;
+
+	while (i < len - 1) {
+		statustext.text[i] = string[i];
+
+		if (string[i] == '\0')
+			break;
+
+		i++;
+	}
+
+	if (i > 1) {
+		/* Enforce null termination */
+		statustext.text[i] = '\0';
+		mavlink_message_t msg;
+
+		mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
+		mavlink_missionlib_send_message(&msg);
+		return OK;
+
+	} else {
+		return 1;
+	}
+}
+
 void publish_mission()
 {
 	/* Initialize mission publication if necessary */
@@ -119,7 +154,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli
 
 	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->index = mavlink_mission_item->seq;
 	mission_item->origin = ORIGIN_MAVLINK;
 
 	return OK;
@@ -151,33 +186,22 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio
 	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;
+	// mavlink_mission_item->seq = mission_item->index;
 
 	return OK;
 }
 
 void mavlink_wpm_init(mavlink_wpm_storage *state)
 {
-	// Set all waypoints to zero
-
-	// Set count to zero
 	state->size = 0;
 	state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
 	state->current_state = MAVLINK_WPM_STATE_IDLE;
 	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->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
 }
 
 /*
@@ -188,24 +212,14 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
 	mavlink_message_t msg;
 	mavlink_mission_ack_t wpa;
 
-	wpa.target_system = wpm->current_partner_sysid;
-	wpa.target_component = wpm->current_partner_compid;
+	wpa.target_system = sysid;
+	wpa.target_component = compid;
 	wpa.type = type;
 
 	mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa);
 	mavlink_missionlib_send_message(&msg);
 
-	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
-
-// 	if (MAVLINK_WPM_TEXT_FEEDBACK) {
-// #ifdef MAVLINK_WPM_NO_PRINTF
-// 		mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
-// #else
-
-// 		if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
-
-// #endif
-// 	}
+	if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system);
 }
 
 /*
@@ -220,45 +234,19 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
 void mavlink_wpm_send_waypoint_current(uint16_t seq)
 {
 	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
-
 		mavlink_message_t msg;
 		mavlink_mission_current_t wpc;
 
-		wpc.seq = cur->seq;
+		wpc.seq = seq;
 
 		mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
 		mavlink_missionlib_send_message(&msg);
 
-		// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
-
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Set current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq);
-
-	} else {
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds\n");
-	}
-}
-
-/*
- *  @brief Directs the MAV to fly to a position
- *
- *  Sends a message to the controller, advising it to fly to the coordinates
- *  of the waypoint with a given orientation
- *
- *  @param seq The waypoint sequence number the MAV should fly to.
- */
-void mavlink_wpm_send_setpoint(uint16_t seq)
-{
-	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
-		mavlink_missionlib_current_waypoint_changed(cur->seq, cur->param1,
-				cur->param2, cur->param3, cur->param4, cur->x,
-				cur->y, cur->z, cur->frame, cur->command);
-
-		// wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp();
+		if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq);
 
 	} else {
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n");
+		mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
+		if (verbose) warnx("ERROR: index out of bounds");
 	}
 }
 
@@ -267,36 +255,48 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou
 	mavlink_message_t msg;
 	mavlink_mission_count_t wpc;
 
-	wpc.target_system = wpm->current_partner_sysid;
-	wpc.target_component = wpm->current_partner_compid;
+	wpc.target_system = sysid;
+	wpc.target_component = compid;
 	wpc.count = mission.count;
 
 	mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
 	mavlink_missionlib_send_message(&msg);
 
-	if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system);
-
-	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
+	if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system);
 }
 
-void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, mavlink_mission_item_t *wp)
+void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
 {
 
-	// if (seq < wpm->size) {
-		mavlink_message_t msg;
-	// 	mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
-		wp->target_system = wpm->current_partner_sysid;
-		wp->target_component = wpm->current_partner_compid;
-		mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp);
-		mavlink_missionlib_send_message(&msg);
+	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 (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
+	if (dm_read(dm_current, seq, &mission_item, len) == len) {
 
-		// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
+		/* create mission_item_s from mavlink_mission_item_t */
+		mavlink_mission_item_t wp;
+		map_mission_item_to_mavlink_mission_item(&mission_item, &wp);
 
-	// } else {
-	// 	if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n");
-	// }
+		mavlink_message_t msg;
+		wp.target_system = sysid;
+		wp.target_component = compid;
+		wp.seq = seq;
+		mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp);
+		mavlink_missionlib_send_message(&msg);
+
+		if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system);
+	} else {
+		mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
+		if (verbose) warnx("ERROR: could not read WP%u", seq);
+	}
 }
 
 void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
@@ -304,18 +304,17 @@ void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t s
 	if (seq < wpm->max_size) {
 		mavlink_message_t msg;
 		mavlink_mission_request_t wpr;
-		wpr.target_system = wpm->current_partner_sysid;
-		wpr.target_component = wpm->current_partner_compid;
+		wpr.target_system = sysid;
+		wpr.target_component = compid;
 		wpr.seq = seq;
 		mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr);
 		mavlink_missionlib_send_message(&msg);
 
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system);
-
-		// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
+		if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system);
 
 	} else {
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n");
+		mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
+		if (verbose) warnx("ERROR: Waypoint index exceeds list capacity");
 	}
 }
 
@@ -336,234 +335,33 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
 	mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached);
 	mavlink_missionlib_send_message(&msg);
 
-	if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq);
-
-	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
+	if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq);
 }
 
 
-// void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance)
-// {
-// 	static uint16_t counter;
-
-// 	if ((!global_pos->valid && !local_pos->xy_valid) ||
-// 		/* no waypoint */
-// 		wpm->size == 0) {
-// 		/* nothing to check here, return */
-// 		return;
-// 	}
-
-// 	if (wpm->current_active_wp_id < wpm->size) {
-
-// 		float orbit;
-// 		if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) {
-
-// 			orbit = wpm->waypoints[wpm->current_active_wp_id].param2;
-
-// 		} else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
-
-// 			orbit = wpm->waypoints[wpm->current_active_wp_id].param3;
-// 		} else {
-
-// 			// XXX set default orbit via param
-// 			orbit = 15.0f;
-// 		}
-
-// 		/* keep vertical orbit */
-// 		float vertical_switch_distance = orbit;
-
-// 		/* Take the larger turn distance - orbit or turn_distance */
-// 		if (orbit < turn_distance)
-// 			orbit = turn_distance;
-
-// 		int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
-// 		float dist = -1.0f;
-
-// 		float dist_xy = -1.0f;
-// 		float dist_z = -1.0f;
-
-// 		if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
-// 			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z);
-
-// 		} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-// 			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z);
-
-// 		} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
-// 			dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z);
-
-// 		} else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
-// 			/* Check if conditions of mission item are satisfied */
-// 			// XXX TODO
-// 		}
-
-// 		if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
-// 			wpm->pos_reached = true;
-// 		}
-
-// 		// check if required yaw reached
-// 		float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
-// 		float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
-// 		if (fabsf(yaw_err) < 0.05f) {
-// 			wpm->yaw_reached = true;
-// 		}
-// 	}
-
-// 	//check if the current waypoint was reached
-// 	if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) {
-// 		if (wpm->current_active_wp_id < wpm->size) {
-// 			mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-
-// 			if (wpm->timestamp_firstinside_orbit == 0) {
-// 				// Announce that last waypoint was reached
-// 				mavlink_wpm_send_waypoint_reached(cur_wp->seq);
-// 				wpm->timestamp_firstinside_orbit = now;
-// 			}
-
-// 			// check if the MAV was long enough inside the waypoint orbit
-// 			//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
-
-// 			bool time_elapsed = false;
-
-// 			if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
-// 				time_elapsed = true;
-// 			} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
-// 				time_elapsed = true;
-// 			}
-
-// 			if (time_elapsed) {
-
-// 				/* safeguard against invalid missions with last wp autocontinue on */
-// 				if (wpm->current_active_wp_id == wpm->size - 1) {
-// 					/* stop handling missions here */
-// 					cur_wp->autocontinue = false;
-// 				}
-
-// 				if (cur_wp->autocontinue) {
-
-// 					cur_wp->current = 0;
-
-// 					float navigation_lat = -1.0f;
-// 					float navigation_lon = -1.0f;
-// 					float navigation_alt = -1.0f;
-// 					int navigation_frame = -1;
-
-// 					/* initialize to current position in case we don't find a suitable navigation waypoint */
-// 					if (global_pos->valid) {
-// 						navigation_lat = global_pos->lat/1e7;
-// 						navigation_lon = global_pos->lon/1e7;
-// 						navigation_alt = global_pos->alt;
-// 						navigation_frame = MAV_FRAME_GLOBAL;
-// 					} else if (local_pos->xy_valid && local_pos->z_valid) {
-// 						navigation_lat = local_pos->x;
-// 						navigation_lon = local_pos->y;
-// 						navigation_alt = local_pos->z;
-// 						navigation_frame = MAV_FRAME_LOCAL_NED;
-// 					}
-
-// 					/* guard against missions without final land waypoint */
-// 					/* only accept supported navigation waypoints, skip unknown ones */
-// 					do {
-
-// 						/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
-// 						if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
-// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM ||
-// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) {
-
-// 							/* this is a navigation waypoint */
-// 							navigation_frame = cur_wp->frame;
-// 							navigation_lat = cur_wp->x;
-// 							navigation_lon = cur_wp->y;
-// 							navigation_alt = cur_wp->z;
-// 						}
-
-// 						if (wpm->current_active_wp_id == wpm->size - 1) {
-
-// 							/* if we're not landing at the last nav waypoint, we're falling back to loiter */
-// 							if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) {
-// 								/* the last waypoint was reached, if auto continue is
-// 								 * activated AND it is NOT a land waypoint, keep the system loitering there.
-// 								 */
-// 								cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
-// 								cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
-// 								cur_wp->frame = navigation_frame;
-// 								cur_wp->x = navigation_lat;
-// 								cur_wp->y = navigation_lon;
-// 								cur_wp->z = navigation_alt;
-// 							}
-
-// 							/* we risk an endless loop for missions without navigation waypoints, abort. */
-// 							break;
-
-// 						} else {
-// 							if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
-// 								wpm->current_active_wp_id++;
-// 						}
-			
-// 					} while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
-// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM));
-
-// 					// Fly to next waypoint
-// 					wpm->timestamp_firstinside_orbit = 0;
-// 					mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-// 					mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-// 					wpm->waypoints[wpm->current_active_wp_id].current = true;
-// 					wpm->pos_reached = false;
-// 					wpm->yaw_reached = false;
-// 					printf("Set new waypoint (%u)\n", wpm->current_active_wp_id);
-// 				}
-// 			}
-// 		}
-
-// 	} else {
-// 		wpm->timestamp_lastoutside_orbit = now;
-// 	}
-
-// 	counter++;
-// }
-
-
-int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap)
+void mavlink_waypoint_eventloop(uint64_t now)
 {
 	/* check for timed-out operations */
 	if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
 
-#ifdef MAVLINK_WPM_NO_PRINTF
-		mavlink_missionlib_send_gcs_string("Operation timeout switching -> IDLE");
-#else
+		mavlink_missionlib_send_gcs_string("Operation timeout");
 
-		if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_state);
+		if (verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state);
 
-#endif
 		wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-		// wpm->current_count = 0;
 		wpm->current_partner_sysid = 0;
 		wpm->current_partner_compid = 0;
-		// wpm->current_wp_id = -1;
-
-		// if (wpm->size == 0) {
-		// 	wpm->current_active_wp_id = -1;
-		// }
 	}
-
-	// check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance);
-
-	return OK;
 }
 
 
-void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos)
+void mavlink_wpm_message_handler(const mavlink_message_t *msg)
 {
-	uint64_t now = mavlink_missionlib_get_system_timestamp();
+	uint64_t now = hrt_absolute_time();
 
 	switch (msg->msgid) {
 
-	case MAVLINK_MSG_ID_MISSION_ACK: {
+		case MAVLINK_MSG_ID_MISSION_ACK: {
 			mavlink_mission_ack_t wpa;
 			mavlink_msg_mission_ack_decode(msg, &wpa);
 
@@ -573,8 +371,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 				if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
 					if (wpm->current_wp_id == wpm->size - 1) {
 
-						// mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
-
 						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 						wpm->current_wp_id = 0;
 					}
@@ -582,12 +378,13 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 			} else {
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
+				if (verbose) warnx("REJ. WP CMD: curr partner id mismatch");
 			}
 
 			break;
 		}
 
-	case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
+		case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
 			mavlink_mission_set_current_t wpc;
 			mavlink_msg_mission_set_current_decode(msg, &wpc);
 
@@ -596,52 +393,32 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
 					if (wpc.seq < wpm->size) {
-						// if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n");
-						// wpm->current_active_wp_id = wpc.seq;
-						// uint32_t i;
-
-						// for (i = 0; i < wpm->size; i++) {
-						// 	if (i == wpm->current_active_wp_id) {
-						// 		wpm->waypoints[i].current = true;
-
-						// 	} else {
-						// 		wpm->waypoints[i].current = false;
-						// 	}
-						// }
-
-						// mavlink_missionlib_send_gcs_string("NEW WP SET");
-
-						// wpm->yaw_reached = false;
-						// wpm->pos_reached = false;
-						
 
 						mission.current_index = wpc.seq;
-
 						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;
+						mavlink_wpm_send_waypoint_current(wpc.seq);
 
 					} else {
 						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
+						if (verbose) warnx("IGN WP CURR CMD: Not in list");
 					}
 
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
+					if (verbose) warnx("IGN WP CURR CMD: Busy");
 
 				}
 
 			} else {
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
+				if (verbose) warnx("REJ. WP CMD: target id mismatch");
 			}
 
 			break;
 		}
 
-	case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
+		case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
 			mavlink_mission_request_list_t wprl;
 			mavlink_msg_mission_request_list_decode(msg, &wprl);
 
@@ -650,532 +427,304 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
 					if (wpm->size > 0) {
-						//if (verbose && wpm->current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
-//                        if (verbose && wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
+						
 						wpm->current_state = MAVLINK_WPM_STATE_SENDLIST;
 						wpm->current_wp_id = 0;
 						wpm->current_partner_sysid = msg->sysid;
 						wpm->current_partner_compid = msg->compid;
 
 					} else {
-						// if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
+						if (verbose) warnx("No waypoints send");
 					}
 
 					wpm->current_count = wpm->size;
 					mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count);
 
 				} else {
-					// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm->current_state);
+					mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
+					if (verbose) warnx("IGN REQUEST LIST: Busy");
 				}
 			} else {
-				// if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n");
+				mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch");
+				if (verbose) warnx("REJ. REQUEST LIST: target id mismatch");
 			}
 
 			break;
 		}
 
-	case MAVLINK_MSG_ID_MISSION_REQUEST: {
+		case MAVLINK_MSG_ID_MISSION_REQUEST: {
 			mavlink_mission_request_t wpr;
 			mavlink_msg_mission_request_decode(msg, &wpr);
 
 			if (msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
 				wpm->timestamp_lastaction = now;
 
-				//ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
-				if ((wpm->current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm->current_wp_id || wpr.seq == wpm->current_wp_id + 1) && wpr.seq < wpm->size)) {
-					if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
-
-#endif
-					}
+				if (wpr.seq >= wpm->size) {
 
-					if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id + 1) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
-
-#endif
-					}
-
-					if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ");
-#else
+					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
+					if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq);
+					break;
+				}
 
-						if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+				/* 
+				 * Ensure that we are in the correct state and that the first request has id 0 
+				 * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
+				 */
+				if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
 
-#endif
+					if (wpr.seq == 0) {
+						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
+						wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
+					} else {
+						mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
+						if (verbose) warnx("REJ. WP CMD: First id != 0");
+						break;
 					}
 
-					wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
-					wpm->current_wp_id = wpr.seq;
+				} else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
 
-					mavlink_mission_item_t wp;
+					if (wpr.seq == wpm->current_wp_id) {
 
-					struct mission_item_s mission_item;
-					ssize_t len = sizeof(struct mission_item_s);
-					
-					dm_item_t dm_current;
+						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
 
-					if (wpm->current_dataman_id == 0) {
-						dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
-					} else {
-						dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
-					}
+					} else if (wpr.seq == wpm->current_wp_id + 1) {
 
-					if (dm_read(dm_current, wpr.seq, &mission_item, len) == len) {
+						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
 					
-						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);
+						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
+						if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1);
+						break;
 					}
 
 				} else {
-					// if (verbose)
-					{
-						if (!(wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-							mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-#else
-
-							if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm->current_state);
-
-#endif
-							break;
-
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
-							if (wpr.seq != 0) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-								mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
-#else
-
-								if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
-
-#endif
-							}
-
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
-							if (wpr.seq != wpm->current_wp_id && wpr.seq != wpm->current_wp_id + 1) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-								mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
-#else
 
-								if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1);
-
-#endif
-
-							} else if (wpr.seq >= wpm->size) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-								mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
-#else
-
-								if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
+					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
+					if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state);
+					break;
+				}
 
-#endif
-							}
+				wpm->current_wp_id = wpr.seq;
+				wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
 
-						} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-							mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?");
-#else
+				if (wpr.seq < wpm->size) {
 
-							if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n");
+					mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid,wpm->current_wp_id);
 
-#endif
-						}
-					}
+				} else {
+					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
+					if (verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq);
 				}
 
+
 			} else {
 				//we we're target but already communicating with someone else
 				if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-#else
 
-					if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm->current_partner_sysid);
-
-#endif
+					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
+					if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid);
 
 				} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-					mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-#else
 
-					if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
-
-#endif
+					mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
+					if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
 				}
-
 			}
-
 			break;
 		}
 
-	case MAVLINK_MSG_ID_MISSION_COUNT: {
+		case MAVLINK_MSG_ID_MISSION_COUNT: {
 			mavlink_mission_count_t wpc;
 			mavlink_msg_mission_count_decode(msg, &wpc);
 
 			if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
 				wpm->timestamp_lastaction = now;
 
-				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id == 0)) {
-//                	printf("wpc count in: %d\n",wpc.count);
-//                	printf("Comp id: %d\n",msg->compid);
-//                	printf("Current partner sysid: %d\n",wpm->current_partner_sysid);
-
-					if (wpc.count > 0) {
-						if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-							mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST");
-#else
-
-							if (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							
-						}
-
-						if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-							mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
-#else
-
-							if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
-
-#endif
-						}
-
-						wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
-						wpm->current_wp_id = 0;
-						wpm->current_partner_sysid = msg->sysid;
-						wpm->current_partner_compid = msg->compid;
-						wpm->current_count = wpc.count;
-
-						if (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
-
-						if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n");
-
-#endif
-						wpm->rcv_size = 0;
-						//while(waypoints_receive_buffer->size() > 0)
-//                        {
-//                            delete waypoints_receive_buffer->back();
-//                            waypoints_receive_buffer->pop_back();
-//                        }
+				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
 
-						mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
+					if (wpc.count > NUM_MISSIONS_SUPPORTED) {
+						if (verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
+						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE);
+						break;
+					}
 
-					} else if (wpc.count == 0) {
-#ifdef MAVLINK_WPM_NO_PRINTF
+					if (wpc.count == 0) {
 						mavlink_missionlib_send_gcs_string("COUNT 0");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n");
-
-#endif
-						wpm->rcv_size = 0;
-						//while(waypoints_receive_buffer->size() > 0)
-//                        {
-//                            delete waypoints->back();
-//                            waypoints->pop_back();
-//                        }
-						// wpm->current_active_wp_id = -1;
-						// wpm->yaw_reached = false;
-						// wpm->pos_reached = false;
+						if (verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE");
 						break;
-
-					} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("IGN WP CMD");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
-
-#endif
 					}
+					
+					if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid);
 
-				} else {
-					if (!(wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_GETLIST)) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm->current_state);
-
-#endif
-
-					} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id != 0) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-#else
+					wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
+					wpm->current_wp_id = 0;
+					wpm->current_partner_sysid = msg->sysid;
+					wpm->current_partner_compid = msg->compid;
+					wpm->current_count = wpc.count;
 
-						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm->current_wp_id);
+					mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
 
-#endif
+				} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
 
+					if (wpm->current_wp_id == 0) {
+						mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
+						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid);
 					} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n");
-
-#endif
+						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
+						if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id);
 					}
+				} else {
+						mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
+						if (verbose) warnx("IGN MISSION_COUNT CMD: Busy");
 				}
-
 			} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-#else
 
-				if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
-
-#endif
+				mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch");
+				if (verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
 			}
-
 		}
 		break;
 
-	case MAVLINK_MSG_ID_MISSION_ITEM: {
+		case MAVLINK_MSG_ID_MISSION_ITEM: {
 			mavlink_mission_item_t wp;
 			mavlink_msg_mission_item_decode(msg, &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);
-
-//            if((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/))
 			if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) {
 
 				wpm->timestamp_lastaction = now;
 
-//                printf("wpm->current_state=%u, wp.seq = %d, wpm->current_wp_id=%d\n", wpm->current_state, wp.seq, wpm->current_wp_id);
-
-//                wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO
+				/*
+				 * ensure that we are in the correct state and that the first waypoint has id 0
+				 * and the following waypoints have the correct ids
+				 */
 
-				//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
-				if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) ||
-				    (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id &&
-				    wp.seq < wpm->current_count)) {
-					//mavlink_missionlib_send_gcs_string("DEBUG 2");
+				if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
 
-//                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid);
-//                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid);
-//                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid);
-//
-					wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
-					// mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]);
-					// memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
+				 	if (wp.seq != 0) {
+				 		mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0");
+				 		warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq);
+				 		break;
+				 	}
+				} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
 
-					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;
+					if (wp.seq >= wpm->current_count) {
+						mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds");
+						warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq);
 						break;
 					}
 
-					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;
+					if (wp.seq != wpm->current_wp_id) {
+						warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, wpm->current_wp_id);
+						mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
 						break;
 					}
+				}
 
-					if (wp.current) {
-						mission.current_index = wp.seq;
-					}
+				wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
 
-					wpm->current_wp_id = wp.seq + 1;
+				struct mission_item_s mission_item;
 
-					// if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
-//					printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
+				int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item);
 
-//					printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count);
-					if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-						// mavlink_missionlib_send_gcs_string("GOT ALL WPS");
-						// if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_count);
+				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;
+				}
 
-						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0);
+				ssize_t len = sizeof(struct mission_item_s);
 
-						// if (wpm->current_active_wp_id > wpm->rcv_size - 1) {
-						// 	wpm->current_active_wp_id = wpm->rcv_size - 1;
-						// }
+				dm_item_t dm_next;
 
-						// bool copy_error = false;
+				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;
+				}
 
-						// // switch the waypoints list
-						// // FIXME CHECK!!!
-						// uint32_t i;
+				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;
+				}
 
-						// 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;
-						// 	}
+				if (wp.current) {
+					mission.current_index = wp.seq;
+				}
 
-						// }
-						// TODO: update count?
+				wpm->current_wp_id = wp.seq + 1;
 
+				if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
+					
+					if (verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count);
 
-						mission.count = wpm->current_count;
-						
-						publish_mission();
+					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
 
-						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;
-						// }
+					mission.count = wpm->current_count;
+					
+					publish_mission();
 
-						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
+					wpm->current_dataman_id = mission.dataman_id;
+					wpm->size = wpm->current_count;
 
-					} else {
-						mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
-					}
+					wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 
 				} else {
-
-					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);
-// 						}
-// 					}
+					mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
 				}
+
 			} else {
-				//we we're target but already communicating with someone else
-				if ((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
-					// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm->current_partner_sysid);
-				} else if (wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/) {
-					// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
-				}
+				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
+				if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
 			}
 
 			break;
 		}
 
-	case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
+		case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
 			mavlink_mission_clear_all_t wpca;
 			mavlink_msg_mission_clear_all_decode(msg, &wpca);
 
-			if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-				wpm->timestamp_lastaction = now;
+			if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
 
-				// if (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;
+				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
+					wpm->timestamp_lastaction = now;
 
-				/* prepare mission topic */
-				mission.dataman_id = -1;
-				mission.count = 0;
-				mission.current_index = -1;
+					wpm->size = 0;
 
-				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);
+					/* prepare mission topic */
+					mission.dataman_id = -1;
+					mission.count = 0;
+					mission.current_index = -1;
+					publish_mission();
+
+					if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) {
+						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
+					} else {
+						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
+					}
+
+					
 				} else {
-					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
+					mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy");
+					if (verbose) warnx("IGN WP CLEAR CMD: Busy");
 				}
 
-				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");
+
+				mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch");
+				if (verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
 			}
 
 			break;
 		}
 
 	default: {
-			// if (debug) // printf("Waypoint: received message of unknown type");
+			/* other messages might should get caught by mavlink and others */
 			break;
 		}
 	}
-
-	// check_waypoints_reached(now, global_pos, local_pos);
 }
diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
index 801bc0bcf..f8b58c7d9 100644
--- a/src/modules/mavlink/waypoints.h
+++ b/src/modules/mavlink/waypoints.h
@@ -46,19 +46,10 @@
  or in the same folder as this source file */
 
 #include <v1.0/mavlink_types.h>
-
-// #ifndef MAVLINK_SEND_UART_BYTES
-// #define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
-// #endif
-//extern mavlink_system_t mavlink_system;
 #include "mavlink_bridge_header.h"
 #include <stdbool.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/navigation_capabilities.h>
 #include <uORB/topics/mission.h>
 
-// FIXME XXX - TO BE MOVED TO XML
 enum MAVLINK_WPM_STATES {
 	MAVLINK_WPM_STATE_IDLE = 0,
 	MAVLINK_WPM_STATE_SENDLIST,
@@ -79,44 +70,24 @@ enum MAVLINK_WPM_CODES {
 };
 
 
-/* WAYPOINT MANAGER - MISSION LIB */
-
-#define MAVLINK_WPM_MAX_WP_COUNT 15
-// #define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE				  ///< Enable double buffer and in-flight updates
-#ifndef MAVLINK_WPM_TEXT_FEEDBACK
-#define MAVLINK_WPM_TEXT_FEEDBACK 0						  ///< Report back status information as text
-#endif
+#define MAVLINK_WPM_MAX_WP_COUNT 255
 #define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000         ///< Protocol communication timeout in useconds
 #define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000           ///< When to send a new setpoint
 #define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000
 
 
 struct mavlink_wpm_storage {
-	mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT];      ///< Currently active waypoints
-// #ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
-	// mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT];  ///< Receive buffer for next waypoints
-// #endif
 	uint16_t size;
 	uint16_t max_size;
-	uint16_t rcv_size;
 	enum MAVLINK_WPM_STATES current_state;
 	int16_t current_wp_id;							///< Waypoint in current transmission
-	// int16_t current_active_wp_id;					///< Waypoint the system is currently heading towards
 	uint16_t current_count;
 	uint8_t current_partner_sysid;
 	uint8_t current_partner_compid;
 	uint64_t timestamp_lastaction;
-	// uint64_t timestamp_last_send_setpoint;
-	// uint64_t timestamp_firstinside_orbit;
-	// uint64_t timestamp_lastoutside_orbit;
+	uint64_t timestamp_last_send_setpoint;
 	uint32_t timeout;
 	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;
@@ -126,13 +97,16 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio
 
 
 void mavlink_wpm_init(mavlink_wpm_storage *state);
-int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position,
-			       struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap);
-void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos ,
-				 struct vehicle_local_position_s *local_pos);
+void mavlink_waypoint_eventloop(uint64_t now);
+void mavlink_wpm_message_handler(const mavlink_message_t *msg);
 
 extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		float param2, float param3, float param4, float param5_lat_x,
 		float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
 
+static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
+
+void mavlink_missionlib_send_message(mavlink_message_t *msg);
+int mavlink_missionlib_send_gcs_string(const char *string);
+
 #endif /* WAYPOINTS_H_ */
diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c
index 0edb53a3e..ab9ce45f3 100644
--- a/src/modules/mavlink_onboard/mavlink.c
+++ b/src/modules/mavlink_onboard/mavlink.c
@@ -441,7 +441,8 @@ int mavlink_thread_main(int argc, char *argv[])
 			get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode);
 
 			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
+			// TODO fix navigation state, use control_mode topic
+			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, 0, mavlink_state);
 
 			/* send status (values already copied in the section above) */
 			mavlink_msg_sys_status_send(chan,
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
index 60a211817..111e9197f 100644
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c
@@ -316,7 +316,7 @@ mc_thread_main(int argc, char *argv[])
 				}
 
 			} else {
-				if (!control_mode.flag_control_auto_enabled) {
+				if (!control_mode.flag_control_attitude_enabled) {
 					/* no control, try to stay on place */
 					if (!control_mode.flag_control_velocity_enabled) {
 						/* no velocity control, reset attitude setpoint */
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..2ca650420
--- /dev/null
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -0,0 +1,553 @@
+/****************************************************************************
+ *
+ *   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;
+			}
+			/* AUTO not implemented */
+
+			/* 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/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 7be9229c9..0a0ee2541 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -61,6 +61,7 @@
 #include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
@@ -145,8 +146,10 @@ private:
 	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 */
+	orb_advert_t	_control_mode_pub;			/**< publish vehicle control mode topic */
 
 	struct vehicle_status_s				_vstatus;		/**< vehicle status */
+	struct vehicle_control_mode_s		_control_mode;		/**< vehicle control mode */
 	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
 	struct home_position_s				_home_pos;		/**< home position for RTL */
 	struct mission_item_triplet_s			_mission_item_triplet;	/**< triplet of mission items */
@@ -168,6 +171,8 @@ private:
 
 	MissionFeasibilityChecker missionFeasiblityChecker;
 
+	uint64_t	_set_nav_state_timestamp;		/**< timestamp of last handled navigation state request */
+
 	struct {
 		float min_altitude;
 		float loiter_radius;
@@ -192,21 +197,10 @@ private:
 		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];
+	static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT];
 
 	/**
 	 * Update our local parameter cache.
@@ -300,6 +294,10 @@ private:
 	 */
 	void		publish_mission_item_triplet();
 
+	/**
+	 * Publish vehicle_control_mode topic for controllers
+	 */
+	void		publish_control_mode();
 
 
 	/**
@@ -328,7 +326,7 @@ Navigator	*g_navigator;
 Navigator::Navigator() : 
 
 /* state machine transition table */
-	StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT),
+	StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
 
 	_task_should_exit(false),
 	_navigator_task(-1),
@@ -347,6 +345,7 @@ Navigator::Navigator() :
 	_triplet_pub(-1),
 	_fence_pub(-1),
 	_mission_result_pub(-1),
+	_control_mode_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@@ -357,7 +356,8 @@ Navigator::Navigator() :
 	_mission(),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
-	_time_first_inside_orbit(0)
+	_time_first_inside_orbit(0),
+	_set_nav_state_timestamp(0)
 {
 	memset(&_fence, 0, sizeof(_fence));
 
@@ -375,7 +375,7 @@ Navigator::Navigator() :
 	memset(&_mission_result, 0, sizeof(struct mission_result_s));
 
 	/* Initialize state machine */
-	myState = STATE_INIT;
+	myState = NAV_STATE_INIT;
 }
 
 Navigator::~Navigator()
@@ -513,7 +513,6 @@ Navigator::task_main()
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
-
 	
 	/* copy all topics first time */
 	vehicle_status_update();
@@ -524,9 +523,6 @@ Navigator::task_main()
 	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);
 
@@ -575,49 +571,41 @@ Navigator::task_main()
 
 			/* 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);
-						}
+				if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
+					/* commander requested new navigation mode, try to set it */
+					_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
+
+					switch (_vstatus.set_nav_state) {
+					case NAV_STATE_INIT:
+					case NAV_STATE_NONE:
+						/* nothing to do */
 						break;
 
-					case NAVIGATION_STATE_AUTO_LOITER:
-
+					case NAV_STATE_LOITER:
 						dispatch(EVENT_LOITER_REQUESTED);
 						break;
 
-
-					case NAVIGATION_STATE_AUTO_RTL:
-						
-						dispatch(EVENT_RTL_REQUESTED);
+					case NAV_STATE_MISSION:
+						dispatch(EVENT_MISSION_REQUESTED);
 						break;
 
-					case NAVIGATION_STATE_AUTO_LAND:
-
-						/* TODO add this */
-						
+					case NAV_STATE_RTL:
+						dispatch(EVENT_RTL_REQUESTED);
 						break;
 
 					default:
-						warnx("ERROR: Navigation state not supported");
+						warnx("ERROR: Requested navigation state not supported");
 						break;
+					}
+
+				} else {
+					/* 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;
 				}
 
 			} else {
@@ -632,10 +620,8 @@ Navigator::task_main()
 			}
 		}
 
-
 		/* only update parameters if it changed */
 		if (fds[0].revents & POLLIN) {
-
 			parameters_update();
 			/* note that these new parameters won't be in effect until a mission triplet is published again */
 		}
@@ -670,9 +656,7 @@ Navigator::task_main()
 			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)) {
+				    (myState == NAV_STATE_MISSION)) {
 
 					/* advance by one mission item */
 					_mission.move_to_next();
@@ -688,6 +672,9 @@ Navigator::task_main()
 				}
 			}
 		}
+
+		publish_control_mode();
+
 		perf_end(_loop_perf);
 	}
 
@@ -740,25 +727,25 @@ Navigator::status()
 	}
 
 	switch (myState) {
-		case STATE_INIT:
+		case NAV_STATE_INIT:
 			warnx("State: Init");
 			break;
-		case STATE_NONE:
+		case NAV_STATE_NONE:
 			warnx("State: None");
 			break;
-		case STATE_LOITER:
+		case NAV_STATE_LOITER:
 			warnx("State: Loiter");
 			break;
-		case STATE_MISSION:
+		case NAV_STATE_MISSION:
 			warnx("State: Mission");
 			break;
-		case STATE_MISSION_LOITER:
+		case NAV_STATE_MISSION_LOITER:
 			warnx("State: Loiter after Mission");
 			break;
-		case STATE_RTL:
+		case NAV_STATE_RTL:
 			warnx("State: RTL");
 			break;
-		case STATE_RTL_LOITER:
+		case NAV_STATE_RTL_LOITER:
 			warnx("State: Loiter after RTL");
 			break;
 		default:
@@ -857,76 +844,76 @@ Navigator::fence_point(int argc, char *argv[])
 
 
 
-StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = {
+StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][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},
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
+		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_MISSION_FINISHED */		{NO_ACTION, NAV_STATE_INIT},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_INIT},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_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},
+		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_NONE},
+		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_MISSION_FINISHED */		{NO_ACTION, NAV_STATE_NONE},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_NONE},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_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},
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
+		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_MISSION_FINISHED */		{NO_ACTION, NAV_STATE_LOITER},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_LOITER},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_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},
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
+		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, NAV_STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_MISSION_FINISHED */		{ACTION(&Navigator::start_mission_loiter), NAV_STATE_MISSION_LOITER},
+		/* EVENT_MISSION_CHANGED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_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},
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
+		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_MISSION_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, NAV_STATE_MISSION_LOITER},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_MISSION_FINISHED */		{NO_ACTION, NAV_STATE_MISSION_LOITER},
+		/* EVENT_MISSION_CHANGED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_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},
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
+		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_RTL},
+		/* EVENT_MISSION_FINISHED */		{ACTION(&Navigator::start_rtl_loiter), NAV_STATE_RTL_LOITER},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_RTL},
+		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_loiter), NAV_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},
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
+		/* EVENT_LOITER_REQUESTED */		{NO_ACTION,  NAV_STATE_RTL_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_RTL_LOITER},
+		/* EVENT_MISSION_FINISHED */		{NO_ACTION, NAV_STATE_RTL_LOITER},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_RTL_LOITER},
+		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
 	},
 };
 
@@ -949,7 +936,7 @@ Navigator::start_loiter()
 
 	_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;
+	_mission_item_triplet.current.yaw = 0.0f;	// TODO use current yaw sp here or set to undefined?
 
 	get_loiter_item(&_mission_item_triplet.current);
 
@@ -990,9 +977,8 @@ Navigator::start_mission()
 			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 */
+		/* since a mission is started without knowledge if there are more mission items available, this can fail */
 		_mission_item_triplet.current_valid = false;
-		warnx("ERROR: current WP can't be set");
 	}
 
 	ret = _mission.get_next_mission_item(&_mission_item_triplet.next);
@@ -1239,6 +1225,76 @@ Navigator::publish_mission_item_triplet()
 	}
 }
 
+void
+Navigator::publish_control_mode()
+{
+	/* update vehicle_control_mode topic*/
+	_control_mode.main_state = _vstatus.main_state;
+	_control_mode.nav_state = static_cast<nav_state_t>(myState);
+	_control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR;
+	_control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing;
+	_control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON;
+
+	_control_mode.flag_control_offboard_enabled = false;
+	_control_mode.flag_control_flighttermination_enabled = false;
+
+	switch (_vstatus.main_state) {
+	case MAIN_STATE_MANUAL:
+		_control_mode.flag_control_manual_enabled = true;
+		_control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing;
+		_control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing;
+		_control_mode.flag_control_altitude_enabled = false;
+		_control_mode.flag_control_climb_rate_enabled = false;
+		_control_mode.flag_control_position_enabled = false;
+		_control_mode.flag_control_velocity_enabled = false;
+		break;
+
+	case MAIN_STATE_SEATBELT:
+		_control_mode.flag_control_manual_enabled = true;
+		_control_mode.flag_control_rates_enabled = true;
+		_control_mode.flag_control_attitude_enabled = true;
+		_control_mode.flag_control_altitude_enabled = true;
+		_control_mode.flag_control_climb_rate_enabled = true;
+		_control_mode.flag_control_position_enabled = false;
+		_control_mode.flag_control_velocity_enabled = false;
+		break;
+
+	case MAIN_STATE_EASY:
+		_control_mode.flag_control_manual_enabled = true;
+		_control_mode.flag_control_rates_enabled = true;
+		_control_mode.flag_control_attitude_enabled = true;
+		_control_mode.flag_control_altitude_enabled = true;
+		_control_mode.flag_control_climb_rate_enabled = true;
+		_control_mode.flag_control_position_enabled = true;
+		_control_mode.flag_control_velocity_enabled = true;
+		break;
+
+	case MAIN_STATE_AUTO:
+		_control_mode.flag_control_manual_enabled = false;
+		_control_mode.flag_control_rates_enabled = true;
+		_control_mode.flag_control_attitude_enabled = true;
+		_control_mode.flag_control_position_enabled = true;
+		_control_mode.flag_control_velocity_enabled = true;
+		_control_mode.flag_control_altitude_enabled = true;
+		_control_mode.flag_control_climb_rate_enabled = true;
+		break;
+
+	default:
+		break;
+	}
+
+	_control_mode.timestamp = hrt_absolute_time();
+
+	/* lazily publish the mission triplet only once available */
+	if (_control_mode_pub > 0) {
+		/* publish the mission triplet */
+		orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode);
+
+	} else {
+		/* advertise and publish */
+		_control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode);
+	}
+}
 
 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 &&
@@ -1251,11 +1307,9 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const
 	    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) {
+	    fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON) {
 		return true;
 	} else {
-		warnx("a.index %d, b.index %d", a.index, b.index);
 		return false;
 	}
 }
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 916b893c4..752960bda 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -582,6 +582,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			/* this option is normally set last */
 			if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
 				uint8_t count = 0;
+				bool disabled = false;
 
 				/* assert min..center..max ordering */
 				if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
@@ -600,7 +601,10 @@ 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_RC_MAPPED_CONTROL_CHANNELS) {
+
+				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
+					disabled = true;
+				} else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
 					count++;
 				}
 
@@ -608,7 +612,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				if (count) {
 					isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
 					r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
-				} else {
+				} else if (!disabled) {
 					conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
 				}
 			}
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index c6648de05..10404211e 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -973,7 +973,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 				// Don't orb_copy, it's already done few lines above
 				log_msg.msg_type = LOG_STAT_MSG;
 				log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
-				log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
+				// TODO use control_mode topic
+				//log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
 				log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
 				log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
 				log_msg.body.log_STAT.battery_current = buf_status.battery_current;
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index c54128cb5..763723554 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -104,49 +104,49 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
 PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
 PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f);
+PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f);
 
 PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f);
+PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f);
 
 PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC3_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC3_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC3_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f);
+PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f);
 
 PARAM_DEFINE_FLOAT(RC4_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC4_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC4_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f);
+PARAM_DEFINE_FLOAT(RC4_DZ, 10.0f);
 
 PARAM_DEFINE_FLOAT(RC5_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC5_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC5_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC5_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC5_DZ,  0.0f);
+PARAM_DEFINE_FLOAT(RC5_DZ,  10.0f);
 
 PARAM_DEFINE_FLOAT(RC6_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC6_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC6_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC6_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f);
+PARAM_DEFINE_FLOAT(RC6_DZ, 10.0f);
 
 PARAM_DEFINE_FLOAT(RC7_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC7_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC7_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC7_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f);
+PARAM_DEFINE_FLOAT(RC7_DZ, 10.0f);
 
 PARAM_DEFINE_FLOAT(RC8_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC8_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC8_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC8_REV, 1.0f);
-PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f);
+PARAM_DEFINE_FLOAT(RC8_DZ, 10.0f);
 
 PARAM_DEFINE_FLOAT(RC9_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC9_TRIM, 1500);
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index dcdb234fa..9b4250115 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -91,7 +91,6 @@ struct mission_item_s
 	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		*/
 };
 
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index e26fb97c8..5ce968f67 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -48,6 +48,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
+#include "vehicle_status.h"
 
 /**
  * @addtogroup topics @{
@@ -59,10 +60,25 @@
  *
  * Encodes the complete system state and is set by the commander app.
  */
+
+typedef enum {
+	NAV_STATE_INIT = 0,
+	NAV_STATE_NONE,
+	NAV_STATE_LOITER,
+	NAV_STATE_MISSION,
+	NAV_STATE_MISSION_LOITER,
+	NAV_STATE_RTL,
+	NAV_STATE_RTL_LOITER,
+	NAV_STATE_MAX
+} nav_state_t;
+
 struct vehicle_control_mode_s
 {
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
+	main_state_t main_state;
+	nav_state_t nav_state;
+
 	bool flag_armed;
 
 	bool flag_external_manual_override_ok;	/**< external override non-fatal for system. Only true for fixed wing */
@@ -79,9 +95,6 @@ struct vehicle_control_mode_s
 	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 1c3763924..ae3a24a1b 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -66,20 +66,6 @@ typedef enum {
 	MAIN_STATE_AUTO,
 } main_state_t;
 
-/* navigation state machine */
-typedef enum {
-	NAVIGATION_STATE_DIRECT = 0,		// true manual control, no any stabilization
-	NAVIGATION_STATE_STABILIZE,		// attitude stabilization
-	NAVIGATION_STATE_ALTHOLD,		// attitude + altitude stabilization
-	NAVIGATION_STATE_VECTOR,		// attitude + altitude + position stabilization
-	NAVIGATION_STATE_AUTO_READY,	// AUTO, landed, reeady for takeoff
-	NAVIGATION_STATE_AUTO_TAKEOFF,	// detect takeoff using land detector and switch to desired AUTO mode
-	NAVIGATION_STATE_AUTO_LOITER,	// pause mission
-	NAVIGATION_STATE_AUTO_MISSION,	// fly mission
-	NAVIGATION_STATE_AUTO_RTL,		// Return To Launch, when home position switch to LAND
-	NAVIGATION_STATE_AUTO_LAND		// land and switch to AUTO_READY when landed (detect using land detector)
-} navigation_state_t;
-
 typedef enum {
 	ARMING_STATE_INIT = 0,
 	ARMING_STATE_STANDBY,
@@ -95,7 +81,6 @@ typedef enum {
 	HIL_STATE_ON
 } hil_state_t;
 
-
 typedef enum {
 	FLIGHTTERMINATION_STATE_OFF = 0,
 	FLIGHTTERMINATION_STATE_ON
@@ -182,7 +167,8 @@ struct vehicle_status_s
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
 	main_state_t main_state;				/**< main state machine */
-	navigation_state_t navigation_state;	/**< navigation state machine */
+	unsigned int set_nav_state;	/**< set navigation state machine to specified value */
+	uint64_t set_nav_state_timestamp;	/**< timestamp of latest change of set_nav_state */
 	arming_state_t arming_state;			/**< current arming state */
 	hil_state_t hil_state;					/**< current hil state */