Merge branch 'master' into vector_control

10 files changed, 551 insertions, 527 deletions

diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 919d01561..7c10e297b 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -64,6 +64,7 @@
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
 #include <mavlink/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
 
 #include "waypoints.h"
 #include "orb_topics.h"
@@ -181,102 +182,80 @@ set_hil_on_off(bool hil_enabled)
 }
 
 void
-get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
+get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
 {
 	/* reset MAVLink mode bitfield */
-	*mavlink_mode = 0;
+	*mavlink_base_mode = 0;
+	*mavlink_custom_mode = 0;
 
-	/* set mode flags independent of system state */
+	/**
+	 * Set mode flags
+	 **/
 
 	/* HIL */
-	if (v_status.flag_hil_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+	if (v_status.hil_state == HIL_STATE_ON) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 	}
 
-	/* manual input */
-	if (v_status.flag_control_manual_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+	/* arming state */
+	if (v_status.arming_state == ARMING_STATE_ARMED
+			|| v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
-	/* attitude or rate control */
-	if (v_status.flag_control_attitude_enabled ||
-	    v_status.flag_control_rates_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-	}
-
-	/* vector control */
-	if (v_status.flag_control_velocity_enabled ||
-	    v_status.flag_control_position_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
-	}
-
-	/* autonomous mode */
-	if (v_status.state_machine == SYSTEM_STATE_AUTO) {
-		*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
-	}
-
-	/* set arming state */
-	if (armed.armed) {
-		*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-
-	} else {
-		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
-	}
-
-	switch (v_status.state_machine) {
-	case SYSTEM_STATE_PREFLIGHT:
-		if (v_status.flag_preflight_gyro_calibration ||
-		    v_status.flag_preflight_mag_calibration ||
-		    v_status.flag_preflight_accel_calibration) {
-			*mavlink_state = MAV_STATE_CALIBRATING;
-
-		} else {
-			*mavlink_state = MAV_STATE_UNINIT;
+	/* main state */
+	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+	union px4_custom_mode custom_mode;
+	custom_mode.data = 0;
+	if (v_status.main_state == MAIN_STATE_MANUAL) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+	} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
+	} else if (v_status.main_state == MAIN_STATE_EASY) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
+	} else if (v_status.main_state == MAIN_STATE_AUTO) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
 		}
-
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		*mavlink_state = MAV_STATE_STANDBY;
-		break;
-
-	case SYSTEM_STATE_GROUND_READY:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_STABILIZED:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_AUTO:
+	}
+	*mavlink_custom_mode = custom_mode.data;
+
+	/**
+	 * Set mavlink state
+	 **/
+
+	/* set calibration state */
+	if (v_status.arming_state == ARMING_STATE_INIT
+			|| v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
+			|| v_status.arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
+		*mavlink_state = MAV_STATE_UNINIT;
+	} else if (v_status.arming_state == ARMING_STATE_ARMED) {
 		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_MISSION_ABORT:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_LANDING:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_CUTOFF:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_GROUND_ERROR:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_REBOOT:
+	} else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+		*mavlink_state = MAV_STATE_CRITICAL;
+	} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
+		*mavlink_state = MAV_STATE_STANDBY;
+	} else if (v_status.arming_state == ARMING_STATE_REBOOT) {
 		*mavlink_state = MAV_STATE_POWEROFF;
-		break;
+	} else {
+		warnx("Unknown mavlink state");
+		*mavlink_state = MAV_STATE_CRITICAL;
 	}
-
 }
 
 
@@ -312,6 +291,9 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m
 		orb_set_interval(subs->act_2_sub, min_interval);
 		orb_set_interval(subs->act_3_sub, min_interval);
 		orb_set_interval(subs->actuators_sub, min_interval);
+		orb_set_interval(subs->actuators_effective_sub, min_interval);
+		orb_set_interval(subs->spa_sub, min_interval);
+		orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval);
 		break;
 
 	case MAVLINK_MSG_ID_MANUAL_CONTROL:
@@ -534,7 +516,7 @@ void mavlink_update_system(void)
 int mavlink_thread_main(int argc, char *argv[])
 {
 	/* initialize mavlink text message buffering */
-	mavlink_logbuffer_init(&lb, 5);
+	mavlink_logbuffer_init(&lb, 10);
 
 	int ch;
 	char *device_name = "/dev/ttyS1";
@@ -549,7 +531,7 @@ int mavlink_thread_main(int argc, char *argv[])
 		case 'b':
 			baudrate = strtoul(optarg, NULL, 10);
 
-			if (baudrate == 0)
+			if (baudrate < 9600 || baudrate > 921600)
 				errx(1, "invalid baud rate '%s'", optarg);
 
 			break;
@@ -568,6 +550,7 @@ int mavlink_thread_main(int argc, char *argv[])
 
 		default:
 			usage();
+			break;
 		}
 	}
 
@@ -674,23 +657,27 @@ int mavlink_thread_main(int argc, char *argv[])
 
 			/* translate the current system state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
-			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+			uint8_t mavlink_base_mode = 0;
+			uint32_t mavlink_custom_mode = 0;
+			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
+			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
 
 			/* switch HIL mode if required */
-			set_hil_on_off(v_status.flag_hil_enabled);
+			if (v_status.hil_state == HIL_STATE_ON)
+				set_hil_on_off(true);
+			else if (v_status.hil_state == HIL_STATE_OFF)
+				set_hil_on_off(false);
 
 			/* send status (values already copied in the section above) */
 			mavlink_msg_sys_status_send(chan,
 						    v_status.onboard_control_sensors_present,
 						    v_status.onboard_control_sensors_enabled,
 						    v_status.onboard_control_sensors_health,
-						    v_status.load,
-						    v_status.voltage_battery * 1000.0f,
-						    v_status.current_battery * 1000.0f,
+						    v_status.load * 1000.0f,
+						    v_status.battery_voltage * 1000.0f,
+						    v_status.battery_current * 1000.0f,
 						    v_status.battery_remaining,
 						    v_status.drop_rate_comm,
 						    v_status.errors_comm,
@@ -703,25 +690,25 @@ int mavlink_thread_main(int argc, char *argv[])
 
 		lowspeed_counter++;
 
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
 
 		/* 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);
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
 
 		/* 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);
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
 
 		/* sleep quarter the time */
 		usleep(25000);
 
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
 
 		if (baudrate > 57600) {
 			mavlink_pm_queued_send();
@@ -751,9 +738,12 @@ int mavlink_thread_main(int argc, char *argv[])
 	/* Reset the UART flags to original state */
 	tcsetattr(uart, TCSANOW, &uart_config_original);
 
+	/* destroy log buffer */
+	//mavlink_logbuffer_destroy(&lb);
+
 	thread_running = false;
 
-	exit(0);
+	return 0;
 }
 
 static void
@@ -777,7 +767,7 @@ int mavlink_main(int argc, char *argv[])
 
 		/* this is not an error */
 		if (thread_running)
-			errx(0, "mavlink already running\n");
+			errx(0, "mavlink already running");
 
 		thread_should_exit = false;
 		mavlink_task = task_spawn_cmd("mavlink",
@@ -786,15 +776,25 @@ int mavlink_main(int argc, char *argv[])
 					  2048,
 					  mavlink_thread_main,
 					  (const char **)argv);
+
+		while (!thread_running) {
+			usleep(200);
+		}
+
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
+
+		/* this is not an error */
+		if (!thread_running)
+			errx(0, "mavlink already stopped");
+
 		thread_should_exit = true;
 
 		while (thread_running) {
 			usleep(200000);
-			printf(".");
+			warnx(".");
 		}
 
 		warnx("terminated.");
diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c
deleted file mode 100644
index 192195856..000000000
--- a/src/modules/mavlink/mavlink_log.c
+++ /dev/null
@@ -1,129 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 mavlink_log.c
- * MAVLink text logging.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include <string.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdarg.h>
-
-#include <mavlink/mavlink_log.h>
-
-static FILE* text_recorder_fd = NULL;
-
-void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
-{
-	lb->size  = size;
-	lb->start = 0;
-	lb->count = 0;
-	lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage));
-	text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w");
-}
-
-int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
-{
-	return lb->count == (int)lb->size;
-}
-
-int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
-{
-	return lb->count == 0;
-}
-
-void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
-{
-	int end = (lb->start + lb->count) % lb->size;
-	memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage));
-
-	if (mavlink_logbuffer_is_full(lb)) {
-		lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
-
-	} else {
-		++lb->count;
-	}
-}
-
-int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
-{
-	if (!mavlink_logbuffer_is_empty(lb)) {
-		memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage));
-		lb->start = (lb->start + 1) % lb->size;
-		--lb->count;
-
-		if (text_recorder_fd) {
-			fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd);
-			fputc("\n", text_recorder_fd);
-			fsync(text_recorder_fd);
-		}
-
-		return 0;
-
-	} else {
-		return 1;
-	}
-}
-
-void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
-{
-	va_list ap;
-	va_start(ap, fmt);
-	int end = (lb->start + lb->count) % lb->size;
-	lb->elems[end].severity = severity;
-	vsnprintf(lb->elems[end].text, sizeof(lb->elems[0].text), fmt, ap);
-	va_end(ap);
-
-	/* increase count */
-	if (mavlink_logbuffer_is_full(lb)) {
-		lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
-
-	} else {
-		++lb->count;
-	}
-}
-
-__EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...)
-{
-	va_list ap;
-	va_start(ap, fmt);
-	char text[MAVLINK_LOG_MAXLEN + 1];
-	vsnprintf(text, sizeof(text), fmt, ap);
-	va_end(ap);
-	ioctl(_fd, severity, (unsigned long)&text[0]);
-}
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 01bbabd46..c51a6de08 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -50,6 +50,10 @@
 #include <mqueue.h>
 #include <string.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
@@ -67,6 +71,7 @@
 #include <systemlib/err.h>
 #include <systemlib/airspeed.h>
 #include <mavlink/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
 
 __BEGIN_DECLS
 
@@ -97,11 +102,22 @@ struct vehicle_global_position_s hil_global_pos;
 struct vehicle_attitude_s hil_attitude;
 struct vehicle_gps_position_s hil_gps;
 struct sensor_combined_s hil_sensors;
+struct battery_status_s	hil_battery_status;
 static orb_advert_t pub_hil_global_pos = -1;
 static orb_advert_t pub_hil_attitude = -1;
 static orb_advert_t pub_hil_gps = -1;
 static orb_advert_t pub_hil_sensors = -1;
+static orb_advert_t pub_hil_gyro = -1;
+static orb_advert_t pub_hil_accel = -1;
+static orb_advert_t pub_hil_mag = -1;
+static orb_advert_t pub_hil_baro = -1;
 static orb_advert_t pub_hil_airspeed = -1;
+static orb_advert_t pub_hil_battery = -1;
+
+/* packet counter */
+static int hil_counter = 0;
+static int hil_frames = 0;
+static uint64_t old_timestamp = 0;
 
 static orb_advert_t cmd_pub = -1;
 static orb_advert_t flow_pub = -1;
@@ -140,7 +156,8 @@ handle_message(mavlink_message_t *msg)
 				vcmd.param5 = cmd_mavlink.param5;
 				vcmd.param6 = cmd_mavlink.param6;
 				vcmd.param7 = cmd_mavlink.param7;
-				vcmd.command = cmd_mavlink.command;
+				// XXX do proper translation
+				vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
 				vcmd.target_system = cmd_mavlink.target_system;
 				vcmd.target_component = cmd_mavlink.target_component;
 				vcmd.source_system = msg->sysid;
@@ -150,6 +167,7 @@ handle_message(mavlink_message_t *msg)
 				/* check if topic is advertised */
 				if (cmd_pub <= 0) {
 					cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+
 				} else {
 					/* publish */
 					orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
@@ -188,15 +206,17 @@ handle_message(mavlink_message_t *msg)
 		mavlink_set_mode_t new_mode;
 		mavlink_msg_set_mode_decode(msg, &new_mode);
 
+		union px4_custom_mode custom_mode;
+		custom_mode.data = new_mode.custom_mode;
 		/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
 		vcmd.param1 = new_mode.base_mode;
-		vcmd.param2 = new_mode.custom_mode;
+		vcmd.param2 = custom_mode.main_mode;
 		vcmd.param3 = 0;
 		vcmd.param4 = 0;
 		vcmd.param5 = 0;
 		vcmd.param6 = 0;
 		vcmd.param7 = 0;
-		vcmd.command = MAV_CMD_DO_SET_MODE;
+		vcmd.command = VEHICLE_CMD_DO_SET_MODE;
 		vcmd.target_system = new_mode.target_system;
 		vcmd.target_component = MAV_COMP_ID_ALL;
 		vcmd.source_system = msg->sysid;
@@ -349,11 +369,6 @@ handle_message(mavlink_message_t *msg)
 			mavlink_hil_sensor_t imu;
 			mavlink_msg_hil_sensor_decode(msg, &imu);
 
-			/* packet counter */
-			static uint16_t hil_counter = 0;
-			static uint16_t hil_frames = 0;
-			static uint64_t old_timestamp = 0;
-
 			/* sensors general */
 			hil_sensors.timestamp = hrt_absolute_time();
 
@@ -380,9 +395,9 @@ handle_message(mavlink_message_t *msg)
 			hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
 
 			/* adc */
-			hil_sensors.adc_voltage_v[0] = 0;
-			hil_sensors.adc_voltage_v[1] = 0;
-			hil_sensors.adc_voltage_v[2] = 0;
+			hil_sensors.adc_voltage_v[0] = 0.0f;
+			hil_sensors.adc_voltage_v[1] = 0.0f;
+			hil_sensors.adc_voltage_v[2] = 0.0f;
 
 			/* magnetometer */
 			float mga2ga = 1.0e-3f;
@@ -407,41 +422,140 @@ handle_message(mavlink_message_t *msg)
 			hil_sensors.accelerometer_counter = hil_counter;
 
 			/* differential pressure */
-			hil_sensors.differential_pressure_pa = imu.diff_pressure;
+			hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
 			hil_sensors.differential_pressure_counter = hil_counter;
 
 			/* airspeed from differential pressure, ambient pressure and temp */
 			struct airspeed_s airspeed;
-			airspeed.timestamp = hrt_absolute_time();
 
-			float ias = calc_indicated_airspeed(imu.diff_pressure);
+			float ias = calc_indicated_airspeed(hil_sensors.differential_pressure_pa);
 			// XXX need to fix this
 			float tas = ias;
 
+			airspeed.timestamp = hrt_absolute_time();
 			airspeed.indicated_airspeed_m_s = ias;
 			airspeed.true_airspeed_m_s = tas;
 
 			if (pub_hil_airspeed < 0) {
 				pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
+
 			} else {
 				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
+
 			//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
 
-			/* publish */
+			/* individual sensor publications */
+			struct gyro_report gyro;
+			gyro.x_raw = imu.xgyro / mrad2rad;
+			gyro.y_raw = imu.ygyro / mrad2rad;
+			gyro.z_raw = imu.zgyro / mrad2rad;
+			gyro.x = imu.xgyro;
+			gyro.y = imu.ygyro;
+			gyro.z = imu.zgyro;
+			gyro.temperature = imu.temperature;
+			gyro.timestamp = hrt_absolute_time();
+
+			if (pub_hil_gyro < 0) {
+				pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+
+			} else {
+				orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro);
+			}
+
+			struct accel_report accel;
+
+			accel.x_raw = imu.xacc / mg2ms2;
+
+			accel.y_raw = imu.yacc / mg2ms2;
+
+			accel.z_raw = imu.zacc / mg2ms2;
+
+			accel.x = imu.xacc;
+
+			accel.y = imu.yacc;
+
+			accel.z = imu.zacc;
+
+			accel.temperature = imu.temperature;
+
+			accel.timestamp = hrt_absolute_time();
+
+			if (pub_hil_accel < 0) {
+				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+
+			} else {
+				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+			}
+
+			struct mag_report mag;
+
+			mag.x_raw = imu.xmag / mga2ga;
+
+			mag.y_raw = imu.ymag / mga2ga;
+
+			mag.z_raw = imu.zmag / mga2ga;
+
+			mag.x = imu.xmag;
+
+			mag.y = imu.ymag;
+
+			mag.z = imu.zmag;
+
+			mag.timestamp = hrt_absolute_time();
+
+			if (pub_hil_mag < 0) {
+				pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag);
+
+			} else {
+				orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag);
+			}
+
+			struct baro_report baro;
+
+			baro.pressure = imu.abs_pressure;
+
+			baro.altitude = imu.pressure_alt;
+
+			baro.temperature = imu.temperature;
+
+			baro.timestamp = hrt_absolute_time();
+
+			if (pub_hil_baro < 0) {
+				pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro);
+
+			} else {
+				orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro);
+			}
+
+			/* publish combined sensor topic */
 			if (pub_hil_sensors > 0) {
 				orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
+
 			} else {
 				pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
 			}
 
+			/* fill in HIL battery status */
+			hil_battery_status.timestamp = hrt_absolute_time();
+			hil_battery_status.voltage_v = 11.1f;
+			hil_battery_status.current_a = 10.0f;
+
+			/* lazily publish the battery voltage */
+			if (pub_hil_battery > 0) {
+				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+
+			} else {
+				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			}
+
 			// increment counters
 			hil_counter++;
 			hil_frames++;
 
 			// output
 			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil sensor at %d hz\n", hil_frames/10);
+				printf("receiving hil sensor at %d hz\n", hil_frames / 10);
 				old_timestamp = timestamp;
 				hil_frames = 0;
 			}
@@ -466,9 +580,11 @@ handle_message(mavlink_message_t *msg)
 
 			/* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */
 			float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f;
+
 			/* go back to -PI..PI */
 			if (heading_rad > M_PI_F)
 				heading_rad -= 2.0f * M_PI_F;
+
 			hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
 			hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m
 			hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m
@@ -481,6 +597,7 @@ handle_message(mavlink_message_t *msg)
 			/* publish GPS measurement data */
 			if (pub_hil_gps > 0) {
 				orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
+
 			} else {
 				pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
 			}
@@ -499,10 +616,12 @@ handle_message(mavlink_message_t *msg)
 
 			if (pub_hil_airspeed < 0) {
 				pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
+
 			} else {
 				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
 
+			hil_global_pos.valid = true;
 			hil_global_pos.lat = hil_state.lat;
 			hil_global_pos.lon = hil_state.lon;
 			hil_global_pos.alt = hil_state.alt / 1000.0f;
@@ -515,6 +634,7 @@ handle_message(mavlink_message_t *msg)
 
 			if (pub_hil_global_pos > 0) {
 				orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
+
 			} else {
 				pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
 			}
@@ -526,8 +646,8 @@ handle_message(mavlink_message_t *msg)
 
 			/* set rotation matrix */
 			for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-				hil_attitude.R[i][j] = C_nb(i, j);
-			
+					hil_attitude.R[i][j] = C_nb(i, j);
+
 			hil_attitude.R_valid = true;
 
 			/* set quaternion */
@@ -549,9 +669,48 @@ handle_message(mavlink_message_t *msg)
 
 			if (pub_hil_attitude > 0) {
 				orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
+
 			} else {
 				pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
 			}
+
+			struct accel_report accel;
+
+			accel.x_raw = hil_state.xacc / 9.81f * 1e3f;
+
+			accel.y_raw = hil_state.yacc / 9.81f * 1e3f;
+
+			accel.z_raw = hil_state.zacc / 9.81f * 1e3f;
+
+			accel.x = hil_state.xacc;
+
+			accel.y = hil_state.yacc;
+
+			accel.z = hil_state.zacc;
+
+			accel.temperature = 25.0f;
+
+			accel.timestamp = hrt_absolute_time();
+
+			if (pub_hil_accel < 0) {
+				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+
+			} else {
+				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+			}
+
+			/* fill in HIL battery status */
+			hil_battery_status.timestamp = hrt_absolute_time();
+			hil_battery_status.voltage_v = 11.1f;
+			hil_battery_status.current_a = 10.0f;
+
+			/* lazily publish the battery voltage */
+			if (pub_hil_battery > 0) {
+				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+
+			} else {
+				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			}
 		}
 
 		if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
@@ -618,17 +777,23 @@ receive_thread(void *arg)
 
 	mavlink_message_t msg;
 
-	prctl(PR_SET_NAME, "mavlink uart rcv", getpid());
+	prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid());
 
-	while (!thread_should_exit) {
+	struct pollfd fds[1];
+	fds[0].fd = uart_fd;
+	fds[0].events = POLLIN;
 
-		struct pollfd fds[1];
-		fds[0].fd = uart_fd;
-		fds[0].events = POLLIN;
+	ssize_t nread = 0;
 
+	while (!thread_should_exit) {
 		if (poll(fds, 1, timeout) > 0) {
+			if (nread < sizeof(buf)) {
+				/* to avoid reading very small chunks wait for data before reading */
+				usleep(1000);
+			}
+
 			/* non-blocking read. read may return negative values */
-			ssize_t nread = read(uart_fd, buf, sizeof(buf));
+			nread = read(uart_fd, buf, sizeof(buf));
 
 			/* if read failed, this loop won't execute */
 			for (ssize_t i = 0; i < nread; i++) {
@@ -636,10 +801,10 @@ receive_thread(void *arg)
 					/* handle generic messages and commands */
 					handle_message(&msg);
 
-					/* Handle packet with waypoint component */
+					/* handle packet with waypoint component */
 					mavlink_wpm_message_handler(&msg, &global_pos, &local_pos);
 
-					/* Handle packet with parameter component */
+					/* handle packet with parameter component */
 					mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
 				}
 			}
@@ -660,6 +825,7 @@ receive_start(int uart)
 	fcntl(uart, F_SETFL, flags | O_NONBLOCK);
 
 	struct sched_param param;
+	(void)pthread_attr_getschedparam(&receiveloop_attr, &param);
 	param.sched_priority = SCHED_PRIORITY_MAX - 40;
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
@@ -667,5 +833,7 @@ receive_start(int uart)
 
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
+
+	pthread_attr_destroy(&receiveloop_attr);
 	return thread;
 }
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index be88b8794..d37b18a19 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -167,9 +167,31 @@ bool set_special_fields(float param1, float param2, float param3, float param4,
 	sp->loiter_direction = (param3 >= 0) ? 1 : -1;
 
 	sp->param1 = param1;
-	sp->param1 = param2;
-	sp->param1 = param3;
-	sp->param1 = param4;
+	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;
 }
 
 /**
@@ -198,7 +220,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		sp.lon = param6_lon_y * 1e7f;
 		sp.altitude = param7_alt_z;
 		sp.altitude_is_relative = false;
-		sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
 		set_special_fields(param1, param2, param3, param4, command, &sp);
 
 		/* Initialize setpoint publication if necessary */
@@ -223,10 +245,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		int last_setpoint_index = -1;
 		bool last_setpoint_valid = false;
 
-		/* at first waypoint, but cycled once through mission */
-		if (index == 0 && last_waypoint_index > 0) {
-			last_setpoint_index = last_waypoint_index;
-		} else {
+		if (index > 0) {
 			last_setpoint_index = index - 1;
 		}
 
@@ -251,10 +270,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		int next_setpoint_index = -1;
 		bool next_setpoint_valid = false;
 
-		/* at last waypoint, try to re-loop through mission as default */
-		if (index == (wpm->size - 1) && wpm->size > 1) {
-			next_setpoint_index = 0;
-		} else if (wpm->size > 1) {
+		/* next waypoint */
+		if (wpm->size > 1) {
 			next_setpoint_index = index + 1;
 		}
 
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index bfccb2d38..5d3d6a73c 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -39,7 +39,6 @@ MODULE_COMMAND	 = mavlink
 SRCS		 += mavlink.c \
 			   missionlib.c \
 			   mavlink_parameters.c \
-			   mavlink_log.c \
 			   mavlink_receiver.cpp \
 			   orb_listener.c \
 			   waypoints.c
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index edb8761b8..f6860930c 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -67,11 +67,13 @@ extern bool gcs_link;
 
 struct vehicle_global_position_s global_pos;
 struct vehicle_local_position_s local_pos;
+struct navigation_capabilities_s nav_cap;
 struct vehicle_status_s v_status;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
 struct actuator_armed_s armed;
-struct actuator_controls_effective_s actuators_0;
+struct actuator_controls_effective_s actuators_effective_0;
+struct actuator_controls_s actuators_0;
 struct vehicle_attitude_s att;
 
 struct mavlink_subscriptions mavlink_subs;
@@ -98,6 +100,8 @@ struct listener {
 	uintptr_t	arg;
 };
 
+uint16_t cm_uint16_from_m_float(float m);
+
 static void	l_sensor_combined(const struct listener *l);
 static void	l_vehicle_attitude(const struct listener *l);
 static void	l_vehicle_gps_position(const struct listener *l);
@@ -112,12 +116,14 @@ static void	l_attitude_setpoint(const struct listener *l);
 static void	l_actuator_outputs(const struct listener *l);
 static void	l_actuator_armed(const struct listener *l);
 static void	l_manual_control_setpoint(const struct listener *l);
+static void	l_vehicle_attitude_controls_effective(const struct listener *l);
 static void	l_vehicle_attitude_controls(const struct listener *l);
 static void	l_debug_key_value(const struct listener *l);
 static void	l_optical_flow(const struct listener *l);
 static void	l_vehicle_rates_setpoint(const struct listener *l);
 static void	l_home(const struct listener *l);
 static void	l_airspeed(const struct listener *l);
+static void	l_nav_cap(const struct listener *l);
 
 static const struct listener listeners[] = {
 	{l_sensor_combined,		&mavlink_subs.sensor_sub,	0},
@@ -138,15 +144,30 @@ static const struct listener listeners[] = {
 	{l_actuator_armed,		&mavlink_subs.armed_sub,	0},
 	{l_manual_control_setpoint,	&mavlink_subs.man_control_sp_sub, 0},
 	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_sub,	0},
+	{l_vehicle_attitude_controls_effective,	&mavlink_subs.actuators_effective_sub,	0},
 	{l_debug_key_value,		&mavlink_subs.debug_key_value,	0},
 	{l_optical_flow,		&mavlink_subs.optical_flow,	0},
 	{l_vehicle_rates_setpoint,	&mavlink_subs.rates_setpoint_sub,	0},
 	{l_home,			&mavlink_subs.home_sub,		0},
 	{l_airspeed,			&mavlink_subs.airspeed_sub,		0},
+	{l_nav_cap,			&mavlink_subs.navigation_capabilities_sub,		0},
 };
 
 static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
 
+uint16_t
+cm_uint16_from_m_float(float m)
+{
+	if (m < 0.0f) {
+		return 0;
+
+	} else if (m > 655.35f) {
+		return 65535;
+	}
+
+	return (uint16_t)(m * 100.0f);
+}
+
 void
 l_sensor_combined(const struct listener *l)
 {
@@ -232,8 +253,10 @@ l_vehicle_gps_position(const struct listener *l)
 
 	/* GPS COG is 0..2PI in degrees * 1e2 */
 	float cog_deg = gps.cog_rad;
+
 	if (cog_deg > M_PI_F)
 		cog_deg -= 2.0f * M_PI_F;
+
 	cog_deg *= M_RAD_TO_DEG_F;
 
 
@@ -244,10 +267,10 @@ l_vehicle_gps_position(const struct listener *l)
 				     gps.lat,
 				     gps.lon,
 				     gps.alt,
-				     gps.eph_m * 1e2f, // from m to cm
-				     gps.epv_m * 1e2f, // from m to cm
+				     cm_uint16_from_m_float(gps.eph_m),
+				     cm_uint16_from_m_float(gps.epv_m),
 				     gps.vel_m_s * 1e2f, // from m/s to cm/s
-				     cog_deg * 1e2f, // from rad to deg * 100
+				     cog_deg * 1e2f, // from deg to deg * 100
 				     gps.satellites_visible);
 
 	/* update SAT info every 10 seconds */
@@ -272,19 +295,23 @@ l_vehicle_status(const struct listener *l)
 	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
 
 	/* enable or disable HIL */
-	set_hil_on_off(v_status.flag_hil_enabled);
+	if (v_status.hil_state == HIL_STATE_ON)
+		set_hil_on_off(true);
+	else if (v_status.hil_state == HIL_STATE_OFF)
+		set_hil_on_off(false);
 
 	/* translate the current syste state to mavlink state and mode */
 	uint8_t mavlink_state = 0;
-	uint8_t mavlink_mode = 0;
-	get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+	uint8_t mavlink_base_mode = 0;
+	uint32_t mavlink_custom_mode = 0;
+	get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 	/* send heartbeat */
 	mavlink_msg_heartbeat_send(chan,
 				   mavlink_system.type,
 				   MAV_AUTOPILOT_PX4,
-				   mavlink_mode,
-				   v_status.state_machine,
+				   mavlink_base_mode,
+				   mavlink_custom_mode,
 				   mavlink_state);
 }
 
@@ -334,7 +361,7 @@ l_global_position(const struct listener *l)
 	int16_t vz = (int16_t)(global_pos.vz * 100.0f);
 
 	/* heading in degrees * 10, from 0 to 36.000) */
-	uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
+	uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
 
 	mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
 					     timestamp / 1000,
@@ -470,8 +497,9 @@ l_actuator_outputs(const struct listener *l)
 
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
-			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+			uint8_t mavlink_base_mode = 0;
+			uint32_t mavlink_custom_mode = 0;
+			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 			/* HIL message as per MAVLink spec */
 
@@ -488,7 +516,7 @@ l_actuator_outputs(const struct listener *l)
 							      -1,
 							      -1,
 							      -1,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
@@ -502,7 +530,7 @@ l_actuator_outputs(const struct listener *l)
 							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
 							      -1,
 							      -1,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
@@ -516,7 +544,7 @@ l_actuator_outputs(const struct listener *l)
 							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
 							      ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
 							      ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else {
@@ -530,7 +558,7 @@ l_actuator_outputs(const struct listener *l)
 							      (act_outputs.output[5] - 1500.0f) / 500.0f,
 							      (act_outputs.output[6] - 1500.0f) / 500.0f,
 							      (act_outputs.output[7] - 1500.0f) / 500.0f,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 			}
 		}
@@ -562,28 +590,54 @@ l_manual_control_setpoint(const struct listener *l)
 }
 
 void
-l_vehicle_attitude_controls(const struct listener *l)
+l_vehicle_attitude_controls_effective(const struct listener *l)
 {
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0);
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0);
 
 	if (gcs_link) {
 		/* send, add spaces so that string buffer is at least 10 chars long */
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl0    ",
-						   actuators_0.control_effective[0]);
+						   actuators_effective_0.control_effective[0]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl1    ",
-						   actuators_0.control_effective[1]);
+						   actuators_effective_0.control_effective[1]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl2     ",
-						   actuators_0.control_effective[2]);
+						   actuators_effective_0.control_effective[2]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl3     ",
-						   actuators_0.control_effective[3]);
+						   actuators_effective_0.control_effective[3]);
+	}
+}
+
+void
+l_vehicle_attitude_controls(const struct listener *l)
+{
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0);
+
+	if (gcs_link) {
+		/* send, add spaces so that string buffer is at least 10 chars long */
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl0    ",
+						   actuators_0.control[0]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl1    ",
+						   actuators_0.control[1]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl2     ",
+						   actuators_0.control[2]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl3     ",
+						   actuators_0.control[3]);
 	}
 }
 
@@ -632,19 +686,32 @@ l_airspeed(const struct listener *l)
 	orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
 
 	float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
-	float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1);
-	float alt = global_pos.alt;
-	float climb = global_pos.vz;
+	uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
+	float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
+	float alt = global_pos.relative_alt;
+	float climb = -global_pos.vz;
+
+	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
+}
+
+void
+l_nav_cap(const struct listener *l)
+{
+
+	orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap);
+
+	mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+				   hrt_absolute_time() / 1000,
+				   "turn dist",
+				   nav_cap.turn_distance);
 
-	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed,
-		((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb);
 }
 
 static void *
 uorb_receive_thread(void *arg)
 {
 	/* Set thread name */
-	prctl(PR_SET_NAME, "mavlink orb rcv", getpid());
+	prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid());
 
 	/*
 	 * set up poll to block for new data,
@@ -673,7 +740,7 @@ uorb_receive_thread(void *arg)
 
 		/* handle the poll result */
 		if (poll_ret == 0) {
-			mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
+			/* silent */
 
 		} else if (poll_ret < 0) {
 			mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
@@ -768,7 +835,10 @@ uorb_receive_start(void)
 	orb_set_interval(mavlink_subs.man_control_sp_sub, 100);	/* 10Hz updates */
 
 	/* --- ACTUATOR CONTROL VALUE --- */
-	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	orb_set_interval(mavlink_subs.actuators_effective_sub, 100);	/* 10Hz updates */
+
+	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */
 
 	/* --- DEBUG VALUE OUTPUT --- */
@@ -783,6 +853,11 @@ uorb_receive_start(void)
 	mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	orb_set_interval(mavlink_subs.airspeed_sub, 200); 	/* 5Hz updates */
 
+	/* --- NAVIGATION CAPABILITIES --- */
+	mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
+	orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); 	/* 2Hz updates */
+	nav_cap.turn_distance = 0.0f;
+
 	/* start the listener loop */
 	pthread_attr_t uorb_attr;
 	pthread_attr_init(&uorb_attr);
@@ -792,5 +867,7 @@ uorb_receive_start(void)
 
 	pthread_t thread;
 	pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
+
+	pthread_attr_destroy(&uorb_attr);
 	return thread;
 }
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index 73e278dc6..91773843b 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -55,15 +55,19 @@
 #include <uORB/topics/vehicle_global_position_set_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
 #include <drivers/drv_rc_input.h>
+#include <uORB/topics/navigation_capabilities.h>
 
 struct mavlink_subscriptions {
 	int sensor_sub;
@@ -75,8 +79,10 @@ struct mavlink_subscriptions {
 	int act_3_sub;
 	int gps_sub;
 	int man_control_sp_sub;
-	int armed_sub;
+	int safety_sub;
 	int actuators_sub;
+	int armed_sub;
+	int actuators_effective_sub;
 	int local_pos_sub;
 	int spa_sub;
 	int spl_sub;
@@ -87,6 +93,7 @@ struct mavlink_subscriptions {
 	int rates_setpoint_sub;
 	int home_sub;
 	int airspeed_sub;
+	int navigation_capabilities_sub;
 };
 
 extern struct mavlink_subscriptions mavlink_subs;
@@ -97,6 +104,9 @@ extern struct vehicle_global_position_s global_pos;
 /** Local position */
 extern struct vehicle_local_position_s local_pos;
 
+/** navigation capabilities */
+extern struct navigation_capabilities_s nav_cap;
+
 /** Vehicle status */
 extern struct vehicle_status_s v_status;
 
diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h
index a4ff06a88..5e5ee8261 100644
--- a/src/modules/mavlink/util.h
+++ b/src/modules/mavlink/util.h
@@ -51,4 +51,4 @@ extern mavlink_wpm_storage *wpm;
 /**
  * Translate the custom state into standard mavlink modes and state.
  */
-extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode);
+extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index eea928a17..7e4a2688f 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -246,64 +246,19 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
 	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
 }
 
-//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z)
-//{
-//    if (seq < wpm->size)
-//    {
-//        mavlink_mission_item_t *cur = waypoints->at(seq);
-//
-//        const PxVector3 A(cur->x, cur->y, cur->z);
-//        const PxVector3 C(x, y, z);
-//
-//        // seq not the second last waypoint
-//        if ((uint16_t)(seq+1) < wpm->size)
-//        {
-//            mavlink_mission_item_t *next = waypoints->at(seq+1);
-//            const PxVector3 B(next->x, next->y, next->z);
-//            const float r = (B-A).dot(C-A) / (B-A).lengthSquared();
-//            if (r >= 0 && r <= 1)
-//            {
-//                const PxVector3 P(A + r*(B-A));
-//                return (P-C).length();
-//            }
-//            else if (r < 0.f)
-//            {
-//                return (C-A).length();
-//            }
-//            else
-//            {
-//                return (C-B).length();
-//            }
-//        }
-//        else
-//        {
-//            return (C-A).length();
-//        }
-//    }
-//    else
-//    {
-//        // if (verbose) // printf("ERROR: index out of bounds\n");
-//    }
-//    return -1.f;
-//}
-
 /*
  * Calculate distance in global frame.
  *
  * The distance calculation is based on the WGS84 geoid (GPS)
  */
-float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt)
+float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z)
 {
-	//TODO: implement for z once altidude contoller is implemented
-
-	static uint16_t counter;
 
-//	if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y);
 	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
+		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
 
-		double current_x_rad = cur->x / 180.0 * M_PI;
-		double current_y_rad = cur->y / 180.0 * M_PI;
+		double current_x_rad = wp->x / 180.0 * M_PI;
+		double current_y_rad = wp->y / 180.0 * M_PI;
 		double x_rad = lat / 180.0 * M_PI;
 		double y_rad = lon / 180.0 * M_PI;
 
@@ -315,20 +270,24 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float
 
 		const double radius_earth = 6371000.0;
 
-		return radius_earth * c;
+		float dxy = radius_earth * c;
+		float dz = alt - wp->z;
+
+		*dist_xy = fabsf(dxy);
+		*dist_z = fabsf(dz);
+
+		return sqrtf(dxy * dxy + dz * dz);
 
 	} else {
 		return -1.0f;
 	}
 
-	counter++;
-
 }
 
 /*
  * Calculate distance in local frame (NED)
  */
-float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z)
+float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z)
 {
 	if (seq < wpm->size) {
 		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
@@ -337,17 +296,27 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float
 		float dy = (cur->y - y);
 		float dz = (cur->z - z);
 
-		return sqrt(dx * dx + dy * dy + dz * dz);
+		*dist_xy = sqrtf(dx * dx + dy * dy);
+		*dist_z = fabsf(dz);
+
+		return sqrtf(dx * dx + dy * dy + dz * dz);
 
 	} else {
 		return -1.0f;
 	}
 }
 
-void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos)
+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;
@@ -366,38 +335,47 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 			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 = 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 = 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 = 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 <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw
-
+		if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
 			wpm->pos_reached = true;
-
 		}
 
-//		else
-//		{
-//			if(counter % 100 == 0)
-//				printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame);
-//		}
+		// 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->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]);
 
@@ -412,28 +390,77 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 
 			bool time_elapsed = false;
 
-			if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) {
-				if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
-					time_elapsed = true;
-				}
-			} else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
+			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 {
 
-						if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
-							/* the last waypoint was reached, if auto continue is
-							 * activated restart the waypoint list from the beginning
-							 */
-							wpm->current_active_wp_id = 0;
+						/* 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)
@@ -465,7 +492,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 }
 
 
-int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position)
+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)
 {
 	/* check for timed-out operations */
 	if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
@@ -488,12 +515,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
 		}
 	}
 
-	// Do NOT continously send the current WP, since we're not loosing messages
-	// if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) {
-	// 	mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-	// }
-
-	check_waypoints_reached(now, global_position , local_position);
+	check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance);
 
 	return OK;
 }
@@ -504,115 +526,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 	uint64_t now = mavlink_missionlib_get_system_timestamp();
 
 	switch (msg->msgid) {
-//		case MAVLINK_MSG_ID_ATTITUDE:
-//        {
-//            if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size)
-//            {
-//                mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-//                if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED)
-//                {
-//                    mavlink_attitude_t att;
-//                    mavlink_msg_attitude_decode(msg, &att);
-//                    float yaw_tolerance = wpm->accept_range_yaw;
-//                    //compare current yaw
-//                    if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*FM_PI)
-//                    {
-//                        if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
-//                            wpm->yaw_reached = true;
-//                    }
-//                    else if(att.yaw - yaw_tolerance < 0.0f)
-//                    {
-//                        float lowerBound = 360.0f + att.yaw - yaw_tolerance;
-//                        if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
-//                            wpm->yaw_reached = true;
-//                    }
-//                    else
-//                    {
-//                        float upperBound = att.yaw + yaw_tolerance - 2.f*FM_PI;
-//                        if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
-//                            wpm->yaw_reached = true;
-//                    }
-//                }
-//            }
-//            break;
-//        }
-//
-//		case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
-//        {
-//            if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size)
-//            {
-//                mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-//
-//                if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED)
-//                {
-//                    mavlink_local_position_ned_t pos;
-//                    mavlink_msg_local_position_ned_decode(msg, &pos);
-//                    //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
-//
-//                    wpm->pos_reached = false;
-//
-//                    // compare current position (given in message) with current waypoint
-//                    float orbit = wp->param1;
-//
-//                    float dist;
-////                    if (wp->param2 == 0)
-////                    {
-////						// FIXME segment distance
-////                        //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z);
-////                    }
-////                    else
-////                    {
-//                        dist = mavlink_wpm_distance_to_point(wpm->current_active_wp_id, pos.x, pos.y, pos.z);
-////                    }
-//
-//                    if (dist >= 0.f && dist <= orbit && wpm->yaw_reached)
-//                    {
-//                        wpm->pos_reached = true;
-//                    }
-//                }
-//            }
-//            break;
-//        }
-
-//		case MAVLINK_MSG_ID_CMD: // special action from ground station
-//        {
-//            mavlink_cmd_t action;
-//            mavlink_msg_cmd_decode(msg, &action);
-//            if(action.target == mavlink_system.sysid)
-//            {
-//                // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
-//                switch (action.action)
-//                {
-//						//				case MAV_ACTION_LAUNCH:
-//						//					// if (verbose) std::cerr << "Launch received" << std::endl;
-//						//					current_active_wp_id = 0;
-//						//					if (wpm->size>0)
-//						//					{
-//						//						setActive(waypoints[current_active_wp_id]);
-//						//					}
-//						//					else
-//						//						// if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
-//						//					break;
-//
-//						//				case MAV_ACTION_CONTINUE:
-//						//					// if (verbose) std::c
-//						//					err << "Continue received" << std::endl;
-//						//					idle = false;
-//						//					setActive(waypoints[current_active_wp_id]);
-//						//					break;
-//
-//						//				case MAV_ACTION_HALT:
-//						//					// if (verbose) std::cerr << "Halt received" << std::endl;
-//						//					idle = true;
-//						//					break;
-//
-//						//				default:
-//						//					// if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
-//						//					break;
-//                }
-//            }
-//            break;
-//        }
 
 	case MAVLINK_MSG_ID_MISSION_ACK: {
 			mavlink_mission_ack_t wpa;
@@ -623,26 +536,16 @@ 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) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
-#else
 
-						if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n");
+						mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
 
-#endif
 						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 						wpm->current_wp_id = 0;
 					}
 				}
 
 			} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
-#else
-
-				if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
-
-#endif
 			}
 
 			break;
@@ -670,13 +573,8 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 							}
 						}
 
-#ifdef MAVLINK_WPM_NO_PRINTF
 						mavlink_missionlib_send_gcs_string("NEW WP SET");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm->current_active_wp_id);
 
-#endif
 						wpm->yaw_reached = false;
 						wpm->pos_reached = false;
 						mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
@@ -684,33 +582,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 						wpm->timestamp_firstinside_orbit = 0;
 
 					} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
 						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n");
-
-#endif
 					}
 
 				} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
 					mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
-#else
-
-					if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n");
 
-#endif
 				}
 
 			} 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
 			}
 
 			break;
@@ -1157,5 +1038,5 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 		}
 	}
 
-	check_waypoints_reached(now, global_pos, local_pos);
+	// check_waypoints_reached(now, global_pos, local_pos);
 }
diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
index 96a0ecd30..d7d6b31dc 100644
--- a/src/modules/mavlink/waypoints.h
+++ b/src/modules/mavlink/waypoints.h
@@ -55,6 +55,7 @@
 #include <stdbool.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/navigation_capabilities.h>
 
 // FIXME XXX - TO BE MOVED TO XML
 enum MAVLINK_WPM_STATES {
@@ -120,7 +121,7 @@ typedef struct mavlink_wpm_storage mavlink_wpm_storage;
 
 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 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);