merged upstream/master into sbus2_sensorssbus2_sensors

27 files changed, 7040 insertions, 4197 deletions

diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 20853379d..e49288a74 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,66 +33,41 @@
 
 /**
  * @file mavlink.c
- * MAVLink 1.0 protocol implementation.
+ * Adapter functions expected by the protocol library
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #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/param/param.h>
-#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"
-#include "missionlib.h"
-#include "mavlink_hil.h"
-#include "util.h"
-#include "waypoints.h"
-#include "mavlink_parameters.h"
 
 /* define MAVLink specific parameters */
+/**
+ * MAVLink system ID
+ * @group MAVLink
+ */
 PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
+/**
+ * MAVLink component ID
+ * @group MAVLink
+ */
 PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
+/**
+ * MAVLink type
+ * @group MAVLink
+ */
 PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
-
-__EXPORT int mavlink_main(int argc, char *argv[]);
-
-static int mavlink_thread_main(int argc, char *argv[]);
-
-/* thread state */
-volatile bool thread_should_exit = false;
-static volatile bool thread_running = false;
-static int mavlink_task;
-
-/* pthreads */
-static pthread_t receive_thread;
-static pthread_t uorb_receive_thread;
-
-/* terminate MAVLink on user request - disabled by default */
-static bool mavlink_link_termination_allowed = false;
+/**
+ * Use/Accept HIL GPS message (even if not in HIL mode)
+ * If set to 1 incomming HIL GPS messages are parsed.
+ * @group MAVLink
+ */
+PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
 
 mavlink_system_t mavlink_system = {
 	100,
@@ -104,357 +78,6 @@ mavlink_system_t mavlink_system = {
 	0
 }; // System ID, 1-255, Component/Subsystem ID, 1-255
 
-/* XXX not widely used */
-uint8_t chan = MAVLINK_COMM_0;
-
-/* XXX probably should be in a header... */
-extern pthread_t receive_start(int uart);
-
-/* Allocate storage space for waypoints */
-static mavlink_wpm_storage wpm_s;
-mavlink_wpm_storage *wpm = &wpm_s;
-
-bool mavlink_hil_enabled = false;
-
-/* protocol interface */
-static int uart;
-static int baudrate;
-bool gcs_link = true;
-
-/* interface mode */
-static enum {
-	MAVLINK_INTERFACE_MODE_OFFBOARD,
-	MAVLINK_INTERFACE_MODE_ONBOARD
-} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD;
-
-static struct mavlink_logbuffer lb;
-
-static void mavlink_update_system(void);
-static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
-static void usage(void);
-int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval);
-
-
-
-int
-set_hil_on_off(bool hil_enabled)
-{
-	int ret = OK;
-
-	/* Enable HIL */
-	if (hil_enabled && !mavlink_hil_enabled) {
-
-		mavlink_hil_enabled = true;
-
-		/* ramp up some HIL-related subscriptions */
-		unsigned hil_rate_interval;
-
-		if (baudrate < 19200) {
-			/* 10 Hz */
-			hil_rate_interval = 100;
-
-		} else if (baudrate < 38400) {
-			/* 10 Hz */
-			hil_rate_interval = 100;
-
-		} else if (baudrate < 115200) {
-			/* 20 Hz */
-			hil_rate_interval = 50;
-
-		} else {
-			/* 200 Hz */
-			hil_rate_interval = 5;
-		}
-
-		orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
-	}
-
-	if (!hil_enabled && mavlink_hil_enabled) {
-		mavlink_hil_enabled = false;
-		orb_set_interval(mavlink_subs.spa_sub, 200);
-
-	} else {
-		ret = ERROR;
-	}
-
-	return ret;
-}
-
-void
-get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
-{
-	/* reset MAVLink mode bitfield */
-	*mavlink_base_mode = 0;
-	*mavlink_custom_mode = 0;
-
-	/**
-	 * Set mode flags
-	 **/
-
-	/* HIL */
-	if (v_status.hil_state == HIL_STATE_ON) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_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;
-	}
-
-	/* 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;
-		}
-	}
-	*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;
-	} 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;
-	} else {
-		warnx("Unknown mavlink state");
-		*mavlink_state = MAV_STATE_CRITICAL;
-	}
-}
-
-
-int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
-{
-	int ret = OK;
-
-	switch (mavlink_msg_id) {
-	case MAVLINK_MSG_ID_SCALED_IMU:
-		/* sensor sub triggers scaled IMU */
-		orb_set_interval(subs->sensor_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_HIGHRES_IMU:
-		/* sensor sub triggers highres IMU */
-		orb_set_interval(subs->sensor_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_RAW_IMU:
-		/* sensor sub triggers RAW IMU */
-		orb_set_interval(subs->sensor_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_ATTITUDE:
-		/* attitude sub triggers attitude */
-		orb_set_interval(subs->att_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
-		/* actuator_outputs triggers this message */
-		orb_set_interval(subs->act_0_sub, min_interval);
-		orb_set_interval(subs->act_1_sub, min_interval);
-		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:
-		/* manual_control_setpoint triggers this message */
-		orb_set_interval(subs->man_control_sp_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
-		orb_set_interval(subs->debug_key_value, min_interval);
-		break;
-
-	default:
-		/* not found */
-		ret = ERROR;
-		break;
-	}
-
-	return ret;
-}
-
-
-/****************************************************************************
- * MAVLink text message logger
- ****************************************************************************/
-
-static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
-
-static const struct file_operations mavlink_fops = {
-	.ioctl = mavlink_dev_ioctl
-};
-
-static int
-mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
-{
-	static unsigned int total_counter = 0;
-
-	switch (cmd) {
-	case (int)MAVLINK_IOC_SEND_TEXT_INFO:
-	case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL:
-	case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
-			const char *txt = (const char *)arg;
-			struct mavlink_logmessage msg;
-			strncpy(msg.text, txt, sizeof(msg.text));
-			mavlink_logbuffer_write(&lb, &msg);
-			total_counter++;
-			return OK;
-		}
-
-	default:
-		return ENOTTY;
-	}
-}
-
-#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
-{
-	/* process baud rate */
-	int speed;
-
-	switch (baud) {
-	case 0:      speed = B0;      break;
-
-	case 50:     speed = B50;     break;
-
-	case 75:     speed = B75;     break;
-
-	case 110:    speed = B110;    break;
-
-	case 134:    speed = B134;    break;
-
-	case 150:    speed = B150;    break;
-
-	case 200:    speed = B200;    break;
-
-	case 300:    speed = B300;    break;
-
-	case 600:    speed = B600;    break;
-
-	case 1200:   speed = B1200;   break;
-
-	case 1800:   speed = B1800;   break;
-
-	case 2400:   speed = B2400;   break;
-
-	case 4800:   speed = B4800;   break;
-
-	case 9600:   speed = B9600;   break;
-
-	case 19200:  speed = B19200;  break;
-
-	case 38400:  speed = B38400;  break;
-
-	case 57600:  speed = B57600;  break;
-
-	case 115200: speed = B115200; break;
-
-	case 230400: speed = B230400; break;
-
-	case 460800: speed = B460800; break;
-
-	case 921600: speed = B921600; break;
-
-	default:
-		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
-		return -EINVAL;
-	}
-
-	/* open uart */
-	warnx("UART is %s, baudrate is %d\n", uart_name, baud);
-	uart = open(uart_name, O_RDWR | O_NOCTTY);
-
-	/* Try to set baud rate */
-	struct termios uart_config;
-	int termios_state;
-	*is_usb = false;
-
-	/* Back up the original uart configuration to restore it after exit */
-	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
-		close(uart);
-		return -1;
-	}
-
-	/* Fill the struct for the new configuration */
-	tcgetattr(uart, &uart_config);
-
-	/* Clear ONLCR flag (which appends a CR for every LF) */
-	uart_config.c_oflag &= ~ONLCR;
-
-	/* USB serial is indicated by /dev/ttyACM0*/
-	if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) {
-
-		/* Set baud rate */
-		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-			warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
-			close(uart);
-			return -1;
-		}
-
-	}
-
-	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
-		close(uart);
-		return -1;
-	}
-
-	return uart;
-}
-
-void
-mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
-{
-	write(uart, ch, (size_t)(sizeof(uint8_t) * length));
-}
-
 /*
  * Internal function to give access to the channel status for each channel
  */
@@ -472,347 +95,3 @@ extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
 	static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
 	return &m_mavlink_buffer[channel];
 }
-
-void mavlink_update_system(void)
-{
-	static bool initialized = false;
-	static param_t param_system_id;
-	static param_t param_component_id;
-	static param_t param_system_type;
-
-	if (!initialized) {
-		param_system_id = param_find("MAV_SYS_ID");
-		param_component_id = param_find("MAV_COMP_ID");
-		param_system_type = param_find("MAV_TYPE");
-		initialized = true;
-	}
-
-	/* update system and component id */
-	int32_t system_id;
-	param_get(param_system_id, &system_id);
-
-	if (system_id > 0 && system_id < 255) {
-		mavlink_system.sysid = system_id;
-	}
-
-	int32_t component_id;
-	param_get(param_component_id, &component_id);
-
-	if (component_id > 0 && component_id < 255) {
-		mavlink_system.compid = component_id;
-	}
-
-	int32_t system_type;
-	param_get(param_system_type, &system_type);
-
-	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
-		mavlink_system.type = system_type;
-	}
-}
-
-/**
- * MAVLink Protocol main function.
- */
-int mavlink_thread_main(int argc, char *argv[])
-{
-	/* initialize mavlink text message buffering */
-	mavlink_logbuffer_init(&lb, 10);
-
-	int ch;
-	char *device_name = "/dev/ttyS1";
-	baudrate = 57600;
-
-	/* work around some stupidity in task_create's argv handling */
-	argc -= 2;
-	argv += 2;
-
-	while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
-		switch (ch) {
-		case 'b':
-			baudrate = strtoul(optarg, NULL, 10);
-
-			if (baudrate < 9600 || baudrate > 921600)
-				errx(1, "invalid baud rate '%s'", optarg);
-
-			break;
-
-		case 'd':
-			device_name = optarg;
-			break;
-
-		case 'e':
-			mavlink_link_termination_allowed = true;
-			break;
-
-		case 'o':
-			mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
-			break;
-
-		default:
-			usage();
-			break;
-		}
-	}
-
-	struct termios uart_config_original;
-
-	bool usb_uart;
-
-	/* print welcome text */
-	warnx("MAVLink v1.0 serial interface starting...");
-
-	/* inform about mode */
-	warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
-
-	/* Flush stdout in case MAVLink is about to take it over */
-	fflush(stdout);
-
-	/* default values for arguments */
-	uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
-
-	if (uart < 0)
-		err(1, "could not open %s", device_name);
-
-	/* create the device node that's used for sending text log messages, etc. */
-	register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL);
-
-	/* Initialize system properties */
-	mavlink_update_system();
-
-	/* start the MAVLink receiver */
-	receive_thread = receive_start(uart);
-
-	/* start the ORB receiver */
-	uorb_receive_thread = uorb_receive_start();
-
-	/* initialize waypoint manager */
-	mavlink_wpm_init(wpm);
-
-	/* all subscriptions are now active, set up initial guess about rate limits */
-	if (baudrate >= 230400) {
-		/* 200 Hz / 5 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
-		/* 50 Hz / 20 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30);
-		/* 20 Hz / 50 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
-		/* 10 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100);
-		/* 10 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
-
-	} else if (baudrate >= 115200) {
-		/* 20 Hz / 50 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200);
-		/* 2 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
-
-	} else if (baudrate >= 57600) {
-		/* 10 Hz / 100 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300);
-		/* 10 Hz / 100 ms ATTITUDE */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
-		/* 2 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
-		/* 2 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500);
-
-	} else {
-		/* very low baud rate, limit to 1 Hz / 1000 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
-		/* 1 Hz / 1000 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
-		/* 0.5 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
-		/* 0.1 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
-	}
-
-	thread_running = true;
-
-	/* arm counter to go off immediately */
-	unsigned lowspeed_counter = 10;
-
-	while (!thread_should_exit) {
-
-		/* 1 Hz */
-		if (lowspeed_counter == 10) {
-			mavlink_update_system();
-
-			/* translate the current system state to mavlink state and mode */
-			uint8_t mavlink_state = 0;
-			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_base_mode, mavlink_custom_mode, mavlink_state);
-
-			/* switch HIL mode if required */
-			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 * 1000.0f,
-						    v_status.battery_voltage * 1000.0f,
-						    v_status.battery_current * 100.0f,
-						    v_status.battery_remaining * 100.0f,
-						    v_status.drop_rate_comm,
-						    v_status.errors_comm,
-						    v_status.errors_count1,
-						    v_status.errors_count2,
-						    v_status.errors_count3,
-						    v_status.errors_count4);
-			lowspeed_counter = 0;
-		}
-
-		lowspeed_counter++;
-
-		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, &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, &nav_cap);
-
-		/* sleep quarter the time */
-		usleep(25000);
-
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
-
-		if (baudrate > 57600) {
-			mavlink_pm_queued_send();
-		}
-
-		/* sleep 10 ms */
-		usleep(10000);
-
-		/* send one string at 10 Hz */
-		if (!mavlink_logbuffer_is_empty(&lb)) {
-			struct mavlink_logmessage msg;
-			int lb_ret = mavlink_logbuffer_read(&lb, &msg);
-
-			if (lb_ret == OK) {
-				mavlink_missionlib_send_gcs_string(msg.text);
-			}
-		}
-
-		/* sleep 15 ms */
-		usleep(15000);
-	}
-
-	/* wait for threads to complete */
-	pthread_join(receive_thread, NULL);
-	pthread_join(uorb_receive_thread, NULL);
-
-	/* Reset the UART flags to original state */
-	tcsetattr(uart, TCSANOW, &uart_config_original);
-
-	/* destroy log buffer */
-	//mavlink_logbuffer_destroy(&lb);
-
-	thread_running = false;
-
-	return 0;
-}
-
-static void
-usage()
-{
-	fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n"
-		"       mavlink stop\n"
-		"       mavlink status\n");
-	exit(1);
-}
-
-int mavlink_main(int argc, char *argv[])
-{
-
-	if (argc < 2) {
-		warnx("missing command");
-		usage();
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		/* this is not an error */
-		if (thread_running)
-			errx(0, "mavlink already running");
-
-		thread_should_exit = false;
-		mavlink_task = task_spawn_cmd("mavlink",
-					  SCHED_DEFAULT,
-					  SCHED_PRIORITY_DEFAULT,
-					  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);
-			warnx(".");
-		}
-
-		warnx("terminated.");
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			errx(0, "running");
-
-		} else {
-			errx(1, "not running");
-		}
-	}
-
-	warnx("unrecognized command");
-	usage();
-	/* not getting here */
-	return 0;
-}
-
diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index 149efda60..374d1511c 100644
--- a/src/modules/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,6 +42,8 @@
 #ifndef MAVLINK_BRIDGE_HEADER_H
 #define MAVLINK_BRIDGE_HEADER_H
 
+__BEGIN_DECLS
+
 #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
 
 /* use efficient approach, see mavlink_helpers.h */
@@ -73,11 +74,13 @@ extern mavlink_system_t mavlink_system;
  * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
  * @param ch Character to send
  */
-extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
+void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
 
 extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
 extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
 
 #include <v1.0/common/mavlink.h>
 
+__END_DECLS
+
 #endif /* MAVLINK_BRIDGE_HEADER_H */
diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp
new file mode 100644
index 000000000..fccd4d9a5
--- /dev/null
+++ b/src/modules/mavlink/mavlink_commands.cpp
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_commands.cpp
+ * Mavlink commands stream implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "mavlink_commands.h"
+
+MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0)
+{
+	_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
+}
+
+void
+MavlinkCommandsStream::update(const hrt_abstime t)
+{
+	struct vehicle_command_s cmd;
+
+	if (_cmd_sub->update(&_cmd_time, &cmd)) {
+		/* only send commands for other systems/components */
+		if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
+			mavlink_msg_command_long_send(_channel,
+						      cmd.target_system,
+						      cmd.target_component,
+						      cmd.command,
+						      cmd.confirmation,
+						      cmd.param1,
+						      cmd.param2,
+						      cmd.param3,
+						      cmd.param4,
+						      cmd.param5,
+						      cmd.param6,
+						      cmd.param7);
+		}
+	}
+}
diff --git a/src/modules/mavlink/missionlib.h b/src/modules/mavlink/mavlink_commands.h
index c7d8f90c5..abdba34b9 100644
--- a/src/modules/mavlink/missionlib.h
+++ b/src/modules/mavlink/mavlink_commands.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,20 +32,34 @@
  ****************************************************************************/
 
 /**
- * @file missionlib.h
- * MAVLink mission helper library
+ * @file mavlink_commands.h
+ * Mavlink commands stream definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#pragma once
+#ifndef MAVLINK_COMMANDS_H_
+#define MAVLINK_COMMANDS_H_
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_command.h>
+
+class Mavlink;
+class MavlinkCommansStream;
+
+#include "mavlink_main.h"
+
+class MavlinkCommandsStream
+{
+private:
+	MavlinkOrbSubscription *_cmd_sub;
+	struct vehicle_command_s *_cmd;
+	mavlink_channel_t _channel;
+	uint64_t _cmd_time;
 
-#include "mavlink_bridge_header.h"
+public:
+	MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
+	void update(const hrt_abstime t);
+};
 
-//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);
+#endif /* MAVLINK_COMMANDS_H_ */
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
new file mode 100644
index 000000000..675a6870e
--- /dev/null
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -0,0 +1,423 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <crc32.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <fcntl.h>
+
+#include "mavlink_ftp.h"
+
+MavlinkFTP *MavlinkFTP::_server;
+
+MavlinkFTP *
+MavlinkFTP::getServer()
+{
+	// XXX this really cries out for some locking...
+	if (_server == nullptr) {
+		_server = new MavlinkFTP;
+	}
+	return _server;
+}
+
+MavlinkFTP::MavlinkFTP()
+{
+	// initialise the request freelist
+	dq_init(&_workFree);
+	sem_init(&_lock, 0, 1);
+    
+    // initialize session list
+    for (size_t i=0; i<kMaxSession; i++) {
+        _session_fds[i] = -1;
+    }
+
+	// drop work entries onto the free list
+	for (unsigned i = 0; i < kRequestQueueSize; i++) {
+		_qFree(&_workBufs[i]);
+	}
+
+}
+
+void
+MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
+{
+	// get a free request
+	auto req = _dqFree();
+
+	// if we couldn't get a request slot, just drop it
+	if (req != nullptr) {
+
+		// decode the request
+		if (req->decode(mavlink, msg)) {
+
+			// and queue it for the worker
+			work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
+		} else {
+			_qFree(req);
+		}
+	}
+}
+
+void
+MavlinkFTP::_workerTrampoline(void *arg)
+{
+	auto req = reinterpret_cast<Request *>(arg);
+	auto server = MavlinkFTP::getServer();
+
+	// call the server worker with the work item
+	server->_worker(req);
+}
+
+void
+MavlinkFTP::_worker(Request *req)
+{
+	auto hdr = req->header();
+	ErrorCode errorCode = kErrNone;
+	uint32_t messageCRC;
+
+	// basic sanity checks; must validate length before use
+	if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
+		errorCode = kErrNoRequest;
+		goto out;
+	}
+
+	// check request CRC to make sure this is one of ours
+	messageCRC = hdr->crc32;
+	hdr->crc32 = 0;
+	if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
+		errorCode = kErrNoRequest;
+		goto out;
+		warnx("ftp: bad crc");
+	}
+
+	//printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
+
+	switch (hdr->opcode) {
+	case kCmdNone:
+		break;
+
+	case kCmdTerminate:
+		errorCode = _workTerminate(req);
+		break;
+
+	case kCmdReset:
+		errorCode = _workReset();
+		break;
+
+	case kCmdList:
+		errorCode = _workList(req);
+		break;
+
+	case kCmdOpen:
+		errorCode = _workOpen(req, false);
+		break;
+
+	case kCmdCreate:
+		errorCode = _workOpen(req, true);
+		break;
+
+	case kCmdRead:
+		errorCode = _workRead(req);
+		break;
+
+	case kCmdWrite:
+		errorCode = _workWrite(req);
+		break;
+
+	case kCmdRemove:
+		errorCode = _workRemove(req);
+		break;
+
+	default:
+		errorCode = kErrNoRequest;
+		break;	
+	}
+
+out:
+	// handle success vs. error
+	if (errorCode == kErrNone) {
+		hdr->opcode = kRspAck;
+		//warnx("FTP: ack\n");
+	} else {
+		warnx("FTP: nak %u", errorCode);
+		hdr->opcode = kRspNak;
+		hdr->size = 1;
+		hdr->data[0] = errorCode;
+	}
+
+	// respond to the request
+	_reply(req);
+
+	// free the request buffer back to the freelist
+	_qFree(req);
+}
+
+void
+MavlinkFTP::_reply(Request *req)
+{
+	auto hdr = req->header();
+
+	// message is assumed to be already constructed in the request buffer, so generate the CRC
+	hdr->crc32 = 0;
+	hdr->crc32 = crc32(req->rawData(), req->dataSize());
+
+	// then pack and send the reply back to the request source
+	req->reply();
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workList(Request *req)
+{
+	auto hdr = req->header();
+    
+    char dirPath[kMaxDataLength];
+    strncpy(dirPath, req->dataAsCString(), kMaxDataLength);
+    
+	DIR *dp = opendir(dirPath);
+
+	if (dp == nullptr) {
+		warnx("FTP: can't open path '%s'", dirPath);
+		return kErrNotDir;
+	}
+    
+	//warnx("FTP: list %s offset %d", dirPath, hdr->offset);
+
+	ErrorCode errorCode = kErrNone;
+	struct dirent entry, *result = nullptr;
+	unsigned offset = 0;
+
+	// move to the requested offset
+	seekdir(dp, hdr->offset);
+
+	for (;;) {
+		// read the directory entry
+		if (readdir_r(dp, &entry, &result)) {
+			warnx("FTP: list %s readdir_r failure\n", dirPath);
+			errorCode = kErrIO;
+			break;
+		}
+
+		// no more entries?
+		if (result == nullptr) {
+			if (hdr->offset != 0 && offset == 0) {
+				// User is requesting subsequent dir entries but there were none. This means the user asked
+				// to seek past EOF.
+				errorCode = kErrEOF;
+			}
+			// Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that
+			break;
+		}
+
+		// name too big to fit?
+		if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) {
+			break;
+		}
+
+		// store the type marker
+		switch (entry.d_type) {
+		case DTYPE_FILE:
+			hdr->data[offset++] = kDirentFile;
+			break;
+		case DTYPE_DIRECTORY:
+			hdr->data[offset++] = kDirentDir;
+			break;
+		default:
+			hdr->data[offset++] = kDirentUnknown;
+			break;
+		}
+
+		// copy the name, which we know will fit
+		strcpy((char *)&hdr->data[offset], entry.d_name);
+		//printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
+		offset += strlen(entry.d_name) + 1;
+	}
+
+	closedir(dp);
+	hdr->size = offset;
+
+	return errorCode;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workOpen(Request *req, bool create)
+{
+	auto hdr = req->header();
+    
+	int session_index = _findUnusedSession();
+	if (session_index < 0) {
+		return kErrNoSession;
+	}
+
+	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
+    
+	int fd = ::open(req->dataAsCString(), oflag);
+	if (fd < 0) {
+		return create ? kErrPerm : kErrNotFile;
+	}
+	_session_fds[session_index] = fd;
+
+	hdr->session = session_index;
+	hdr->size = 0;
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRead(Request *req)
+{
+	auto hdr = req->header();
+
+	int session_index = hdr->session;
+
+	if (!_validSession(session_index)) {
+		return kErrNoSession;
+	}
+
+	// Seek to the specified position
+	//warnx("seek %d", hdr->offset);
+	if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
+		// Unable to see to the specified location
+		warnx("seek fail");
+		return kErrEOF;
+	}
+
+	int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength);
+	if (bytes_read < 0) {
+		// Negative return indicates error other than eof
+		warnx("read fail %d", bytes_read);
+		return kErrIO;
+	}
+
+	hdr->size = bytes_read;
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workWrite(Request *req)
+{
+#if 0
+    // NYI: Coming soon
+	auto hdr = req->header();
+
+	// look up session
+	auto session = getSession(hdr->session);
+	if (session == nullptr) {
+		return kErrNoSession;
+	}
+
+	// append to file
+	int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
+
+	if (result < 0) {
+		// XXX might also be no space, I/O, etc.
+		return kErrNotAppend;
+	}
+
+	hdr->size = result;
+	return kErrNone;
+#else
+	return kErrPerm;
+#endif
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRemove(Request *req)
+{
+	//auto hdr = req->header();
+
+	// for now, send error reply
+	return kErrPerm;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workTerminate(Request *req)
+{
+	auto hdr = req->header();
+    
+	if (!_validSession(hdr->session)) {
+		return kErrNoSession;
+	}
+    
+	::close(_session_fds[hdr->session]);
+
+	return kErrNone;
+}
+
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workReset(void)
+{
+	for (size_t i=0; i<kMaxSession; i++) {
+		if (_session_fds[i] != -1) {
+			::close(_session_fds[i]);
+			_session_fds[i] = -1;
+		}
+	}
+
+	return kErrNone;
+}
+
+bool
+MavlinkFTP::_validSession(unsigned index)
+{
+	if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
+		return false;
+	}
+	return true;
+}
+
+int
+MavlinkFTP::_findUnusedSession(void)
+{
+	for (size_t i=0; i<kMaxSession; i++) {
+		if (_session_fds[i] == -1) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+char *
+MavlinkFTP::Request::dataAsCString()
+{
+	// guarantee nul termination
+	if (header()->size < kMaxDataLength) {
+		requestData()[header()->size] = '\0';
+	} else {
+		requestData()[kMaxDataLength - 1] = '\0';
+	}
+
+	// and return data
+	return (char *)&(header()->data[0]);
+}
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
new file mode 100644
index 000000000..59237a4c4
--- /dev/null
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -0,0 +1,227 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+/**
+ * @file mavlink_ftp.h
+ *
+ * MAVLink remote file server.
+ *
+ * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
+ * a session ID and sequence number.
+ *
+ * A limited number of requests (currently 2) may be outstanding at a time.
+ * Additional messages will be discarded.
+ *
+ * Messages consist of a fixed header, followed by a data area.
+ *
+ */
+ 
+#include <dirent.h>
+#include <queue.h>
+
+#include <nuttx/wqueue.h>
+#include <systemlib/err.h>
+
+#include "mavlink_messages.h"
+
+class MavlinkFTP
+{
+public:
+	MavlinkFTP();
+
+	static MavlinkFTP	*getServer();
+
+	// static interface
+	void			handle_message(Mavlink* mavlink,
+					mavlink_message_t *msg);
+
+private:
+
+	static const unsigned	kRequestQueueSize = 2;
+
+	static MavlinkFTP	*_server;
+
+	struct RequestHeader
+	{
+		uint8_t		magic;
+		uint8_t		session;
+		uint8_t		opcode;
+		uint8_t		size;
+		uint32_t	crc32;
+		uint32_t	offset;
+		uint8_t		data[];
+	};
+
+	enum Opcode : uint8_t
+	{
+		kCmdNone,	// ignored, always acked
+		kCmdTerminate,	// releases sessionID, closes file
+		kCmdReset,	// terminates all sessions
+		kCmdList,	// list files in <path> from <offset>
+		kCmdOpen,	// opens <path> for reading, returns <session>
+		kCmdRead,	// reads <size> bytes from <offset> in <session>
+		kCmdCreate,	// creates <path> for writing, returns <session>
+		kCmdWrite,	// appends <size> bytes at <offset> in <session>
+		kCmdRemove,	// remove file (only if created by server?)
+
+		kRspAck,
+		kRspNak
+	};
+
+	enum ErrorCode : uint8_t
+	{
+		kErrNone,
+		kErrNoRequest,
+		kErrNoSession,
+		kErrSequence,
+		kErrNotDir,
+		kErrNotFile,
+		kErrEOF,
+		kErrNotAppend,
+		kErrTooBig,
+		kErrIO,
+		kErrPerm
+	};
+
+    int _findUnusedSession(void);
+    bool _validSession(unsigned index);
+
+    static const unsigned kMaxSession = 2;
+    int	_session_fds[kMaxSession];
+
+	class Request
+	{
+	public:
+		union {
+			dq_entry_t	entry;
+			work_s		work;
+		};
+
+		bool		decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
+			if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
+				_mavlink = mavlink;
+				mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
+				return true;
+			}
+			return false;
+		}
+
+		void		reply() {
+
+			// XXX the proper way would be an IOCTL / uORB call, rather than exploiting the
+			// flat memory architecture, as we're operating between threads here.
+			mavlink_message_t msg;
+			msg.checksum = 0;
+			unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
+				_mavlink->get_channel(), &msg, sequence(), rawData());
+
+			_mavlink->lockMessageBufferMutex();
+			bool success = _mavlink->message_buffer_write(&msg, len);
+			_mavlink->unlockMessageBufferMutex();
+
+			if (!success) {
+				warnx("FTP TX ERR");
+			} 
+			// else {
+				// warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
+				// 	_mavlink->get_system_id(),
+				// 	_mavlink->get_component_id(),
+				// 	_mavlink->get_channel(),
+				// 	len,
+				// 	msg.checksum);
+			// }
+		}
+
+		uint8_t		*rawData() { return &_message.data[0]; }
+		RequestHeader 	*header()  { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
+		uint8_t         *requestData() { return &(header()->data[0]); }
+		unsigned	dataSize() { return header()->size + sizeof(RequestHeader); }
+		uint16_t	sequence() const { return _message.seqnr; }
+		mavlink_channel_t channel() { return _mavlink->get_channel(); }
+
+		char		*dataAsCString();
+
+	private:
+		Mavlink			*_mavlink;
+		mavlink_encapsulated_data_t _message;
+
+	};
+
+	static const uint8_t	kProtocolMagic = 'f';
+	static const char	kDirentFile = 'F';
+	static const char	kDirentDir = 'D';
+	static const char	kDirentUnknown = 'U';
+	static const uint8_t	kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
+
+	/// Request worker; runs on the low-priority work queue to service
+	/// remote requests.
+	///
+	static void		_workerTrampoline(void *arg);
+	void			_worker(Request *req);
+
+	/// Reply to a request (XXX should be a Request method)
+	///
+	void			_reply(Request *req);
+
+	ErrorCode		_workList(Request *req);
+	ErrorCode		_workOpen(Request *req, bool create);
+	ErrorCode		_workRead(Request *req);
+	ErrorCode		_workWrite(Request *req);
+	ErrorCode		_workRemove(Request *req);
+	ErrorCode		_workTerminate(Request *req);
+	ErrorCode		_workReset();
+
+	// work freelist
+	Request			_workBufs[kRequestQueueSize];
+	dq_queue_t		_workFree;
+	sem_t			_lock;
+
+	void			_qLock() { do {} while (sem_wait(&_lock) != 0); }
+	void			_qUnlock() { sem_post(&_lock); }
+
+	void			_qFree(Request *req) {
+		_qLock();
+		dq_addlast(&req->entry, &_workFree);
+		_qUnlock();
+	}
+
+	Request			*_dqFree() {
+		_qLock();
+		auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree));
+		_qUnlock();
+		return req;
+	}
+    
+};
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
new file mode 100644
index 000000000..cd0581af4
--- /dev/null
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -0,0 +1,1759 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * 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_main.cpp
+ * MAVLink 1.0 protocol implementation.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <assert.h>
+#include <math.h>
+#include <poll.h>
+#include <termios.h>
+#include <time.h>
+#include <math.h> /* isinf / isnan checks */
+
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <geo/geo.h>
+#include <dataman/dataman.h>
+#include <mathlib/mathlib.h>
+#include <mavlink/mavlink_log.h>
+
+#include <uORB/topics/parameter_update.h>
+
+#include "mavlink_bridge_header.h"
+#include "mavlink_main.h"
+#include "mavlink_messages.h"
+#include "mavlink_receiver.h"
+#include "mavlink_rate_limiter.h"
+#include "mavlink_commands.h"
+
+#ifndef MAVLINK_CRC_EXTRA
+ #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
+#endif
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#define DEFAULT_DEVICE_NAME	"/dev/ttyS1"
+#define MAX_DATA_RATE	10000	// max data rate in bytes/s
+#define MAIN_LOOP_DELAY 10000	// 100 Hz @ 1000 bytes/s data rate
+
+static Mavlink *_mavlink_instances = nullptr;
+
+/* TODO: if this is a class member it crashes */
+static struct file_operations fops;
+
+/**
+ * mavlink app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
+
+extern mavlink_system_t mavlink_system;
+
+static uint64_t last_write_success_times[6] = {0};
+static uint64_t last_write_try_times[6] = {0};
+
+/*
+ * Internal function to send the bytes through the right serial port
+ */
+void
+mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
+{
+
+	Mavlink *instance;
+
+	switch (channel) {
+	case MAVLINK_COMM_0:
+		instance = Mavlink::get_instance(0);
+		break;
+
+	case MAVLINK_COMM_1:
+		instance = Mavlink::get_instance(1);
+		break;
+
+	case MAVLINK_COMM_2:
+		instance = Mavlink::get_instance(2);
+		break;
+
+	case MAVLINK_COMM_3:
+		instance = Mavlink::get_instance(3);
+		break;
+#ifdef MAVLINK_COMM_4
+
+	case MAVLINK_COMM_4:
+		instance = Mavlink::get_instance(4);
+		break;
+#endif
+#ifdef MAVLINK_COMM_5
+
+	case MAVLINK_COMM_5:
+		instance = Mavlink::get_instance(5);
+		break;
+#endif
+#ifdef MAVLINK_COMM_6
+
+	case MAVLINK_COMM_6:
+		instance = Mavlink::get_instance(6);
+		break;
+#endif
+		default:
+		return;
+	}
+
+	int uart = instance->get_uart_fd();
+
+	ssize_t desired = (sizeof(uint8_t) * length);
+
+	/*
+	 * Check if the OS buffer is full and disable HW
+	 * flow control if it continues to be full
+	 */
+	int buf_free = 0;
+
+	if (instance->get_flow_control_enabled()
+		&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
+
+		/* Disable hardware flow control:
+		 * if no successful write since a defined time
+		 * and if the last try was not the last successful write
+		 */
+		if (last_write_try_times[(unsigned)channel] != 0 &&
+			hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
+			last_write_success_times[(unsigned)channel] !=
+			last_write_try_times[(unsigned)channel])
+		{
+			warnx("DISABLING HARDWARE FLOW CONTROL");
+			instance->enable_flow_control(false);
+		}
+
+	}
+
+	/* If the wait until transmit flag is on, only transmit after we've received messages.
+	   Otherwise, transmit all the time. */
+	if (instance->should_transmit()) {
+		last_write_try_times[(unsigned)channel] = hrt_absolute_time();
+
+		/* check if there is space in the buffer, let it overflow else */
+		if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
+
+			if (buf_free < desired) {
+				/* we don't want to send anything just in half, so return */
+				instance->count_txerr();
+				return;
+			}
+		}
+
+		ssize_t ret = write(uart, ch, desired);
+		if (ret != desired) {
+			instance->count_txerr();
+		} else {
+			last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
+		}
+	}
+}
+
+static void usage(void);
+
+Mavlink::Mavlink() :
+	_device_name(DEFAULT_DEVICE_NAME),
+	_task_should_exit(false),
+	next(nullptr),
+	_mavlink_fd(-1),
+	_task_running(false),
+	_hil_enabled(false),
+	_use_hil_gps(false),
+	_is_usb_uart(false),
+	_wait_to_transmit(false),
+	_received_messages(false),
+	_main_loop_delay(1000),
+	_subscriptions(nullptr),
+	_streams(nullptr),
+	_mission_manager(nullptr),
+	_mission_pub(-1),
+	_mission_result_sub(-1),
+	_mode(MAVLINK_MODE_NORMAL),
+	_total_counter(0),
+	_verbose(false),
+	_forwarding_on(false),
+	_passing_on(false),
+	_ftp_on(false),
+	_uart_fd(-1),
+	_mavlink_param_queue_index(0),
+	_subscribe_to_stream(nullptr),
+	_subscribe_to_stream_rate(0.0f),
+	_flow_control_enabled(true),
+	_message_buffer({}),
+	_param_initialized(false),
+	_param_system_id(0),
+	_param_component_id(0),
+	_param_system_type(0),
+	_param_use_hil_gps(0),
+
+/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
+	_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
+{
+	fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
+
+	_instance_id = Mavlink::instance_count();
+
+	/* set channel according to instance id */
+	switch (_instance_id) {
+	case 0:
+		_channel = MAVLINK_COMM_0;
+		break;
+
+	case 1:
+		_channel = MAVLINK_COMM_1;
+		break;
+
+	case 2:
+		_channel = MAVLINK_COMM_2;
+		break;
+
+	case 3:
+		_channel = MAVLINK_COMM_3;
+		break;
+#ifdef MAVLINK_COMM_4
+
+	case 4:
+		_channel = MAVLINK_COMM_4;
+		break;
+#endif
+#ifdef MAVLINK_COMM_5
+
+	case 5:
+		_channel = MAVLINK_COMM_5;
+		break;
+#endif
+#ifdef MAVLINK_COMM_6
+
+	case 6:
+		_channel = MAVLINK_COMM_6;
+		break;
+#endif
+
+	default:
+		errx(1, "instance ID is out of range");
+		break;
+	}
+}
+
+Mavlink::~Mavlink()
+{
+	perf_free(_loop_perf);
+	perf_free(_txerr_perf);
+
+	if (_task_running) {
+		/* task wakes up every 10ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				//TODO store main task handle in Mavlink instance to allow killing task
+				//task_delete(_mavlink_task);
+				break;
+			}
+		} while (_task_running);
+	}
+
+	LL_DELETE(_mavlink_instances, this);
+}
+
+void
+Mavlink::count_txerr()
+{
+	perf_count(_txerr_perf);
+}
+
+void
+Mavlink::set_mode(enum MAVLINK_MODE mode)
+{
+	_mode = mode;
+}
+
+int
+Mavlink::instance_count()
+{
+	unsigned inst_index = 0;
+	Mavlink *inst;
+
+	LL_FOREACH(::_mavlink_instances, inst) {
+		inst_index++;
+	}
+
+	return inst_index;
+}
+
+Mavlink *
+Mavlink::get_instance(unsigned instance)
+{
+	Mavlink *inst;
+	unsigned inst_index = 0;
+	LL_FOREACH(::_mavlink_instances, inst) {
+		if (instance == inst_index) {
+			return inst;
+		}
+
+		inst_index++;
+	}
+
+	return nullptr;
+}
+
+Mavlink *
+Mavlink::get_instance_for_device(const char *device_name)
+{
+	Mavlink *inst;
+
+	LL_FOREACH(::_mavlink_instances, inst) {
+		if (strcmp(inst->_device_name, device_name) == 0) {
+			return inst;
+		}
+	}
+
+	return nullptr;
+}
+
+int
+Mavlink::destroy_all_instances()
+{
+	/* start deleting from the end */
+	Mavlink *inst_to_del = nullptr;
+	Mavlink *next_inst = ::_mavlink_instances;
+
+	unsigned iterations = 0;
+
+	warnx("waiting for instances to stop");
+
+	while (next_inst != nullptr) {
+		inst_to_del = next_inst;
+		next_inst = inst_to_del->next;
+
+		/* set flag to stop thread and wait for all threads to finish */
+		inst_to_del->_task_should_exit = true;
+
+		while (inst_to_del->_task_running) {
+			printf(".");
+			fflush(stdout);
+			usleep(10000);
+			iterations++;
+
+			if (iterations > 1000) {
+				warnx("ERROR: Couldn't stop all mavlink instances.");
+				return ERROR;
+			}
+		}
+	}
+
+	printf("\n");
+	warnx("all instances stopped");
+	return OK;
+}
+
+bool
+Mavlink::instance_exists(const char *device_name, Mavlink *self)
+{
+	Mavlink *inst = ::_mavlink_instances;
+
+	while (inst != nullptr) {
+
+		/* don't compare with itself */
+		if (inst != self && !strcmp(device_name, inst->_device_name)) {
+			return true;
+		}
+
+		inst = inst->next;
+	}
+
+	return false;
+}
+
+void
+Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
+{
+
+	Mavlink *inst;
+	LL_FOREACH(_mavlink_instances, inst) {
+		if (inst != self) {
+			inst->pass_message(msg);
+		}
+	}
+}
+
+int
+Mavlink::get_uart_fd(unsigned index)
+{
+	Mavlink *inst = get_instance(index);
+
+	if (inst) {
+		return inst->get_uart_fd();
+	}
+
+	return -1;
+}
+
+int
+Mavlink::get_uart_fd()
+{
+	return _uart_fd;
+}
+
+int
+Mavlink::get_instance_id()
+{
+	return _instance_id;
+}
+
+mavlink_channel_t
+Mavlink::get_channel()
+{
+	return _channel;
+}
+
+/****************************************************************************
+ * MAVLink text message logger
+ ****************************************************************************/
+
+int
+Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	case (int)MAVLINK_IOC_SEND_TEXT_INFO:
+	case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL:
+	case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
+
+			const char *txt = (const char *)arg;
+//		printf("logmsg: %s\n", txt);
+			struct mavlink_logmessage msg;
+			strncpy(msg.text, txt, sizeof(msg.text));
+			msg.severity = (unsigned char)cmd;
+
+			Mavlink *inst;
+			LL_FOREACH(_mavlink_instances, inst) {
+				if (!inst->_task_should_exit) {
+					mavlink_logbuffer_write(&inst->_logbuffer, &msg);
+					inst->_total_counter++;
+				}
+			}
+
+			return OK;
+		}
+
+	default:
+		return ENOTTY;
+	}
+}
+
+void Mavlink::mavlink_update_system(void)
+{
+	if (!_param_initialized) {
+		_param_system_id = param_find("MAV_SYS_ID");
+		_param_component_id = param_find("MAV_COMP_ID");
+		_param_system_type = param_find("MAV_TYPE");
+		_param_use_hil_gps = param_find("MAV_USEHILGPS");
+		_param_initialized = true;
+	}
+
+	/* update system and component id */
+	int32_t system_id;
+	param_get(_param_system_id, &system_id);
+
+	if (system_id > 0 && system_id < 255) {
+		mavlink_system.sysid = system_id;
+	}
+
+	int32_t component_id;
+	param_get(_param_component_id, &component_id);
+
+	if (component_id > 0 && component_id < 255) {
+		mavlink_system.compid = component_id;
+	}
+
+	int32_t system_type;
+	param_get(_param_system_type, &system_type);
+
+	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
+		mavlink_system.type = system_type;
+	}
+
+	int32_t use_hil_gps;
+	param_get(_param_use_hil_gps, &use_hil_gps);
+
+	_use_hil_gps = (bool)use_hil_gps;
+}
+
+int Mavlink::get_system_id()
+{
+	return mavlink_system.sysid;
+}
+
+int Mavlink::get_component_id()
+{
+	return mavlink_system.compid;
+}
+
+int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
+{
+	/* process baud rate */
+	int speed;
+
+	switch (baud) {
+	case 0:      speed = B0;      break;
+
+	case 50:     speed = B50;     break;
+
+	case 75:     speed = B75;     break;
+
+	case 110:    speed = B110;    break;
+
+	case 134:    speed = B134;    break;
+
+	case 150:    speed = B150;    break;
+
+	case 200:    speed = B200;    break;
+
+	case 300:    speed = B300;    break;
+
+	case 600:    speed = B600;    break;
+
+	case 1200:   speed = B1200;   break;
+
+	case 1800:   speed = B1800;   break;
+
+	case 2400:   speed = B2400;   break;
+
+	case 4800:   speed = B4800;   break;
+
+	case 9600:   speed = B9600;   break;
+
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	case 230400: speed = B230400; break;
+
+	case 460800: speed = B460800; break;
+
+	case 921600: speed = B921600; break;
+
+	default:
+		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud);
+		return -EINVAL;
+	}
+
+	/* open uart */
+	_uart_fd = open(uart_name, O_RDWR | O_NOCTTY);
+
+	if (_uart_fd < 0) {
+		return _uart_fd;
+	}
+
+
+	/* Try to set baud rate */
+	struct termios uart_config;
+	int termios_state;
+	*is_usb = false;
+
+	/* Back up the original uart configuration to restore it after exit */
+	if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) {
+		warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
+		close(_uart_fd);
+		return -1;
+	}
+
+	/* Fill the struct for the new configuration */
+	tcgetattr(_uart_fd, &uart_config);
+
+	/* Clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+
+	/* USB serial is indicated by /dev/ttyACM0*/
+	if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) {
+
+		/* Set baud rate */
+		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+			warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
+			close(_uart_fd);
+			return -1;
+		}
+
+	}
+
+	if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
+		warnx("ERR SET CONF %s\n", uart_name);
+		close(_uart_fd);
+		return -1;
+	}
+
+	if (!_is_usb_uart) {
+		/*
+		 * Setup hardware flow control. If the port has no RTS pin this call will fail,
+		 * which is not an issue, but requires a separate call so we can fail silently.
+		 */
+		(void)tcgetattr(_uart_fd, &uart_config);
+		uart_config.c_cflag |= CRTS_IFLOW;
+		(void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
+
+		/* setup output flow control */
+		if (enable_flow_control(true)) {
+			warnx("hardware flow control not supported");
+		}
+	}
+
+	return _uart_fd;
+}
+
+int
+Mavlink::enable_flow_control(bool enabled)
+{
+	// We can't do this on USB - skip
+	if (_is_usb_uart) {
+		return OK;
+	}
+
+	struct termios uart_config;
+
+	int ret = tcgetattr(_uart_fd, &uart_config);
+
+	if (enabled) {
+		uart_config.c_cflag |= CRTSCTS;
+
+	} else {
+		uart_config.c_cflag &= ~CRTSCTS;
+	}
+
+	ret = tcsetattr(_uart_fd, TCSANOW, &uart_config);
+
+	if (!ret) {
+		_flow_control_enabled = enabled;
+	}
+
+	return ret;
+}
+
+int
+Mavlink::set_hil_enabled(bool hil_enabled)
+{
+	int ret = OK;
+
+	/* enable HIL */
+	if (hil_enabled && !_hil_enabled) {
+		_hil_enabled = true;
+		float rate_mult = _datarate / 1000.0f;
+		configure_stream("HIL_CONTROLS", 15.0f * rate_mult);
+	}
+
+	/* disable HIL */
+	if (!hil_enabled && _hil_enabled) {
+		_hil_enabled = false;
+		configure_stream("HIL_CONTROLS", 0.0f);
+
+	} else {
+		ret = ERROR;
+	}
+
+	return ret;
+}
+
+void
+Mavlink::send_message(const mavlink_message_t *msg)
+{
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+
+	uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
+	mavlink_send_uart_bytes(_channel, buf, len);
+}
+
+void
+Mavlink::handle_message(const mavlink_message_t *msg)
+{
+	/* handle packet with mission manager */
+	_mission_manager->handle_message(msg);
+
+	/* handle packet with parameter component */
+	mavlink_pm_message_handler(_channel, msg);
+
+	if (get_forwarding_on()) {
+		/* forward any messages to other mavlink instances */
+		Mavlink::forward_message(msg, this);
+	}
+}
+
+int
+Mavlink::mavlink_pm_queued_send()
+{
+	if (_mavlink_param_queue_index < param_count()) {
+		mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index));
+		_mavlink_param_queue_index++;
+		return 0;
+
+	} else {
+		return 1;
+	}
+}
+
+void Mavlink::mavlink_pm_start_queued_send()
+{
+	_mavlink_param_queue_index = 0;
+}
+
+int Mavlink::mavlink_pm_send_param_for_index(uint16_t index)
+{
+	return mavlink_pm_send_param(param_for_index(index));
+}
+
+int Mavlink::mavlink_pm_send_param_for_name(const char *name)
+{
+	return mavlink_pm_send_param(param_find(name));
+}
+
+int Mavlink::mavlink_pm_send_param(param_t param)
+{
+	if (param == PARAM_INVALID) { return 1; }
+
+	/* buffers for param transmission */
+	char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
+	float val_buf;
+	mavlink_message_t tx_msg;
+
+	/* query parameter type */
+	param_type_t type = param_type(param);
+	/* copy parameter name */
+	strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+
+	/*
+	 * Map onboard parameter type to MAVLink type,
+	 * endianess matches (both little endian)
+	 */
+	uint8_t mavlink_type;
+
+	if (type == PARAM_TYPE_INT32) {
+		mavlink_type = MAVLINK_TYPE_INT32_T;
+
+	} else if (type == PARAM_TYPE_FLOAT) {
+		mavlink_type = MAVLINK_TYPE_FLOAT;
+
+	} else {
+		mavlink_type = MAVLINK_TYPE_FLOAT;
+	}
+
+	/*
+	 * get param value, since MAVLink encodes float and int params in the same
+	 * space during transmission, copy param onto float val_buf
+	 */
+
+	int ret;
+
+	if ((ret = param_get(param, &val_buf)) != OK) {
+		return ret;
+	}
+
+	mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
+					  mavlink_system.compid,
+					  _channel,
+					  &tx_msg,
+					  name_buf,
+					  val_buf,
+					  mavlink_type,
+					  param_count(),
+					  param_get_index(param));
+	send_message(&tx_msg);
+	return OK;
+}
+
+void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
+{
+	switch (msg->msgid) {
+	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
+			mavlink_param_request_list_t req;
+			mavlink_msg_param_request_list_decode(msg, &req);
+			if (req.target_system == mavlink_system.sysid &&
+					(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
+				/* Start sending parameters */
+				mavlink_pm_start_queued_send();
+				send_statustext_info("[pm] sending list");
+			}
+		} break;
+
+	case MAVLINK_MSG_ID_PARAM_SET: {
+
+			/* Handle parameter setting */
+
+			if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
+				mavlink_param_set_t mavlink_param_set;
+				mavlink_msg_param_set_decode(msg, &mavlink_param_set);
+
+				if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
+					/* local name buffer to enforce null-terminated string */
+					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+					strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+					/* enforce null termination */
+					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+					/* attempt to find parameter, set and send it */
+					param_t param = param_find(name);
+
+					if (param == PARAM_INVALID) {
+						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
+						sprintf(buf, "[pm] unknown: %s", name);
+						send_statustext_info(buf);
+
+					} else {
+						/* set and send parameter */
+						param_set(param, &(mavlink_param_set.param_value));
+						mavlink_pm_send_param(param);
+					}
+				}
+			}
+		} break;
+
+	case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
+			mavlink_param_request_read_t mavlink_param_request_read;
+			mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
+
+			if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
+				/* when no index is given, loop through string ids and compare them */
+				if (mavlink_param_request_read.param_index == -1) {
+					/* local name buffer to enforce null-terminated string */
+					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+					strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+					/* enforce null termination */
+					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+					/* attempt to find parameter and send it */
+					mavlink_pm_send_param_for_name(name);
+
+				} else {
+					/* when index is >= 0, send this parameter again */
+					mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
+				}
+			}
+
+		} break;
+	}
+}
+
+int
+Mavlink::send_statustext_info(const char *string)
+{
+	return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string);
+}
+
+int
+Mavlink::send_statustext_critical(const char *string)
+{
+	return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string);
+}
+
+int
+Mavlink::send_statustext_emergency(const char *string)
+{
+	return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string);
+}
+
+int
+Mavlink::send_statustext(unsigned severity, 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';
+
+		/* Map severity */
+		switch (severity) {
+			case MAVLINK_IOC_SEND_TEXT_INFO:
+				statustext.severity = MAV_SEVERITY_INFO;
+				break;
+			case MAVLINK_IOC_SEND_TEXT_CRITICAL:
+				statustext.severity = MAV_SEVERITY_CRITICAL;
+				break;
+			case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
+				statustext.severity = MAV_SEVERITY_EMERGENCY;
+				break;
+		}
+
+		mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
+		return OK;
+
+	} else {
+		return ERROR;
+	}
+}
+
+MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
+{
+	/* check if already subscribed to this topic */
+	MavlinkOrbSubscription *sub;
+
+	LL_FOREACH(_subscriptions, sub) {
+		if (sub->get_topic() == topic) {
+			/* already subscribed */
+			return sub;
+		}
+	}
+
+	/* add new subscription */
+	MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic);
+
+	LL_APPEND(_subscriptions, sub_new);
+
+	return sub_new;
+}
+
+int
+Mavlink::configure_stream(const char *stream_name, const float rate)
+{
+	/* calculate interval in us, 0 means disabled stream */
+	unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0;
+
+	/* search if stream exists */
+	MavlinkStream *stream;
+	LL_FOREACH(_streams, stream) {
+		if (strcmp(stream_name, stream->get_name()) == 0) {
+			if (interval > 0) {
+				/* set new interval */
+				stream->set_interval(interval);
+
+			} else {
+				/* delete stream */
+				LL_DELETE(_streams, stream);
+				delete stream;
+				warnx("deleted stream %s", stream->get_name());
+			}
+
+			return OK;
+		}
+	}
+
+	if (interval == 0) {
+		/* stream was not active and is requested to be disabled, do nothing */
+		return OK;
+	}
+
+	/* search for stream with specified name in supported streams list */
+	for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
+
+		if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
+			/* create new instance */
+			stream = streams_list[i]->new_instance();
+			stream->set_channel(get_channel());
+			stream->set_interval(interval);
+			stream->subscribe(this);
+			LL_APPEND(_streams, stream);
+
+			return OK;
+		}
+	}
+
+	/* if we reach here, the stream list does not contain the stream */
+	warnx("stream %s not found", stream_name);
+
+	return ERROR;
+}
+
+void
+Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
+{
+	/* orb subscription must be done from the main thread,
+	 * set _subscribe_to_stream and _subscribe_to_stream_rate fields
+	 * which polled in mavlink main loop */
+	if (!_task_should_exit) {
+		/* wait for previous subscription completion */
+		while (_subscribe_to_stream != nullptr) {
+			usleep(MAIN_LOOP_DELAY / 2);
+		}
+
+		/* copy stream name */
+		unsigned n = strlen(stream_name) + 1;
+		char *s = new char[n];
+		strcpy(s, stream_name);
+
+		/* set subscription task */
+		_subscribe_to_stream_rate = rate;
+		_subscribe_to_stream = s;
+
+		/* wait for subscription */
+		do {
+			usleep(MAIN_LOOP_DELAY / 2);
+		} while (_subscribe_to_stream != nullptr);
+	}
+}
+
+int
+Mavlink::message_buffer_init(int size)
+{
+
+	_message_buffer.size = size;
+	_message_buffer.write_ptr = 0;
+	_message_buffer.read_ptr = 0;
+	_message_buffer.data = (char*)malloc(_message_buffer.size);
+
+	int ret;
+	if (_message_buffer.data == 0) {
+		ret = ERROR;
+		_message_buffer.size = 0;
+	} else {
+		ret = OK;
+	}
+
+	return ret;
+}
+
+void
+Mavlink::message_buffer_destroy()
+{
+	_message_buffer.size = 0;
+	_message_buffer.write_ptr = 0;
+	_message_buffer.read_ptr = 0;
+	free(_message_buffer.data);
+}
+
+int
+Mavlink::message_buffer_count()
+{
+	int n = _message_buffer.write_ptr - _message_buffer.read_ptr;
+
+	if (n < 0) {
+		n += _message_buffer.size;
+	}
+
+	return n;
+}
+
+int
+Mavlink::message_buffer_is_empty()
+{
+	return _message_buffer.read_ptr == _message_buffer.write_ptr;
+}
+
+
+bool
+Mavlink::message_buffer_write(const void *ptr, int size)
+{
+	// bytes available to write
+	int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1;
+
+	if (available < 0) {
+		available += _message_buffer.size;
+	}
+
+	if (size > available) {
+		// buffer overflow
+		return false;
+	}
+
+	char *c = (char *) ptr;
+	int n = _message_buffer.size - _message_buffer.write_ptr;	// bytes to end of the buffer
+
+	if (n < size) {
+		// message goes over end of the buffer
+		memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n);
+		_message_buffer.write_ptr = 0;
+
+	} else {
+		n = 0;
+	}
+
+	// now: n = bytes already written
+	int p = size - n;	// number of bytes to write
+	memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p);
+	_message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size;
+	return true;
+}
+
+int
+Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part)
+{
+	// bytes available to read
+	int available = _message_buffer.write_ptr - _message_buffer.read_ptr;
+
+	if (available == 0) {
+		return 0;	// buffer is empty
+	}
+
+	int n = 0;
+
+	if (available > 0) {
+		// read pointer is before write pointer, all available bytes can be read
+		n = available;
+		*is_part = false;
+
+	} else {
+		// read pointer is after write pointer, read bytes from read_ptr to end of the buffer
+		n = _message_buffer.size - _message_buffer.read_ptr;
+		*is_part = _message_buffer.write_ptr > 0;
+	}
+
+	*ptr = &(_message_buffer.data[_message_buffer.read_ptr]);
+	return n;
+}
+
+void
+Mavlink::message_buffer_mark_read(int n)
+{
+	_message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size;
+}
+
+void
+Mavlink::pass_message(const mavlink_message_t *msg)
+{
+	if (_passing_on) {
+		/* size is 8 bytes plus variable payload */
+		int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len;
+		pthread_mutex_lock(&_message_buffer_mutex);
+		message_buffer_write(msg, size);
+		pthread_mutex_unlock(&_message_buffer_mutex);
+	}
+}
+
+float
+Mavlink::get_rate_mult()
+{
+	return _datarate / 1000.0f;
+}
+
+int
+Mavlink::task_main(int argc, char *argv[])
+{
+	int ch;
+	_baudrate = 57600;
+	_datarate = 0;
+	_mode = MAVLINK_MODE_NORMAL;
+
+	/* work around some stupidity in task_create's argv handling */
+	argc -= 2;
+	argv += 2;
+
+	/* don't exit from getopt loop to leave getopt global variables in consistent state,
+	 * set error flag instead */
+	bool err_flag = false;
+
+	while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) {
+		switch (ch) {
+		case 'b':
+			_baudrate = strtoul(optarg, NULL, 10);
+
+			if (_baudrate < 9600 || _baudrate > 921600) {
+				warnx("invalid baud rate '%s'", optarg);
+				err_flag = true;
+			}
+
+			break;
+
+		case 'r':
+			_datarate = strtoul(optarg, NULL, 10);
+
+			if (_datarate < 10 || _datarate > MAX_DATA_RATE) {
+				warnx("invalid data rate '%s'", optarg);
+				err_flag = true;
+			}
+
+			break;
+
+		case 'd':
+			_device_name = optarg;
+			break;
+
+//		case 'e':
+//			mavlink_link_termination_allowed = true;
+//			break;
+
+		case 'm':
+			if (strcmp(optarg, "custom") == 0) {
+				_mode = MAVLINK_MODE_CUSTOM;
+
+			} else if (strcmp(optarg, "camera") == 0) {
+				_mode = MAVLINK_MODE_CAMERA;
+			}
+
+			break;
+
+		case 'f':
+			_forwarding_on = true;
+			break;
+
+		case 'p':
+			_passing_on = true;
+			break;
+
+		case 'v':
+			_verbose = true;
+			break;
+
+		case 'w':
+			_wait_to_transmit = true;
+			break;
+
+		case 'x':
+			_ftp_on = true;
+			break;
+
+		default:
+			err_flag = true;
+			break;
+		}
+	}
+
+	if (err_flag) {
+		usage();
+		return ERROR;
+	}
+
+	if (_datarate == 0) {
+		/* convert bits to bytes and use 1/2 of bandwidth by default */
+		_datarate = _baudrate / 20;
+	}
+
+	if (_datarate > MAX_DATA_RATE) {
+		_datarate = MAX_DATA_RATE;
+	}
+
+	if (Mavlink::instance_exists(_device_name, this)) {
+		warnx("mavlink instance for %s already running", _device_name);
+		return ERROR;
+	}
+
+	/* inform about mode */
+	switch (_mode) {
+	case MAVLINK_MODE_NORMAL:
+		warnx("mode: NORMAL");
+		break;
+
+	case MAVLINK_MODE_CUSTOM:
+		warnx("mode: CUSTOM");
+		break;
+
+	case MAVLINK_MODE_CAMERA:
+		warnx("mode: CAMERA");
+		break;
+
+	default:
+		warnx("ERROR: Unknown mode");
+		break;
+	}
+
+	warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
+
+	/* flush stdout in case MAVLink is about to take it over */
+	fflush(stdout);
+
+	struct termios uart_config_original;
+
+	/* default values for arguments */
+	_uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &_is_usb_uart);
+
+	if (_uart_fd < 0) {
+		warn("could not open %s", _device_name);
+		return ERROR;
+	}
+
+	/* initialize mavlink text message buffering */
+	mavlink_logbuffer_init(&_logbuffer, 5);
+
+	/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
+	if (_passing_on || _ftp_on) {
+		/* initialize message buffer if multiplexing is on or its needed for FTP.
+		 * make space for two messages plus off-by-one space as we use the empty element
+		 * marker ring buffer approach.
+		 */
+		if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) {
+			errx(1, "can't allocate message buffer, exiting");
+		}
+
+		/* initialize message buffer mutex */
+		pthread_mutex_init(&_message_buffer_mutex, NULL);
+	}
+
+	/* create the device node that's used for sending text log messages, etc. */
+	register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
+
+	/* initialize logging device */
+	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	/* Initialize system properties */
+	mavlink_update_system();
+
+	/* start the MAVLink receiver */
+	_receive_thread = MavlinkReceiver::receive_start(this);
+
+	_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
+
+	/* create mission manager */
+	_mission_manager = new MavlinkMissionManager(this);
+	_mission_manager->set_verbose(_verbose);
+
+	_task_running = true;
+
+	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
+	uint64_t param_time = 0;
+	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
+	uint64_t status_time = 0;
+
+	struct vehicle_status_s status;
+	status_sub->update(&status_time, &status);
+
+	MavlinkCommandsStream commands_stream(this, _channel);
+
+	/* add default streams depending on mode and intervals depending on datarate */
+	float rate_mult = get_rate_mult();
+
+	configure_stream("HEARTBEAT", 1.0f);
+
+	switch (_mode) {
+	case MAVLINK_MODE_NORMAL:
+		configure_stream("SYS_STATUS", 1.0f);
+		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
+		configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
+		configure_stream("ATTITUDE", 10.0f * rate_mult);
+		configure_stream("VFR_HUD", 10.0f * rate_mult);
+		configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
+		configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
+		configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
+		configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
+		configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
+		configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
+		configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
+		configure_stream("DISTANCE_SENSOR", 0.5f);
+		break;
+
+	case MAVLINK_MODE_CAMERA:
+		configure_stream("SYS_STATUS", 1.0f);
+		configure_stream("ATTITUDE", 15.0f * rate_mult);
+		configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult);
+		configure_stream("CAMERA_CAPTURE", 1.0f);
+		break;
+
+	default:
+		break;
+	}
+
+	/* don't send parameters on startup without request */
+	_mavlink_param_queue_index = param_count();
+
+	MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
+
+	/* set main loop delay depending on data rate to minimize CPU overhead */
+	_main_loop_delay = MAIN_LOOP_DELAY / rate_mult;
+
+	/* now the instance is fully initialized and we can bump the instance count */
+	LL_APPEND(_mavlink_instances, this);
+
+	while (!_task_should_exit) {
+		/* main loop */
+		usleep(_main_loop_delay);
+
+		perf_begin(_loop_perf);
+
+		hrt_abstime t = hrt_absolute_time();
+
+		if (param_sub->update(&param_time, nullptr)) {
+			/* parameters updated */
+			mavlink_update_system();
+		}
+
+		if (status_sub->update(&status_time, &status)) {
+			/* switch HIL mode if required */
+			set_hil_enabled(status.hil_state == HIL_STATE_ON);
+		}
+
+		/* update commands stream */
+		commands_stream.update(t);
+
+		/* check for requested subscriptions */
+		if (_subscribe_to_stream != nullptr) {
+			if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
+				if (_subscribe_to_stream_rate > 0.0f) {
+					warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate);
+
+				} else {
+					warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
+				}
+
+			} else {
+				warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name);
+			}
+
+			delete _subscribe_to_stream;
+			_subscribe_to_stream = nullptr;
+		}
+
+		/* update streams */
+		MavlinkStream *stream;
+		LL_FOREACH(_streams, stream) {
+			stream->update(t);
+		}
+
+		if (fast_rate_limiter.check(t)) {
+			mavlink_pm_queued_send();
+			_mission_manager->eventloop();
+
+			if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
+				struct mavlink_logmessage msg;
+				int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
+
+				if (lb_ret == OK) {
+					send_statustext(msg.severity, msg.text);
+				}
+			}
+		}
+
+		/* pass messages from other UARTs or FTP worker */
+		if (_passing_on || _ftp_on) {
+
+			bool is_part;
+			uint8_t *read_ptr;
+            uint8_t *write_ptr;
+
+			pthread_mutex_lock(&_message_buffer_mutex);
+			int available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
+			pthread_mutex_unlock(&_message_buffer_mutex);
+
+			if (available > 0) {
+                // Reconstruct message from buffer
+
+				mavlink_message_t msg;
+                write_ptr = (uint8_t*)&msg;
+
+                // Pull a single message from the buffer
+                size_t read_count = available;
+                if (read_count > sizeof(mavlink_message_t)) {
+                    read_count = sizeof(mavlink_message_t);
+                }
+                
+                memcpy(write_ptr, read_ptr, read_count);
+                
+                // We hold the mutex until after we complete the second part of the buffer. If we don't
+                // we may end up breaking the empty slot overflow detection semantics when we mark the
+                // possibly partial read below.
+                pthread_mutex_lock(&_message_buffer_mutex);
+                
+				message_buffer_mark_read(read_count);
+
+				/* write second part of buffer if there is some */
+				if (is_part && read_count < sizeof(mavlink_message_t)) {
+                    write_ptr += read_count;
+					available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
+                    read_count = sizeof(mavlink_message_t) - read_count;
+                    memcpy(write_ptr, read_ptr, read_count);
+					message_buffer_mark_read(available);
+				}
+                
+                pthread_mutex_unlock(&_message_buffer_mutex);
+
+                _mavlink_resend_uart(_channel, &msg);
+			}
+		}
+
+		perf_end(_loop_perf);
+	}
+
+	delete _mission_manager;
+
+	delete _subscribe_to_stream;
+	_subscribe_to_stream = nullptr;
+
+	/* delete streams */
+	MavlinkStream *stream_to_del = nullptr;
+	MavlinkStream *stream_next = _streams;
+
+	while (stream_next != nullptr) {
+		stream_to_del = stream_next;
+		stream_next = stream_to_del->next;
+		delete stream_to_del;
+	}
+
+	_streams = nullptr;
+
+	/* delete subscriptions */
+	MavlinkOrbSubscription *sub_to_del = nullptr;
+	MavlinkOrbSubscription *sub_next = _subscriptions;
+
+	while (sub_next != nullptr) {
+		sub_to_del = sub_next;
+		sub_next = sub_to_del->next;
+		delete sub_to_del;
+	}
+
+	_subscriptions = nullptr;
+
+	warnx("waiting for UART receive thread");
+
+	/* wait for threads to complete */
+	pthread_join(_receive_thread, NULL);
+
+	/* reset the UART flags to original state */
+	tcsetattr(_uart_fd, TCSANOW, &uart_config_original);
+
+	/* close UART */
+	close(_uart_fd);
+
+	/* close mavlink logging device */
+	close(_mavlink_fd);
+
+	if (_passing_on || _ftp_on) {
+		message_buffer_destroy();
+		pthread_mutex_destroy(&_message_buffer_mutex);
+	}
+	/* destroy log buffer */
+	mavlink_logbuffer_destroy(&_logbuffer);
+
+	warnx("exiting");
+	_task_running = false;
+
+	return OK;
+}
+
+int Mavlink::start_helper(int argc, char *argv[])
+{
+	/* create the instance in task context */
+	Mavlink *instance = new Mavlink();
+
+	int res;
+
+	if (!instance) {
+
+		/* out of memory */
+		res = -ENOMEM;
+		warnx("OUT OF MEM");
+	} else {
+		/* this will actually only return once MAVLink exits */
+		res = instance->task_main(argc, argv);
+
+		/* delete instance on main thread end */
+		delete instance;
+	}
+
+	return res;
+}
+
+int
+Mavlink::start(int argc, char *argv[])
+{
+	// Wait for the instance count to go up one
+	// before returning to the shell
+	int ic = Mavlink::instance_count();
+
+	// Instantiate thread
+	char buf[24];
+	sprintf(buf, "mavlink_if%d", ic);
+
+	// This is where the control flow splits
+	// between the starting task and the spawned
+	// task - start_helper() only returns
+	// when the started task exits.
+	task_spawn_cmd(buf,
+		       SCHED_DEFAULT,
+		       SCHED_PRIORITY_DEFAULT,
+		       2700,
+		       (main_t)&Mavlink::start_helper,
+		       (const char **)argv);
+
+	// Ensure that this shell command
+	// does not return before the instance
+	// is fully initialized. As this is also
+	// the only path to create a new instance,
+	// this is effectively a lock on concurrent
+	// instance starting. XXX do a real lock.
+
+	// Sleep 500 us between each attempt
+	const unsigned sleeptime = 500;
+
+	// Wait 100 ms max for the startup.
+	const unsigned limit = 100 * 1000 / sleeptime;
+
+	unsigned count = 0;
+
+	while (ic == Mavlink::instance_count() && count < limit) {
+		::usleep(sleeptime);
+		count++;
+	}
+
+	return OK;
+}
+
+void
+Mavlink::display_status()
+{
+	warnx("running");
+}
+
+int
+Mavlink::stream_command(int argc, char *argv[])
+{
+	const char *device_name = DEFAULT_DEVICE_NAME;
+	float rate = -1.0f;
+	const char *stream_name = nullptr;
+
+	argc -= 2;
+	argv += 2;
+
+	/* don't exit from getopt loop to leave getopt global variables in consistent state,
+	 * set error flag instead */
+	bool err_flag = false;
+
+	int i = 0;
+
+	while (i < argc) {
+
+		if (0 == strcmp(argv[i], "-r") && i < argc - 1) {
+			rate = strtod(argv[i + 1], nullptr);
+
+			if (rate < 0.0f) {
+				err_flag = true;
+			}
+
+			i++;
+
+		} else if (0 == strcmp(argv[i], "-d") && i < argc - 1) {
+			device_name = argv[i + 1];
+			i++;
+
+		} else if (0 == strcmp(argv[i], "-s") && i < argc - 1) {
+			stream_name = argv[i + 1];
+			i++;
+
+		} else {
+			err_flag = true;
+		}
+
+		i++;
+	}
+
+	if (!err_flag && rate >= 0.0f && stream_name != nullptr) {
+		Mavlink *inst = get_instance_for_device(device_name);
+
+		if (inst != nullptr) {
+			inst->configure_stream_threadsafe(stream_name, rate);
+
+		} else {
+
+			// If the link is not running we should complain, but not fall over
+			// because this is so easy to get wrong and not fatal. Warning is sufficient.
+			errx(0, "mavlink for device %s is not running", device_name);
+		}
+
+	} else {
+		errx(1, "usage: mavlink stream [-d device] -s stream -r rate");
+	}
+
+	return OK;
+}
+
+static void usage()
+{
+	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
+}
+
+int mavlink_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		usage();
+		exit(1);
+	}
+
+	if (!strcmp(argv[1], "start")) {
+		return Mavlink::start(argc, argv);
+
+	} else if (!strcmp(argv[1], "stop")) {
+		warnx("mavlink stop is deprecated, use stop-all instead");
+		usage();
+		exit(1);
+
+	} else if (!strcmp(argv[1], "stop-all")) {
+		return Mavlink::destroy_all_instances();
+
+		// } else if (!strcmp(argv[1], "status")) {
+		// 	mavlink::g_mavlink->status();
+
+	} else if (!strcmp(argv[1], "stream")) {
+		return Mavlink::stream_command(argc, argv);
+
+	} else {
+		usage();
+		exit(1);
+	}
+
+	return 0;
+}
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
new file mode 100644
index 000000000..acfc8b90e
--- /dev/null
+++ b/src/modules/mavlink/mavlink_main.h
@@ -0,0 +1,375 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_main.h
+ * MAVLink 1.0 protocol interface definition.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <nuttx/fs/fs.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+#include <pthread.h>
+#include <mavlink/mavlink_log.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+
+#include "mavlink_bridge_header.h"
+#include "mavlink_orb_subscription.h"
+#include "mavlink_stream.h"
+#include "mavlink_messages.h"
+#include "mavlink_mission.h"
+
+
+class Mavlink
+{
+
+public:
+	/**
+	 * Constructor
+	 */
+	Mavlink();
+
+	/**
+	 * Destructor, also kills the mavlinks task.
+	 */
+	~Mavlink();
+
+	/**
+	* Start the mavlink task.
+	 *
+	 * @return		OK on success.
+	 */
+	static int		start(int argc, char *argv[]);
+
+	/**
+	 * Display the mavlink status.
+	 */
+	void			display_status();
+
+	static int		stream_command(int argc, char *argv[]);
+
+	static int		instance_count();
+
+	static Mavlink		*new_instance();
+
+	static Mavlink		*get_instance(unsigned instance);
+
+	static Mavlink		*get_instance_for_device(const char *device_name);
+
+	static int		destroy_all_instances();
+
+	static bool		instance_exists(const char *device_name, Mavlink *self);
+
+	static void		forward_message(const mavlink_message_t *msg, Mavlink *self);
+
+	static int		get_uart_fd(unsigned index);
+
+	int			get_uart_fd();
+
+	/**
+	 * Get the MAVLink system id.
+	 *
+	 * @return		The system ID of this vehicle
+	 */
+	int			get_system_id();
+
+	/**
+	 * Get the MAVLink component id.
+	 *
+	 * @return		The component ID of this vehicle
+	 */
+	int			get_component_id();
+
+	const char *_device_name;
+
+	enum MAVLINK_MODE {
+		MAVLINK_MODE_NORMAL = 0,
+		MAVLINK_MODE_CUSTOM,
+		MAVLINK_MODE_CAMERA
+	};
+
+	void			set_mode(enum MAVLINK_MODE);
+	enum MAVLINK_MODE	get_mode() { return _mode; }
+
+	bool			get_hil_enabled() { return _hil_enabled; }
+
+	bool			get_use_hil_gps() { return _use_hil_gps; }
+
+	bool			get_flow_control_enabled() { return _flow_control_enabled; }
+
+	bool			get_forwarding_on() { return _forwarding_on; }
+
+	static int		start_helper(int argc, char *argv[]);
+
+	/**
+	 * Handle parameter related messages.
+	 */
+	void			mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
+
+	void			get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
+
+	/**
+	 * Enable / disable Hardware in the Loop simulation mode.
+	 *
+	 * @param hil_enabled	The new HIL enable/disable state.
+	 * @return		OK if the HIL state changed, ERROR if the
+	 *			requested change could not be made or was
+	 *			redundant.
+	 */
+	int			set_hil_enabled(bool hil_enabled);
+
+	void			send_message(const mavlink_message_t *msg);
+
+	void			handle_message(const mavlink_message_t *msg);
+
+	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
+
+	int			get_instance_id();
+
+	/**
+	 * Enable / disable hardware flow control.
+	 *
+	 * @param enabled	True if hardware flow control should be enabled
+	 */
+	int			enable_flow_control(bool enabled);
+
+	mavlink_channel_t	get_channel();
+
+	void			configure_stream_threadsafe(const char *stream_name, float rate);
+
+	bool			_task_should_exit;	/**< if true, mavlink task should exit */
+
+	int			get_mavlink_fd() { return _mavlink_fd; }
+
+	/**
+	 * Send a status text with loglevel INFO
+	 *
+	 * @param string the message to send (will be capped by mavlink max string length)
+	 */
+	int			send_statustext_info(const char *string);
+
+	/**
+	 * Send a status text with loglevel CRITICAL
+	 *
+	 * @param string the message to send (will be capped by mavlink max string length)
+	 */
+	int			send_statustext_critical(const char *string);
+
+	/**
+	 * Send a status text with loglevel EMERGENCY
+	 *
+	 * @param string the message to send (will be capped by mavlink max string length)
+	 */
+	int			send_statustext_emergency(const char *string);
+
+	/**
+	 * Send a status text with loglevel
+	 *
+	 * @param string the message to send (will be capped by mavlink max string length)
+	 * @param severity the log level, one of 
+	 */
+	int			send_statustext(unsigned severity, const char *string);
+	MavlinkStream *		get_streams() const { return _streams; }
+
+	float			get_rate_mult();
+
+	/* Functions for waiting to start transmission until message received. */
+	void			set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
+	bool			get_has_received_messages() { return _received_messages; }
+	void			set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
+	bool			get_wait_to_transmit() { return _wait_to_transmit; }
+	bool			should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
+
+	bool			message_buffer_write(const void *ptr, int size);
+    
+	void			lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
+	void			unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
+
+	/**
+	 * Count a transmision error
+	 */
+	void			count_txerr();
+
+protected:
+	Mavlink			*next;
+
+private:
+	int			_instance_id;
+
+	int			_mavlink_fd;
+	bool			_task_running;
+
+	/* states */
+	bool			_hil_enabled;		/**< Hardware In the Loop mode */
+	bool			_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
+	bool			_is_usb_uart;		/**< Port is USB */
+	bool        		_wait_to_transmit;  	/**< Wait to transmit until received messages. */
+	bool        		_received_messages;	/**< Whether we've received valid mavlink messages. */
+
+	unsigned		_main_loop_delay;	/**< mainloop delay, depends on data rate */
+
+	MavlinkOrbSubscription	*_subscriptions;
+	MavlinkStream		*_streams;
+
+	MavlinkMissionManager *_mission_manager;
+
+	orb_advert_t	_mission_pub;
+	int			_mission_result_sub;
+	MAVLINK_MODE _mode;
+
+	mavlink_channel_t _channel;
+
+	struct mavlink_logbuffer _logbuffer;
+	unsigned int		_total_counter;
+
+	pthread_t		_receive_thread;
+
+	bool			_verbose;
+	bool			_forwarding_on;
+	bool			_passing_on;
+	bool			_ftp_on;
+	int			_uart_fd;
+	int			_baudrate;
+	int			_datarate;
+
+	/**
+	 * If the queue index is not at 0, the queue sending
+	 * logic will send parameters from the current index
+	 * to len - 1, the end of the param list.
+	 */
+	unsigned int		_mavlink_param_queue_index;
+
+	bool			mavlink_link_termination_allowed;
+
+	char 			*_subscribe_to_stream;
+	float			_subscribe_to_stream_rate;
+
+	bool			_flow_control_enabled;
+
+	struct mavlink_message_buffer {
+		int write_ptr;
+		int read_ptr;
+		int size;
+		char *data;
+	};
+
+	mavlink_message_buffer	_message_buffer;
+
+	pthread_mutex_t		_message_buffer_mutex;
+
+	bool			_param_initialized;
+	param_t			_param_system_id;
+	param_t			_param_component_id;
+	param_t			_param_system_type;
+	param_t			_param_use_hil_gps;
+
+	perf_counter_t		_loop_perf;			/**< loop performance counter */
+	perf_counter_t		_txerr_perf;			/**< TX error counter */
+
+	/**
+	 * Send one parameter.
+	 *
+	 * @param param		The parameter id to send.
+	 * @return		zero on success, nonzero on failure.
+	 */
+	int			mavlink_pm_send_param(param_t param);
+
+	/**
+	 * Send one parameter identified by index.
+	 *
+	 * @param index		The index of the parameter to send.
+	 * @return		zero on success, nonzero else.
+	 */
+	int			mavlink_pm_send_param_for_index(uint16_t index);
+
+	/**
+	 * Send one parameter identified by name.
+	 *
+	 * @param name		The index of the parameter to send.
+	 * @return		zero on success, nonzero else.
+	 */
+	int			mavlink_pm_send_param_for_name(const char *name);
+
+	/**
+	 * Send a queue of parameters, one parameter per function call.
+	 *
+	 * @return		zero on success, nonzero on failure
+	 */
+	int			mavlink_pm_queued_send(void);
+
+	/**
+	 * Start sending the parameter queue.
+	 *
+	 * This function will not directly send parameters, but instead
+	 * activate the sending of one parameter on each call of
+	 * mavlink_pm_queued_send().
+	 * @see 		mavlink_pm_queued_send()
+	 */
+	void			mavlink_pm_start_queued_send();
+
+	void			mavlink_update_system();
+
+	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
+
+	int configure_stream(const char *stream_name, const float rate);
+
+	int message_buffer_init(int size);
+
+	void message_buffer_destroy();
+
+	int message_buffer_count();
+
+	int message_buffer_is_empty();
+
+	int message_buffer_get_ptr(void **ptr, bool *is_part);
+
+	void message_buffer_mark_read(int n);
+
+	void pass_message(const mavlink_message_t *msg);
+
+	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
+
+	/**
+	 * Main mavlink task.
+	 */
+	int		task_main(int argc, char *argv[]);
+};
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
new file mode 100644
index 000000000..7c864f127
--- /dev/null
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -0,0 +1,1796 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_messages.cpp
+ * MAVLink 1.0 message formatters implementation.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <stdio.h>
+#include <commander/px4_custom_mode.h>
+#include <lib/geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/position_setpoint_triplet.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 <uORB/topics/navigation_capabilities.h>
+#include <drivers/drv_rc_input.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_range_finder.h>
+
+#include <systemlib/err.h>
+
+#include "mavlink_messages.h"
+
+
+static uint16_t cm_uint16_from_m_float(float m);
+static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
+				   uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
+
+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 get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
+			    uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
+{
+	*mavlink_state = 0;
+	*mavlink_base_mode = 0;
+	*mavlink_custom_mode = 0;
+
+	/* HIL */
+	if (status->hil_state == HIL_STATE_ON) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+	}
+
+	/* arming state */
+	if (status->arming_state == ARMING_STATE_ARMED
+	    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+	}
+
+	/* main state */
+	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+
+	union px4_custom_mode custom_mode;
+	custom_mode.data = 0;
+
+	switch (status->nav_state) {
+
+		case NAVIGATION_STATE_MANUAL:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+			                      | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+			break;
+
+		case NAVIGATION_STATE_ACRO:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
+			break;
+
+		case NAVIGATION_STATE_ALTCTL:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
+			break;
+
+		case NAVIGATION_STATE_POSCTL:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
+			break;
+
+		case NAVIGATION_STATE_AUTO_MISSION:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+			break;
+
+		case NAVIGATION_STATE_AUTO_LOITER:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+			break;
+
+		case NAVIGATION_STATE_AUTO_RTL:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+			break;
+
+		case NAVIGATION_STATE_LAND:
+			/* fallthrough */
+		case NAVIGATION_STATE_DESCEND:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
+			break;
+
+		case NAVIGATION_STATE_AUTO_RTGS:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS;
+			break;
+
+		case NAVIGATION_STATE_TERMINATION:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+			break;
+
+		case NAVIGATION_STATE_OFFBOARD:
+			*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_OFFBOARD;
+			break;
+
+		case NAVIGATION_STATE_MAX:
+			/* this is an unused case, ignore */
+			break;
+
+	}
+
+	*mavlink_custom_mode = custom_mode.data;
+
+	/* set system state */
+	if (status->arming_state == ARMING_STATE_INIT
+	    || status->arming_state == ARMING_STATE_IN_AIR_RESTORE
+	    || status->arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
+		*mavlink_state = MAV_STATE_UNINIT;
+
+	} else if (status->arming_state == ARMING_STATE_ARMED) {
+		*mavlink_state = MAV_STATE_ACTIVE;
+
+	} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		*mavlink_state = MAV_STATE_CRITICAL;
+
+	} else if (status->arming_state == ARMING_STATE_STANDBY) {
+		*mavlink_state = MAV_STATE_STANDBY;
+
+	} else if (status->arming_state == ARMING_STATE_REBOOT) {
+		*mavlink_state = MAV_STATE_POWEROFF;
+
+	} else {
+		*mavlink_state = MAV_STATE_CRITICAL;
+	}
+}
+
+
+class MavlinkStreamHeartbeat : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamHeartbeat::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "HEARTBEAT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HEARTBEAT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamHeartbeat();
+	}
+
+private:
+	MavlinkOrbSubscription *status_sub;
+	MavlinkOrbSubscription *pos_sp_triplet_sub;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
+		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_status_s status;
+		struct position_setpoint_triplet_s pos_sp_triplet;
+
+		/* always send the heartbeat, independent of the update status of the topics */
+		if (!status_sub->update(&status)) {
+			/* if topic update failed fill it with defaults */
+			memset(&status, 0, sizeof(status));
+		}
+
+		if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+			/* if topic update failed fill it with defaults */
+			memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
+		}
+
+		uint8_t mavlink_state = 0;
+		uint8_t mavlink_base_mode = 0;
+		uint32_t mavlink_custom_mode = 0;
+		get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+
+		mavlink_msg_heartbeat_send(_channel,
+					   mavlink_system.type,
+					   MAV_AUTOPILOT_PX4,
+					   mavlink_base_mode,
+					   mavlink_custom_mode,
+					   mavlink_state);
+	}
+};
+
+
+class MavlinkStreamSysStatus : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamSysStatus::get_name_static();
+	}
+
+	static const char *get_name_static ()
+	{
+		return "SYS_STATUS";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_SYS_STATUS;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamSysStatus();
+	}
+
+private:
+	MavlinkOrbSubscription *status_sub;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_status_s status;
+
+		if (status_sub->update(&status)) {
+			mavlink_msg_sys_status_send(_channel,
+							status.onboard_control_sensors_present,
+							status.onboard_control_sensors_enabled,
+							status.onboard_control_sensors_health,
+							status.load * 1000.0f,
+							status.battery_voltage * 1000.0f,
+							status.battery_current * 100.0f,
+							status.battery_remaining * 100.0f,
+							status.drop_rate_comm,
+							status.errors_comm,
+							status.errors_count1,
+							status.errors_count2,
+							status.errors_count3,
+							status.errors_count4);
+		}
+	}
+};
+
+
+class MavlinkStreamHighresIMU : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamHighresIMU::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "HIGHRES_IMU";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HIGHRES_IMU;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamHighresIMU();
+	}
+
+private:
+	MavlinkOrbSubscription *sensor_sub;
+	uint64_t sensor_time;
+
+	uint64_t accel_timestamp;
+	uint64_t gyro_timestamp;
+	uint64_t mag_timestamp;
+	uint64_t baro_timestamp;
+
+protected:
+	explicit MavlinkStreamHighresIMU() : MavlinkStream(),
+		sensor_time(0),
+		accel_timestamp(0),
+		gyro_timestamp(0),
+		mag_timestamp(0),
+		baro_timestamp(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct sensor_combined_s sensor;
+
+		if (sensor_sub->update(&sensor_time, &sensor)) {
+			uint16_t fields_updated = 0;
+
+			if (accel_timestamp != sensor.accelerometer_timestamp) {
+				/* mark first three dimensions as changed */
+				fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
+				accel_timestamp = sensor.accelerometer_timestamp;
+			}
+
+			if (gyro_timestamp != sensor.timestamp) {
+				/* mark second group dimensions as changed */
+				fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
+				gyro_timestamp = sensor.timestamp;
+			}
+
+			if (mag_timestamp != sensor.magnetometer_timestamp) {
+				/* mark third group dimensions as changed */
+				fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
+				mag_timestamp = sensor.magnetometer_timestamp;
+			}
+
+			if (baro_timestamp != sensor.baro_timestamp) {
+				/* mark last group dimensions as changed */
+				fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
+				baro_timestamp = sensor.baro_timestamp;
+			}
+
+			mavlink_msg_highres_imu_send(_channel,
+						     sensor.timestamp,
+						     sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2],
+						     sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2],
+						     sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2],
+						     sensor.baro_pres_mbar, sensor.differential_pressure_pa,
+						     sensor.baro_alt_meter, sensor.baro_temp_celcius,
+						     fields_updated);
+		}
+	}
+};
+
+
+class MavlinkStreamAttitude : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamAttitude::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "ATTITUDE";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ATTITUDE;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamAttitude();
+	}
+
+private:
+	MavlinkOrbSubscription *att_sub;
+	uint64_t att_time;
+
+protected:
+	explicit MavlinkStreamAttitude() : MavlinkStream(),
+		att_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_attitude_s att;
+
+		if (att_sub->update(&att_time, &att)) {
+			mavlink_msg_attitude_send(_channel,
+						  att.timestamp / 1000,
+						  att.roll, att.pitch, att.yaw,
+						  att.rollspeed, att.pitchspeed, att.yawspeed);
+		}
+	}
+};
+
+
+class MavlinkStreamAttitudeQuaternion : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamAttitudeQuaternion::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "ATTITUDE_QUATERNION";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamAttitudeQuaternion();
+	}
+
+private:
+	MavlinkOrbSubscription *att_sub;
+	uint64_t att_time;
+
+protected:
+	explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(),
+		att_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_attitude_s att;
+
+		if (att_sub->update(&att_time, &att)) {
+			mavlink_msg_attitude_quaternion_send(_channel,
+							     att.timestamp / 1000,
+							     att.q[0],
+							     att.q[1],
+							     att.q[2],
+							     att.q[3],
+							     att.rollspeed,
+							     att.pitchspeed,
+							     att.yawspeed);
+		}
+	}
+};
+
+
+class MavlinkStreamVFRHUD : public MavlinkStream
+{
+public:
+
+	const char *get_name() const
+	{
+		return MavlinkStreamVFRHUD::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "VFR_HUD";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_VFR_HUD;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamVFRHUD();
+	}
+
+private:
+	MavlinkOrbSubscription *att_sub;
+	uint64_t att_time;
+
+	MavlinkOrbSubscription *pos_sub;
+	uint64_t pos_time;
+
+	MavlinkOrbSubscription *armed_sub;
+	uint64_t armed_time;
+
+	MavlinkOrbSubscription *act_sub;
+	uint64_t act_time;
+
+	MavlinkOrbSubscription *airspeed_sub;
+	uint64_t airspeed_time;
+
+protected:
+	explicit MavlinkStreamVFRHUD() : MavlinkStream(),
+		att_time(0),
+		pos_time(0),
+		armed_time(0),
+		act_time(0),
+		airspeed_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
+		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
+		armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed));
+		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0));
+		airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_attitude_s att;
+		struct vehicle_global_position_s pos;
+		struct actuator_armed_s armed;
+		struct actuator_controls_s act;
+		struct airspeed_s airspeed;
+
+		bool updated = att_sub->update(&att_time, &att);
+		updated |= pos_sub->update(&pos_time, &pos);
+		updated |= armed_sub->update(&armed_time, &armed);
+		updated |= act_sub->update(&act_time, &act);
+		updated |= airspeed_sub->update(&airspeed_time, &airspeed);
+
+		if (updated) {
+			float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
+			uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
+			float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
+
+			mavlink_msg_vfr_hud_send(_channel,
+						 airspeed.true_airspeed_m_s,
+						 groundspeed,
+						 heading,
+						 throttle,
+						 pos.alt,
+						 -pos.vel_d);
+		}
+	}
+};
+
+
+class MavlinkStreamGPSRawInt : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamGPSRawInt::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "GPS_RAW_INT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GPS_RAW_INT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamGPSRawInt();
+	}
+
+private:
+	MavlinkOrbSubscription *gps_sub;
+	uint64_t gps_time;
+
+protected:
+	explicit MavlinkStreamGPSRawInt() : MavlinkStream(),
+		gps_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_gps_position_s gps;
+
+		if (gps_sub->update(&gps_time, &gps)) {
+			mavlink_msg_gps_raw_int_send(_channel,
+						     gps.timestamp_position,
+						     gps.fix_type,
+						     gps.lat,
+						     gps.lon,
+						     gps.alt,
+						     cm_uint16_from_m_float(gps.eph),
+						     cm_uint16_from_m_float(gps.epv),
+						     gps.vel_m_s * 100.0f,
+						     _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+						     gps.satellites_used);
+		}
+	}
+};
+
+
+class MavlinkStreamGlobalPositionInt : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamGlobalPositionInt::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "GLOBAL_POSITION_INT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamGlobalPositionInt();
+	}
+
+private:
+	MavlinkOrbSubscription *pos_sub;
+	uint64_t pos_time;
+
+	MavlinkOrbSubscription *home_sub;
+	uint64_t home_time;
+
+protected:
+	explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(),
+		pos_time(0),
+		home_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
+		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_global_position_s pos;
+		struct home_position_s home;
+
+		bool updated = pos_sub->update(&pos_time, &pos);
+		updated |= home_sub->update(&home_time, &home);
+
+		if (updated) {
+			mavlink_msg_global_position_int_send(_channel,
+							     pos.timestamp / 1000,
+							     pos.lat * 1e7,
+							     pos.lon * 1e7,
+							     pos.alt * 1000.0f,
+							     (pos.alt - home.alt) * 1000.0f,
+							     pos.vel_n * 100.0f,
+							     pos.vel_e * 100.0f,
+							     pos.vel_d * 100.0f,
+							     _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
+		}
+	}
+};
+
+
+class MavlinkStreamLocalPositionNED : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamLocalPositionNED::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "LOCAL_POSITION_NED";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamLocalPositionNED();
+	}
+
+private:
+	MavlinkOrbSubscription *pos_sub;
+	uint64_t pos_time;
+
+protected:
+	explicit MavlinkStreamLocalPositionNED() : MavlinkStream(),
+		pos_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_local_position_s pos;
+
+		if (pos_sub->update(&pos_time, &pos)) {
+			mavlink_msg_local_position_ned_send(_channel,
+							    pos.timestamp / 1000,
+							    pos.x,
+							    pos.y,
+							    pos.z,
+							    pos.vx,
+							    pos.vy,
+							    pos.vz);
+		}
+	}
+};
+
+
+
+class MavlinkStreamViconPositionEstimate : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamViconPositionEstimate::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "VICON_POSITION_ESTIMATE";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamViconPositionEstimate();
+	}
+
+private:
+	MavlinkOrbSubscription *pos_sub;
+	uint64_t pos_time;
+
+protected:
+	explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(),
+		pos_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_vicon_position_s pos;
+
+		if (pos_sub->update(&pos_time, &pos)) {
+			mavlink_msg_vicon_position_estimate_send(_channel,
+								pos.timestamp / 1000,
+								pos.x,
+								pos.y,
+								pos.z,
+								pos.roll,
+								pos.pitch,
+								pos.yaw);
+		}
+	}
+};
+
+
+class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamGPSGlobalOrigin::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "GPS_GLOBAL_ORIGIN";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamGPSGlobalOrigin();
+	}
+
+private:
+	MavlinkOrbSubscription *home_sub;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		/* we're sending the GPS home periodically to ensure the
+		 * the GCS does pick it up at one point */
+		if (home_sub->is_published()) {
+			struct home_position_s home;
+
+			if (home_sub->update(&home)) {
+				mavlink_msg_gps_global_origin_send(_channel,
+								   (int32_t)(home.lat * 1e7),
+								   (int32_t)(home.lon * 1e7),
+								   (int32_t)(home.alt) * 1000.0f);
+			}
+		}
+	}
+};
+
+template <int N>
+class MavlinkStreamServoOutputRaw : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamServoOutputRaw<N>::get_name_static();
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
+	}
+
+	static const char *get_name_static()
+	{
+		switch (N) {
+			case 0:
+			return "SERVO_OUTPUT_RAW_0";
+
+			case 1:
+			return "SERVO_OUTPUT_RAW_1";
+
+			case 2:
+			return "SERVO_OUTPUT_RAW_2";
+
+			case 3:
+			return "SERVO_OUTPUT_RAW_3";
+		}
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamServoOutputRaw<N>();
+	}
+
+private:
+	MavlinkOrbSubscription *act_sub;
+	uint64_t act_time;
+
+protected:
+	explicit MavlinkStreamServoOutputRaw() : MavlinkStream(),
+		act_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		orb_id_t act_topics[] = {
+			ORB_ID(actuator_outputs_0),
+			ORB_ID(actuator_outputs_1),
+			ORB_ID(actuator_outputs_2),
+			ORB_ID(actuator_outputs_3)
+		};
+
+		act_sub = mavlink->add_orb_subscription(act_topics[N]);
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct actuator_outputs_s act;
+
+		if (act_sub->update(&act_time, &act)) {
+			mavlink_msg_servo_output_raw_send(_channel,
+							  act.timestamp / 1000,
+							  N,
+							  act.output[0],
+							  act.output[1],
+							  act.output[2],
+							  act.output[3],
+							  act.output[4],
+							  act.output[5],
+							  act.output[6],
+							  act.output[7]);
+		}
+	}
+};
+
+
+class MavlinkStreamHILControls : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamHILControls::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "HIL_CONTROLS";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HIL_CONTROLS;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamHILControls();
+	}
+
+private:
+	MavlinkOrbSubscription *status_sub;
+	uint64_t status_time;
+
+	MavlinkOrbSubscription *pos_sp_triplet_sub;
+	uint64_t pos_sp_triplet_time;
+
+	MavlinkOrbSubscription *act_sub;
+	uint64_t act_time;
+
+protected:
+	explicit MavlinkStreamHILControls() : MavlinkStream(),
+		status_time(0),
+		pos_sp_triplet_time(0),
+		act_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
+		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
+		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_status_s status;
+		struct position_setpoint_triplet_s pos_sp_triplet;
+		struct actuator_outputs_s act;
+
+		bool updated = act_sub->update(&act_time, &act);
+		updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet);
+		updated |= status_sub->update(&status_time, &status);
+
+		if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
+			/* translate the current syste state to mavlink state and mode */
+			uint8_t mavlink_state;
+			uint8_t mavlink_base_mode;
+			uint32_t mavlink_custom_mode;
+			get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+
+			float out[8];
+
+			const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
+
+			/* scale outputs depending on system type */
+			if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
+				mavlink_system.type == MAV_TYPE_HEXAROTOR ||
+				mavlink_system.type == MAV_TYPE_OCTOROTOR) {
+				/* multirotors: set number of rotor outputs depending on type */
+
+				unsigned n;
+
+				switch (mavlink_system.type) {
+				case MAV_TYPE_QUADROTOR:
+					n = 4;
+					break;
+
+				case MAV_TYPE_HEXAROTOR:
+					n = 6;
+					break;
+
+				default:
+					n = 8;
+					break;
+				}
+
+				for (unsigned i = 0; i < 8; i++) {
+					if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
+						if (i < n) {
+							/* scale PWM out 900..2100 us to 0..1 for rotors */
+							out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+
+						} else {
+							/* scale PWM out 900..2100 us to -1..1 for other channels */
+							out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+						}
+
+					} else {
+						/* send 0 when disarmed */
+						out[i] = 0.0f;
+					}
+				}
+
+			} else {
+				/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
+
+				for (unsigned i = 0; i < 8; i++) {
+					if (i != 3) {
+						/* scale PWM out 900..2100 us to -1..1 for normal channels */
+						out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+
+					} else {
+						/* scale PWM out 900..2100 us to 0..1 for throttle */
+						out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+					}
+
+				}
+			}
+
+			mavlink_msg_hil_controls_send(_channel,
+						      hrt_absolute_time(),
+						      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
+						      mavlink_base_mode,
+						      0);
+		}
+	}
+};
+
+
+class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamGlobalPositionSetpointInt::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "GLOBAL_POSITION_SETPOINT_INT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamGlobalPositionSetpointInt();
+	}
+
+private:
+	MavlinkOrbSubscription *pos_sp_triplet_sub;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct position_setpoint_triplet_s pos_sp_triplet;
+
+		if (pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+			mavlink_msg_global_position_setpoint_int_send(_channel,
+					MAV_FRAME_GLOBAL,
+					(int32_t)(pos_sp_triplet.current.lat * 1e7),
+					(int32_t)(pos_sp_triplet.current.lon * 1e7),
+					(int32_t)(pos_sp_triplet.current.alt * 1000),
+					(int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f));
+		}
+	}
+};
+
+
+class MavlinkStreamLocalPositionSetpoint : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamLocalPositionSetpoint::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "LOCAL_POSITION_SETPOINT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamLocalPositionSetpoint();
+	}
+
+private:
+	MavlinkOrbSubscription *pos_sp_sub;
+	uint64_t pos_sp_time;
+
+protected:
+	explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(),
+		pos_sp_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_local_position_setpoint_s pos_sp;
+
+		if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) {
+			mavlink_msg_local_position_setpoint_send(_channel,
+					MAV_FRAME_LOCAL_NED,
+					pos_sp.x,
+					pos_sp.y,
+					pos_sp.z,
+					pos_sp.yaw);
+		}
+	}
+};
+
+
+class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "ROLL_PITCH_YAW_THRUST_SETPOINT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamRollPitchYawThrustSetpoint();
+	}
+
+private:
+	MavlinkOrbSubscription *att_sp_sub;
+	uint64_t att_sp_time;
+
+protected:
+	explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(),
+		att_sp_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_attitude_setpoint_s att_sp;
+
+		if (att_sp_sub->update(&att_sp_time, &att_sp)) {
+			mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
+					att_sp.timestamp / 1000,
+					att_sp.roll_body,
+					att_sp.pitch_body,
+					att_sp.yaw_body,
+					att_sp.thrust);
+		}
+	}
+};
+
+
+class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
+	}
+
+private:
+	MavlinkOrbSubscription *att_rates_sp_sub;
+	uint64_t att_rates_sp_time;
+
+protected:
+	explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(),
+		att_rates_sp_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_rates_setpoint_s att_rates_sp;
+
+		if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) {
+			mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
+					att_rates_sp.timestamp / 1000,
+					att_rates_sp.roll,
+					att_rates_sp.pitch,
+					att_rates_sp.yaw,
+					att_rates_sp.thrust);
+		}
+	}
+};
+
+
+class MavlinkStreamRCChannelsRaw : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamRCChannelsRaw::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "RC_CHANNELS_RAW";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_RC_CHANNELS_RAW;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamRCChannelsRaw();
+	}
+
+private:
+	MavlinkOrbSubscription *rc_sub;
+	uint64_t rc_time;
+
+protected:
+	explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(),
+		rc_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct rc_input_values rc;
+
+		if (rc_sub->update(&rc_time, &rc)) {
+			const unsigned port_width = 8;
+
+			// Deprecated message (but still needed for compatibility!)
+			for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) {
+				/* Channels are sent in MAVLink main loop at a fixed interval */
+				mavlink_msg_rc_channels_raw_send(_channel,
+								 rc.timestamp_publication / 1000,
+								 i,
+								 (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX,
+								 (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX,
+								 rc.rssi);
+			}
+
+			// New message
+			mavlink_msg_rc_channels_send(_channel,
+					rc.timestamp_publication / 1000,
+					rc.channel_count,
+					((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX),
+					((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX),
+					((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX),
+					((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX),
+					((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX),
+					((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX),
+					((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX),
+					((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX),
+					((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX),
+					((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX),
+					((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX),
+					((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX),
+					((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX),
+					((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX),
+					((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX),
+					((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX),
+					((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX),
+					((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX),
+					rc.rssi);
+		}
+	}
+};
+
+
+class MavlinkStreamManualControl : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamManualControl::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "MANUAL_CONTROL";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_MANUAL_CONTROL;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamManualControl();
+	}
+
+private:
+	MavlinkOrbSubscription *manual_sub;
+	uint64_t manual_time;
+
+protected:
+	explicit MavlinkStreamManualControl() : MavlinkStream(),
+		manual_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct manual_control_setpoint_s manual;
+
+		if (manual_sub->update(&manual_time, &manual)) {
+			mavlink_msg_manual_control_send(_channel,
+							mavlink_system.sysid,
+							manual.x * 1000,
+							manual.y * 1000,
+							manual.z * 1000,
+							manual.r * 1000,
+							0);
+		}
+	}
+};
+
+
+class MavlinkStreamOpticalFlow : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamOpticalFlow::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "OPTICAL_FLOW";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_OPTICAL_FLOW;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamOpticalFlow();
+	}
+
+private:
+	MavlinkOrbSubscription *flow_sub;
+	uint64_t flow_time;
+
+protected:
+	explicit MavlinkStreamOpticalFlow() : MavlinkStream(),
+		flow_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct optical_flow_s flow;
+
+		if (flow_sub->update(&flow_time, &flow)) {
+			mavlink_msg_optical_flow_send(_channel,
+						      flow.timestamp,
+						      flow.sensor_id,
+						      flow.flow_raw_x, flow.flow_raw_y,
+						      flow.flow_comp_x_m, flow.flow_comp_y_m,
+						      flow.quality,
+						      flow.ground_distance_m);
+		}
+	}
+};
+
+class MavlinkStreamAttitudeControls : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamAttitudeControls::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "ATTITUDE_CONTROLS";
+	}
+
+	uint8_t get_id()
+	{
+		return 0;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamAttitudeControls();
+	}
+
+private:
+	MavlinkOrbSubscription *att_ctrl_sub;
+	uint64_t att_ctrl_time;
+
+protected:
+	explicit MavlinkStreamAttitudeControls() : MavlinkStream(),
+		att_ctrl_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct actuator_controls_s att_ctrl;
+
+		if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) {
+			/* send, add spaces so that string buffer is at least 10 chars long */
+			mavlink_msg_named_value_float_send(_channel,
+							   att_ctrl.timestamp / 1000,
+							   "rll ctrl    ",
+							   att_ctrl.control[0]);
+			mavlink_msg_named_value_float_send(_channel,
+							   att_ctrl.timestamp / 1000,
+							   "ptch ctrl    ",
+							   att_ctrl.control[1]);
+			mavlink_msg_named_value_float_send(_channel,
+							   att_ctrl.timestamp / 1000,
+							   "yaw ctrl     ",
+							   att_ctrl.control[2]);
+			mavlink_msg_named_value_float_send(_channel,
+							   att_ctrl.timestamp / 1000,
+							   "thr ctrl     ",
+							   att_ctrl.control[3]);
+		}
+	}
+};
+
+class MavlinkStreamNamedValueFloat : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamNamedValueFloat::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "NAMED_VALUE_FLOAT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamNamedValueFloat();
+	}
+
+private:
+	MavlinkOrbSubscription *debug_sub;
+	uint64_t debug_time;
+
+protected:
+	explicit MavlinkStreamNamedValueFloat() : MavlinkStream(),
+		debug_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct debug_key_value_s debug;
+
+		if (debug_sub->update(&debug_time, &debug)) {
+			/* enforce null termination */
+			debug.key[sizeof(debug.key) - 1] = '\0';
+
+			mavlink_msg_named_value_float_send(_channel,
+							   debug.timestamp_ms,
+							   debug.key,
+							   debug.value);
+		}
+	}
+};
+
+class MavlinkStreamCameraCapture : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamCameraCapture::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "CAMERA_CAPTURE";
+	}
+
+	uint8_t get_id()
+	{
+		return 0;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamCameraCapture();
+	}
+
+private:
+	MavlinkOrbSubscription *status_sub;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_status_s status;
+		(void)status_sub->update(&status);
+
+		if (status.arming_state == ARMING_STATE_ARMED
+		    || status.arming_state == ARMING_STATE_ARMED_ERROR) {
+
+			/* send camera capture on */
+			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
+
+		} else {
+			/* send camera capture off */
+			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
+		}
+	}
+};
+
+class MavlinkStreamDistanceSensor : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamDistanceSensor::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "DISTANCE_SENSOR";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_DISTANCE_SENSOR;
+	}
+
+	static MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamDistanceSensor();
+	}
+
+private:
+	MavlinkOrbSubscription *range_sub;
+	uint64_t range_time;
+
+protected:
+	explicit MavlinkStreamDistanceSensor() : MavlinkStream(),
+		range_time(0)
+	{}
+
+	void subscribe(Mavlink *mavlink)
+	{
+		range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
+	}
+
+	void send(const hrt_abstime t)
+	{
+		struct range_finder_report range;
+
+		if (range_sub->update(&range_time, &range)) {
+
+			uint8_t type;
+
+			switch (range.type) {
+				case RANGE_FINDER_TYPE_LASER:
+				type = MAV_DISTANCE_SENSOR_LASER;
+				break;
+			}
+
+			uint8_t id = 0;
+			uint8_t orientation = 0;
+			uint8_t covariance = 20;
+
+			mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation,
+				range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance);
+		}
+	}
+};
+
+
+StreamListItem *streams_list[] = {
+	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
+	new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static),
+	new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static),
+	new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static),
+	new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static),
+	new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static),
+	new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static),
+	new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
+	new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
+	new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static),
+	new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static),
+	new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static),
+	new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static),
+	new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static),
+	new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static),
+	new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
+	new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
+	new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
+	new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
+	new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
+	new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
+	new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static),
+	new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),
+	nullptr
+};
diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/mavlink_messages.h
index 5e5ee8261..ee64d0e42 100644
--- a/src/modules/mavlink/util.h
+++ b/src/modules/mavlink/mavlink_messages.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,22 +32,30 @@
  ****************************************************************************/
 
 /**
- * @file util.h
- * Utility and helper functions and data.
+ * @file mavlink_messages.h
+ * MAVLink 1.0 message formatters definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#pragma once
+#ifndef MAVLINK_MESSAGES_H_
+#define MAVLINK_MESSAGES_H_
 
-/** MAVLink communications channel */
-extern uint8_t chan;
+#include "mavlink_stream.h"
 
-/** Shutdown marker */
-extern volatile bool thread_should_exit;
+class StreamListItem {
 
-/** Waypoint storage */
-extern mavlink_wpm_storage *wpm;
+public:
+	MavlinkStream* (*new_instance)();
+	const char* (*get_name)();
 
-/**
- * Translate the custom state into standard mavlink modes and state.
- */
-extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
+	StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
+		new_instance(inst),
+		get_name(name) {};
+
+	~StreamListItem() {};
+};
+
+extern StreamListItem *streams_list[];
+
+#endif /* MAVLINK_MESSAGES_H_ */
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
new file mode 100644
index 000000000..068774c47
--- /dev/null
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -0,0 +1,828 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_mission.cpp
+ * MAVLink mission manager implementation.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "mavlink_mission.h"
+#include "mavlink_main.h"
+
+#include <math.h>
+#include <lib/geo/geo.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+
+#include <dataman/dataman.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+
+MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
+	_mavlink(mavlink),
+	_state(MAVLINK_WPM_STATE_IDLE),
+	_time_last_recv(0),
+	_time_last_sent(0),
+	_action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT),
+	_retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT),
+	_max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX),
+	_dataman_id(0),
+	_count(0),
+	_current_seq(0),
+	_transfer_dataman_id(0),
+	_transfer_count(0),
+	_transfer_seq(0),
+	_transfer_current_seq(0),
+	_transfer_partner_sysid(0),
+	_transfer_partner_compid(0),
+	_offboard_mission_sub(-1),
+	_mission_result_sub(-1),
+	_offboard_mission_pub(-1),
+	_slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()),
+	_verbose(false),
+	_channel(mavlink->get_channel()),
+	_comp_id(MAV_COMP_ID_MISSIONPLANNER)
+{
+	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
+	_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
+
+	init_offboard_mission();
+}
+
+MavlinkMissionManager::~MavlinkMissionManager()
+{
+	close(_offboard_mission_pub);
+	close(_mission_result_sub);
+}
+
+void
+MavlinkMissionManager::init_offboard_mission()
+{
+	mission_s mission_state;
+	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) {
+		_dataman_id = mission_state.dataman_id;
+		_count = mission_state.count;
+		_current_seq = mission_state.current_seq;
+
+		warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq);
+
+	} else {
+		_dataman_id = 0;
+		_count = 0;
+		_current_seq = 0;
+		warnx("offboard mission init: ERROR, reading mission state failed");
+	}
+}
+
+/**
+ * Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes.
+ */
+int
+MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int seq)
+{
+	struct mission_s mission;
+
+	mission.dataman_id = dataman_id;
+	mission.count = count;
+	mission.current_seq = seq;
+
+	/* update mission state in dataman */
+	int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
+
+	if (res == sizeof(mission_s)) {
+		/* update active mission state */
+		_dataman_id = dataman_id;
+		_count = count;
+		_current_seq = seq;
+
+		/* mission state saved successfully, publish offboard_mission topic */
+		if (_offboard_mission_pub < 0) {
+			_offboard_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
+
+		} else {
+			orb_publish(ORB_ID(offboard_mission), _offboard_mission_pub, &mission);
+		}
+
+		return OK;
+
+	} else {
+		warnx("ERROR: can't save mission state");
+		_mavlink->send_statustext(MAV_SEVERITY_CRITICAL, "ERROR: can't save mission state");
+
+		return ERROR;
+	}
+}
+
+void
+MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type)
+{
+	mavlink_message_t msg;
+	mavlink_mission_ack_t wpa;
+
+	wpa.target_system = sysid;
+	wpa.target_component = compid;
+	wpa.type = type;
+
+	mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa);
+	_mavlink->send_message(&msg);
+
+	if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
+}
+
+
+void
+MavlinkMissionManager::send_mission_current(uint16_t seq)
+{
+	if (seq < _count) {
+		mavlink_message_t msg;
+		mavlink_mission_current_t wpc;
+
+		wpc.seq = seq;
+
+		mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
+		_mavlink->send_message(&msg);
+
+	} else if (seq == 0 && _count == 0) {
+		/* don't broadcast if no WPs */
+
+	} else {
+		if (_verbose) { warnx("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq); }
+
+		_mavlink->send_statustext_critical("ERROR: wp index out of bounds");
+	}
+}
+
+
+void
+MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count)
+{
+	_time_last_sent = hrt_absolute_time();
+
+	mavlink_message_t msg;
+	mavlink_mission_count_t wpc;
+
+	wpc.target_system = sysid;
+	wpc.target_component = compid;
+	wpc.count = _count;
+
+	mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
+	_mavlink->send_message(&msg);
+
+	if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); }
+}
+
+
+void
+MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq)
+{
+	dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_dataman_id);
+	struct mission_item_s mission_item;
+
+	if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) {
+		_time_last_sent = hrt_absolute_time();
+
+		/* create mission_item_s from mavlink_mission_item_t */
+		mavlink_mission_item_t wp;
+		format_mavlink_mission_item(&mission_item, &wp);
+
+		mavlink_message_t msg;
+		wp.target_system = sysid;
+		wp.target_component = compid;
+		wp.seq = seq;
+		wp.current = (_current_seq == seq) ? 1 : 0;
+		mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp);
+		_mavlink->send_message(&msg);
+
+		if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); }
+
+	} else {
+		send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+		_mavlink->send_statustext_critical("Unable to read from micro SD");
+
+		if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); }
+	}
+}
+
+
+void
+MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq)
+{
+	if (seq < _max_count) {
+		_time_last_sent = hrt_absolute_time();
+
+		mavlink_message_t msg;
+		mavlink_mission_request_t wpr;
+		wpr.target_system = sysid;
+		wpr.target_component = compid;
+		wpr.seq = seq;
+		mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr);
+		_mavlink->send_message(&msg);
+
+		if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
+
+	} else {
+		_mavlink->send_statustext_critical("ERROR: Waypoint index exceeds list capacity");
+
+		if (_verbose) { warnx("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); }
+	}
+}
+
+
+void
+MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
+{
+	mavlink_message_t msg;
+	mavlink_mission_item_reached_t wp_reached;
+
+	wp_reached.seq = seq;
+
+	mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached);
+	_mavlink->send_message(&msg);
+
+	if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); }
+}
+
+
+void
+MavlinkMissionManager::eventloop()
+{
+	hrt_abstime now = hrt_absolute_time();
+
+	bool updated = false;
+	orb_check(_mission_result_sub, &updated);
+
+	if (updated) {
+		mission_result_s mission_result;
+		orb_copy(ORB_ID(mission_result), _mission_result_sub, &mission_result);
+
+		_current_seq = mission_result.seq_current;
+
+		if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); }
+
+		if (mission_result.reached) {
+			send_mission_item_reached((uint16_t)mission_result.seq_reached);
+		}
+
+		send_mission_current(_current_seq);
+
+	} else {
+		if (_slow_rate_limiter.check(now)) {
+			send_mission_current(_current_seq);
+		}
+	}
+
+	/* check for timed-out operations */
+	if (_state != MAVLINK_WPM_STATE_IDLE && hrt_elapsed_time(&_time_last_recv) > _action_timeout) {
+		_mavlink->send_statustext_critical("Operation timeout");
+
+		if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); }
+
+		_state = MAVLINK_WPM_STATE_IDLE;
+
+	} else if (_state == MAVLINK_WPM_STATE_GETLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) {
+		/* try to request item again after timeout */
+		send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
+
+	} else if (_state == MAVLINK_WPM_STATE_SENDLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) {
+		if (_transfer_seq == 0) {
+			/* try to send items count again after timeout */
+			send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count);
+
+		} else {
+			/* try to send item again after timeout */
+			send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq - 1);
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_message(const mavlink_message_t *msg)
+{
+	switch (msg->msgid) {
+	case MAVLINK_MSG_ID_MISSION_ACK:
+		handle_mission_ack(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
+		handle_mission_set_current(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
+		handle_mission_request_list(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_REQUEST:
+		handle_mission_request(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_COUNT:
+		handle_mission_count(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_ITEM:
+		handle_mission_item(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
+		handle_mission_clear_all(msg);
+		break;
+
+	default:
+		break;
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
+{
+	mavlink_mission_ack_t wpa;
+	mavlink_msg_mission_ack_decode(msg, &wpa);
+
+	if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) {
+		if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) {
+			if (_state == MAVLINK_WPM_STATE_SENDLIST) {
+				_time_last_recv = hrt_absolute_time();
+
+				if (_transfer_seq == _count) {
+					if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); }
+
+				} else {
+					_mavlink->send_statustext_critical("WPM: ERR: not all items sent -> IDLE");
+
+					if (_verbose) { warnx("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); }
+				}
+
+				_state = MAVLINK_WPM_STATE_IDLE;
+			}
+
+		} else {
+			_mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch");
+
+			if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); }
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
+{
+	mavlink_mission_set_current_t wpc;
+	mavlink_msg_mission_set_current_decode(msg, &wpc);
+
+	if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
+		if (_state == MAVLINK_WPM_STATE_IDLE) {
+			_time_last_recv = hrt_absolute_time();
+
+			if (wpc.seq < _count) {
+				if (update_active_mission(_dataman_id, _count, wpc.seq) == OK) {
+					if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); }
+
+				} else {
+					if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); }
+
+					_mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID");
+				}
+
+			} else {
+				if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); }
+
+				_mavlink->send_statustext_critical("WPM: WP CURR CMD: Not in list");
+			}
+
+		} else {
+			if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); }
+
+			_mavlink->send_statustext_critical("WPM: IGN WP CURR CMD: Busy");
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
+{
+	mavlink_mission_request_list_t wprl;
+	mavlink_msg_mission_request_list_decode(msg, &wprl);
+
+	if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
+		if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
+			_time_last_recv = hrt_absolute_time();
+
+			if (_count > 0) {
+				_state = MAVLINK_WPM_STATE_SENDLIST;
+				_transfer_seq = 0;
+				_transfer_count = _count;
+				_transfer_partner_sysid = msg->sysid;
+				_transfer_partner_compid = msg->compid;
+
+				if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); }
+
+			} else {
+				if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); }
+
+				_mavlink->send_statustext_info("WPM: mission is empty");
+			}
+
+			send_mission_count(msg->sysid, msg->compid, _count);
+
+		} else {
+			if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); }
+
+			_mavlink->send_statustext_critical("IGN REQUEST LIST: Busy");
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg)
+{
+	mavlink_mission_request_t wpr;
+	mavlink_msg_mission_request_decode(msg, &wpr);
+
+	if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
+		if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) {
+			if (_state == MAVLINK_WPM_STATE_SENDLIST) {
+				_time_last_recv = hrt_absolute_time();
+
+				/* _transfer_seq contains sequence of expected request */
+				if (wpr.seq == _transfer_seq && _transfer_seq < _transfer_count) {
+					if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u", wpr.seq, msg->sysid); }
+
+					_transfer_seq++;
+
+				} else if (wpr.seq == _transfer_seq - 1) {
+					if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u (again)", wpr.seq, msg->sysid); }
+
+				} else {
+					if (_transfer_seq > 0 && _transfer_seq < _transfer_count) {
+						if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i or %i", wpr.seq, msg->sysid, _transfer_seq - 1, _transfer_seq); }
+
+					} else if (_transfer_seq <= 0) {
+						if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq); }
+
+					} else {
+						if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq - 1); }
+					}
+
+					_state = MAVLINK_WPM_STATE_IDLE;
+
+					send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+					_mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected");
+					return;
+				}
+
+				/* double check bounds in case of items count changed */
+				if (wpr.seq < _count) {
+					send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq);
+
+				} else {
+					if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [%u, %u]", (unsigned)wpr.seq, (unsigned)wpr.seq, (unsigned)_count - 1); }
+
+					_state = MAVLINK_WPM_STATE_IDLE;
+
+					send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+					_mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected");
+				}
+
+			} else if (_state == MAVLINK_WPM_STATE_IDLE) {
+				if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: no transfer"); }
+
+				_mavlink->send_statustext_critical("IGN MISSION_ITEM_REQUEST: No active transfer");
+
+			} else {
+				if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _state); }
+
+				_mavlink->send_statustext_critical("WPM: REJ. CMD: Busy");
+			}
+
+		} else {
+			_mavlink->send_statustext_critical("WPM: REJ. CMD: partner id mismatch");
+
+			if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); }
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
+{
+	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*/) {
+		if (_state == MAVLINK_WPM_STATE_IDLE) {
+			_time_last_recv = hrt_absolute_time();
+
+			if (wpc.count > _max_count) {
+				if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, _max_count); }
+
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE);
+				return;
+			}
+
+			if (wpc.count == 0) {
+				if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); }
+
+				/* alternate dataman ID anyway to let navigator know about changes */
+				update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0);
+				_mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION");
+
+				// TODO send ACK?
+				return;
+			}
+
+			if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); }
+
+			_state = MAVLINK_WPM_STATE_GETLIST;
+			_transfer_seq = 0;
+			_transfer_partner_sysid = msg->sysid;
+			_transfer_partner_compid = msg->compid;
+			_transfer_count = wpc.count;
+			_transfer_dataman_id = _dataman_id == 0 ? 1 : 0;	// use inactive storage for transmission
+			_transfer_current_seq = -1;
+
+		} else if (_state == MAVLINK_WPM_STATE_GETLIST) {
+			_time_last_recv = hrt_absolute_time();
+
+			if (_transfer_seq == 0) {
+				/* looks like our MISSION_REQUEST was lost, try again */
+				if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); }
+
+				_mavlink->send_statustext_info("WP CMD OK TRY AGAIN");
+
+			} else {
+				if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); }
+
+				_mavlink->send_statustext_critical("WPM: REJ. CMD: Busy");
+				return;
+			}
+
+		} else {
+			if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _state); }
+
+			_mavlink->send_statustext_critical("WPM: IGN MISSION_COUNT: Busy");
+			return;
+		}
+
+		send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
+{
+	mavlink_mission_item_t wp;
+	mavlink_msg_mission_item_decode(msg, &wp);
+
+	if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) {
+		if (_state == MAVLINK_WPM_STATE_GETLIST) {
+			_time_last_recv = hrt_absolute_time();
+
+			if (wp.seq != _transfer_seq) {
+				if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); }
+
+				/* don't send request here, it will be performed in eventloop after timeout */
+				return;
+			}
+
+		} else if (_state == MAVLINK_WPM_STATE_IDLE) {
+			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); }
+
+			_mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer");
+			return;
+
+		} else {
+			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); }
+
+			_mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy");
+			return;
+		}
+
+		struct mission_item_s mission_item;
+		int ret = parse_mavlink_mission_item(&wp, &mission_item);
+
+		if (ret != OK) {
+			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); }
+
+			_mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy");
+
+			send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret);
+			_state = MAVLINK_WPM_STATE_IDLE;
+			return;
+		}
+
+		dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id);
+
+		if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
+			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); }
+
+			send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+			_mavlink->send_statustext_critical("Unable to write on micro SD");
+			_state = MAVLINK_WPM_STATE_IDLE;
+			return;
+		}
+
+		/* waypoint marked as current */
+		if (wp.current) {
+			_transfer_current_seq = wp.seq;
+		}
+
+		if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); }
+
+		_transfer_seq = wp.seq + 1;
+
+		if (_transfer_seq == _transfer_count) {
+			/* got all new mission items successfully */
+			if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); }
+
+			_mavlink->send_statustext_info("WPM: Transfer complete.");
+
+			_state = MAVLINK_WPM_STATE_IDLE;
+
+			if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) {
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
+
+			} else {
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+			}
+
+		} else {
+			/* request next item */
+			send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
+{
+	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 */) {
+
+		if (_state == MAVLINK_WPM_STATE_IDLE) {
+			/* don't touch mission items storage itself, but only items count in mission state */
+			_time_last_recv = hrt_absolute_time();
+
+			if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == OK) {
+				if (_verbose) { warnx("WPM: CLEAR_ALL OK"); }
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
+
+			} else {
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+			}
+
+		} else {
+			_mavlink->send_statustext_critical("WPM: IGN CLEAR CMD: Busy");
+
+			if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); }
+		}
+	}
+}
+
+int
+MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
+{
+	/* only support global waypoints for now */
+	switch (mavlink_mission_item->frame) {
+	case MAV_FRAME_GLOBAL:
+		mission_item->lat = (double)mavlink_mission_item->x;
+		mission_item->lon = (double)mavlink_mission_item->y;
+		mission_item->altitude = mavlink_mission_item->z;
+		mission_item->altitude_is_relative = false;
+		break;
+
+	case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+		mission_item->lat = (double)mavlink_mission_item->x;
+		mission_item->lon = (double)mavlink_mission_item->y;
+		mission_item->altitude = mavlink_mission_item->z;
+		mission_item->altitude_is_relative = true;
+		break;
+
+	case MAV_FRAME_LOCAL_NED:
+	case MAV_FRAME_LOCAL_ENU:
+		return MAV_MISSION_UNSUPPORTED_FRAME;
+
+	case MAV_FRAME_MISSION:
+	default:
+		return MAV_MISSION_ERROR;
+	}
+
+	switch (mavlink_mission_item->command) {
+	case MAV_CMD_NAV_TAKEOFF:
+		mission_item->pitch_min = mavlink_mission_item->param1;
+		break;
+	case MAV_CMD_DO_JUMP:
+		mission_item->do_jump_mission_index = mavlink_mission_item->param1;
+		mission_item->do_jump_current_count = 0;
+		mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
+		break;
+	default:
+		mission_item->acceptance_radius = mavlink_mission_item->param2;
+		mission_item->time_inside = mavlink_mission_item->param1;
+		break;
+	}
+
+	mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+	mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
+	mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
+	mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+
+	mission_item->autocontinue = mavlink_mission_item->autocontinue;
+	// mission_item->index = mavlink_mission_item->seq;
+	mission_item->origin = ORIGIN_MAVLINK;
+
+	/* reset DO_JUMP count */
+	mission_item->do_jump_current_count = 0;
+
+	return MAV_MISSION_ACCEPTED;
+}
+
+
+int
+MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
+{
+	if (mission_item->altitude_is_relative) {
+		mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
+
+	} else {
+		mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+	}
+
+	switch (mission_item->nav_cmd) {
+	case NAV_CMD_TAKEOFF:
+		mavlink_mission_item->param1 = mission_item->pitch_min;
+		break;
+
+	case NAV_CMD_DO_JUMP:
+		mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
+		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
+		break;
+
+	default:
+		mavlink_mission_item->param2 = mission_item->acceptance_radius;
+		mavlink_mission_item->param1 = mission_item->time_inside;
+		break;
+	}
+
+	mavlink_mission_item->x = (float)mission_item->lat;
+	mavlink_mission_item->y = (float)mission_item->lon;
+	mavlink_mission_item->z = mission_item->altitude;
+
+	mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+	mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
+	mavlink_mission_item->command = mission_item->nav_cmd;
+	mavlink_mission_item->autocontinue = mission_item->autocontinue;
+	// mavlink_mission_item->seq = mission_item->index;
+
+	return OK;
+}
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
new file mode 100644
index 000000000..f63c32f24
--- /dev/null
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_mission.h
+ * MAVLink mission manager interface definition.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#pragma once
+
+#include "mavlink_bridge_header.h"
+#include "mavlink_rate_limiter.h"
+#include <uORB/uORB.h>
+
+// FIXME XXX - TO BE MOVED TO XML
+enum MAVLINK_WPM_STATES {
+	MAVLINK_WPM_STATE_IDLE = 0,
+	MAVLINK_WPM_STATE_SENDLIST,
+	MAVLINK_WPM_STATE_GETLIST,
+	MAVLINK_WPM_STATE_ENUM_END
+};
+
+enum MAVLINK_WPM_CODES {
+	MAVLINK_WPM_CODE_OK = 0,
+	MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
+	MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
+	MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
+	MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
+	MAVLINK_WPM_CODE_ENUM_END
+};
+
+#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000    ///< Protocol communication action timeout in useconds
+#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000        ///< Protocol communication retry timeout in useconds
+
+class Mavlink;
+
+class MavlinkMissionManager {
+public:
+	MavlinkMissionManager(Mavlink *parent);
+
+	~MavlinkMissionManager();
+
+	void init_offboard_mission();
+
+	int update_active_mission(int dataman_id, unsigned count, int seq);
+
+	void send_message(mavlink_message_t *msg);
+
+	/**
+	 *  @brief Sends an waypoint ack message
+	 */
+	void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type);
+
+	/**
+	 *  @brief Broadcasts the new target waypoint and directs the MAV to fly there
+	 *
+	 *  This function broadcasts its new active waypoint sequence number and
+	 *  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 send_mission_current(uint16_t seq);
+
+	void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count);
+
+	void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq);
+
+	void send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq);
+
+	/**
+	 *  @brief emits a message that a waypoint reached
+	 *
+	 *  This function broadcasts a message that a waypoint is reached.
+	 *
+	 *  @param seq The waypoint sequence number the MAV has reached.
+	 */
+	void send_mission_item_reached(uint16_t seq);
+
+	void eventloop();
+
+	void handle_message(const mavlink_message_t *msg);
+
+	void handle_mission_ack(const mavlink_message_t *msg);
+
+	void handle_mission_set_current(const mavlink_message_t *msg);
+
+	void handle_mission_request_list(const mavlink_message_t *msg);
+
+	void handle_mission_request(const mavlink_message_t *msg);
+
+	void handle_mission_count(const mavlink_message_t *msg);
+
+	void handle_mission_item(const mavlink_message_t *msg);
+
+	void handle_mission_clear_all(const mavlink_message_t *msg);
+
+	/**
+	 * Parse mavlink MISSION_ITEM message to get mission_item_s.
+	 */
+	int parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
+
+	/**
+	 * Format mission_item_s as mavlink MISSION_ITEM message.
+	 */
+	int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
+
+	void set_verbose(bool v) { _verbose = v; }
+
+private:
+	Mavlink*		_mavlink;
+
+	enum MAVLINK_WPM_STATES _state;					///< Current state
+
+	uint64_t		_time_last_recv;
+	uint64_t		_time_last_sent;
+
+	uint32_t		_action_timeout;
+	uint32_t		_retry_timeout;
+	unsigned		_max_count;				///< Maximum number of mission items
+
+	int			_dataman_id;				///< Dataman storage ID for active mission
+	unsigned		_count;					///< Count of items in active mission
+	int			_current_seq;				///< Current item sequence in active mission
+
+	int			_transfer_dataman_id;			///< Dataman storage ID for current transmission
+	unsigned		_transfer_count;			///< Items count in current transmission
+	unsigned		_transfer_seq;				///< Item sequence in current transmission
+	unsigned		_transfer_current_seq;			///< Current item ID for current transmission (-1 means not initialized)
+	unsigned		_transfer_partner_sysid;		///< Partner system ID for current transmission
+	unsigned		_transfer_partner_compid;		///< Partner component ID for current transmission
+
+	int			_offboard_mission_sub;
+	int			_mission_result_sub;
+	orb_advert_t		_offboard_mission_pub;
+
+	MavlinkRateLimiter	_slow_rate_limiter;
+
+	bool _verbose;
+
+	mavlink_channel_t	_channel;
+	uint8_t			_comp_id;
+};
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
new file mode 100644
index 000000000..734f0903a
--- /dev/null
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_orb_subscription.cpp
+ * uORB subscription implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <uORB/uORB.h>
+#include <stdio.h>
+
+#include "mavlink_orb_subscription.h"
+
+MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
+	next(nullptr),
+	_topic(topic),
+	_fd(orb_subscribe(_topic)),
+	_published(false)
+{
+}
+
+MavlinkOrbSubscription::~MavlinkOrbSubscription()
+{
+	close(_fd);
+}
+
+orb_id_t
+MavlinkOrbSubscription::get_topic() const
+{
+	return _topic;
+}
+
+bool
+MavlinkOrbSubscription::update(uint64_t *time, void* data)
+{
+	// TODO this is NOT atomic operation, we can get data newer than time
+	// if topic was published between orb_stat and orb_copy calls.
+
+	uint64_t time_topic;
+	if (orb_stat(_fd, &time_topic)) {
+		/* error getting last topic publication time */
+		time_topic = 0;
+	}
+
+	if (orb_copy(_topic, _fd, data)) {
+		/* error copying topic data */
+		memset(data, 0, _topic->o_size);
+		return false;
+
+	} else {
+		/* data copied successfully */
+		_published = true;
+		if (time_topic != *time) {
+			*time = time_topic;
+			return true;
+
+		} else {
+			return false;
+		}
+	}
+}
+
+bool
+MavlinkOrbSubscription::update(void* data)
+{
+	return !orb_copy(_topic, _fd, data);
+}
+
+bool
+MavlinkOrbSubscription::is_published()
+{
+	if (_published) {
+		return true;
+	}
+
+	bool updated;
+	orb_check(_fd, &updated);
+
+	if (updated) {
+		_published = true;
+	}
+
+	return _published;
+}
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
new file mode 100644
index 000000000..71efb43af
--- /dev/null
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_orb_subscription.h
+ * uORB subscription definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef MAVLINK_ORB_SUBSCRIPTION_H_
+#define MAVLINK_ORB_SUBSCRIPTION_H_
+
+#include <systemlib/uthash/utlist.h>
+#include <drivers/drv_hrt.h>
+
+
+class MavlinkOrbSubscription
+{
+public:
+	MavlinkOrbSubscription *next;	///< pointer to next subscription in list
+
+	MavlinkOrbSubscription(const orb_id_t topic);
+	~MavlinkOrbSubscription();
+
+	/**
+	 * Check if subscription updated and get data.
+	 *
+	 * @return true only if topic was updated and data copied to buffer successfully.
+	 * If topic was not updated since last check it will return false but still copy the data.
+	 * If no data available data buffer will be filled with zeroes.
+	 */
+	bool update(uint64_t *time, void* data);
+
+	/**
+	 * Copy topic data to given buffer.
+	 *
+	 * @return true only if topic data copied successfully.
+	 */
+	bool update(void* data);
+
+	/**
+	 * Check if the topic has been published.
+	 *
+	 * This call will return true if the topic was ever published.
+	 * @return true if the topic has been published at least once.
+	 */
+	bool is_published();
+	orb_id_t get_topic() const;
+
+private:
+	const orb_id_t _topic;		///< topic metadata
+	int _fd;			///< subscription handle
+	bool _published;		///< topic was ever published
+};
+
+
+#endif /* MAVLINK_ORB_SUBSCRIPTION_H_ */
diff --git a/src/modules/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c
deleted file mode 100644
index 18ca7a854..000000000
--- a/src/modules/mavlink/mavlink_parameters.c
+++ /dev/null
@@ -1,230 +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_parameters.c
- * MAVLink parameter protocol implementation (BSD-relicensed).
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include "mavlink_bridge_header.h"
-#include "mavlink_parameters.h"
-#include <uORB/uORB.h>
-#include "math.h" /* isinf / isnan checks */
-#include <assert.h>
-#include <stdio.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <stdbool.h>
-#include <string.h>
-#include <errno.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <sys/stat.h>
-
-extern mavlink_system_t mavlink_system;
-
-extern int mavlink_missionlib_send_message(mavlink_message_t *msg);
-extern int mavlink_missionlib_send_gcs_string(const char *string);
-
-/**
- * If the queue index is not at 0, the queue sending
- * logic will send parameters from the current index
- * to len - 1, the end of the param list.
- */
-static unsigned int mavlink_param_queue_index = 0;
-
-/**
- * Callback for param interface.
- */
-void mavlink_pm_callback(void *arg, param_t param);
-
-void mavlink_pm_callback(void *arg, param_t param)
-{
-	mavlink_pm_send_param(param);
-	usleep(*(unsigned int *)arg);
-}
-
-void mavlink_pm_send_all_params(unsigned int delay)
-{
-	unsigned int dbuf = delay;
-	param_foreach(&mavlink_pm_callback, &dbuf, false);
-}
-
-int mavlink_pm_queued_send()
-{
-	if (mavlink_param_queue_index < param_count()) {
-		mavlink_pm_send_param(param_for_index(mavlink_param_queue_index));
-		mavlink_param_queue_index++;
-		return 0;
-
-	} else {
-		return 1;
-	}
-}
-
-void mavlink_pm_start_queued_send()
-{
-	mavlink_param_queue_index = 0;
-}
-
-int mavlink_pm_send_param_for_index(uint16_t index)
-{
-	return mavlink_pm_send_param(param_for_index(index));
-}
-
-int mavlink_pm_send_param_for_name(const char *name)
-{
-	return mavlink_pm_send_param(param_find(name));
-}
-
-int mavlink_pm_send_param(param_t param)
-{
-	if (param == PARAM_INVALID) return 1;
-
-	/* buffers for param transmission */
-	static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
-	float val_buf;
-	static mavlink_message_t tx_msg;
-
-	/* query parameter type */
-	param_type_t type = param_type(param);
-	/* copy parameter name */
-	strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-
-	/*
-	 * Map onboard parameter type to MAVLink type,
-	 * endianess matches (both little endian)
-	 */
-	uint8_t mavlink_type;
-
-	if (type == PARAM_TYPE_INT32) {
-		mavlink_type = MAVLINK_TYPE_INT32_T;
-
-	} else if (type == PARAM_TYPE_FLOAT) {
-		mavlink_type = MAVLINK_TYPE_FLOAT;
-
-	} else {
-		mavlink_type = MAVLINK_TYPE_FLOAT;
-	}
-
-	/*
-	 * get param value, since MAVLink encodes float and int params in the same
-	 * space during transmission, copy param onto float val_buf
-	 */
-
-	int ret;
-
-	if ((ret = param_get(param, &val_buf)) != OK) {
-		return ret;
-	}
-
-	mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
-					  mavlink_system.compid,
-					  MAVLINK_COMM_0,
-					  &tx_msg,
-					  name_buf,
-					  val_buf,
-					  mavlink_type,
-					  param_count(),
-					  param_get_index(param));
-	ret = mavlink_missionlib_send_message(&tx_msg);
-	return ret;
-}
-
-void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
-{
-	switch (msg->msgid) {
-	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
-			/* Start sending parameters */
-			mavlink_pm_start_queued_send();
-			mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
-		} break;
-
-	case MAVLINK_MSG_ID_PARAM_SET: {
-
-			/* Handle parameter setting */
-
-			if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
-				mavlink_param_set_t mavlink_param_set;
-				mavlink_msg_param_set_decode(msg, &mavlink_param_set);
-
-				if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
-					/* local name buffer to enforce null-terminated string */
-					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
-					strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					/* enforce null termination */
-					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					/* attempt to find parameter, set and send it */
-					param_t param = param_find(name);
-
-					if (param == PARAM_INVALID) {
-						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
-						sprintf(buf, "[mavlink pm] unknown: %s", name);
-						mavlink_missionlib_send_gcs_string(buf);
-
-					} else {
-						/* set and send parameter */
-						param_set(param, &(mavlink_param_set.param_value));
-						mavlink_pm_send_param(param);
-					}
-				}
-			}
-		} break;
-
-	case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
-			mavlink_param_request_read_t mavlink_param_request_read;
-			mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
-
-			if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
-				/* when no index is given, loop through string ids and compare them */
-				if (mavlink_param_request_read.param_index == -1) {
-					/* local name buffer to enforce null-terminated string */
-					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
-					strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					/* enforce null termination */
-					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					/* attempt to find parameter and send it */
-					mavlink_pm_send_param_for_name(name);
-
-				} else {
-					/* when index is >= 0, send this parameter again */
-					mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
-				}
-			}
-
-		} break;
-	}
-}
diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h
deleted file mode 100644
index b1e38bcc8..000000000
--- a/src/modules/mavlink/mavlink_parameters.h
+++ /dev/null
@@ -1,104 +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_parameters.h
- * MAVLink parameter protocol definitions (BSD-relicensed).
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-/* This assumes you have the mavlink headers on your include path
- or in the same folder as this source file */
-
-
-#include <v1.0/mavlink_types.h>
-#include <stdbool.h>
-#include <systemlib/param/param.h>
-
-/**
- * Handle parameter related messages.
- */
-void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
-
-/**
- * Send all parameters at once.
- *
- * This function blocks until all parameters have been sent.
- * it delays each parameter by the passed amount of microseconds.
- *
- * @param delay		The delay in us between sending all parameters.
- */
-void mavlink_pm_send_all_params(unsigned int delay);
-
-/**
- * Send one parameter.
- *
- * @param param		The parameter id to send.
- * @return		zero on success, nonzero on failure.
- */
-int mavlink_pm_send_param(param_t param);
-
-/**
- * Send one parameter identified by index.
- *
- * @param index		The index of the parameter to send.
- * @return		zero on success, nonzero else.
- */
-int mavlink_pm_send_param_for_index(uint16_t index);
-
-/**
- * Send one parameter identified by name.
- *
- * @param name		The index of the parameter to send.
- * @return		zero on success, nonzero else.
- */
-int mavlink_pm_send_param_for_name(const char *name);
-
-/**
- * Send a queue of parameters, one parameter per function call.
- *
- * @return		zero on success, nonzero on failure
- */
-int mavlink_pm_queued_send(void);
-
-/**
- * Start sending the parameter queue.
- *
- * This function will not directly send parameters, but instead
- * activate the sending of one parameter on each call of
- * mavlink_pm_queued_send().
- * @see 		mavlink_pm_queued_send()
- */
-void mavlink_pm_start_queued_send(void);
diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp
new file mode 100644
index 000000000..9cdda8b17
--- /dev/null
+++ b/src/modules/mavlink/mavlink_rate_limiter.cpp
@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_rate_limiter.cpp
+ * Message rate limiter implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "mavlink_rate_limiter.h"
+
+MavlinkRateLimiter::MavlinkRateLimiter() : _last_sent(0), _interval(1000000)
+{
+}
+
+MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval)
+{
+}
+
+MavlinkRateLimiter::~MavlinkRateLimiter()
+{
+}
+
+void
+MavlinkRateLimiter::set_interval(unsigned int interval)
+{
+	_interval = interval;
+}
+
+bool
+MavlinkRateLimiter::check(hrt_abstime t)
+{
+	uint64_t dt = t - _last_sent;
+
+	if (dt > 0 && dt >= _interval) {
+		_last_sent = (t / _interval) * _interval;
+		return true;
+	}
+
+	return false;
+}
diff --git a/src/modules/mavlink/mavlink_hil.h b/src/modules/mavlink/mavlink_rate_limiter.h
index 744ed7d94..0b37538e6 100644
--- a/src/modules/mavlink/mavlink_hil.h
+++ b/src/modules/mavlink/mavlink_rate_limiter.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,20 +32,31 @@
  ****************************************************************************/
 
 /**
- * @file mavlink_hil.h
- * Hardware-in-the-loop simulation support.
+ * @file mavlink_rate_limiter.h
+ * Message rate limiter definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#pragma once
+#ifndef MAVLINK_RATE_LIMITER_H_
+#define MAVLINK_RATE_LIMITER_H_
 
-extern bool mavlink_hil_enabled;
+#include <drivers/drv_hrt.h>
 
-/**
- * Enable / disable Hardware in the Loop simulation mode.
- *
- * @param hil_enabled	The new HIL enable/disable state.
- * @return		OK if the HIL state changed, ERROR if the
- *			requested change could not be made or was
- *			redundant.
- */
-extern int set_hil_on_off(bool hil_enabled);
+
+class MavlinkRateLimiter
+{
+private:
+	hrt_abstime _last_sent;
+	hrt_abstime _interval;
+
+public:
+	MavlinkRateLimiter();
+	MavlinkRateLimiter(unsigned int interval);
+	~MavlinkRateLimiter();
+	void set_interval(unsigned int interval);
+	bool check(hrt_abstime t);
+};
+
+
+#endif /* MAVLINK_RATE_LIMITER_H_ */
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7b6fad658..99ec98ee9 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,10 +32,11 @@
  ****************************************************************************/
 
 /**
- * @file mavlink_receiver.c
+ * @file mavlink_receiver.cpp
  * MAVLink protocol message receive and dispatch
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 /* XXX trim includes */
@@ -77,272 +77,334 @@
 __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"
-
-extern bool gcs_link;
+#include "mavlink_receiver.h"
+#include "mavlink_main.h"
 
 __END_DECLS
 
-/* XXX should be in a header somewhere */
-extern "C" pthread_t receive_start(int uart);
-
-static void handle_message(mavlink_message_t *msg);
-static void *receive_thread(void *arg);
-
-static mavlink_status_t status;
-static struct vehicle_vicon_position_s vicon_position;
-static struct vehicle_command_s vcmd;
-static struct offboard_control_setpoint_s offboard_control_sp;
-
-struct vehicle_global_position_s hil_global_pos;
-struct vehicle_local_position_s hil_local_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_local_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;
-
-static orb_advert_t offboard_control_sp_pub = -1;
-static orb_advert_t vicon_position_pub = -1;
-static orb_advert_t telemetry_status_pub = -1;
-
-// variables for HIL reference position
-static int32_t lat0 = 0;
-static int32_t lon0 = 0;
-static double alt0 = 0;
-
-static void
-handle_message(mavlink_message_t *msg)
+static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
+
+MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
+	_mavlink(parent),
+
+	_global_pos_pub(-1),
+	_local_pos_pub(-1),
+	_attitude_pub(-1),
+	_gps_pub(-1),
+	_sensors_pub(-1),
+	_gyro_pub(-1),
+	_accel_pub(-1),
+	_mag_pub(-1),
+	_baro_pub(-1),
+	_airspeed_pub(-1),
+	_battery_pub(-1),
+	_cmd_pub(-1),
+	_flow_pub(-1),
+	_offboard_control_sp_pub(-1),
+	_local_pos_sp_pub(-1),
+	_global_vel_sp_pub(-1),
+	_att_sp_pub(-1),
+	_rates_sp_pub(-1),
+	_vicon_position_pub(-1),
+	_telemetry_status_pub(-1),
+	_rc_pub(-1),
+	_manual_pub(-1),
+	_telemetry_heartbeat_time(0),
+	_radio_status_available(false),
+	_control_mode_sub(-1),
+	_hil_frames(0),
+	_old_timestamp(0),
+	_hil_local_proj_inited(0),
+	_hil_local_alt0(0.0)
 {
-	if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
+	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	memset(&hil_local_pos, 0, sizeof(hil_local_pos));
+	memset(&_control_mode, 0, sizeof(_control_mode));
 
-		mavlink_command_long_t cmd_mavlink;
-		mavlink_msg_command_long_decode(msg, &cmd_mavlink);
+	// make sure the FTP server is started
+	(void)MavlinkFTP::getServer();
+}
 
-		if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
-				|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
-			//check for MAVLINK terminate command
-			if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
-				/* This is the link shutdown command, terminate mavlink */
-				printf("[mavlink] Terminating .. \n");
-				fflush(stdout);
-				usleep(50000);
+MavlinkReceiver::~MavlinkReceiver()
+{
+}
 
-				/* terminate other threads and this thread */
-				thread_should_exit = true;
+void
+MavlinkReceiver::handle_message(mavlink_message_t *msg)
+{
+	switch (msg->msgid) {
+	case MAVLINK_MSG_ID_COMMAND_LONG:
+		handle_message_command_long(msg);
+		break;
 
-			} else {
+	case MAVLINK_MSG_ID_OPTICAL_FLOW:
+		handle_message_optical_flow(msg);
+		break;
 
-				/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
-				vcmd.param1 = cmd_mavlink.param1;
-				vcmd.param2 = cmd_mavlink.param2;
-				vcmd.param3 = cmd_mavlink.param3;
-				vcmd.param4 = cmd_mavlink.param4;
-				vcmd.param5 = cmd_mavlink.param5;
-				vcmd.param6 = cmd_mavlink.param6;
-				vcmd.param7 = cmd_mavlink.param7;
-				// 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;
-				vcmd.source_component = msg->compid;
-				vcmd.confirmation =  cmd_mavlink.confirmation;
-
-				/* check if topic is advertised */
-				if (cmd_pub <= 0) {
-					cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+	case MAVLINK_MSG_ID_SET_MODE:
+		handle_message_set_mode(msg);
+		break;
 
-				} else {
-					/* publish */
-					orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
-				}
-			}
-		}
-	}
+	case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
+		handle_message_vicon_position_estimate(msg);
+		break;
 
-	if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) {
-		mavlink_optical_flow_t flow;
-		mavlink_msg_optical_flow_decode(msg, &flow);
+	case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST:
+		handle_message_quad_swarm_roll_pitch_yaw_thrust(msg);
+		break;
 
-		struct optical_flow_s f;
+	case MAVLINK_MSG_ID_RADIO_STATUS:
+		handle_message_radio_status(msg);
+		break;
 
-		f.timestamp = flow.time_usec;
-		f.flow_raw_x = flow.flow_x;
-		f.flow_raw_y = flow.flow_y;
-		f.flow_comp_x_m = flow.flow_comp_m_x;
-		f.flow_comp_y_m = flow.flow_comp_m_y;
-		f.ground_distance_m = flow.ground_distance;
-		f.quality = flow.quality;
-		f.sensor_id = flow.sensor_id;
+	case MAVLINK_MSG_ID_MANUAL_CONTROL:
+		handle_message_manual_control(msg);
+		break;
 
-		/* check if topic is advertised */
-		if (flow_pub <= 0) {
-			flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
+	case MAVLINK_MSG_ID_HEARTBEAT:
+		handle_message_heartbeat(msg);
+		break;
 
-		} else {
-			/* publish */
-			orb_publish(ORB_ID(optical_flow), flow_pub, &f);
-		}
-	}
+	case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
+		handle_message_request_data_stream(msg);
+		break;
 
-	if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) {
-		/* Set mode on request */
-		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 = custom_mode.main_mode;
-		vcmd.param3 = 0;
-		vcmd.param4 = 0;
-		vcmd.param5 = 0;
-		vcmd.param6 = 0;
-		vcmd.param7 = 0;
-		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;
-		vcmd.source_component = msg->compid;
-		vcmd.confirmation = 1;
-
-		/* check if topic is advertised */
-		if (cmd_pub <= 0) {
-			cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+	case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
+		MavlinkFTP::getServer()->handle_message(_mavlink, msg);
+		break;
 
-		} else {
-			/* create command */
-			orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
-		}
+	default:
+		break;
 	}
 
-	/* Handle Vicon position estimates */
-	if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
-		mavlink_vicon_position_estimate_t pos;
-		mavlink_msg_vicon_position_estimate_decode(msg, &pos);
-
-		vicon_position.timestamp = hrt_absolute_time();
-
-		vicon_position.x = pos.x;
-		vicon_position.y = pos.y;
-		vicon_position.z = pos.z;
+	/*
+	 * Only decode hil messages in HIL mode.
+	 *
+	 * The HIL mode is enabled by the HIL bit flag
+	 * in the system mode. Either send a set mode
+	 * COMMAND_LONG message or a SET_MODE message
+	 *
+	 * Accept HIL GPS messages if use_hil_gps flag is true.
+	 * This allows to provide fake gps measurements to the system.
+	 */
+	if (_mavlink->get_hil_enabled()) {
+		switch (msg->msgid) {
+		case MAVLINK_MSG_ID_HIL_SENSOR:
+			handle_message_hil_sensor(msg);
+			break;
+
+		case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
+			handle_message_hil_state_quaternion(msg);
+			break;
+
+		default:
+			break;
+		}
+	}
 
-		vicon_position.roll = pos.roll;
-		vicon_position.pitch = pos.pitch;
-		vicon_position.yaw = pos.yaw;
 
-		if (vicon_position_pub <= 0) {
-			vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
+   	if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) {
+		switch (msg->msgid) {
+		case MAVLINK_MSG_ID_HIL_GPS:
+			handle_message_hil_gps(msg);
+			break;
 
-		} else {
-			orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
+		default:
+			break;
 		}
+
 	}
 
-	/* Handle quadrotor motor setpoints */
+    /* If we've received a valid message, mark the flag indicating so.
+	   This is used in the '-w' command-line flag. */
+	_mavlink->set_has_received_messages(true);
+}
 
-	if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
-		mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
-		mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
+void
+MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
+{
+	/* command */
+	mavlink_command_long_t cmd_mavlink;
+	mavlink_msg_command_long_decode(msg, &cmd_mavlink);
+
+	if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
+			|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
+		//check for MAVLINK terminate command
+		if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
+			/* This is the link shutdown command, terminate mavlink */
+			warnx("terminated by remote command");
+			fflush(stdout);
+			usleep(50000);
+
+			/* terminate other threads and this thread */
+			_mavlink->_task_should_exit = true;
 
-		if (mavlink_system.sysid < 4) {
+		} else {
 
-			/* switch to a receiving link mode */
-			gcs_link = false;
+			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
+				warnx("ignoring CMD spoofed with same SYS/COMP ID");
+				return;
+			}
 
-			/*
-			 * rate control mode - defined by MAVLink
-			 */
+			struct vehicle_command_s vcmd;
+			memset(&vcmd, 0, sizeof(vcmd));
+
+			/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
+			vcmd.param1 = cmd_mavlink.param1;
+			vcmd.param2 = cmd_mavlink.param2;
+			vcmd.param3 = cmd_mavlink.param3;
+			vcmd.param4 = cmd_mavlink.param4;
+			vcmd.param5 = cmd_mavlink.param5;
+			vcmd.param6 = cmd_mavlink.param6;
+			vcmd.param7 = cmd_mavlink.param7;
+			// 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;
+			vcmd.source_component = msg->compid;
+			vcmd.confirmation =  cmd_mavlink.confirmation;
+
+			if (_cmd_pub < 0) {
+				_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
 
-			uint8_t ml_mode = 0;
-			bool ml_armed = false;
+			} else {
+				orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
+			}
+		}
+	}
+}
 
-			switch (quad_motors_setpoint.mode) {
-			case 0:
-				ml_armed = false;
-				break;
+void
+MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
+{
+	/* optical flow */
+	mavlink_optical_flow_t flow;
+	mavlink_msg_optical_flow_decode(msg, &flow);
+
+	struct optical_flow_s f;
+	memset(&f, 0, sizeof(f));
+
+	f.timestamp = hrt_absolute_time();
+	f.flow_timestamp = flow.time_usec;
+	f.flow_raw_x = flow.flow_x;
+	f.flow_raw_y = flow.flow_y;
+	f.flow_comp_x_m = flow.flow_comp_m_x;
+	f.flow_comp_y_m = flow.flow_comp_m_y;
+	f.ground_distance_m = flow.ground_distance;
+	f.quality = flow.quality;
+	f.sensor_id = flow.sensor_id;
+
+	if (_flow_pub < 0) {
+		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
+
+	} else {
+		orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
+	}
+}
 
-			case 1:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
-				ml_armed = true;
+void
+MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
+{
+	mavlink_set_mode_t new_mode;
+	mavlink_msg_set_mode_decode(msg, &new_mode);
+
+	struct vehicle_command_s vcmd;
+	memset(&vcmd, 0, sizeof(vcmd));
+
+	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 = custom_mode.main_mode;
+	vcmd.param3 = 0;
+	vcmd.param4 = 0;
+	vcmd.param5 = 0;
+	vcmd.param6 = 0;
+	vcmd.param7 = 0;
+	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;
+	vcmd.source_component = msg->compid;
+	vcmd.confirmation = 1;
+
+	if (_cmd_pub < 0) {
+		_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+
+	} else {
+		orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
+	}
+}
 
-				break;
+void
+MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
+{
+	mavlink_vicon_position_estimate_t pos;
+	mavlink_msg_vicon_position_estimate_decode(msg, &pos);
 
-			case 2:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
-				ml_armed = true;
+	struct vehicle_vicon_position_s vicon_position;
+	memset(&vicon_position, 0, sizeof(vicon_position));
 
-				break;
+	vicon_position.timestamp = hrt_absolute_time();
+	vicon_position.x = pos.x;
+	vicon_position.y = pos.y;
+	vicon_position.z = pos.z;
+	vicon_position.roll = pos.roll;
+	vicon_position.pitch = pos.pitch;
+	vicon_position.yaw = pos.yaw;
 
-			case 3:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
-				break;
+	if (_vicon_position_pub < 0) {
+		_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
 
-			case 4:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
-				break;
-			}
+	} else {
+		orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position);
+	}
+}
 
-			offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1]   / (float)INT16_MAX;
-			offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1]  / (float)INT16_MAX;
-			offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1]    / (float)INT16_MAX;
-			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
+void
+MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg)
+{
+	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control;
+	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control);
 
-			if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
-				ml_armed = false;
-			}
+	/* Only accept system IDs from 1 to 4 */
+	if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) {
+		struct offboard_control_setpoint_s offboard_control_sp;
+		memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
 
-			offboard_control_sp.armed = ml_armed;
-			offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode);
+		/* Convert values * 1000 back */
+		offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f;
+		offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f;
+		offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f;
+		offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f;
 
-			offboard_control_sp.timestamp = hrt_absolute_time();
+		offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode;
 
-			/* check if topic has to be advertised */
-			if (offboard_control_sp_pub <= 0) {
-				offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+		offboard_control_sp.timestamp = hrt_absolute_time();
 
-			} else {
-				/* Publish */
-				orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
-			}
+		if (_offboard_control_sp_pub < 0) {
+			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+
+		} else {
+			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
 		}
 	}
+}
 
-	/* handle status updates of the radio */
-	if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
-
-		struct telemetry_status_s tstatus;
-
+void
+MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
+{
+	/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
+	if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
 		mavlink_radio_status_t rstatus;
 		mavlink_msg_radio_status_decode(msg, &rstatus);
 
-		/* publish telemetry status topic */
+		struct telemetry_status_s tstatus;
+		memset(&tstatus, 0, sizeof(tstatus));
+
 		tstatus.timestamp = hrt_absolute_time();
+		tstatus.heartbeat_time = _telemetry_heartbeat_time;
 		tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
 		tstatus.rssi = rstatus.rssi;
 		tstatus.remote_rssi = rstatus.remrssi;
@@ -352,455 +414,499 @@ handle_message(mavlink_message_t *msg)
 		tstatus.rxerrors = rstatus.rxerrors;
 		tstatus.fixed = rstatus.fixed;
 
-		if (telemetry_status_pub == 0) {
-			telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
+		if (_telemetry_status_pub < 0) {
+			_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
 
 		} else {
-			orb_publish(ORB_ID(telemetry_status), telemetry_status_pub, &tstatus);
+			orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
 		}
-	}
-
-	/*
-	 * Only decode hil messages in HIL mode.
-	 *
-	 * The HIL mode is enabled by the HIL bit flag
-	 * in the system mode. Either send a set mode
-	 * COMMAND_LONG message or a SET_MODE message
-	 */
 
-	if (mavlink_hil_enabled) {
-
-		uint64_t timestamp = hrt_absolute_time();
-
-		if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) {
-
-			mavlink_hil_sensor_t imu;
-			mavlink_msg_hil_sensor_decode(msg, &imu);
-
-			/* sensors general */
-			hil_sensors.timestamp = hrt_absolute_time();
-
-			/* hil gyro */
-			static const float mrad2rad = 1.0e-3f;
-			hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad;
-			hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad;
-			hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad;
-			hil_sensors.gyro_rad_s[0] = imu.xgyro;
-			hil_sensors.gyro_rad_s[1] = imu.ygyro;
-			hil_sensors.gyro_rad_s[2] = imu.zgyro;
-			hil_sensors.gyro_counter = hil_counter;
-
-			/* accelerometer */
-			static const float mg2ms2 = 9.8f / 1000.0f;
-			hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
-			hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
-			hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
-			hil_sensors.accelerometer_m_s2[0] = imu.xacc;
-			hil_sensors.accelerometer_m_s2[1] = imu.yacc;
-			hil_sensors.accelerometer_m_s2[2] = imu.zacc;
-			hil_sensors.accelerometer_mode = 0; // TODO what is this?
-			hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
-			hil_sensors.accelerometer_counter = hil_counter;
-
-			/* adc */
-			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;
-			hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga;
-			hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga;
-			hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga;
-			hil_sensors.magnetometer_ga[0] = imu.xmag;
-			hil_sensors.magnetometer_ga[1] = imu.ymag;
-			hil_sensors.magnetometer_ga[2] = imu.zmag;
-			hil_sensors.magnetometer_range_ga = 32.7f; // int16
-			hil_sensors.magnetometer_mode = 0; // TODO what is this
-			hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
-			hil_sensors.magnetometer_counter = hil_counter;
-
-			/* baro */
-			hil_sensors.baro_pres_mbar = imu.abs_pressure;
-			hil_sensors.baro_alt_meter = imu.pressure_alt;
-			hil_sensors.baro_temp_celcius = imu.temperature;
-			hil_sensors.baro_counter = hil_counter;
-
-			/* differential 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;
-
-			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);
+		/* this means that heartbeats alone won't be published to the radio status no more */
+		_radio_status_available = true;
+	}
+}
 
-			} else {
-				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
-			}
+void
+MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
+{
+	mavlink_manual_control_t man;
+	mavlink_msg_manual_control_decode(msg, &man);
 
-			//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
+	struct manual_control_setpoint_s manual;
+	memset(&manual, 0, sizeof(manual));
 
-			/* 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();
+	manual.timestamp = hrt_absolute_time();
+	manual.x = man.x / 1000.0f;
+	manual.y = man.y / 1000.0f;
+	manual.r = man.r / 1000.0f;
+	manual.z = man.z / 1000.0f;
 
-			if (pub_hil_gyro < 0) {
-				pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+	warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
 
-			} else {
-				orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro);
-			}
+	if (_manual_pub < 0) {
+		_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
 
-			struct accel_report accel;
+	} else {
+		orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
+	}
+}
 
-			accel.x_raw = imu.xacc / mg2ms2;
+void
+MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
+{
+	/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
+	if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
+		mavlink_heartbeat_t hb;
+		mavlink_msg_heartbeat_decode(msg, &hb);
 
-			accel.y_raw = imu.yacc / mg2ms2;
+		/* ignore own heartbeats, accept only heartbeats from GCS */
+		if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
+			_telemetry_heartbeat_time = hrt_absolute_time();
 
-			accel.z_raw = imu.zacc / mg2ms2;
+			/* if no radio status messages arrive, lets at least publish that heartbeats were received */
+			if (!_radio_status_available) {
 
-			accel.x = imu.xacc;
+				struct telemetry_status_s tstatus;
+				memset(&tstatus, 0, sizeof(tstatus));
 
-			accel.y = imu.yacc;
+				tstatus.timestamp = _telemetry_heartbeat_time;
+				tstatus.heartbeat_time = _telemetry_heartbeat_time;
+				tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
 
-			accel.z = imu.zacc;
+				if (_telemetry_status_pub < 0) {
+					_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
 
-			accel.temperature = imu.temperature;
+				} else {
+					orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
+				}
+			}
+		}
+	}
+}
 
-			accel.timestamp = hrt_absolute_time();
+void
+MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
+{
+	mavlink_request_data_stream_t req;
+	mavlink_msg_request_data_stream_decode(msg, &req);
 
-			if (pub_hil_accel < 0) {
-				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+	if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid) {
+		float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f;
 
-			} else {
-				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+		MavlinkStream *stream;
+		LL_FOREACH(_mavlink->get_streams(), stream) {
+			if (req.req_stream_id == stream->get_id()) {
+				_mavlink->configure_stream_threadsafe(stream->get_name(), rate);
 			}
+		}
+	}
+}
 
-			struct mag_report mag;
-
-			mag.x_raw = imu.xmag / mga2ga;
+void
+MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
+{
+	mavlink_hil_sensor_t imu;
+	mavlink_msg_hil_sensor_decode(msg, &imu);
 
-			mag.y_raw = imu.ymag / mga2ga;
+	uint64_t timestamp = hrt_absolute_time();
 
-			mag.z_raw = imu.zmag / mga2ga;
+	/* airspeed */
+	{
+		struct airspeed_s airspeed;
+		memset(&airspeed, 0, sizeof(airspeed));
 
-			mag.x = imu.xmag;
+		float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f);
+		// XXX need to fix this
+		float tas = ias;
 
-			mag.y = imu.ymag;
+		airspeed.timestamp = timestamp;
+		airspeed.indicated_airspeed_m_s = ias;
+		airspeed.true_airspeed_m_s = tas;
 
-			mag.z = imu.zmag;
+		if (_airspeed_pub < 0) {
+			_airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed);
 
-			mag.timestamp = hrt_absolute_time();
+		} else {
+			orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed);
+		}
+	}
 
-			if (pub_hil_mag < 0) {
-				pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag);
+	/* gyro */
+	{
+		struct gyro_report gyro;
+		memset(&gyro, 0, sizeof(gyro));
 
-			} else {
-				orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag);
-			}
+		gyro.timestamp = timestamp;
+		gyro.x_raw = imu.xgyro * 1000.0f;
+		gyro.y_raw = imu.ygyro * 1000.0f;
+		gyro.z_raw = imu.zgyro * 1000.0f;
+		gyro.x = imu.xgyro;
+		gyro.y = imu.ygyro;
+		gyro.z = imu.zgyro;
+		gyro.temperature = imu.temperature;
 
-			struct baro_report baro;
+		if (_gyro_pub < 0) {
+			_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
 
-			baro.pressure = imu.abs_pressure;
+		} else {
+			orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro);
+		}
+	}
 
-			baro.altitude = imu.pressure_alt;
+	/* accelerometer */
+	{
+		struct accel_report accel;
+		memset(&accel, 0, sizeof(accel));
 
-			baro.temperature = imu.temperature;
+		accel.timestamp = timestamp;
+		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;
 
-			baro.timestamp = hrt_absolute_time();
+		if (_accel_pub < 0) {
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
 
-			if (pub_hil_baro < 0) {
-				pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro);
+		} else {
+			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
+		}
+	}
 
-			} else {
-				orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro);
-			}
+	/* magnetometer */
+	{
+		struct mag_report mag;
+		memset(&mag, 0, sizeof(mag));
 
-			/* publish combined sensor topic */
-			if (pub_hil_sensors > 0) {
-				orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
+		mag.timestamp = timestamp;
+		mag.x_raw = imu.xmag * 1000.0f;
+		mag.y_raw = imu.ymag * 1000.0f;
+		mag.z_raw = imu.zmag * 1000.0f;
+		mag.x = imu.xmag;
+		mag.y = imu.ymag;
+		mag.z = imu.zmag;
 
-			} else {
-				pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
-			}
+		if (_mag_pub < 0) {
+			_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
 
-			/* 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;
+		} else {
+			orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
+		}
+	}
 
-			/* lazily publish the battery voltage */
-			if (pub_hil_battery > 0) {
-				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+	/* baro */
+	{
+		struct baro_report baro;
+		memset(&baro, 0, sizeof(baro));
 
-			} else {
-				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
-			}
+		baro.timestamp = timestamp;
+		baro.pressure = imu.abs_pressure;
+		baro.altitude = imu.pressure_alt;
+		baro.temperature = imu.temperature;
 
-			// increment counters
-			hil_counter++;
-			hil_frames++;
+		if (_baro_pub < 0) {
+			_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
 
-			// output
-			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil sensor at %d hz\n", hil_frames / 10);
-				old_timestamp = timestamp;
-				hil_frames = 0;
-			}
+		} else {
+			orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
 		}
+	}
 
-		if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) {
-
-			mavlink_hil_gps_t gps;
-			mavlink_msg_hil_gps_decode(msg, &gps);
-
-			/* gps */
-			hil_gps.timestamp_position = gps.time_usec;
-			hil_gps.time_gps_usec = gps.time_usec;
-			hil_gps.lat = gps.lat;
-			hil_gps.lon = gps.lon;
-			hil_gps.alt = gps.alt;
-			hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
-			hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
-			hil_gps.s_variance_m_s = 5.0f;
-			hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
-			hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
-
-			/* 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
-			hil_gps.vel_ned_valid = true;
-			/* COG (course over ground) is spec'ed as -PI..+PI */
-			hil_gps.cog_rad = heading_rad;
-			hil_gps.fix_type = gps.fix_type;
-			hil_gps.satellites_visible = gps.satellites_visible;
-
-			/* 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);
-			}
+	/* sensor combined */
+	{
+		struct sensor_combined_s hil_sensors;
+		memset(&hil_sensors, 0, sizeof(hil_sensors));
+
+		hil_sensors.timestamp = timestamp;
+
+		hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f;
+		hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f;
+		hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f;
+		hil_sensors.gyro_rad_s[0] = imu.xgyro;
+		hil_sensors.gyro_rad_s[1] = imu.ygyro;
+		hil_sensors.gyro_rad_s[2] = imu.zgyro;
+
+		hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
+		hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
+		hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
+		hil_sensors.accelerometer_m_s2[0] = imu.xacc;
+		hil_sensors.accelerometer_m_s2[1] = imu.yacc;
+		hil_sensors.accelerometer_m_s2[2] = imu.zacc;
+		hil_sensors.accelerometer_mode = 0; // TODO what is this?
+		hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
+		hil_sensors.accelerometer_timestamp = timestamp;
+
+		hil_sensors.adc_voltage_v[0] = 0.0f;
+		hil_sensors.adc_voltage_v[1] = 0.0f;
+		hil_sensors.adc_voltage_v[2] = 0.0f;
+
+		hil_sensors.magnetometer_raw[0] = imu.xmag * 1000.0f;
+		hil_sensors.magnetometer_raw[1] = imu.ymag * 1000.0f;
+		hil_sensors.magnetometer_raw[2] = imu.zmag * 1000.0f;
+		hil_sensors.magnetometer_ga[0] = imu.xmag;
+		hil_sensors.magnetometer_ga[1] = imu.ymag;
+		hil_sensors.magnetometer_ga[2] = imu.zmag;
+		hil_sensors.magnetometer_range_ga = 32.7f; // int16
+		hil_sensors.magnetometer_mode = 0; // TODO what is this
+		hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
+		hil_sensors.magnetometer_timestamp = timestamp;
+
+		hil_sensors.baro_pres_mbar = imu.abs_pressure;
+		hil_sensors.baro_alt_meter = imu.pressure_alt;
+		hil_sensors.baro_temp_celcius = imu.temperature;
+		hil_sensors.baro_timestamp = timestamp;
+
+		hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
+		hil_sensors.differential_pressure_timestamp = timestamp;
+
+		/* publish combined sensor topic */
+		if (_sensors_pub < 0) {
+			_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
 
+		} else {
+			orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
 		}
+	}
 
-		if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) {
-
-			mavlink_hil_state_quaternion_t hil_state;
-			mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
-
-			struct airspeed_s airspeed;
-			airspeed.timestamp = hrt_absolute_time();
-			airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
-			airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
-
-			if (pub_hil_airspeed < 0) {
-				pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
+	/* battery status */
+	{
+		struct battery_status_s hil_battery_status;
+		memset(&hil_battery_status, 0, sizeof(hil_battery_status));
 
-			} else {
-				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
-			}
+		hil_battery_status.timestamp = timestamp;
+		hil_battery_status.voltage_v = 11.1f;
+		hil_battery_status.voltage_filtered_v = 11.1f;
+		hil_battery_status.current_a = 10.0f;
+		hil_battery_status.discharged_mah = -1.0f;
 
-			uint64_t timestamp = hrt_absolute_time();
-
-			// publish global position	
-			if (pub_hil_global_pos > 0) {
-				orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
-				// global position packet
-				hil_global_pos.timestamp = timestamp;
-				hil_global_pos.valid = true;
-				hil_global_pos.lat = hil_state.lat;
-				hil_global_pos.lon = hil_state.lon;
-				hil_global_pos.alt = hil_state.alt / 1000.0f;
-				hil_global_pos.vx = hil_state.vx / 100.0f;
-				hil_global_pos.vy = hil_state.vy / 100.0f;
-				hil_global_pos.vz = hil_state.vz / 100.0f;
+		if (_battery_pub < 0) {
+			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 
-			} else {
-				pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
-			}
-
-			// publish local position
-			if (pub_hil_local_pos > 0) {
-				float x;
-				float y;
-				bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve?
-				double lat = hil_state.lat*1e-7;
-				double lon = hil_state.lon*1e-7;
-				map_projection_project(lat, lon, &x, &y); 
-				hil_local_pos.timestamp = timestamp;
-				hil_local_pos.xy_valid = true;
-				hil_local_pos.z_valid = true;
-				hil_local_pos.v_xy_valid = true;
-				hil_local_pos.v_z_valid = true;
-				hil_local_pos.x = x;
-				hil_local_pos.y = y;
-				hil_local_pos.z = alt0 - hil_state.alt/1000.0f;
-				hil_local_pos.vx = hil_state.vx/100.0f;
-				hil_local_pos.vy = hil_state.vy/100.0f;
-				hil_local_pos.vz = hil_state.vz/100.0f;
-				hil_local_pos.yaw = hil_attitude.yaw;
-				hil_local_pos.xy_global = true;
-				hil_local_pos.z_global = true;
-				hil_local_pos.ref_timestamp = timestamp;
-				hil_local_pos.ref_lat = hil_state.lat;
-				hil_local_pos.ref_lon = hil_state.lon;
-				hil_local_pos.ref_alt = alt0;
-				hil_local_pos.landed = landed;
-				orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos);
-			} else {
-				pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
-				lat0 = hil_state.lat;
-				lon0 = hil_state.lon;
-				alt0 = hil_state.alt / 1000.0f;
-				map_projection_init(hil_state.lat, hil_state.lon);
-			}
+		} else {
+			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
+		}
+	}
 
-			/* Calculate Rotation Matrix */
-			math::Quaternion q(hil_state.attitude_quaternion);
-			math::Dcm C_nb(q);
-			math::EulerAngles euler(C_nb);
+	/* increment counters */
+	_hil_frames++;
 
-			/* 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);
+	/* print HIL sensors rate */
+	if ((timestamp - _old_timestamp) > 10000000) {
+		printf("receiving HIL sensors at %d hz\n", _hil_frames / 10);
+		_old_timestamp = timestamp;
+		_hil_frames = 0;
+	}
+}
 
-			hil_attitude.R_valid = true;
+void
+MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
+{
+	mavlink_hil_gps_t gps;
+	mavlink_msg_hil_gps_decode(msg, &gps);
 
-			/* set quaternion */
-			hil_attitude.q[0] = q(0);
-			hil_attitude.q[1] = q(1);
-			hil_attitude.q[2] = q(2);
-			hil_attitude.q[3] = q(3);
-			hil_attitude.q_valid = true;
+	uint64_t timestamp = hrt_absolute_time();
 
-			hil_attitude.roll = euler.getPhi();
-			hil_attitude.pitch = euler.getTheta();
-			hil_attitude.yaw = euler.getPsi();
-			hil_attitude.rollspeed = hil_state.rollspeed;
-			hil_attitude.pitchspeed = hil_state.pitchspeed;
-			hil_attitude.yawspeed = hil_state.yawspeed;
+	struct vehicle_gps_position_s hil_gps;
+	memset(&hil_gps, 0, sizeof(hil_gps));
 
-			/* set timestamp and notify processes (broadcast) */
-			hil_attitude.timestamp = hrt_absolute_time();
+	hil_gps.timestamp_time = timestamp;
+	hil_gps.time_gps_usec = gps.time_usec;
 
-			if (pub_hil_attitude > 0) {
-				orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
+	hil_gps.timestamp_position = timestamp;
+	hil_gps.lat = gps.lat;
+	hil_gps.lon = gps.lon;
+	hil_gps.alt = gps.alt;
+	hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m
+	hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m
 
-			} else {
-				pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
-			}
+	hil_gps.timestamp_variance = timestamp;
+	hil_gps.s_variance_m_s = 5.0f;
 
-			struct accel_report accel;
+	hil_gps.timestamp_velocity = timestamp;
+	hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
+	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
+	hil_gps.vel_ned_valid = true;
+	hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
 
-			accel.x_raw = hil_state.xacc / 9.81f * 1e3f;
+	hil_gps.fix_type = gps.fix_type;
+	hil_gps.satellites_used = gps.satellites_visible;  //TODO: rename mavlink_hil_gps_t sats visible to used?
 
-			accel.y_raw = hil_state.yacc / 9.81f * 1e3f;
+	if (_gps_pub < 0) {
+		_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
 
-			accel.z_raw = hil_state.zacc / 9.81f * 1e3f;
-
-			accel.x = hil_state.xacc;
-
-			accel.y = hil_state.yacc;
+	} else {
+		orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
+	}
+}
 
-			accel.z = hil_state.zacc;
+void
+MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
+{
+	mavlink_hil_state_quaternion_t hil_state;
+	mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
 
-			accel.temperature = 25.0f;
+	uint64_t timestamp = hrt_absolute_time();
 
-			accel.timestamp = hrt_absolute_time();
+	/* airspeed */
+	{
+		struct airspeed_s airspeed;
+		memset(&airspeed, 0, sizeof(airspeed));
 
-			if (pub_hil_accel < 0) {
-				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+		airspeed.timestamp = timestamp;
+		airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
+		airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
 
-			} else {
-				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
-			}
+		if (_airspeed_pub < 0) {
+			_airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed);
 
-			/* 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;
+		} else {
+			orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed);
+		}
+	}
 
-			/* lazily publish the battery voltage */
-			if (pub_hil_battery > 0) {
-				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+	/* attitude */
+	struct vehicle_attitude_s hil_attitude;
+	{
+		memset(&hil_attitude, 0, sizeof(hil_attitude));
+		math::Quaternion q(hil_state.attitude_quaternion);
+		math::Matrix<3, 3> C_nb = q.to_dcm();
+		math::Vector<3> euler = C_nb.to_euler();
+
+		hil_attitude.timestamp = timestamp;
+		memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R));
+		hil_attitude.R_valid = true;
+
+		hil_attitude.q[0] = q(0);
+		hil_attitude.q[1] = q(1);
+		hil_attitude.q[2] = q(2);
+		hil_attitude.q[3] = q(3);
+		hil_attitude.q_valid = true;
+
+		hil_attitude.roll = euler(0);
+		hil_attitude.pitch = euler(1);
+		hil_attitude.yaw = euler(2);
+		hil_attitude.rollspeed = hil_state.rollspeed;
+		hil_attitude.pitchspeed = hil_state.pitchspeed;
+		hil_attitude.yawspeed = hil_state.yawspeed;
+
+		if (_attitude_pub < 0) {
+			_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
 
-			} else {
-				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
-			}
+		} else {
+			orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
 		}
+	}
 
-		if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
-			mavlink_manual_control_t man;
-			mavlink_msg_manual_control_decode(msg, &man);
-
-			struct rc_channels_s rc_hil;
-			memset(&rc_hil, 0, sizeof(rc_hil));
-			static orb_advert_t rc_pub = 0;
+	/* global position */
+	{
+		struct vehicle_global_position_s hil_global_pos;
+		memset(&hil_global_pos, 0, sizeof(hil_global_pos));
+
+		hil_global_pos.timestamp = timestamp;
+		hil_global_pos.lat = hil_state.lat;
+		hil_global_pos.lon = hil_state.lon;
+		hil_global_pos.alt = hil_state.alt / 1000.0f;
+		hil_global_pos.vel_n = hil_state.vx / 100.0f;
+		hil_global_pos.vel_e = hil_state.vy / 100.0f;
+		hil_global_pos.vel_d = hil_state.vz / 100.0f;
+		hil_global_pos.yaw = hil_attitude.yaw;
+		hil_global_pos.eph = 2.0f;
+		hil_global_pos.epv = 4.0f;
+
+		if (_global_pos_pub < 0) {
+			_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
 
-			rc_hil.timestamp = hrt_absolute_time();
-			rc_hil.chan_count = 4;
+		} else {
+			orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
+		}
+	}
 
-			rc_hil.chan[0].scaled = man.x / 1000.0f;
-			rc_hil.chan[1].scaled = man.y / 1000.0f;
-			rc_hil.chan[2].scaled = man.r / 1000.0f;
-			rc_hil.chan[3].scaled = man.z / 1000.0f;
+	/* local position */
+	{
+		double lat = hil_state.lat * 1e-7;
+		double lon = hil_state.lon * 1e-7;
+
+		if (!_hil_local_proj_inited) {
+			_hil_local_proj_inited = true;
+			_hil_local_alt0 = hil_state.alt / 1000.0f;
+			map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon);
+			hil_local_pos.ref_timestamp = timestamp;
+			hil_local_pos.ref_lat = lat;
+			hil_local_pos.ref_lon = lon;
+			hil_local_pos.ref_alt = _hil_local_alt0;
+		}
 
-			struct manual_control_setpoint_s mc;
-			static orb_advert_t mc_pub = 0;
+		float x;
+		float y;
+		map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
+		hil_local_pos.timestamp = timestamp;
+		hil_local_pos.xy_valid = true;
+		hil_local_pos.z_valid = true;
+		hil_local_pos.v_xy_valid = true;
+		hil_local_pos.v_z_valid = true;
+		hil_local_pos.x = x;
+		hil_local_pos.y = y;
+		hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f;
+		hil_local_pos.vx = hil_state.vx / 100.0f;
+		hil_local_pos.vy = hil_state.vy / 100.0f;
+		hil_local_pos.vz = hil_state.vz / 100.0f;
+		hil_local_pos.yaw = hil_attitude.yaw;
+		hil_local_pos.xy_global = true;
+		hil_local_pos.z_global = true;
+
+		bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
+		hil_local_pos.landed = landed;
+
+		if (_local_pos_pub < 0) {
+			_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
 
-			int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+		} else {
+			orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
+		}
+	}
 
-			/* get a copy first, to prevent altering values that are not sent by the mavlink command */
-			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc);
+	/* accelerometer */
+	{
+		struct accel_report accel;
+		memset(&accel, 0, sizeof(accel));
 
-			mc.timestamp = rc_hil.timestamp;
-			mc.roll = man.x / 1000.0f;
-			mc.pitch = man.y / 1000.0f;
-			mc.yaw = man.r / 1000.0f;
-			mc.throttle = man.z / 1000.0f;
+		accel.timestamp = timestamp;
+		accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f;
+		accel.y_raw = hil_state.yacc / CONSTANTS_ONE_G * 1e3f;
+		accel.z_raw = hil_state.zacc / CONSTANTS_ONE_G * 1e3f;
+		accel.x = hil_state.xacc;
+		accel.y = hil_state.yacc;
+		accel.z = hil_state.zacc;
+		accel.temperature = 25.0f;
 
-			/* fake RC channels with manual control input from simulator */
+		if (_accel_pub < 0) {
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
 
+		} else {
+			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
+		}
+	}
 
-			if (rc_pub == 0) {
-				rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil);
+	/* battery status */
+	{
+		struct battery_status_s	hil_battery_status;
+		memset(&hil_battery_status, 0, sizeof(hil_battery_status));
 
-			} else {
-				orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil);
-			}
+		hil_battery_status.timestamp = timestamp;
+		hil_battery_status.voltage_v = 11.1f;
+		hil_battery_status.voltage_filtered_v = 11.1f;
+		hil_battery_status.current_a = 10.0f;
+		hil_battery_status.discharged_mah = -1.0f;
 
-			if (mc_pub == 0) {
-				mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc);
+		if (_battery_pub < 0) {
+			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 
-			} else {
-				orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc);
-			}
+		} else {
+			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
 		}
 	}
 }
@@ -809,17 +915,20 @@ handle_message(mavlink_message_t *msg)
 /**
  * Receive data from UART.
  */
-static void *
-receive_thread(void *arg)
+void *
+MavlinkReceiver::receive_thread(void *arg)
 {
-	int uart_fd = *((int *)arg);
+	int uart_fd = _mavlink->get_uart_fd();
 
-	const int timeout = 1000;
+	const int timeout = 500;
 	uint8_t buf[32];
 
 	mavlink_message_t msg;
 
-	prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid());
+	/* set thread name */
+	char thread_name[24];
+	sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
+	prctl(PR_SET_NAME, thread_name, getpid());
 
 	struct pollfd fds[1];
 	fds[0].fd = uart_fd;
@@ -827,27 +936,23 @@ receive_thread(void *arg)
 
 	ssize_t nread = 0;
 
-	while (!thread_should_exit) {
+	while (!_mavlink->_task_should_exit) {
 		if (poll(fds, 1, timeout) > 0) {
-			if (nread < sizeof(buf)) {
+
+			/* non-blocking read. read may return negative values */
+			if ((nread = read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
 				/* to avoid reading very small chunks wait for data before reading */
 				usleep(1000);
 			}
 
-			/* non-blocking read. read may return negative values */
-			nread = read(uart_fd, buf, sizeof(buf));
-
 			/* if read failed, this loop won't execute */
 			for (ssize_t i = 0; i < nread; i++) {
-				if (mavlink_parse_char(chan, buf[i], &msg, &status)) {
+				if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) {
 					/* handle generic messages and commands */
 					handle_message(&msg);
 
-					/* handle packet with waypoint component */
-					mavlink_wpm_message_handler(&msg, &global_pos, &local_pos);
-
-					/* handle packet with parameter component */
-					mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
+					/* handle packet with parent object */
+					_mavlink->handle_message(&msg);
 				}
 			}
 		}
@@ -856,25 +961,40 @@ receive_thread(void *arg)
 	return NULL;
 }
 
+void MavlinkReceiver::print_status()
+{
+
+}
+
+void *MavlinkReceiver::start_helper(void *context)
+{
+	MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context);
+
+	rcv->receive_thread(NULL);
+
+	delete rcv;
+
+	return nullptr;
+}
+
 pthread_t
-receive_start(int uart)
+MavlinkReceiver::receive_start(Mavlink *parent)
 {
 	pthread_attr_t receiveloop_attr;
 	pthread_attr_init(&receiveloop_attr);
 
 	// set to non-blocking read
-	int flags = fcntl(uart, F_GETFL, 0);
-	fcntl(uart, F_SETFL, flags | O_NONBLOCK);
+	int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0);
+	fcntl(parent->get_uart_fd(), 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);
 
-	pthread_attr_setstacksize(&receiveloop_attr, 3000);
-
+	pthread_attr_setstacksize(&receiveloop_attr, 2900);
 	pthread_t thread;
-	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
+	pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
 
 	pthread_attr_destroy(&receiveloop_attr);
 	return thread;
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/mavlink_receiver.h
index 91773843b..8d38b9973 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,29 +32,34 @@
  ****************************************************************************/
 
 /**
- * @file orb_topics.h
- * Common sets of topics subscribed to or published by the MAVLink driver,
- * and structures maintained by those subscriptions.
+ * @file mavlink_orb_listener.h
+ * MAVLink 1.0 uORB listener definition
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
+
 #pragma once
 
+#include <systemlib/perf_counter.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/rc_channels.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls_effective.h>
@@ -66,55 +70,90 @@
 #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;
-	int att_sub;
-	int global_pos_sub;
-	int act_0_sub;
-	int act_1_sub;
-	int act_2_sub;
-	int act_3_sub;
-	int gps_sub;
-	int man_control_sp_sub;
-	int safety_sub;
-	int actuators_sub;
-	int armed_sub;
-	int actuators_effective_sub;
-	int local_pos_sub;
-	int spa_sub;
-	int spl_sub;
-	int spg_sub;
-	int debug_key_value;
-	int input_rc_sub;
-	int optical_flow;
-	int rates_setpoint_sub;
-	int home_sub;
-	int airspeed_sub;
-	int navigation_capabilities_sub;
-};
+#include "mavlink_ftp.h"
 
-extern struct mavlink_subscriptions mavlink_subs;
+class Mavlink;
 
-/** Global position */
-extern struct vehicle_global_position_s global_pos;
+class MavlinkReceiver
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MavlinkReceiver(Mavlink *parent);
 
-/** Local position */
-extern struct vehicle_local_position_s local_pos;
+	/**
+	 * Destructor, also kills the mavlinks task.
+	 */
+	~MavlinkReceiver();
 
-/** navigation capabilities */
-extern struct navigation_capabilities_s nav_cap;
+	/**
+	* Start the mavlink task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
 
-/** Vehicle status */
-extern struct vehicle_status_s v_status;
+	/**
+	 * Display the mavlink status.
+	 */
+	void		print_status();
 
-/** RC channels */
-extern struct rc_channels_s rc;
+	static pthread_t receive_start(Mavlink *parent);
 
-/** Actuator armed state */
-extern struct actuator_armed_s armed;
+	static void *start_helper(void *context);
 
-/** Worker thread starter */
-extern pthread_t uorb_receive_start(void);
+private:
+	Mavlink	*_mavlink;
+
+	void handle_message(mavlink_message_t *msg);
+	void handle_message_command_long(mavlink_message_t *msg);
+	void handle_message_optical_flow(mavlink_message_t *msg);
+	void handle_message_set_mode(mavlink_message_t *msg);
+	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
+	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
+	void handle_message_radio_status(mavlink_message_t *msg);
+	void handle_message_manual_control(mavlink_message_t *msg);
+	void handle_message_heartbeat(mavlink_message_t *msg);
+	void handle_message_request_data_stream(mavlink_message_t *msg);
+	void handle_message_hil_sensor(mavlink_message_t *msg);
+	void handle_message_hil_gps(mavlink_message_t *msg);
+	void handle_message_hil_state_quaternion(mavlink_message_t *msg);
+
+	void *receive_thread(void *arg);
+
+	mavlink_status_t status;
+	struct vehicle_local_position_s hil_local_pos;
+	struct vehicle_control_mode_s _control_mode;
+	orb_advert_t _global_pos_pub;
+	orb_advert_t _local_pos_pub;
+	orb_advert_t _attitude_pub;
+	orb_advert_t _gps_pub;
+	orb_advert_t _sensors_pub;
+	orb_advert_t _gyro_pub;
+	orb_advert_t _accel_pub;
+	orb_advert_t _mag_pub;
+	orb_advert_t _baro_pub;
+	orb_advert_t _airspeed_pub;
+	orb_advert_t _battery_pub;
+	orb_advert_t _cmd_pub;
+	orb_advert_t _flow_pub;
+	orb_advert_t _offboard_control_sp_pub;
+	orb_advert_t _local_pos_sp_pub;
+	orb_advert_t _global_vel_sp_pub;
+	orb_advert_t _att_sp_pub;
+	orb_advert_t _rates_sp_pub;
+	orb_advert_t _vicon_position_pub;
+	orb_advert_t _telemetry_status_pub;
+	orb_advert_t _rc_pub;
+	orb_advert_t _manual_pub;
+	hrt_abstime _telemetry_heartbeat_time;
+	bool _radio_status_available;
+	int _control_mode_sub;
+	int _hil_frames;
+	uint64_t _old_timestamp;
+	bool _hil_local_proj_inited;
+	float _hil_local_alt0;
+	struct map_projection_reference_s _hil_local_proj_ref;
+};
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
new file mode 100644
index 000000000..7279206db
--- /dev/null
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_stream.cpp
+ * Mavlink messages stream implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <stdlib.h>
+
+#include "mavlink_stream.h"
+#include "mavlink_main.h"
+
+MavlinkStream::MavlinkStream() :
+	next(nullptr),
+	_channel(MAVLINK_COMM_0),
+	_interval(1000000),
+	_last_sent(0)
+{
+}
+
+MavlinkStream::~MavlinkStream()
+{
+}
+
+/**
+ * Set messages interval in ms
+ */
+void
+MavlinkStream::set_interval(const unsigned int interval)
+{
+	_interval = interval;
+}
+
+/**
+ * Set mavlink channel
+ */
+void
+MavlinkStream::set_channel(mavlink_channel_t channel)
+{
+	_channel = channel;
+}
+
+/**
+ * Update subscriptions and send message if necessary
+ */
+int
+MavlinkStream::update(const hrt_abstime t)
+{
+	uint64_t dt = t - _last_sent;
+
+	if (dt > 0 && dt >= _interval) {
+		/* interval expired, send message */
+		send(t);
+		_last_sent = (t / _interval) * _interval;
+
+		return 0;
+	}
+
+	return -1;
+}
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
new file mode 100644
index 000000000..69809a386
--- /dev/null
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_stream.h
+ * Mavlink messages stream definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef MAVLINK_STREAM_H_
+#define MAVLINK_STREAM_H_
+
+#include <drivers/drv_hrt.h>
+
+class Mavlink;
+class MavlinkStream;
+
+#include "mavlink_main.h"
+
+class MavlinkStream
+{
+
+public:
+	MavlinkStream *next;
+
+	MavlinkStream();
+	virtual ~MavlinkStream();
+	void set_interval(const unsigned int interval);
+	void set_channel(mavlink_channel_t channel);
+
+	/**
+	 * @return 0 if updated / sent, -1 if unchanged
+	 */
+	int update(const hrt_abstime t);
+	static MavlinkStream *new_instance();
+	static const char *get_name_static();
+	virtual void subscribe(Mavlink *mavlink) = 0;
+	virtual const char *get_name() const = 0;
+	virtual uint8_t get_id() = 0;
+
+protected:
+	mavlink_channel_t _channel;
+	unsigned int _interval;
+
+	virtual void send(const hrt_abstime t) = 0;
+
+private:
+	hrt_abstime _last_sent;
+};
+
+
+#endif /* MAVLINK_STREAM_H_ */
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
deleted file mode 100644
index 124b3b2ae..000000000
--- a/src/modules/mavlink/missionlib.c
+++ /dev/null
@@ -1,390 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file missionlib.h
- * MAVLink missionlib components
- */
-
-// XXX trim includes
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <math.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <mqueue.h>
-#include <string.h>
-#include "mavlink_bridge_header.h"
-#include <drivers/drv_hrt.h>
-#include <time.h>
-#include <float.h>
-#include <unistd.h>
-#include <nuttx/sched.h>
-#include <sys/prctl.h>
-#include <termios.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <poll.h>
-
-#include <systemlib/err.h>
-#include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <mavlink/mavlink_log.h>
-
-#include "geo/geo.h"
-#include "waypoints.h"
-#include "orb_topics.h"
-#include "missionlib.h"
-#include "mavlink_hil.h"
-#include "util.h"
-#include "waypoints.h"
-#include "mavlink_parameters.h"
-
-static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
-static uint64_t loiter_start_time;
-
-static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
-	struct vehicle_global_position_setpoint_s *sp);
-
-int
-mavlink_missionlib_send_message(mavlink_message_t *msg)
-{
-	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
-
-	mavlink_send_uart_bytes(chan, missionlib_msg_buf, len);
-	return 0;
-}
-
-int
-mavlink_missionlib_send_gcs_string(const char *string)
-{
-	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
-	mavlink_statustext_t statustext;
-	int i = 0;
-
-	while (i < len - 1) {
-		statustext.text[i] = string[i];
-
-		if (string[i] == '\0')
-			break;
-
-		i++;
-	}
-
-	if (i > 1) {
-		/* Enforce null termination */
-		statustext.text[i] = '\0';
-		mavlink_message_t msg;
-
-		mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
-		return mavlink_missionlib_send_message(&msg);
-
-	} else {
-		return 1;
-	}
-}
-
-/**
- * Get system time since boot in microseconds
- *
- * @return the system time since boot in microseconds
- */
-uint64_t mavlink_missionlib_get_system_timestamp()
-{
-	return hrt_absolute_time();
-}
-
-/**
- * Set special vehicle setpoint fields based on current mission item.
- *
- * @return true if the mission item could be interpreted
- * successfully, it return false on failure.
- */
-bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
-	struct vehicle_global_position_setpoint_s *sp)
-{
-	switch (command) {
-		case MAV_CMD_NAV_LOITER_UNLIM:
-			sp->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
-			break;
-		case MAV_CMD_NAV_LOITER_TIME:
-			sp->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
-			loiter_start_time = hrt_absolute_time();
-			break;
-		// case MAV_CMD_NAV_LOITER_TURNS:
-		// 	sp->nav_cmd = NAV_CMD_LOITER_TURN_COUNT;
-		// 	break;
-		case MAV_CMD_NAV_WAYPOINT:
-			sp->nav_cmd = NAV_CMD_WAYPOINT;
-			break;
-		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-			sp->nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
-			break;
-		case MAV_CMD_NAV_LAND:
-			sp->nav_cmd = NAV_CMD_LAND;
-			break;
-		case MAV_CMD_NAV_TAKEOFF:
-			sp->nav_cmd = NAV_CMD_TAKEOFF;
-			break;
-		default:
-			/* abort */
-			return false;
-	}
-
-	sp->loiter_radius = param3;
-	sp->loiter_direction = (param3 >= 0) ? 1 : -1;
-
-	sp->param1 = param1;
-	sp->param2 = param2;
-	sp->param3 = param3;
-	sp->param4 = param4;
-
-
-	/* define the turn distance */
-	float orbit = 15.0f;
-
-	if (command == (int)MAV_CMD_NAV_WAYPOINT) {
-
-		orbit = param2;
-
-	} else if (command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-			command == (int)MAV_CMD_NAV_LOITER_TIME ||
-			command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
-
-		orbit = param3;
-	} else {
-
-		// XXX set default orbit via param
-		// 15 initialized above
-	}
-
-	sp->turn_distance_xy = orbit;
-	sp->turn_distance_z = orbit;
-}
-
-/**
- * This callback is executed each time a waypoint changes.
- *
- * It publishes the vehicle_global_position_setpoint_s or the
- * vehicle_local_position_setpoint_s topic, depending on the type of waypoint
- */
-void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
-		float param2, float param3, float param4, float param5_lat_x,
-		float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command)
-{
-	static orb_advert_t global_position_setpoint_pub = -1;
-	static orb_advert_t global_position_set_triplet_pub = -1;
-	static orb_advert_t local_position_setpoint_pub = -1;
-	static unsigned last_waypoint_index = -1;
-	char buf[50] = {0};
-
-	// XXX include check if WP is supported, jump to next if not
-
-	/* Update controller setpoints */
-	if (frame == (int)MAV_FRAME_GLOBAL) {
-		/* global, absolute waypoint */
-		struct vehicle_global_position_setpoint_s sp;
-		sp.lat = param5_lat_x * 1e7f;
-		sp.lon = param6_lon_y * 1e7f;
-		sp.altitude = param7_alt_z;
-		sp.altitude_is_relative = false;
-		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
-		set_special_fields(param1, param2, param3, param4, command, &sp);
-
-		/* Initialize setpoint publication if necessary */
-		if (global_position_setpoint_pub < 0) {
-			global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
-		}
-
-
-		/* fill triplet: previous, current, next waypoint */
-		struct vehicle_global_position_set_triplet_s triplet;
-
-		/* current waypoint is same as sp */
-		memcpy(&(triplet.current), &sp, sizeof(sp));
-
-		/*
-		 * Check if previous WP (in mission, not in execution order) 
-		 * is available and identify correct index
-		 */
-		int last_setpoint_index = -1;
-		bool last_setpoint_valid = false;
-
-		if (index > 0) {
-			last_setpoint_index = index - 1;
-		}
-
-		while (last_setpoint_index >= 0) {
-
-			if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL &&
-				(wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
-				wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
-				last_setpoint_valid = true;
-				break;
-			}
-
-			last_setpoint_index--;
-		}
-
-		/*
-		 * Check if next WP (in mission, not in execution order) 
-		 * is available and identify correct index
-		 */
-		int next_setpoint_index = -1;
-		bool next_setpoint_valid = false;
-
-		/* next waypoint */
-		if (wpm->size > 1) {
-			next_setpoint_index = index + 1;
-		}
-
-		while (next_setpoint_index < wpm->size) {
-
-			if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
-					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
-				next_setpoint_valid = true;
-				break;
-			}
-
-			next_setpoint_index++;
-		}
-
-		/* populate last and next */
-
-		triplet.previous_valid = false;
-		triplet.next_valid = false;
-
-		if (last_setpoint_valid) {
-			triplet.previous_valid = true;
-			struct vehicle_global_position_setpoint_s sp;
-			sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f;
-			sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
-			sp.altitude = wpm->waypoints[last_setpoint_index].z;
-			sp.altitude_is_relative = false;
-			sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
-			set_special_fields(wpm->waypoints[last_setpoint_index].param1,
-				wpm->waypoints[last_setpoint_index].param2,
-				wpm->waypoints[last_setpoint_index].param3,
-				wpm->waypoints[last_setpoint_index].param4,
-				wpm->waypoints[last_setpoint_index].command, &sp);
-			memcpy(&(triplet.previous), &sp, sizeof(sp));
-		}
-
-		if (next_setpoint_valid) {
-			triplet.next_valid = true;
-			struct vehicle_global_position_setpoint_s sp;
-			sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f;
-			sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
-			sp.altitude = wpm->waypoints[next_setpoint_index].z;
-			sp.altitude_is_relative = false;
-			sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
-			set_special_fields(wpm->waypoints[next_setpoint_index].param1,
-				wpm->waypoints[next_setpoint_index].param2,
-				wpm->waypoints[next_setpoint_index].param3,
-				wpm->waypoints[next_setpoint_index].param4,
-				wpm->waypoints[next_setpoint_index].command, &sp);
-			memcpy(&(triplet.next), &sp, sizeof(sp));
-		}
-
-		/* Initialize triplet publication if necessary */
-		if (global_position_set_triplet_pub < 0) {
-			global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
-		}
-
-		sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
-
-	} else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-		/* global, relative alt (in relation to HOME) waypoint */
-		struct vehicle_global_position_setpoint_s sp;
-		sp.lat = param5_lat_x * 1e7f;
-		sp.lon = param6_lon_y * 1e7f;
-		sp.altitude = param7_alt_z;
-		sp.altitude_is_relative = true;
-		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
-		set_special_fields(param1, param2, param3, param4, command, &sp);
-
-		/* Initialize publication if necessary */
-		if (global_position_setpoint_pub < 0) {
-			global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
-		}
-
-
-
-		sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
-
-	} else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) {
-		/* local, absolute waypoint */
-		struct vehicle_local_position_setpoint_s sp;
-		sp.x = param5_lat_x;
-		sp.y = param6_lon_y;
-		sp.z = param7_alt_z;
-		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
-
-		/* Initialize publication if necessary */
-		if (local_position_setpoint_pub < 0) {
-			local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp);
-		}
-
-		sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
-	} else {
-		warnx("non-navigation WP, ignoring");
-		mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring.");
-		return;
-	}
-
-	/* only set this for known waypoint types (non-navigation types would have returned earlier) */
-	last_waypoint_index = index;
-
-	mavlink_missionlib_send_gcs_string(buf);
-	printf("%s\n", buf);
-	//printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);
-}
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index 5d3d6a73c..d49bbb7f7 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -36,11 +36,19 @@
 #
 
 MODULE_COMMAND	 = mavlink
-SRCS		 += mavlink.c \
-			   missionlib.c \
-			   mavlink_parameters.c \
-			   mavlink_receiver.cpp \
-			   orb_listener.c \
-			   waypoints.c
+SRCS		 += mavlink_main.cpp \
+			mavlink.c \
+			mavlink_receiver.cpp \
+			mavlink_mission.cpp \
+			mavlink_orb_subscription.cpp \
+			mavlink_messages.cpp \
+			mavlink_stream.cpp \
+			mavlink_rate_limiter.cpp \
+			mavlink_commands.cpp \
+			mavlink_ftp.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
+
+MAXOPTIMIZATION	 = -Os
+
+MODULE_STACKSIZE = 1024
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
deleted file mode 100644
index e1dabfd21..000000000
--- a/src/modules/mavlink/orb_listener.c
+++ /dev/null
@@ -1,848 +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 orb_listener.c
- * Monitors ORB topics and sends update messages as appropriate.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-// XXX trim includes
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <math.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <string.h>
-#include "mavlink_bridge_header.h"
-#include <drivers/drv_hrt.h>
-#include <time.h>
-#include <float.h>
-#include <unistd.h>
-#include <sys/prctl.h>
-#include <stdlib.h>
-#include <poll.h>
-#include <lib/geo/geo.h>
-
-#include <mavlink/mavlink_log.h>
-
-#include "waypoints.h"
-#include "orb_topics.h"
-#include "missionlib.h"
-#include "mavlink_hil.h"
-#include "util.h"
-
-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_s actuators_0;
-struct vehicle_attitude_s att;
-struct airspeed_s airspeed;
-
-struct mavlink_subscriptions mavlink_subs;
-
-static int status_sub;
-static int rc_sub;
-
-static unsigned int sensors_raw_counter;
-static unsigned int attitude_counter;
-static unsigned int gps_counter;
-
-/*
- * Last sensor loop time
- * some outputs are better timestamped
- * with this "global" reference.
- */
-static uint64_t last_sensor_timestamp;
-
-static hrt_abstime last_sent_vfr = 0;
-
-static void	*uorb_receive_thread(void *arg);
-
-struct listener {
-	void	(*callback)(const struct listener *l);
-	int		*subp;
-	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);
-static void	l_vehicle_status(const struct listener *l);
-static void	l_rc_channels(const struct listener *l);
-static void	l_input_rc(const struct listener *l);
-static void	l_global_position(const struct listener *l);
-static void	l_local_position(const struct listener *l);
-static void	l_global_position_setpoint(const struct listener *l);
-static void	l_local_position_setpoint(const struct listener *l);
-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(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},
-	{l_vehicle_attitude,		&mavlink_subs.att_sub,		0},
-	{l_vehicle_gps_position,	&mavlink_subs.gps_sub,		0},
-	{l_vehicle_status,		&status_sub,			0},
-	{l_rc_channels,			&rc_sub,			0},
-	{l_input_rc,			&mavlink_subs.input_rc_sub,	0},
-	{l_global_position,		&mavlink_subs.global_pos_sub,	0},
-	{l_local_position,		&mavlink_subs.local_pos_sub,	0},
-	{l_global_position_setpoint,	&mavlink_subs.spg_sub,		0},
-	{l_local_position_setpoint,	&mavlink_subs.spl_sub,		0},
-	{l_attitude_setpoint,		&mavlink_subs.spa_sub,		0},
-	{l_actuator_outputs,		&mavlink_subs.act_0_sub,	0},
-	{l_actuator_outputs,		&mavlink_subs.act_1_sub,	1},
-	{l_actuator_outputs,		&mavlink_subs.act_2_sub,	2},
-	{l_actuator_outputs,		&mavlink_subs.act_3_sub,	3},
-	{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_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)
-{
-	struct sensor_combined_s raw;
-
-	/* copy sensors raw data into local buffer */
-	orb_copy(ORB_ID(sensor_combined), mavlink_subs.sensor_sub, &raw);
-
-	last_sensor_timestamp = raw.timestamp;
-
-	/* mark individual fields as changed */
-	uint16_t fields_updated = 0;
-	static unsigned accel_counter = 0;
-	static unsigned gyro_counter = 0;
-	static unsigned mag_counter = 0;
-	static unsigned baro_counter = 0;
-
-	if (accel_counter != raw.accelerometer_counter) {
-		/* mark first three dimensions as changed */
-		fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
-		accel_counter = raw.accelerometer_counter;
-	}
-
-	if (gyro_counter != raw.gyro_counter) {
-		/* mark second group dimensions as changed */
-		fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
-		gyro_counter = raw.gyro_counter;
-	}
-
-	if (mag_counter != raw.magnetometer_counter) {
-		/* mark third group dimensions as changed */
-		fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
-		mag_counter = raw.magnetometer_counter;
-	}
-
-	if (baro_counter != raw.baro_counter) {
-		/* mark last group dimensions as changed */
-		fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
-		baro_counter = raw.baro_counter;
-	}
-
-	if (gcs_link)
-		mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp,
-					     raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
-					     raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
-					     raw.gyro_rad_s[1], raw.gyro_rad_s[2],
-					     raw.magnetometer_ga[0],
-					     raw.magnetometer_ga[1], raw.magnetometer_ga[2],
-					     raw.baro_pres_mbar, raw.differential_pressure_pa,
-					     raw.baro_alt_meter, raw.baro_temp_celcius,
-					     fields_updated);
-
-	sensors_raw_counter++;
-}
-
-void
-l_vehicle_attitude(const struct listener *l)
-{
-	/* copy attitude data into local buffer */
-	orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
-
-	if (gcs_link) {
-		/* send sensor values */
-		mavlink_msg_attitude_send(MAVLINK_COMM_0,
-					  last_sensor_timestamp / 1000,
-					  att.roll,
-					  att.pitch,
-					  att.yaw,
-					  att.rollspeed,
-					  att.pitchspeed,
-					  att.yawspeed);
-					  	
-		/* limit VFR message rate to 10Hz */
-		hrt_abstime t = hrt_absolute_time();
-		if (t >= last_sent_vfr + 100000) {
-			last_sent_vfr = t;
-			float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
-			uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
-			float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
-			mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
-		}
-		
-		/* send quaternion values if it exists */
-		if(att.q_valid) {
-			mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
-												last_sensor_timestamp / 1000,
-												att.q[0],
-												att.q[1],
-												att.q[2],
-												att.q[3],
-												att.rollspeed,
-												att.pitchspeed,
-												att.yawspeed);
-		}
-	}
-
-	attitude_counter++;
-}
-
-void
-l_vehicle_gps_position(const struct listener *l)
-{
-	struct vehicle_gps_position_s gps;
-
-	/* copy gps data into local buffer */
-	orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
-
-	/* GPS COG is 0..2PI in degrees * 1e2 */
-	float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F;
-
-	/* GPS position */
-	mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
-				     gps.timestamp_position,
-				     gps.fix_type,
-				     gps.lat,
-				     gps.lon,
-				     gps.alt,
-				     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 deg to deg * 100
-				     gps.satellites_visible);
-
-	/* update SAT info every 10 seconds */
-	if (gps.satellite_info_available && (gps_counter % 50 == 0)) {
-		mavlink_msg_gps_status_send(MAVLINK_COMM_0,
-					    gps.satellites_visible,
-					    gps.satellite_prn,
-					    gps.satellite_used,
-					    gps.satellite_elevation,
-					    gps.satellite_azimuth,
-					    gps.satellite_snr);
-	}
-
-	gps_counter++;
-}
-
-void
-l_vehicle_status(const struct listener *l)
-{
-	/* immediately communicate state changes back to user */
-	orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
-	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
-
-	/* enable or disable HIL */
-	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_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_base_mode,
-				   mavlink_custom_mode,
-				   mavlink_state);
-}
-
-void
-l_rc_channels(const struct listener *l)
-{
-	/* copy rc channels into local buffer */
-	orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
-	// XXX Add RC channels scaled message here
-}
-
-void
-l_input_rc(const struct listener *l)
-{
-	/* copy rc channels into local buffer */
-	orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw);
-
-	if (gcs_link) {
-
-		const unsigned port_width = 8;
-
-		for (unsigned i = 0; (i * port_width) < rc_raw.channel_count; i++) {
-			/* Channels are sent in MAVLink main loop at a fixed interval */
-			mavlink_msg_rc_channels_raw_send(chan,
-							 rc_raw.timestamp_publication / 1000,
-							 i,
-							 (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 2) ? rc_raw.values[(i * port_width) + 2] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 3) ? rc_raw.values[(i * port_width) + 3] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 4) ? rc_raw.values[(i * port_width) + 4] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 5) ? rc_raw.values[(i * port_width) + 5] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 6) ? rc_raw.values[(i * port_width) + 6] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 7) ? rc_raw.values[(i * port_width) + 7] : UINT16_MAX,
-							 rc_raw.rssi);
-		}
-	}
-}
-
-void
-l_global_position(const struct listener *l)
-{
-	/* copy global position data into local buffer */
-	orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
-
-	mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
-						 global_pos.timestamp / 1000,
-					     global_pos.lat,
-					     global_pos.lon,
-					     global_pos.alt * 1000.0f,
-					     global_pos.relative_alt * 1000.0f,
-					     global_pos.vx * 100.0f,
-					     global_pos.vy * 100.0f,
-					     global_pos.vz * 100.0f,
-					     _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
-}
-
-void
-l_local_position(const struct listener *l)
-{
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos);
-
-	if (gcs_link)
-		mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
-						    local_pos.timestamp / 1000,
-						    local_pos.x,
-						    local_pos.y,
-						    local_pos.z,
-						    local_pos.vx,
-						    local_pos.vy,
-						    local_pos.vz);
-}
-
-void
-l_global_position_setpoint(const struct listener *l)
-{
-	struct vehicle_global_position_setpoint_s global_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp);
-
-	uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
-
-	if (global_sp.altitude_is_relative)
-		coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
-
-	if (gcs_link)
-		mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
-				coordinate_frame,
-				global_sp.lat,
-				global_sp.lon,
-				global_sp.altitude * 1000.0f,
-				global_sp.yaw * M_RAD_TO_DEG_F * 100.0f);
-}
-
-void
-l_local_position_setpoint(const struct listener *l)
-{
-	struct vehicle_local_position_setpoint_s local_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp);
-
-	if (gcs_link)
-		mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0,
-				MAV_FRAME_LOCAL_NED,
-				local_sp.x,
-				local_sp.y,
-				local_sp.z,
-				local_sp.yaw);
-}
-
-void
-l_attitude_setpoint(const struct listener *l)
-{
-	struct vehicle_attitude_setpoint_s att_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp);
-
-	if (gcs_link)
-		mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0,
-				att_sp.timestamp / 1000,
-				att_sp.roll_body,
-				att_sp.pitch_body,
-				att_sp.yaw_body,
-				att_sp.thrust);
-}
-
-void
-l_vehicle_rates_setpoint(const struct listener *l)
-{
-	struct vehicle_rates_setpoint_s rates_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp);
-
-	if (gcs_link)
-		mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0,
-				rates_sp.timestamp / 1000,
-				rates_sp.roll,
-				rates_sp.pitch,
-				rates_sp.yaw,
-				rates_sp.thrust);
-}
-
-void
-l_actuator_outputs(const struct listener *l)
-{
-	struct actuator_outputs_s act_outputs;
-
-	orb_id_t ids[] = {
-		ORB_ID(actuator_outputs_0),
-		ORB_ID(actuator_outputs_1),
-		ORB_ID(actuator_outputs_2),
-		ORB_ID(actuator_outputs_3)
-	};
-
-	/* copy actuator data into local buffer */
-	orb_copy(ids[l->arg], *l->subp, &act_outputs);
-
-	if (gcs_link) {
-		mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
-						  l->arg /* port number - needs GCS support */,
-							 /* QGC has port number support already */
-						  act_outputs.output[0],
-						  act_outputs.output[1],
-						  act_outputs.output[2],
-						  act_outputs.output[3],
-						  act_outputs.output[4],
-						  act_outputs.output[5],
-						  act_outputs.output[6],
-						  act_outputs.output[7]);
-
-		/* only send in HIL mode and only send first group for HIL */
-		if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
-
-			/* translate the current syste state to mavlink state and mode */
-			uint8_t mavlink_state = 0;
-			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 */
-
-			/* scale / assign outputs depending on system type */
-
-			if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
-				mavlink_msg_hil_controls_send(chan,
-							      hrt_absolute_time(),
-							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-							      -1,
-							      -1,
-							      -1,
-							      -1,
-							      mavlink_base_mode,
-							      0);
-
-			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
-				mavlink_msg_hil_controls_send(chan,
-							      hrt_absolute_time(),
-							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-							      -1,
-							      -1,
-							      mavlink_base_mode,
-							      0);
-
-			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
-				mavlink_msg_hil_controls_send(chan,
-							      hrt_absolute_time(),
-							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
-							      ((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_base_mode,
-							      0);
-
-			} else {
-				mavlink_msg_hil_controls_send(chan,
-							      hrt_absolute_time(),
-							      (act_outputs.output[0] - 1500.0f) / 500.0f,
-							      (act_outputs.output[1] - 1500.0f) / 500.0f,
-							      (act_outputs.output[2] - 1500.0f) / 500.0f,
-							      (act_outputs.output[3] - 1000.0f) / 1000.0f,
-							      (act_outputs.output[4] - 1500.0f) / 500.0f,
-							      (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_base_mode,
-							      0);
-			}
-		}
-	}
-}
-
-void
-l_actuator_armed(const struct listener *l)
-{
-	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
-}
-
-void
-l_manual_control_setpoint(const struct listener *l)
-{
-	struct manual_control_setpoint_s man_control;
-
-	/* copy manual control data into local buffer */
-	orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control);
-
-	if (gcs_link)
-		mavlink_msg_manual_control_send(MAVLINK_COMM_0,
-						mavlink_system.sysid,
-						man_control.roll * 1000,
-						man_control.pitch * 1000,
-						man_control.yaw * 1000,
-						man_control.throttle * 1000,
-						0);
-}
-
-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]);
-	}
-}
-
-void
-l_debug_key_value(const struct listener *l)
-{
-	struct debug_key_value_s debug;
-
-	orb_copy(ORB_ID(debug_key_value), mavlink_subs.debug_key_value, &debug);
-
-	/* Enforce null termination */
-	debug.key[sizeof(debug.key) - 1] = '\0';
-
-	mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-					   last_sensor_timestamp / 1000,
-					   debug.key,
-					   debug.value);
-}
-
-void
-l_optical_flow(const struct listener *l)
-{
-	struct optical_flow_s flow;
-
-	orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow);
-
-	mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y,
-				      flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m);
-}
-
-void
-l_home(const struct listener *l)
-{
-	struct home_position_s home;
-
-	orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
-
-	mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt);
-}
-
-void
-l_airspeed(const struct listener *l)
-{
-	orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
-}
-
-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);
-
-}
-
-static void *
-uorb_receive_thread(void *arg)
-{
-	/* Set thread name */
-	prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid());
-
-	/*
-	 * set up poll to block for new data,
-	 * wait for a maximum of 1000 ms (1 second)
-	 */
-	const int timeout = 1000;
-
-	/*
-	 * Initialise listener array.
-	 *
-	 * Might want to invoke each listener once to set initial state.
-	 */
-	struct pollfd fds[n_listeners];
-
-	for (unsigned i = 0; i < n_listeners; i++) {
-		fds[i].fd = *listeners[i].subp;
-		fds[i].events = POLLIN;
-
-		/* Invoke callback to set initial state */
-		//listeners[i].callback(&listener[i]);
-	}
-
-	while (!thread_should_exit) {
-
-		int poll_ret = poll(fds, n_listeners, timeout);
-
-		/* handle the poll result */
-		if (poll_ret == 0) {
-			/* silent */
-
-		} else if (poll_ret < 0) {
-			mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
-
-		} else {
-
-			for (unsigned i = 0; i < n_listeners; i++) {
-				if (fds[i].revents & POLLIN)
-					listeners[i].callback(&listeners[i]);
-			}
-		}
-	}
-
-	return NULL;
-}
-
-pthread_t
-uorb_receive_start(void)
-{
-	/* --- SENSORS RAW VALUE --- */
-	mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	/* rate limit set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink_subs.sensor_sub, 200);	/* 5Hz updates */
-
-	/* --- ATTITUDE VALUE --- */
-	mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	/* rate limit set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink_subs.att_sub, 200);	/* 5Hz updates */
-
-	/* --- GPS VALUE --- */
-	mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	orb_set_interval(mavlink_subs.gps_sub, 200);	/* 5Hz updates */
-
-	/* --- HOME POSITION --- */
-	mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position));
-	orb_set_interval(mavlink_subs.home_sub, 1000);	/* 1Hz updates */
-
-	/* --- SYSTEM STATE --- */
-	status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	orb_set_interval(status_sub, 300);		/* max 3.33 Hz updates */
-
-	/* --- RC CHANNELS VALUE --- */
-	rc_sub = orb_subscribe(ORB_ID(rc_channels));
-	orb_set_interval(rc_sub, 100);			/* 10Hz updates */
-
-	/* --- RC RAW VALUE --- */
-	mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc));
-	orb_set_interval(mavlink_subs.input_rc_sub, 100);
-
-	/* --- GLOBAL POS VALUE --- */
-	mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	orb_set_interval(mavlink_subs.global_pos_sub, 100);	/* 10 Hz active updates */
-
-	/* --- LOCAL POS VALUE --- */
-	mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	orb_set_interval(mavlink_subs.local_pos_sub, 1000);	/* 1Hz active updates */
-
-	/* --- GLOBAL SETPOINT VALUE --- */
-	mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
-	orb_set_interval(mavlink_subs.spg_sub, 2000);	/* 0.5 Hz updates */
-
-	/* --- LOCAL SETPOINT VALUE --- */
-	mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
-	orb_set_interval(mavlink_subs.spl_sub, 2000);	/* 0.5 Hz updates */
-
-	/* --- ATTITUDE SETPOINT VALUE --- */
-	mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	orb_set_interval(mavlink_subs.spa_sub, 2000);	/* 0.5 Hz updates */
-
-	/* --- RATES SETPOINT VALUE --- */
-	mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000);  /* 0.5 Hz updates */
-
-	/* --- ACTUATOR OUTPUTS --- */
-	mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
-	mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
-	mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2));
-	mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3));
-	/* rate limits set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink_subs.act_0_sub, 100);	/* 10Hz updates */
-	orb_set_interval(mavlink_subs.act_1_sub, 100);	/* 10Hz updates */
-	orb_set_interval(mavlink_subs.act_2_sub, 100);	/* 10Hz updates */
-	orb_set_interval(mavlink_subs.act_3_sub, 100);	/* 10Hz updates */
-
-	/* --- ACTUATOR ARMED VALUE --- */
-	mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(mavlink_subs.armed_sub, 100);	/* 10Hz updates */
-
-	/* --- MAPPED MANUAL CONTROL INPUTS --- */
-	mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	/* rate limits set externally based on interface speed, set a basic default here */
-	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);
-	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */
-
-	/* --- DEBUG VALUE OUTPUT --- */
-	mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
-	orb_set_interval(mavlink_subs.debug_key_value, 100);	/* 10Hz updates */
-
-	/* --- FLOW SENSOR --- */
-	mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
-	orb_set_interval(mavlink_subs.optical_flow, 200); 	/* 5Hz updates */
-
-	/* --- AIRSPEED --- */
-	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);
-
-	/* Set stack size, needs less than 2k */
-	pthread_attr_setstacksize(&uorb_attr, 2048);
-
-	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/waypoints.c b/src/modules/mavlink/waypoints.c
deleted file mode 100644
index 7e4a2688f..000000000
--- a/src/modules/mavlink/waypoints.c
+++ /dev/null
@@ -1,1042 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file waypoints.c
- * MAVLink waypoint protocol implementation (BSD-relicensed).
- */
-
-#include <math.h>
-#include <sys/prctl.h>
-#include <unistd.h>
-#include <stdio.h>
-
-#include "mavlink_bridge_header.h"
-#include "missionlib.h"
-#include "waypoints.h"
-#include "util.h"
-
-#ifndef FM_PI
-#define FM_PI 3.1415926535897932384626433832795f
-#endif
-
-bool debug = false;
-bool verbose = false;
-
-
-#define MAVLINK_WPM_NO_PRINTF
-
-uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
-
-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->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
-	state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT;
-	state->idle = false;      				///< indicates if the system is following the waypoints or is waiting
-	state->current_active_wp_id = -1;		///< id of current waypoint
-	state->yaw_reached = false;						///< boolean for yaw attitude reached
-	state->pos_reached = false;						///< boolean for position reached
-	state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
-	state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
-
-}
-
-/*
- *  @brief Sends an waypoint ack message
- */
-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.type = type;
-
-	mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa);
-	mavlink_missionlib_send_message(&msg);
-
-	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
-
-	if (MAVLINK_WPM_TEXT_FEEDBACK) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-		mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
-#else
-
-		if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
-
-#endif
-		mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
-	}
-}
-
-/*
- *  @brief Broadcasts the new target waypoint and directs the MAV to fly there
- *
- *  This function broadcasts its new active waypoint sequence number and
- *  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_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;
-
-		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();
-
-	} 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");
-	}
-}
-
-void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count)
-{
-	mavlink_message_t msg;
-	mavlink_mission_count_t wpc;
-
-	wpc.target_system = wpm->current_partner_sysid;
-	wpc.target_component = wpm->current_partner_compid;
-	wpc.count = count;
-
-	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"));
-}
-
-void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
-{
-	if (seq < wpm->size) {
-		mavlink_message_t msg;
-		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
-		wp->target_system = wpm->current_partner_sysid;
-		wp->target_component = wpm->current_partner_compid;
-		mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp);
-		mavlink_missionlib_send_message(&msg);
-
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
-
-		// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
-
-	} else {
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n");
-	}
-}
-
-void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
-{
-	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.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"));
-
-	} else {
-		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n");
-	}
-}
-
-/*
- *  @brief emits a message that a waypoint reached
- *
- *  This function broadcasts a message that a waypoint is reached.
- *
- *  @param seq The waypoint sequence number the MAV has reached.
- */
-void mavlink_wpm_send_waypoint_reached(uint16_t seq)
-{
-	mavlink_message_t msg;
-	mavlink_mission_item_reached_t wp_reached;
-
-	wp_reached.seq = seq;
-
-	mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached);
-	mavlink_missionlib_send_message(&msg);
-
-	if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq);
-
-	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
-}
-
-/*
- * Calculate distance in global frame.
- *
- * The distance calculation is based on the WGS84 geoid (GPS)
- */
-float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z)
-{
-
-	if (seq < wpm->size) {
-		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
-
-		double current_x_rad = wp->x / 180.0 * M_PI;
-		double current_y_rad = wp->y / 180.0 * M_PI;
-		double x_rad = lat / 180.0 * M_PI;
-		double y_rad = lon / 180.0 * M_PI;
-
-		double d_lat = x_rad - current_x_rad;
-		double d_lon = y_rad - current_y_rad;
-
-		double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad);
-		double c = 2 * atan2(sqrt(a), sqrt(1 - a));
-
-		const double radius_earth = 6371000.0;
-
-		float dxy = radius_earth * c;
-		float dz = alt - wp->z;
-
-		*dist_xy = fabsf(dxy);
-		*dist_z = fabsf(dz);
-
-		return sqrtf(dxy * dxy + dz * dz);
-
-	} else {
-		return -1.0f;
-	}
-
-}
-
-/*
- * Calculate distance in local frame (NED)
- */
-float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z)
-{
-	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
-
-		float dx = (cur->x - x);
-		float dy = (cur->y - y);
-		float dz = (cur->z - z);
-
-		*dist_xy = sqrtf(dx * dx + dy * dy);
-		*dist_z = fabsf(dz);
-
-		return sqrtf(dx * dx + dy * dy + dz * dz);
-
-	} else {
-		return -1.0f;
-	}
-}
-
-void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance)
-{
-	static uint16_t counter;
-
-	if ((!global_pos->valid && !local_pos->xy_valid) ||
-		/* no waypoint */
-		wpm->size == 0) {
-		/* nothing to check here, return */
-		return;
-	}
-
-	if (wpm->current_active_wp_id < wpm->size) {
-
-		float orbit;
-		if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) {
-
-			orbit = wpm->waypoints[wpm->current_active_wp_id].param2;
-
-		} else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
-
-			orbit = wpm->waypoints[wpm->current_active_wp_id].param3;
-		} else {
-
-			// XXX set default orbit via param
-			orbit = 15.0f;
-		}
-
-		/* keep vertical orbit */
-		float vertical_switch_distance = orbit;
-
-		/* Take the larger turn distance - orbit or turn_distance */
-		if (orbit < turn_distance)
-			orbit = turn_distance;
-
-		int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
-		float dist = -1.0f;
-
-		float dist_xy = -1.0f;
-		float dist_z = -1.0f;
-
-		if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z);
-
-		} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z);
-
-		} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
-			dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z);
-
-		} else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
-			/* Check if conditions of mission item are satisfied */
-			// XXX TODO
-		}
-
-		if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
-			wpm->pos_reached = true;
-		}
-
-		// check if required yaw reached
-		float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
-		float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
-		if (fabsf(yaw_err) < 0.05f) {
-			wpm->yaw_reached = true;
-		}
-	}
-
-	//check if the current waypoint was reached
-	if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) {
-		if (wpm->current_active_wp_id < wpm->size) {
-			mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-
-			if (wpm->timestamp_firstinside_orbit == 0) {
-				// Announce that last waypoint was reached
-				mavlink_wpm_send_waypoint_reached(cur_wp->seq);
-				wpm->timestamp_firstinside_orbit = now;
-			}
-
-			// check if the MAV was long enough inside the waypoint orbit
-			//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
-
-			bool time_elapsed = false;
-
-			if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
-				time_elapsed = true;
-			} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
-				time_elapsed = true;
-			}
-
-			if (time_elapsed) {
-
-				/* safeguard against invalid missions with last wp autocontinue on */
-				if (wpm->current_active_wp_id == wpm->size - 1) {
-					/* stop handling missions here */
-					cur_wp->autocontinue = false;
-				}
-
-				if (cur_wp->autocontinue) {
-
-					cur_wp->current = 0;
-
-					float navigation_lat = -1.0f;
-					float navigation_lon = -1.0f;
-					float navigation_alt = -1.0f;
-					int navigation_frame = -1;
-
-					/* initialize to current position in case we don't find a suitable navigation waypoint */
-					if (global_pos->valid) {
-						navigation_lat = global_pos->lat/1e7;
-						navigation_lon = global_pos->lon/1e7;
-						navigation_alt = global_pos->alt;
-						navigation_frame = MAV_FRAME_GLOBAL;
-					} else if (local_pos->xy_valid && local_pos->z_valid) {
-						navigation_lat = local_pos->x;
-						navigation_lon = local_pos->y;
-						navigation_alt = local_pos->z;
-						navigation_frame = MAV_FRAME_LOCAL_NED;
-					}
-
-					/* guard against missions without final land waypoint */
-					/* only accept supported navigation waypoints, skip unknown ones */
-					do {
-
-						/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
-						if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) {
-
-							/* this is a navigation waypoint */
-							navigation_frame = cur_wp->frame;
-							navigation_lat = cur_wp->x;
-							navigation_lon = cur_wp->y;
-							navigation_alt = cur_wp->z;
-						}
-
-						if (wpm->current_active_wp_id == wpm->size - 1) {
-
-							/* if we're not landing at the last nav waypoint, we're falling back to loiter */
-							if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) {
-								/* the last waypoint was reached, if auto continue is
-								 * activated AND it is NOT a land waypoint, keep the system loitering there.
-								 */
-								cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
-								cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
-								cur_wp->frame = navigation_frame;
-								cur_wp->x = navigation_lat;
-								cur_wp->y = navigation_lon;
-								cur_wp->z = navigation_alt;
-							}
-
-							/* we risk an endless loop for missions without navigation waypoints, abort. */
-							break;
-
-						} else {
-							if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
-								wpm->current_active_wp_id++;
-						}
-			
-					} while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM));
-
-					// Fly to next waypoint
-					wpm->timestamp_firstinside_orbit = 0;
-					mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-					mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-					wpm->waypoints[wpm->current_active_wp_id].current = true;
-					wpm->pos_reached = false;
-					wpm->yaw_reached = false;
-					printf("Set new waypoint (%u)\n", wpm->current_active_wp_id);
-				}
-			}
-		}
-
-	} else {
-		wpm->timestamp_lastoutside_orbit = now;
-	}
-
-	counter++;
-}
-
-
-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) {
-
-#ifdef MAVLINK_WPM_NO_PRINTF
-		mavlink_missionlib_send_gcs_string("Operation timeout switching -> IDLE");
-#else
-
-		if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", 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)
-{
-	uint64_t now = mavlink_missionlib_get_system_timestamp();
-
-	switch (msg->msgid) {
-
-	case MAVLINK_MSG_ID_MISSION_ACK: {
-			mavlink_mission_ack_t wpa;
-			mavlink_msg_mission_ack_decode(msg, &wpa);
-
-			if ((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) {
-				wpm->timestamp_lastaction = now;
-
-				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;
-					}
-				}
-
-			} else {
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
-			}
-
-			break;
-		}
-
-	case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
-			mavlink_mission_set_current_t wpc;
-			mavlink_msg_mission_set_current_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) {
-					if (wpc.seq < wpm->size) {
-						// if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n");
-						wpm->current_active_wp_id = wpc.seq;
-						uint32_t i;
-
-						for (i = 0; i < wpm->size; i++) {
-							if (i == wpm->current_active_wp_id) {
-								wpm->waypoints[i].current = true;
-
-							} else {
-								wpm->waypoints[i].current = false;
-							}
-						}
-
-						mavlink_missionlib_send_gcs_string("NEW WP SET");
-
-						wpm->yaw_reached = false;
-						wpm->pos_reached = false;
-						mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-						mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-						wpm->timestamp_firstinside_orbit = 0;
-
-					} else {
-						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
-					}
-
-				} else {
-					mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
-
-				}
-
-			} else {
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-			}
-
-			break;
-		}
-
-	case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
-			mavlink_mission_request_list_t wprl;
-			mavlink_msg_mission_request_list_decode(msg, &wprl);
-
-			if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
-				wpm->timestamp_lastaction = now;
-
-				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);
-					}
-
-					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);
-				}
-			} else {
-				// if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n");
-			}
-
-			break;
-		}
-
-	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 (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
-
-						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);
-
-#endif
-					}
-
-					wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
-					wpm->current_wp_id = wpr.seq;
-					mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpr.seq);
-
-				} else {
-					// if (verbose)
-					{
-						if (!(wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-							mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-#else
-
-							if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm->current_state);
-
-#endif
-							break;
-
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
-							if (wpr.seq != 0) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-								mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
-#else
-
-								if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
-
-#endif
-							}
-
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
-							if (wpr.seq != wpm->current_wp_id && wpr.seq != wpm->current_wp_id + 1) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-								mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
-#else
-
-								if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1);
-
-#endif
-
-							} else if (wpr.seq >= wpm->size) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-								mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
-#else
-
-								if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
-
-#endif
-							}
-
-						} 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_REQUEST - FIXME: missed error description\n");
-
-#endif
-						}
-					}
-				}
-
-			} 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
-
-				} 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;
-		}
-
-	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;
-
-#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();
-//                        }
-
-						mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
-
-					} else if (wpc.count == 0) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("COUNT 0");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n");
-
-#endif
-						wpm->rcv_size = 0;
-						//while(waypoints_receive_buffer->size() > 0)
-//                        {
-//                            delete waypoints->back();
-//                            waypoints->pop_back();
-//                        }
-						wpm->current_active_wp_id = -1;
-						wpm->yaw_reached = false;
-						wpm->pos_reached = false;
-						break;
-
-					} 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
-					}
-
-				} else {
-					if (!(wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_GETLIST)) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm->current_state);
-
-#endif
-
-					} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id != 0) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm->current_wp_id);
-
-#endif
-
-					} 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
-					}
-				}
-
-			} 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;
-
-	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
-				if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && wp.seq < wpm->current_count)) {
-					//mavlink_missionlib_send_gcs_string("DEBUG 2");
-
-//                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid);
-//                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid);
-//                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid);
-//
-					wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
-					mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]);
-					memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
-//                    printf("WP seq: %d\n",wp.seq);
-					wpm->current_wp_id = wp.seq + 1;
-
-					// if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
-//					printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
-
-//					printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count);
-					if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-						mavlink_missionlib_send_gcs_string("GOT ALL WPS");
-						// if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_count);
-
-						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0);
-
-						if (wpm->current_active_wp_id > wpm->rcv_size - 1) {
-							wpm->current_active_wp_id = wpm->rcv_size - 1;
-						}
-
-						// switch the waypoints list
-						// FIXME CHECK!!!
-						uint32_t i;
-
-						for (i = 0; i < wpm->current_count; ++i) {
-							wpm->waypoints[i] = wpm->rcv_waypoints[i];
-						}
-
-						wpm->size = wpm->current_count;
-
-						//get the new current waypoint
-
-						for (i = 0; i < wpm->size; i++) {
-							if (wpm->waypoints[i].current == 1) {
-								wpm->current_active_wp_id = i;
-								//// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id);
-								wpm->yaw_reached = false;
-								wpm->pos_reached = false;
-								mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-								mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-								wpm->timestamp_firstinside_orbit = 0;
-								break;
-							}
-						}
-
-						if (i == wpm->size) {
-							wpm->current_active_wp_id = -1;
-							wpm->yaw_reached = false;
-							wpm->pos_reached = false;
-							wpm->timestamp_firstinside_orbit = 0;
-						}
-
-						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-
-					} else {
-						mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
-					}
-
-				} else {
-					if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-						//we're done receiving waypoints, answer with ack.
-						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0);
-						printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
-					}
-
-					// if (verbose)
-					{
-						if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) {
-//							 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state);
-							break;
-
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
-							if (!(wp.seq == 0)) {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq);
-							} else {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
-							}
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-							if (!(wp.seq == wpm->current_wp_id)) {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id);
-								mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
-
-							} else if (!(wp.seq < wpm->current_count)) {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq);
-							} else {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
-							}
-						} else {
-//							 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
-						}
-					}
-				}
-			} 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);
-				}
-			}
-
-			break;
-		}
-
-	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 (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
-				// Delete all waypoints
-				wpm->size = 0;
-				wpm->current_active_wp_id = -1;
-				wpm->yaw_reached = false;
-				wpm->pos_reached = false;
-
-			} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
-				// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state);
-			}
-
-			break;
-		}
-
-	default: {
-			// if (debug) // printf("Waypoint: received message of unknown type");
-			break;
-		}
-	}
-
-	// check_waypoints_reached(now, global_pos, local_pos);
-}
diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
deleted file mode 100644
index d7d6b31dc..000000000
--- a/src/modules/mavlink/waypoints.h
+++ /dev/null
@@ -1,132 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 waypoints.h
- * MAVLink waypoint protocol definition (BSD-relicensed).
- */
-
-#ifndef WAYPOINTS_H_
-#define WAYPOINTS_H_
-
-/* This assumes you have the mavlink headers on your include path
- 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>
-
-// FIXME XXX - TO BE MOVED TO XML
-enum MAVLINK_WPM_STATES {
-	MAVLINK_WPM_STATE_IDLE = 0,
-	MAVLINK_WPM_STATE_SENDLIST,
-	MAVLINK_WPM_STATE_SENDLIST_SENDWPS,
-	MAVLINK_WPM_STATE_GETLIST,
-	MAVLINK_WPM_STATE_GETLIST_GETWPS,
-	MAVLINK_WPM_STATE_GETLIST_GOTALL,
-	MAVLINK_WPM_STATE_ENUM_END
-};
-
-enum MAVLINK_WPM_CODES {
-	MAVLINK_WPM_CODE_OK = 0,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
-	MAVLINK_WPM_CODE_ENUM_END
-};
-
-
-/* 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_PROTOCOL_TIMEOUT_DEFAULT 5000000         ///< Protocol communication timeout in useconds
-#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000           ///< When to send a new setpoint
-#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000
-
-
-struct mavlink_wpm_storage {
-	mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT];      ///< Currently active waypoints
-#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
-	mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT];  ///< Receive buffer for next waypoints
-#endif
-	uint16_t size;
-	uint16_t max_size;
-	uint16_t rcv_size;
-	enum MAVLINK_WPM_STATES current_state;
-	int16_t current_wp_id;							///< Waypoint in current transmission
-	int16_t current_active_wp_id;					///< Waypoint the system is currently heading towards
-	uint16_t current_count;
-	uint8_t current_partner_sysid;
-	uint8_t current_partner_compid;
-	uint64_t timestamp_lastaction;
-	uint64_t timestamp_last_send_setpoint;
-	uint64_t timestamp_firstinside_orbit;
-	uint64_t timestamp_lastoutside_orbit;
-	uint32_t timeout;
-	uint32_t delay_setpoint;
-	float accept_range_yaw;
-	float accept_range_distance;
-	bool yaw_reached;
-	bool pos_reached;
-	bool idle;
-};
-
-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 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);
-
-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);
-
-#endif /* WAYPOINTS_H_ */