Cut over MAVLink to new build system

24 files changed, 0 insertions, 5869 deletions

diff --git a/apps/mavlink/.context b/apps/mavlink/.context
deleted file mode 100644
index e69de29bb..000000000
--- a/apps/mavlink/.context
+++ /dev/null
diff --git a/apps/mavlink/.gitignore b/apps/mavlink/.gitignore
deleted file mode 100644
index a02827195..000000000
--- a/apps/mavlink/.gitignore
+++ /dev/null
@@ -1,3 +0,0 @@
-include
-mavlink-*
-pymavlink-*
diff --git a/apps/mavlink/Makefile b/apps/mavlink/Makefile
deleted file mode 100644
index 8ddb69b71..000000000
--- a/apps/mavlink/Makefile
+++ /dev/null
@@ -1,44 +0,0 @@
-############################################################################
-#
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Mavlink Application
-#
-
-APPNAME		 = mavlink
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
-
-INCLUDES	 = $(TOPDIR)/../mavlink/include/mavlink
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
deleted file mode 100644
index 644b779af..000000000
--- a/apps/mavlink/mavlink.c
+++ /dev/null
@@ -1,825 +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.c
- * MAVLink 1.0 protocol implementation.
- *
- * @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 <v1.0/common/mavlink.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 "waypoints.h"
-#include "mavlink_log.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 */
-PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
-PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
-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;
-
-mavlink_system_t mavlink_system = {
-	100,
-	50,
-	MAV_TYPE_FIXED_WING,
-	0,
-	0,
-	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) {
-
-		/* Advertise topics */
-		pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
-		pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
-
-		/* sensore level hil */
-		pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
-		pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
-
-		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_mode)
-{
-	/* reset MAVLink mode bitfield */
-	*mavlink_mode = 0;
-
-	/* set mode flags independent of system state */
-
-	/* HIL */
-	if (v_status.flag_hil_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
-	}
-
-	/* manual input */
-	if (v_status.flag_control_manual_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-	}
-
-	/* attitude or rate control */
-	if (v_status.flag_control_attitude_enabled ||
-	    v_status.flag_control_rates_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-	}
-
-	/* vector control */
-	if (v_status.flag_control_velocity_enabled ||
-	    v_status.flag_control_position_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
-	}
-
-	/* autonomous mode */
-	if (v_status.state_machine == SYSTEM_STATE_AUTO) {
-		*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
-	}
-
-	/* set arming state */
-	if (armed.armed) {
-		*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-
-	} else {
-		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
-	}
-
-	switch (v_status.state_machine) {
-	case SYSTEM_STATE_PREFLIGHT:
-		if (v_status.flag_preflight_gyro_calibration ||
-		    v_status.flag_preflight_mag_calibration ||
-		    v_status.flag_preflight_accel_calibration) {
-			*mavlink_state = MAV_STATE_CALIBRATING;
-
-		} else {
-			*mavlink_state = MAV_STATE_UNINIT;
-		}
-
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		*mavlink_state = MAV_STATE_STANDBY;
-		break;
-
-	case SYSTEM_STATE_GROUND_READY:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_STABILIZED:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_AUTO:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_MISSION_ABORT:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_LANDING:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_CUTOFF:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_GROUND_ERROR:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_REBOOT:
-		*mavlink_state = MAV_STATE_POWEROFF;
-		break;
-	}
-
-}
-
-
-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);
-		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:
-		fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
-		return -EINVAL;
-	}
-
-	/* open uart */
-	printf("[mavlink] 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;
-
-	/* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
-	if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
-		/* Back up the original uart configuration to restore it after exit */
-		if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-			fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %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;
-
-		/* Set baud rate */
-		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-			fprintf(stderr, "[mavlink] 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) {
-			fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
-			close(uart);
-			return -1;
-		}
-
-	} else {
-		*is_usb = true;
-	}
-
-	return uart;
-}
-
-void
-mavlink_send_uart_bytes(mavlink_channel_t channel, 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
- */
-mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
-{
-	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
-	return &m_mavlink_status[channel];
-}
-
-/*
- * Internal function to give access to the channel buffer for each channel
- */
-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, 5);
-
-	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 == 0)
-				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();
-		}
-	}
-
-	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_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
-
-			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
-
-			/* switch HIL mode if required */
-			set_hil_on_off(v_status.flag_hil_enabled);
-
-			/* send status (values already copied in the section above) */
-			mavlink_msg_sys_status_send(chan,
-						    v_status.onboard_control_sensors_present,
-						    v_status.onboard_control_sensors_enabled,
-						    v_status.onboard_control_sensors_health,
-						    v_status.load,
-						    v_status.voltage_battery * 1000.0f,
-						    v_status.current_battery * 1000.0f,
-						    v_status.battery_remaining,
-						    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++;
-
-		/* 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);
-
-		/* sleep quarter the time */
-		usleep(25000);
-
-		/* send parameters at 20 Hz (if queued for sending) */
-		mavlink_pm_queued_send();
-
-		/* sleep quarter the time */
-		usleep(25000);
-
-		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 */
-	if (!usb_uart)
-		tcsetattr(uart, TCSANOW, &uart_config_original);
-
-	thread_running = false;
-
-	exit(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\n");
-
-		thread_should_exit = false;
-		mavlink_task = task_spawn("mavlink",
-					  SCHED_DEFAULT,
-					  SCHED_PRIORITY_DEFAULT,
-					  2048,
-					  mavlink_thread_main,
-					  (const char **)argv);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-
-		while (thread_running) {
-			usleep(200000);
-			printf(".");
-		}
-
-		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/apps/mavlink/mavlink_bridge_header.h b/apps/mavlink/mavlink_bridge_header.h
deleted file mode 100644
index 270ec1727..000000000
--- a/apps/mavlink/mavlink_bridge_header.h
+++ /dev/null
@@ -1,81 +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_bridge_header
- * MAVLink bridge header for UART access.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-/* MAVLink adapter header */
-#ifndef MAVLINK_BRIDGE_HEADER_H
-#define MAVLINK_BRIDGE_HEADER_H
-
-#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-/* use efficient approach, see mavlink_helpers.h */
-#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
-
-#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
-#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
-
-#include "v1.0/mavlink_types.h"
-#include <unistd.h>
-
-
-/* Struct that stores the communication settings of this system.
-   you can also define / alter these settings elsewhere, as long
-   as they're included BEFORE mavlink.h.
-   So you can set the
-
-   mavlink_system.sysid = 100; // System ID, 1-255
-   mavlink_system.compid = 50; // Component/Subsystem ID, 1-255
-
-   Lines also in your main.c, e.g. by reading these parameter from EEPROM.
- */
-extern mavlink_system_t mavlink_system;
-
-/**
- * @brief Send multiple chars (uint8_t) over a comm channel
- *
- * @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, uint8_t *ch, int length);
-
-mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
-mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
-
-#endif /* MAVLINK_BRIDGE_HEADER_H */
diff --git a/apps/mavlink/mavlink_hil.h b/apps/mavlink/mavlink_hil.h
deleted file mode 100644
index 8c7a5b514..000000000
--- a/apps/mavlink/mavlink_hil.h
+++ /dev/null
@@ -1,61 +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 mavlink_hil.h
- * Hardware-in-the-loop simulation support.
- */
-
-#pragma once
-
-extern bool mavlink_hil_enabled;
-
-extern struct vehicle_global_position_s hil_global_pos;
-extern struct vehicle_attitude_s hil_attitude;
-extern struct sensor_combined_s hil_sensors;
-extern struct vehicle_gps_position_s hil_gps;
-extern orb_advert_t pub_hil_global_pos;
-extern orb_advert_t pub_hil_attitude;
-extern orb_advert_t pub_hil_sensors;
-extern orb_advert_t pub_hil_gps;
-
-/**
- * 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);
diff --git a/apps/mavlink/mavlink_log.c b/apps/mavlink/mavlink_log.c
deleted file mode 100644
index ed65fb883..000000000
--- a/apps/mavlink/mavlink_log.c
+++ /dev/null
@@ -1,89 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file mavlink_log.c
- * MAVLink text logging.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include <string.h>
-#include <stdlib.h>
-
-#include "mavlink_log.h"
-
-void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
-{
-	lb->size  = size;
-	lb->start = 0;
-	lb->count = 0;
-	lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage));
-}
-
-int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
-{
-	return lb->count == (int)lb->size;
-}
-
-int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
-{
-	return lb->count == 0;
-}
-
-void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
-{
-	int end = (lb->start + lb->count) % lb->size;
-	memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage));
-
-	if (mavlink_logbuffer_is_full(lb)) {
-		lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
-
-	} else {
-		++lb->count;
-	}
-}
-
-int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
-{
-	if (!mavlink_logbuffer_is_empty(lb)) {
-		memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage));
-		lb->start = (lb->start + 1) % lb->size;
-		--lb->count;
-		return 0;
-
-	} else {
-		return 1;
-	}
-}
diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h
deleted file mode 100644
index 233a76cb3..000000000
--- a/apps/mavlink/mavlink_log.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file mavlink_log.h
- * MAVLink text logging.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#ifndef MAVLINK_LOG
-#define MAVLINK_LOG
-
-/*
- * IOCTL interface for sending log messages.
- */
-#include <sys/ioctl.h>
-
-/*
- * The mavlink log device node; must be opened before messages
- * can be logged.
- */
-#define MAVLINK_LOG_DEVICE			"/dev/mavlink"
-
-#define MAVLINK_IOC_SEND_TEXT_INFO		_IOC(0x1100, 1)
-#define MAVLINK_IOC_SEND_TEXT_CRITICAL		_IOC(0x1100, 2)
-#define MAVLINK_IOC_SEND_TEXT_EMERGENCY		_IOC(0x1100, 3)
-
-/**
- * Send a mavlink emergency message.
- *
- * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
- * @param _text		The text to log;
- */
-#ifdef __cplusplus
-#define mavlink_log_emergency(_fd, _text)	::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
-#else
-#define mavlink_log_emergency(_fd, _text)	ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
-#endif
-
-/**
- * Send a mavlink critical message.
- *
- * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
- * @param _text		The text to log;
- */
-#ifdef __cplusplus
-#define mavlink_log_critical(_fd, _text)	::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
-#else
-#define mavlink_log_critical(_fd, _text)	ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
-#endif
-
-/**
- * Send a mavlink info message.
- *
- * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
- * @param _text		The text to log;
- */
-#ifdef __cplusplus
-#define mavlink_log_info(_fd, _text)		::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
-#else
-#define mavlink_log_info(_fd, _text)		ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
-#endif
-
-struct mavlink_logmessage {
-	char text[51];
-	unsigned char severity;
-};
-
-struct mavlink_logbuffer {
-	unsigned int start;
-	// unsigned int end;
-	unsigned int size;
-	int count;
-	struct mavlink_logmessage *elems;
-};
-
-void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size);
-
-int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb);
-
-int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb);
-
-void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem);
-
-int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem);
-
-#endif
-
diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c
deleted file mode 100644
index 819f3441b..000000000
--- a/apps/mavlink/mavlink_parameters.c
+++ /dev/null
@@ -1,231 +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 <v1.0/common/mavlink.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/apps/mavlink/mavlink_parameters.h b/apps/mavlink/mavlink_parameters.h
deleted file mode 100644
index b1e38bcc8..000000000
--- a/apps/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/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
deleted file mode 100644
index 22c2fcdad..000000000
--- a/apps/mavlink/mavlink_receiver.c
+++ /dev/null
@@ -1,718 +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_receiver.c
- * MAVLink protocol message receive and dispatch
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-/* 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 <v1.0/common/mavlink.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 "waypoints.h"
-#include "mavlink_log.h"
-#include "orb_topics.h"
-#include "missionlib.h"
-#include "mavlink_hil.h"
-#include "mavlink_parameters.h"
-#include "util.h"
-
-/* XXX should be in a header somewhere */
-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_attitude_s hil_attitude;
-struct vehicle_gps_position_s hil_gps;
-struct sensor_combined_s hil_sensors;
-orb_advert_t pub_hil_global_pos = -1;
-orb_advert_t pub_hil_attitude = -1;
-orb_advert_t pub_hil_gps = -1;
-orb_advert_t pub_hil_sensors = -1;
-
-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;
-
-extern bool gcs_link;
-
-static void
-handle_message(mavlink_message_t *msg)
-{
-	if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
-
-		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 */
-				printf("[mavlink] Terminating .. \n");
-				fflush(stdout);
-				usleep(50000);
-
-				/* terminate other threads and this thread */
-				thread_should_exit = true;
-
-			} else {
-
-				/* 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;
-				vcmd.command = 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);
-				}
-
-				/* publish */
-				orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
-			}
-		}
-	}
-
-	if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) {
-		mavlink_optical_flow_t flow;
-		mavlink_msg_optical_flow_decode(msg, &flow);
-
-		struct optical_flow_s f;
-
-		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;
-
-		/* check if topic is advertised */
-		if (flow_pub <= 0) {
-			flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
-
-		} else {
-			/* publish */
-			orb_publish(ORB_ID(optical_flow), flow_pub, &f);
-		}
-	}
-
-	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);
-
-		/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
-		vcmd.param1 = new_mode.base_mode;
-		vcmd.param2 = new_mode.custom_mode;
-		vcmd.param3 = 0;
-		vcmd.param4 = 0;
-		vcmd.param5 = 0;
-		vcmd.param6 = 0;
-		vcmd.param7 = 0;
-		vcmd.command = MAV_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);
-
-		} else {
-			/* create command */
-			orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
-		}
-	}
-
-	/* 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;
-
-		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);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
-		}
-	}
-
-	/* Handle quadrotor motor setpoints */
-
-	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);
-
-		if (mavlink_system.sysid < 4) {
-
-			/* switch to a receiving link mode */
-			gcs_link = false;
-
-			/*
-			 * rate control mode - defined by MAVLink
-			 */
-
-			uint8_t ml_mode = 0;
-			bool ml_armed = false;
-
-			switch (quad_motors_setpoint.mode) {
-			case 0:
-				ml_armed = false;
-				break;
-
-			case 1:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
-				ml_armed = true;
-
-				break;
-
-			case 2:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
-				ml_armed = true;
-
-				break;
-
-			case 3:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
-				break;
-
-			case 4:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
-				break;
-			}
-
-			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;
-
-			if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
-				ml_armed = false;
-			}
-
-			offboard_control_sp.armed = ml_armed;
-			offboard_control_sp.mode = ml_mode;
-
-			offboard_control_sp.timestamp = hrt_absolute_time();
-
-			/* 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);
-
-			} else {
-				/* Publish */
-				orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
-			}
-		}
-	}
-
-	/*
-	 * 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();
-
-		/* TODO, set ground_press/ temp during calib */
-		static const float ground_press = 1013.25f; // mbar
-		static const float ground_tempC = 21.0f;
-		static const float ground_alt = 0.0f;
-		static const float T0 = 273.15;
-		static const float R = 287.05f;
-		static const float g = 9.806f;
-
-		if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) {
-
-			mavlink_raw_imu_t imu;
-			mavlink_msg_raw_imu_decode(msg, &imu);
-
-			/* packet counter */
-			static uint16_t hil_counter = 0;
-			static uint16_t hil_frames = 0;
-			static uint64_t old_timestamp = 0;
-
-			/* sensors general */
-			hil_sensors.timestamp = imu.time_usec;
-
-			/* hil gyro */
-			static const float mrad2rad = 1.0e-3f;
-			hil_sensors.gyro_counter = hil_counter;
-			hil_sensors.gyro_raw[0] = imu.xgyro;
-			hil_sensors.gyro_raw[1] = imu.ygyro;
-			hil_sensors.gyro_raw[2] = imu.zgyro;
-			hil_sensors.gyro_rad_s[0] = imu.xgyro * mrad2rad;
-			hil_sensors.gyro_rad_s[1] = imu.ygyro * mrad2rad;
-			hil_sensors.gyro_rad_s[2] = imu.zgyro * mrad2rad;
-
-			/* accelerometer */
-			hil_sensors.accelerometer_counter = hil_counter;
-			static const float mg2ms2 = 9.8f / 1000.0f;
-			hil_sensors.accelerometer_raw[0] = imu.xacc;
-			hil_sensors.accelerometer_raw[1] = imu.yacc;
-			hil_sensors.accelerometer_raw[2] = imu.zacc;
-			hil_sensors.accelerometer_m_s2[0] = mg2ms2 * imu.xacc;
-			hil_sensors.accelerometer_m_s2[1] = mg2ms2 * imu.yacc;
-			hil_sensors.accelerometer_m_s2[2] = mg2ms2 * imu.zacc;
-			hil_sensors.accelerometer_mode = 0; // TODO what is this?
-			hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
-
-			/* adc */
-			hil_sensors.adc_voltage_v[0] = 0;
-			hil_sensors.adc_voltage_v[1] = 0;
-			hil_sensors.adc_voltage_v[2] = 0;
-
-			/* magnetometer */
-			float mga2ga = 1.0e-3f;
-			hil_sensors.magnetometer_counter = hil_counter;
-			hil_sensors.magnetometer_raw[0] = imu.xmag;
-			hil_sensors.magnetometer_raw[1] = imu.ymag;
-			hil_sensors.magnetometer_raw[2] = imu.zmag;
-			hil_sensors.magnetometer_ga[0] = imu.xmag * mga2ga;
-			hil_sensors.magnetometer_ga[1] = imu.ymag * mga2ga;
-			hil_sensors.magnetometer_ga[2] = imu.zmag * mga2ga;
-			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;
-
-			/* publish */
-			orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
-
-			// increment counters
-			hil_counter += 1 ;
-			hil_frames += 1 ;
-
-			// output
-			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil imu at %d hz\n", hil_frames/10);
-				old_timestamp = timestamp;
-				hil_frames = 0;
-			}
-		}
-
-		if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) {
-
-			mavlink_highres_imu_t imu;
-			mavlink_msg_highres_imu_decode(msg, &imu);
-
-			/* packet counter */
-			static uint16_t hil_counter = 0;
-			static uint16_t hil_frames = 0;
-			static uint64_t old_timestamp = 0;
-
-			/* sensors general */
-			hil_sensors.timestamp = hrt_absolute_time();
-
-			/* hil gyro */
-			static const float mrad2rad = 1.0e-3f;
-			hil_sensors.gyro_counter = hil_counter;
-			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;
-
-			/* accelerometer */
-			hil_sensors.accelerometer_counter = hil_counter;
-			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
-
-			/* adc */
-			hil_sensors.adc_voltage_v[0] = 0;
-			hil_sensors.adc_voltage_v[1] = 0;
-			hil_sensors.adc_voltage_v[2] = 0;
-
-			/* magnetometer */
-			float mga2ga = 1.0e-3f;
-			hil_sensors.magnetometer_counter = hil_counter;
-			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.baro_pres_mbar = imu.abs_pressure;
-
-			float tempC =  imu.temperature;
-			float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;
-			float h =  ground_alt + (R / g) * tempAvgK * logf(ground_press / imu.abs_pressure);
-
-			hil_sensors.baro_alt_meter = h;
-			hil_sensors.baro_temp_celcius = imu.temperature;
-
-			hil_sensors.gyro_counter = hil_counter;
-			hil_sensors.magnetometer_counter = hil_counter;
-			hil_sensors.accelerometer_counter = hil_counter;
-
-			/* publish */
-			orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
-
-			// increment counters
-			hil_counter++;
-			hil_frames++;
-
-			// output
-			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil imu at %d hz\n", hil_frames/10);
-				old_timestamp = timestamp;
-				hil_frames = 0;
-			}
-		}
-
-		if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) {
-
-			mavlink_gps_raw_int_t gps;
-			mavlink_msg_gps_raw_int_decode(msg, &gps);
-
-			/* packet counter */
-			static uint16_t hil_counter = 0;
-			static uint16_t hil_frames = 0;
-			static uint64_t old_timestamp = 0;
-
-			/* 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 = (float)gps.vel * 1e-2f * cosf(heading_rad);
-			hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad);
-			hil_gps.vel_d_m_s = 0.0f;
-			hil_gps.vel_ned_valid = true;
-			/* COG (course over ground) is speced as -PI..+PI */
-			hil_gps.cog_rad = heading_rad;
-			hil_gps.fix_type = gps.fix_type;
-			hil_gps.satellites_visible = gps.satellites_visible;
-
-			/* publish */
-			orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
-
-			// increment counters
-			hil_counter += 1 ;
-			hil_frames += 1 ;
-
-			// output
-			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil gps at %d hz\n", hil_frames/10);
-				old_timestamp = timestamp;
-				hil_frames = 0;
-			}
-		}
-
-		if (msg->msgid == MAVLINK_MSG_ID_RAW_PRESSURE) {
-
-			mavlink_raw_pressure_t press;
-			mavlink_msg_raw_pressure_decode(msg, &press);
-
-			/* packet counter */
-			static uint16_t hil_counter = 0;
-			static uint16_t hil_frames = 0;
-			static uint64_t old_timestamp = 0;
-
-			/* sensors general */
-			hil_sensors.timestamp = press.time_usec;
-
-			/* baro */
-
-			float tempC =  press.temperature / 100.0f;
-			float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;
-			float h =  ground_alt + (R / g) * tempAvgK * logf(ground_press / press.press_abs);
-			hil_sensors.baro_counter = hil_counter;
-			hil_sensors.baro_pres_mbar = press.press_abs;
-			hil_sensors.baro_alt_meter = h;
-			hil_sensors.baro_temp_celcius = tempC;
-
-			/* publish */
-			orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
-
-			// increment counters
-			hil_counter += 1 ;
-			hil_frames += 1 ;
-
-			// output
-			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil pressure at %d hz\n", hil_frames/10);
-				old_timestamp = timestamp;
-				hil_frames = 0;
-			}
-		}
-
-		if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {
-
-			mavlink_hil_state_t hil_state;
-			mavlink_msg_hil_state_decode(msg, &hil_state);
-
-			/* Calculate Rotation Matrix */
-			//TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode
-
-			if (mavlink_system.type == MAV_TYPE_FIXED_WING) {
-				//TODO: assuming low pitch and roll values for now
-				hil_attitude.R[0][0] = cosf(hil_state.yaw);
-				hil_attitude.R[0][1] = sinf(hil_state.yaw);
-				hil_attitude.R[0][2] = 0.0f;
-
-				hil_attitude.R[1][0] = -sinf(hil_state.yaw);
-				hil_attitude.R[1][1] = cosf(hil_state.yaw);
-				hil_attitude.R[1][2] = 0.0f;
-
-				hil_attitude.R[2][0] = 0.0f;
-				hil_attitude.R[2][1] = 0.0f;
-				hil_attitude.R[2][2] = 1.0f;
-
-				hil_attitude.R_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;
-
-
-			/* set timestamp and notify processes (broadcast) */
-			hil_global_pos.timestamp = hrt_absolute_time();
-			orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
-
-			hil_attitude.roll = hil_state.roll;
-			hil_attitude.pitch = hil_state.pitch;
-			hil_attitude.yaw = hil_state.yaw;
-			hil_attitude.rollspeed = hil_state.rollspeed;
-			hil_attitude.pitchspeed = hil_state.pitchspeed;
-			hil_attitude.yawspeed = hil_state.yawspeed;
-
-			/* set timestamp and notify processes (broadcast) */
-			hil_attitude.timestamp = hrt_absolute_time();
-			orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &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;
-
-			rc_hil.timestamp = hrt_absolute_time();
-			rc_hil.chan_count = 4;
-
-			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;
-
-			struct manual_control_setpoint_s mc;
-			static orb_advert_t mc_pub = 0;
-
-			int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-
-			/* 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);
-
-			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;
-
-			/* fake RC channels with manual control input from simulator */
-
-
-			if (rc_pub == 0) {
-				rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil);
-
-			} else {
-				orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil);
-			}
-
-			if (mc_pub == 0) {
-				mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc);
-
-			} else {
-				orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc);
-			}
-		}
-	}
-}
-
-
-/**
- * Receive data from UART.
- */
-static void *
-receive_thread(void *arg)
-{
-	int uart_fd = *((int *)arg);
-
-	const int timeout = 1000;
-	uint8_t buf[32];
-
-	mavlink_message_t msg;
-
-	prctl(PR_SET_NAME, "mavlink uart rcv", getpid());
-
-	while (!thread_should_exit) {
-
-		struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
-
-		if (poll(fds, 1, timeout) > 0) {
-			/* non-blocking read. read may return negative values */
-			ssize_t 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)) {
-					/* 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);
-				}
-			}
-		}
-	}
-
-	return NULL;
-}
-
-pthread_t
-receive_start(int uart)
-{
-	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);
-
-	struct sched_param param;
-	param.sched_priority = SCHED_PRIORITY_MAX - 40;
-	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
-
-	pthread_attr_setstacksize(&receiveloop_attr, 2048);
-
-	pthread_t thread;
-	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
-	return thread;
-}
diff --git a/apps/mavlink/missionlib.c b/apps/mavlink/missionlib.c
deleted file mode 100644
index 8064febc4..000000000
--- a/apps/mavlink/missionlib.c
+++ /dev/null
@@ -1,200 +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 <v1.0/common/mavlink.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 "waypoints.h"
-#include "mavlink_log.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];
-
-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();
-}
-
-/**
- * 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 local_position_setpoint_pub = -1;
-	char buf[50] = {0};
-
-	/* 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 = (param4 / 180.0f) * M_PI_F - M_PI_F;
-
-		/* 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: % 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 = (param4 / 180.0f) * M_PI_F - M_PI_F;
-
-		/* 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 = (param4 / 180.0f) * M_PI_F - 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);
-	}
-
-	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/apps/mavlink/missionlib.h b/apps/mavlink/missionlib.h
deleted file mode 100644
index c2ca735b3..000000000
--- a/apps/mavlink/missionlib.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file missionlib.h
- * MAVLink mission helper library
- */
-
-#pragma once
-
-#include <v1.0/common/mavlink.h>
-
-//extern void	mavlink_wpm_send_message(mavlink_message_t *msg);
-//extern void	mavlink_wpm_send_gcs_string(const char *string);
-//extern uint64_t	mavlink_wpm_get_system_timestamp(void);
-extern int	mavlink_missionlib_send_message(mavlink_message_t *msg);
-extern int	mavlink_missionlib_send_gcs_string(const char *string);
-extern uint64_t	mavlink_missionlib_get_system_timestamp(void);
-extern void	mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
-		float param2, float param3, float param4, float param5_lat_x,
-		float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
deleted file mode 100644
index 5f15facf8..000000000
--- a/apps/mavlink/orb_listener.c
+++ /dev/null
@@ -1,777 +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 <v1.0/common/mavlink.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 "waypoints.h"
-#include "mavlink_log.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 vehicle_status_s v_status;
-struct rc_channels_s rc;
-struct rc_input_values rc_raw;
-struct actuator_armed_s armed;
-
-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 void	*uorb_receive_thread(void *arg);
-
-struct listener {
-	void	(*callback)(const struct listener *l);
-	int		*subp;
-	uintptr_t	arg;
-};
-
-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 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},
-};
-
-static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
-
-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, 0 /* no diff pressure yet */,
-					     raw.baro_alt_meter, raw.baro_temp_celcius,
-					     fields_updated);
-
-	sensors_raw_counter++;
-}
-
-void
-l_vehicle_attitude(const struct listener *l)
-{
-	struct vehicle_attitude_s att;
-
-
-	/* 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);
-
-	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 = gps.cog_rad;
-	if (cog_deg > M_PI_F)
-		cog_deg -= 2.0f * M_PI_F;
-	cog_deg *= 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,
-				     gps.eph_m * 1e2f, // from m to cm
-				     gps.epv_m * 1e2f, // from m to cm
-				     gps.vel_m_s * 1e2f, // from m/s to cm/s
-				     cog_deg * 1e2f, // from rad 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 */
-	set_hil_on_off(v_status.flag_hil_enabled);
-
-	/* translate the current syste state to mavlink state and mode */
-	uint8_t mavlink_state = 0;
-	uint8_t mavlink_mode = 0;
-	get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
-
-	/* send heartbeat */
-	mavlink_msg_heartbeat_send(chan,
-				   mavlink_system.type,
-				   MAV_AUTOPILOT_PX4,
-				   mavlink_mode,
-				   v_status.state_machine,
-				   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)
-		/* Channels are sent in MAVLink main loop at a fixed interval */
-		mavlink_msg_rc_channels_raw_send(chan,
-						 rc_raw.timestamp / 1000,
-						 0,
-						 (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX,
-						 (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX,
-						 (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX,
-						 (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX,
-						 (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX,
-						 (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX,
-						 (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX,
-						 (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX,
-						 255);
-}
-
-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);
-
-	uint64_t timestamp = global_pos.timestamp;
-	int32_t lat = global_pos.lat;
-	int32_t lon = global_pos.lon;
-	int32_t alt = (int32_t)(global_pos.alt * 1000);
-	int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f);
-	int16_t vx = (int16_t)(global_pos.vx * 100.0f);
-	int16_t vy = (int16_t)(global_pos.vy * 100.0f);
-	int16_t vz = (int16_t)(global_pos.vz * 100.0f);
-
-	/* heading in degrees * 10, from 0 to 36.000) */
-	uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
-
-	mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
-					     timestamp / 1000,
-					     lat,
-					     lon,
-					     alt,
-					     relative_alt,
-					     vx,
-					     vy,
-					     vz,
-					     hdg);
-}
-
-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,
-				global_sp.yaw);
-}
-
-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 */,
-						  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 */
-		if (mavlink_hil_enabled && armed.armed) {
-
-			/* translate the current syste state to mavlink state and mode */
-			uint8_t mavlink_state = 0;
-			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
-
-			/* 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_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_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_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_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)
-{
-	struct actuator_controls_effective_s actuators;
-
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators);
-
-	if (gcs_link) {
-		/* send, add spaces so that string buffer is at least 10 chars long */
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl0    ",
-						   actuators.control_effective[0]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl1    ",
-						   actuators.control_effective[1]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl2     ",
-						   actuators.control_effective[2]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl3     ",
-						   actuators.control_effective[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);
-}
-
-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) {
-			mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
-
-		} 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_EFFECTIVE);
-	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 */
-
-	/* 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);
-	return thread;
-}
diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h
deleted file mode 100644
index d61cd43dc..000000000
--- a/apps/mavlink/orb_topics.h
+++ /dev/null
@@ -1,106 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-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 orb_topics.h
- * Common sets of topics subscribed to or published by the MAVLink driver,
- * and structures maintained by those subscriptions.
- */
-#pragma once
-
-#include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/rc_channels.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_global_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_global_position_setpoint.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_rates_setpoint.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/manual_control_setpoint.h>
-#include <uORB/topics/debug_key_value.h>
-#include <drivers/drv_rc_input.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 armed_sub;
-	int actuators_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;
-};
-
-extern struct mavlink_subscriptions mavlink_subs;
-
-/** Global position */
-extern struct vehicle_global_position_s global_pos;
-
-/** Local position */
-extern struct vehicle_local_position_s local_pos;
-
-/** Vehicle status */
-extern struct vehicle_status_s v_status;
-
-/** RC channels */
-extern struct rc_channels_s rc;
-
-/** Actuator armed state */
-extern struct actuator_armed_s armed;
-
-/** Worker thread starter */
-extern pthread_t uorb_receive_start(void);
diff --git a/apps/mavlink/util.h b/apps/mavlink/util.h
deleted file mode 100644
index a4ff06a88..000000000
--- a/apps/mavlink/util.h
+++ /dev/null
@@ -1,54 +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 util.h
- * Utility and helper functions and data.
- */
-
-#pragma once
-
-/** MAVLink communications channel */
-extern uint8_t chan;
-
-/** Shutdown marker */
-extern volatile bool thread_should_exit;
-
-/** Waypoint storage */
-extern mavlink_wpm_storage *wpm;
-
-/**
- * Translate the custom state into standard mavlink modes and state.
- */
-extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode);
diff --git a/apps/mavlink/waypoints.c b/apps/mavlink/waypoints.c
deleted file mode 100644
index a131b143b..000000000
--- a/apps/mavlink/waypoints.c
+++ /dev/null
@@ -1,1134 +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 "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"));
-}
-
-//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z)
-//{
-//    if (seq < wpm->size)
-//    {
-//        mavlink_mission_item_t *cur = waypoints->at(seq);
-//
-//        const PxVector3 A(cur->x, cur->y, cur->z);
-//        const PxVector3 C(x, y, z);
-//
-//        // seq not the second last waypoint
-//        if ((uint16_t)(seq+1) < wpm->size)
-//        {
-//            mavlink_mission_item_t *next = waypoints->at(seq+1);
-//            const PxVector3 B(next->x, next->y, next->z);
-//            const float r = (B-A).dot(C-A) / (B-A).lengthSquared();
-//            if (r >= 0 && r <= 1)
-//            {
-//                const PxVector3 P(A + r*(B-A));
-//                return (P-C).length();
-//            }
-//            else if (r < 0.f)
-//            {
-//                return (C-A).length();
-//            }
-//            else
-//            {
-//                return (C-B).length();
-//            }
-//        }
-//        else
-//        {
-//            return (C-A).length();
-//        }
-//    }
-//    else
-//    {
-//        // if (verbose) // printf("ERROR: index out of bounds\n");
-//    }
-//    return -1.f;
-//}
-
-/*
- * Calculate distance in global frame.
- *
- * The distance calculation is based on the WGS84 geoid (GPS)
- */
-float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt)
-{
-	//TODO: implement for z once altidude contoller is implemented
-
-	static uint16_t counter;
-
-//	if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y);
-	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
-
-		double current_x_rad = cur->x / 180.0 * M_PI;
-		double current_y_rad = cur->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;
-
-		return radius_earth * c;
-
-	} else {
-		return -1.0f;
-	}
-
-	counter++;
-
-}
-
-/*
- * Calculate distance in local frame (NED)
- */
-float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z)
-{
-	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);
-
-		return sqrt(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)
-{
-	static uint16_t counter;
-
-	// Do not flood the precious wireless link with debug data
-	// if (wpm->size > 0 && counter % 10 == 0) {
-	// 	printf("Currect active waypoint id: %i\n", wpm->current_active_wp_id);
-	// }
-
-
-	if (wpm->current_active_wp_id < wpm->size) {
-
-		float orbit = wpm->waypoints[wpm->current_active_wp_id].param2;
-		int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
-		float dist = -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);
-
-		} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt);
-
-		} 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);
-
-		} else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
-			/* Check if conditions of mission item are satisfied */
-			// XXX TODO
-		}
-
-		if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw
-			wpm->pos_reached = true;
-
-			if (counter % 100 == 0)
-				printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit);
-		}
-
-//		else
-//		{
-//			if(counter % 100 == 0)
-//				printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame);
-//		}
-	}
-
-	//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
-				printf("Reached waypoint %u for the first time \n", cur_wp->seq);
-				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))
-			if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param2 * 1000) {
-				printf("Reached waypoint %u long enough \n", cur_wp->seq);
-
-				if (cur_wp->autocontinue) {
-					cur_wp->current = 0;
-
-					if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
-						/* the last waypoint was reached, if auto continue is
-						 * activated restart the waypoint list from the beginning
-						 */
-						wpm->current_active_wp_id = 0;
-
-					} else {
-						if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
-							wpm->current_active_wp_id++;
-					}
-
-					// 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)
-{
-	/* 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;
-		}
-	}
-
-	// Do NOT continously send the current WP, since we're not loosing messages
-	// if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) {
-	// 	mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-	// }
-
-	check_waypoints_reached(now, global_position , local_position);
-
-	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_ATTITUDE:
-//        {
-//            if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size)
-//            {
-//                mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-//                if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED)
-//                {
-//                    mavlink_attitude_t att;
-//                    mavlink_msg_attitude_decode(msg, &att);
-//                    float yaw_tolerance = wpm->accept_range_yaw;
-//                    //compare current yaw
-//                    if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*FM_PI)
-//                    {
-//                        if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
-//                            wpm->yaw_reached = true;
-//                    }
-//                    else if(att.yaw - yaw_tolerance < 0.0f)
-//                    {
-//                        float lowerBound = 360.0f + att.yaw - yaw_tolerance;
-//                        if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
-//                            wpm->yaw_reached = true;
-//                    }
-//                    else
-//                    {
-//                        float upperBound = att.yaw + yaw_tolerance - 2.f*FM_PI;
-//                        if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
-//                            wpm->yaw_reached = true;
-//                    }
-//                }
-//            }
-//            break;
-//        }
-//
-//		case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
-//        {
-//            if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size)
-//            {
-//                mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-//
-//                if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED)
-//                {
-//                    mavlink_local_position_ned_t pos;
-//                    mavlink_msg_local_position_ned_decode(msg, &pos);
-//                    //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
-//
-//                    wpm->pos_reached = false;
-//
-//                    // compare current position (given in message) with current waypoint
-//                    float orbit = wp->param1;
-//
-//                    float dist;
-////                    if (wp->param2 == 0)
-////                    {
-////						// FIXME segment distance
-////                        //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z);
-////                    }
-////                    else
-////                    {
-//                        dist = mavlink_wpm_distance_to_point(wpm->current_active_wp_id, pos.x, pos.y, pos.z);
-////                    }
-//
-//                    if (dist >= 0.f && dist <= orbit && wpm->yaw_reached)
-//                    {
-//                        wpm->pos_reached = true;
-//                    }
-//                }
-//            }
-//            break;
-//        }
-
-//		case MAVLINK_MSG_ID_CMD: // special action from ground station
-//        {
-//            mavlink_cmd_t action;
-//            mavlink_msg_cmd_decode(msg, &action);
-//            if(action.target == mavlink_system.sysid)
-//            {
-//                // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
-//                switch (action.action)
-//                {
-//						//				case MAV_ACTION_LAUNCH:
-//						//					// if (verbose) std::cerr << "Launch received" << std::endl;
-//						//					current_active_wp_id = 0;
-//						//					if (wpm->size>0)
-//						//					{
-//						//						setActive(waypoints[current_active_wp_id]);
-//						//					}
-//						//					else
-//						//						// if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
-//						//					break;
-//
-//						//				case MAV_ACTION_CONTINUE:
-//						//					// if (verbose) std::c
-//						//					err << "Continue received" << std::endl;
-//						//					idle = false;
-//						//					setActive(waypoints[current_active_wp_id]);
-//						//					break;
-//
-//						//				case MAV_ACTION_HALT:
-//						//					// if (verbose) std::cerr << "Halt received" << std::endl;
-//						//					idle = true;
-//						//					break;
-//
-//						//				default:
-//						//					// if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
-//						//					break;
-//                }
-//            }
-//            break;
-//        }
-
-	case MAVLINK_MSG_ID_MISSION_ACK: {
-			mavlink_mission_ack_t wpa;
-			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) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n");
-
-#endif
-						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-						wpm->current_wp_id = 0;
-					}
-				}
-
-			} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
-#else
-
-				if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
-
-#endif
-			}
-
-			break;
-		}
-
-	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;
-							}
-						}
-
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("NEW WP SET");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm->current_active_wp_id);
-
-#endif
-						wpm->yaw_reached = false;
-						wpm->pos_reached = false;
-						mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-						mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-						wpm->timestamp_firstinside_orbit = 0;
-
-					} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n");
-
-#endif
-					}
-
-				} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-					mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
-#else
-
-					if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n");
-
-#endif
-				}
-
-			} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-#else
-
-				if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
-
-#endif
-			}
-
-			break;
-		}
-
-	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/apps/mavlink/waypoints.h b/apps/mavlink/waypoints.h
deleted file mode 100644
index c32ab32e5..000000000
--- a/apps/mavlink/waypoints.h
+++ /dev/null
@@ -1,131 +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 <v1.0/common/mavlink.h>
-#include <stdbool.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.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);
-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_ */
diff --git a/apps/mavlink_onboard/Makefile b/apps/mavlink_onboard/Makefile
deleted file mode 100644
index 8b1cb9b2c..000000000
--- a/apps/mavlink_onboard/Makefile
+++ /dev/null
@@ -1,44 +0,0 @@
-############################################################################
-#
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Mavlink Application
-#
-
-APPNAME		 = mavlink_onboard
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
-
-INCLUDES	 = $(TOPDIR)/../mavlink/include/mavlink
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c
deleted file mode 100644
index 5a2685560..000000000
--- a/apps/mavlink_onboard/mavlink.c
+++ /dev/null
@@ -1,529 +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.c
- * MAVLink 1.0 protocol implementation.
- *
- * @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 <v1.0/common/mavlink.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 "orb_topics.h"
-#include "util.h"
-
-__EXPORT int mavlink_onboard_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;
-
-/* terminate MAVLink on user request - disabled by default */
-static bool mavlink_link_termination_allowed = false;
-
-mavlink_system_t mavlink_system = {
-	100,
-	50,
-	MAV_TYPE_QUADROTOR,
-	0,
-	0,
-	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);
-
-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 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);
-
-/****************************************************************************
- * 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:
-		fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
-		return -EINVAL;
-	}
-
-	/* open uart */
-	printf("[mavlink] 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;
-
-	if (strcmp(uart_name, "/dev/ttyACM0") != OK) {
-		/* Back up the original uart configuration to restore it after exit */
-		if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-			fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %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;
-
-		/* Set baud rate */
-		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-			fprintf(stderr, "[mavlink] 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) {
-			fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
-			close(uart);
-			return -1;
-		}
-
-	} else {
-		*is_usb = true;
-	}
-
-	return uart;
-}
-
-void 
-mavlink_send_uart_bytes(mavlink_channel_t channel, 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
- */
-mavlink_status_t* mavlink_get_channel_status(uint8_t channel)
-{
-	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
-	return &m_mavlink_status[channel];
-}
-
-/*
- * Internal function to give access to the channel buffer for each channel
- */
-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;
-	param_t param_system_id;
-	param_t param_component_id;
-	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");
-	}
-
-	/* 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;
-	}
-}
-
-void
-get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
-	uint8_t *mavlink_state, uint8_t *mavlink_mode)
-{
-	/* reset MAVLink mode bitfield */
-	*mavlink_mode = 0;
-
-	/* set mode flags independent of system state */
-	if (v_status->flag_control_manual_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-	}
-
-	if (v_status->flag_hil_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
-	}
-
-	/* set arming state */
-	if (armed->armed) {
-		*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-	} else {
-		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
-	}
-
-	switch (v_status->state_machine) {
-	case SYSTEM_STATE_PREFLIGHT:
-		if (v_status->flag_preflight_gyro_calibration ||
-		    v_status->flag_preflight_mag_calibration ||
-		    v_status->flag_preflight_accel_calibration) {
-			*mavlink_state = MAV_STATE_CALIBRATING;
-		} else {
-			*mavlink_state = MAV_STATE_UNINIT;
-		}
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		*mavlink_state = MAV_STATE_STANDBY;
-		break;
-
-	case SYSTEM_STATE_GROUND_READY:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-		break;
-
-	case SYSTEM_STATE_STABILIZED:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-		break;
-
-	case SYSTEM_STATE_AUTO:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
-		break;
-
-	case SYSTEM_STATE_MISSION_ABORT:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_LANDING:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_CUTOFF:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_GROUND_ERROR:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_REBOOT:
-		*mavlink_state = MAV_STATE_POWEROFF;
-		break;
-	}
-
-}
-
-/**
- * MAVLink Protocol main function.
- */
-int mavlink_thread_main(int argc, char *argv[])
-{
-	int ch;
-	char *device_name = "/dev/ttyS1";
-	baudrate = 57600;
-
-	struct vehicle_status_s v_status;
-	struct actuator_armed_s armed;
-
-	/* 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 == 0)
-				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();
-		}
-	}
-
-	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);
-
-	/* Initialize system properties */
-	mavlink_update_system();
-
-	/* start the MAVLink receiver */
-	receive_thread = receive_start(uart);
-
-	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_mode = 0;
-			get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
-
-			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
-
-			/* send status (values already copied in the section above) */
-			mavlink_msg_sys_status_send(chan,
-						    v_status.onboard_control_sensors_present,
-						    v_status.onboard_control_sensors_enabled,
-						    v_status.onboard_control_sensors_health,
-						    v_status.load,
-						    v_status.voltage_battery * 1000.0f,
-						    v_status.current_battery * 1000.0f,
-						    v_status.battery_remaining,
-						    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++;
-
-		/* sleep 1000 ms */
-		usleep(1000000);
-	}
-
-	/* wait for threads to complete */
-	pthread_join(receive_thread, NULL);
-
-	/* Reset the UART flags to original state */
-	if (!usb_uart)
-		tcsetattr(uart, TCSANOW, &uart_config_original);
-
-	thread_running = false;
-
-	exit(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_onboard_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\n");
-
-		thread_should_exit = false;
-		mavlink_task = task_spawn("mavlink_onboard",
-					  SCHED_DEFAULT,
-					  SCHED_PRIORITY_DEFAULT,
-					  2048,
-					  mavlink_thread_main,
-					  (const char**)argv);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		while (thread_running) {
-			usleep(200000);
-		}
-		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/apps/mavlink_onboard/mavlink_bridge_header.h b/apps/mavlink_onboard/mavlink_bridge_header.h
deleted file mode 100644
index bf7c5354c..000000000
--- a/apps/mavlink_onboard/mavlink_bridge_header.h
+++ /dev/null
@@ -1,81 +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_bridge_header
- * MAVLink bridge header for UART access.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-/* MAVLink adapter header */
-#ifndef MAVLINK_BRIDGE_HEADER_H
-#define MAVLINK_BRIDGE_HEADER_H
-
-#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
-
-/* use efficient approach, see mavlink_helpers.h */
-#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
-
-#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
-#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
-
-#include "v1.0/mavlink_types.h"
-#include <unistd.h>
-
-
-/* Struct that stores the communication settings of this system.
-   you can also define / alter these settings elsewhere, as long
-   as they're included BEFORE mavlink.h.
-   So you can set the
-
-   mavlink_system.sysid = 100; // System ID, 1-255
-   mavlink_system.compid = 50; // Component/Subsystem ID, 1-255
-
-   Lines also in your main.c, e.g. by reading these parameter from EEPROM.
- */
-extern mavlink_system_t mavlink_system;
-
-/**
- * @brief Send multiple chars (uint8_t) over a comm channel
- *
- * @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, uint8_t *ch, int length);
-
-mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
-mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);
-
-#endif /* MAVLINK_BRIDGE_HEADER_H */
diff --git a/apps/mavlink_onboard/mavlink_receiver.c b/apps/mavlink_onboard/mavlink_receiver.c
deleted file mode 100644
index 0acbea675..000000000
--- a/apps/mavlink_onboard/mavlink_receiver.c
+++ /dev/null
@@ -1,331 +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_receiver.c
- * MAVLink protocol message receive and dispatch
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-/* 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 <v1.0/common/mavlink.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 "util.h"
-#include "orb_topics.h"
-
-/* XXX should be in a header somewhere */
-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_attitude_s hil_attitude;
-orb_advert_t pub_hil_global_pos = -1;
-orb_advert_t pub_hil_attitude = -1;
-
-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;
-
-extern bool gcs_link;
-
-static void
-handle_message(mavlink_message_t *msg)
-{
-	if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
-
-		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 */
-				printf("[mavlink] Terminating .. \n");
-				fflush(stdout);
-				usleep(50000);
-
-				/* terminate other threads and this thread */
-				thread_should_exit = true;
-
-			} else {
-
-				/* 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;
-				vcmd.command = 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);
-				}
-				/* publish */
-				orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
-			}
-		}
-	}
-
-	if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) {
-		mavlink_optical_flow_t flow;
-		mavlink_msg_optical_flow_decode(msg, &flow);
-
-		struct optical_flow_s f;
-
-		f.timestamp = hrt_absolute_time();
-		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;
-
-		/* check if topic is advertised */
-		if (flow_pub <= 0) {
-			flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
-		} else {
-			/* publish */
-			orb_publish(ORB_ID(optical_flow), flow_pub, &f);
-		}
-
-	}
-
-	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);
-
-		/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
-		vcmd.param1 = new_mode.base_mode;
-		vcmd.param2 = new_mode.custom_mode;
-		vcmd.param3 = 0;
-		vcmd.param4 = 0;
-		vcmd.param5 = 0;
-		vcmd.param6 = 0;
-		vcmd.param7 = 0;
-		vcmd.command = MAV_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);
-		} else {
-			/* create command */
-			orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
-		}
-	}
-
-	/* 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.x = pos.x;
-		vicon_position.y = pos.y;
-		vicon_position.z = pos.z;
-
-		if (vicon_position_pub <= 0) {
-			vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
-		} else {
-			orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
-		}
-	}
-
-	/* Handle quadrotor motor setpoints */
-
-	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);
-
-		if (mavlink_system.sysid < 4) {
-
-			/* switch to a receiving link mode */
-			gcs_link = false;
-
-			/* 
-			 * rate control mode - defined by MAVLink
-			 */
-
-			uint8_t ml_mode = 0;
-			bool ml_armed = false;
-
-			switch (quad_motors_setpoint.mode) {
-				case 0:
-					ml_armed = false;
-					break;
-				case 1:
-					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
-					ml_armed = true;
-
-					break;
-				case 2:
-					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
-					ml_armed = true;
-
-					break;
-				case 3:
-					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
-					break;
-				case 4:
-					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
-					break;
-			}
-
-			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;
-
-			if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) {
-				ml_armed = false;
-			}
-
-			offboard_control_sp.armed = ml_armed;
-			offboard_control_sp.mode = ml_mode;
-
-			offboard_control_sp.timestamp = hrt_absolute_time();
-
-			/* 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);
-			} else {
-				/* Publish */
-				orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
-			}
-		}
-	}
-
-}
-
-
-/**
- * Receive data from UART.
- */
-static void *
-receive_thread(void *arg)
-{
-	int uart_fd = *((int*)arg);
-
-	const int timeout = 1000;
-	uint8_t ch;
-
-	mavlink_message_t msg;
-
-	prctl(PR_SET_NAME, "mavlink offb rcv", getpid());
-
-	while (!thread_should_exit) {
-
-		struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
-
-		if (poll(fds, 1, timeout) > 0) {
-			/* non-blocking read until buffer is empty */
-			int nread = 0;
-
-			do {
-				nread = read(uart_fd, &ch, 1);
-
-				if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char
-					/* handle generic messages and commands */
-					handle_message(&msg);
-				}
-			} while (nread > 0);
-		}
-	}
-
-	return NULL;
-}
-
-pthread_t
-receive_start(int uart)
-{
-	pthread_attr_t receiveloop_attr;
-	pthread_attr_init(&receiveloop_attr);
-
-	struct sched_param param;
-  	param.sched_priority = SCHED_PRIORITY_MAX - 40;
-  	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
-
-	pthread_attr_setstacksize(&receiveloop_attr, 2048);
-
-	pthread_t thread;
-	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
-	return thread;
-}
\ No newline at end of file
diff --git a/apps/mavlink_onboard/orb_topics.h b/apps/mavlink_onboard/orb_topics.h
deleted file mode 100644
index f18f56243..000000000
--- a/apps/mavlink_onboard/orb_topics.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-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 orb_topics.h
- * Common sets of topics subscribed to or published by the MAVLink driver,
- * and structures maintained by those subscriptions.
- */
-#pragma once
-
-#include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/rc_channels.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_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_global_position_setpoint.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/optical_flow.h>
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/debug_key_value.h>
-#include <drivers/drv_rc_input.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 armed_sub;
-	int actuators_sub;
-	int local_pos_sub;
-	int spa_sub;
-	int spl_sub;
-	int spg_sub;
-	int debug_key_value;
-	int input_rc_sub;
-};
-
-extern struct mavlink_subscriptions mavlink_subs;
-
-/** Global position */
-extern struct vehicle_global_position_s global_pos;
-
-/** Local position */
-extern struct vehicle_local_position_s local_pos;
-
-/** Vehicle status */
-// extern struct vehicle_status_s v_status;
-
-/** RC channels */
-extern struct rc_channels_s rc;
-
-/** Actuator armed state */
-// extern struct actuator_armed_s armed;
-
-/** Worker thread starter */
-extern pthread_t uorb_receive_start(void);
diff --git a/apps/mavlink_onboard/util.h b/apps/mavlink_onboard/util.h
deleted file mode 100644
index 38a4db372..000000000
--- a/apps/mavlink_onboard/util.h
+++ /dev/null
@@ -1,54 +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 util.h
- * Utility and helper functions and data.
- */
-
-#pragma once
-
-#include "orb_topics.h"
-
-/** MAVLink communications channel */
-extern uint8_t chan;
-
-/** Shutdown marker */
-extern volatile bool thread_should_exit;
-
-/**
- * Translate the custom state into standard mavlink modes and state.
- */
-extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
-	uint8_t *mavlink_state, uint8_t *mavlink_mode);