Cut over MAVLink to new build system

22 files changed, 5751 insertions, 0 deletions

diff --git a/src/modules/mavlink/.gitignore b/src/modules/mavlink/.gitignore
new file mode 100644
index 000000000..a02827195
--- /dev/null
+++ b/src/modules/mavlink/.gitignore
@@ -0,0 +1,3 @@
+include
+mavlink-*
+pymavlink-*
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
new file mode 100644
index 000000000..de78cd139
--- /dev/null
+++ b/src/modules/mavlink/mavlink.c
@@ -0,0 +1,825 @@
+/****************************************************************************
+ *
+ *   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 <mavlink/mavlink_log.h>
+
+#include "waypoints.h"
+#include "orb_topics.h"
+#include "missionlib.h"
+#include "mavlink_hil.h"
+#include "util.h"
+#include "waypoints.h"
+#include "mavlink_parameters.h"
+
+/* define MAVLink specific parameters */
+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/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
new file mode 100644
index 000000000..0010bb341
--- /dev/null
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   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/src/modules/mavlink/mavlink_hil.h b/src/modules/mavlink/mavlink_hil.h
new file mode 100644
index 000000000..8c7a5b514
--- /dev/null
+++ b/src/modules/mavlink/mavlink_hil.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   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/src/modules/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c
new file mode 100644
index 000000000..fa974dc0b
--- /dev/null
+++ b/src/modules/mavlink/mavlink_log.c
@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   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/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/src/modules/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c
new file mode 100644
index 000000000..819f3441b
--- /dev/null
+++ b/src/modules/mavlink/mavlink_parameters.c
@@ -0,0 +1,231 @@
+/****************************************************************************
+ *
+ *   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/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h
new file mode 100644
index 000000000..b1e38bcc8
--- /dev/null
+++ b/src/modules/mavlink/mavlink_parameters.h
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_parameters.h
+ * MAVLink parameter protocol definitions (BSD-relicensed).
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+/* This assumes you have the mavlink headers on your include path
+ or in the same folder as this source file */
+
+
+#include <v1.0/mavlink_types.h>
+#include <stdbool.h>
+#include <systemlib/param/param.h>
+
+/**
+ * Handle parameter related messages.
+ */
+void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
+
+/**
+ * Send all parameters at once.
+ *
+ * This function blocks until all parameters have been sent.
+ * it delays each parameter by the passed amount of microseconds.
+ *
+ * @param delay		The delay in us between sending all parameters.
+ */
+void mavlink_pm_send_all_params(unsigned int delay);
+
+/**
+ * Send one parameter.
+ *
+ * @param param		The parameter id to send.
+ * @return		zero on success, nonzero on failure.
+ */
+int mavlink_pm_send_param(param_t param);
+
+/**
+ * Send one parameter identified by index.
+ *
+ * @param index		The index of the parameter to send.
+ * @return		zero on success, nonzero else.
+ */
+int mavlink_pm_send_param_for_index(uint16_t index);
+
+/**
+ * Send one parameter identified by name.
+ *
+ * @param name		The index of the parameter to send.
+ * @return		zero on success, nonzero else.
+ */
+int mavlink_pm_send_param_for_name(const char *name);
+
+/**
+ * Send a queue of parameters, one parameter per function call.
+ *
+ * @return		zero on success, nonzero on failure
+ */
+int mavlink_pm_queued_send(void);
+
+/**
+ * Start sending the parameter queue.
+ *
+ * This function will not directly send parameters, but instead
+ * activate the sending of one parameter on each call of
+ * mavlink_pm_queued_send().
+ * @see 		mavlink_pm_queued_send()
+ */
+void mavlink_pm_start_queued_send(void);
diff --git a/src/modules/mavlink/mavlink_receiver.c b/src/modules/mavlink/mavlink_receiver.c
new file mode 100644
index 000000000..e62e4dcc4
--- /dev/null
+++ b/src/modules/mavlink/mavlink_receiver.c
@@ -0,0 +1,718 @@
+/****************************************************************************
+ *
+ *   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 <mavlink/mavlink_log.h>
+
+#include "waypoints.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.15f;
+		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/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
new file mode 100644
index 000000000..d369e05ff
--- /dev/null
+++ b/src/modules/mavlink/missionlib.c
@@ -0,0 +1,200 @@
+/****************************************************************************
+ *
+ *   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 <mavlink/mavlink_log.h>
+
+#include "waypoints.h"
+#include "orb_topics.h"
+#include "missionlib.h"
+#include "mavlink_hil.h"
+#include "util.h"
+#include "waypoints.h"
+#include "mavlink_parameters.h"
+
+static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
+
+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/src/modules/mavlink/missionlib.h b/src/modules/mavlink/missionlib.h
new file mode 100644
index 000000000..c2ca735b3
--- /dev/null
+++ b/src/modules/mavlink/missionlib.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   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/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
new file mode 100644
index 000000000..cbf08aeb2
--- /dev/null
+++ b/src/modules/mavlink/module.mk
@@ -0,0 +1,47 @@
+############################################################################
+#
+#   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# 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 protocol to uORB interface process
+#
+
+MODULE_COMMAND	 = mavlink
+SRCS		 += mavlink.c \
+			   missionlib.c \
+			   mavlink_parameters.c \
+			   mavlink_log.c \
+			   mavlink_receiver.c \
+			   orb_listener.c \
+			   waypoints.c
+
+INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
new file mode 100644
index 000000000..295cd5e28
--- /dev/null
+++ b/src/modules/mavlink/orb_listener.c
@@ -0,0 +1,778 @@
+/****************************************************************************
+ *
+ *   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 <mavlink/mavlink_log.h>
+
+#include "waypoints.h"
+#include "orb_topics.h"
+#include "missionlib.h"
+#include "mavlink_hil.h"
+#include "util.h"
+
+extern bool gcs_link;
+
+struct vehicle_global_position_s global_pos;
+struct vehicle_local_position_s local_pos;
+struct 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/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
new file mode 100644
index 000000000..d61cd43dc
--- /dev/null
+++ b/src/modules/mavlink/orb_topics.h
@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ *   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/src/modules/mavlink/util.h b/src/modules/mavlink/util.h
new file mode 100644
index 000000000..a4ff06a88
--- /dev/null
+++ b/src/modules/mavlink/util.h
@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   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/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
new file mode 100644
index 000000000..a131b143b
--- /dev/null
+++ b/src/modules/mavlink/waypoints.c
@@ -0,0 +1,1134 @@
+/****************************************************************************
+ *
+ *   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/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
new file mode 100644
index 000000000..c32ab32e5
--- /dev/null
+++ b/src/modules/mavlink/waypoints.h
@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   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/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c
new file mode 100644
index 000000000..5a2685560
--- /dev/null
+++ b/src/modules/mavlink_onboard/mavlink.c
@@ -0,0 +1,529 @@
+/****************************************************************************
+ *
+ *   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/src/modules/mavlink_onboard/mavlink_bridge_header.h b/src/modules/mavlink_onboard/mavlink_bridge_header.h
new file mode 100644
index 000000000..3ad3bb617
--- /dev/null
+++ b/src/modules/mavlink_onboard/mavlink_bridge_header.h
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   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/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c
new file mode 100644
index 000000000..0acbea675
--- /dev/null
+++ b/src/modules/mavlink_onboard/mavlink_receiver.c
@@ -0,0 +1,331 @@
+/****************************************************************************
+ *
+ *   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/src/modules/mavlink_onboard/module.mk b/src/modules/mavlink_onboard/module.mk
new file mode 100644
index 000000000..c40fa042e
--- /dev/null
+++ b/src/modules/mavlink_onboard/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# 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 protocol to uORB interface process (XXX hack for onboard use)
+#
+
+MODULE_COMMAND	 = mavlink_onboard
+SRCS		 = mavlink.c \
+			   mavlink_receiver.c
+
+INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h
new file mode 100644
index 000000000..f18f56243
--- /dev/null
+++ b/src/modules/mavlink_onboard/orb_topics.h
@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   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/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h
new file mode 100644
index 000000000..38a4db372
--- /dev/null
+++ b/src/modules/mavlink_onboard/util.h
@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   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);