Merge remote-tracking branch 'px4/master' into pwm_ioctls

Conflicts:
	src/drivers/drv_pwm_output.h

98 files changed, 7456 insertions, 1584 deletions

diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 1ec61eb60..5e45cc936 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -68,6 +68,7 @@
 
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
@@ -77,10 +78,9 @@
 
 Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
 	I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
+	_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
+	_max_differential_pressure_pa(0),
 	_sensor_ok(false),
 	_measure_ticks(0),
 	_collect_phase(false),
@@ -88,8 +88,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
 	_airspeed_pub(-1),
 	_conversion_interval(conversion_interval),
 	_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows"))
+	_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -105,7 +104,7 @@ Airspeed::~Airspeed()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 
 	// free perf counters
 	perf_free(_sample_perf);
@@ -123,20 +122,14 @@ Airspeed::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_reports = new struct differential_pressure_s[_num_reports];
-
-	for (unsigned i = 0; i < _num_reports; i++)
-		_reports[i].max_differential_pressure_pa = 0;
-
+	_reports = new RingBuffer(2, sizeof(differential_pressure_s));
 	if (_reports == nullptr)
 		goto out;
 
-	_oldest_report = _next_report = 0;
-
 	/* get a publish handle on the airspeed topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]);
+	differential_pressure_s zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report);
 
 	if (_airspeed_pub < 0)
 		warnx("failed to create airspeed sensor object. Did you start uOrb?");
@@ -229,31 +222,22 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* add one to account for the sentinel in the ring */
-			arg++;
-
 			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
+			if ((arg < 1) || (arg > 100))
 				return -EINVAL;
 
-			/* allocate new buffer */
-			struct differential_pressure_s *buf = new struct differential_pressure_s[arg];
-
-			if (nullptr == buf)
+			irqstate_t flags = irqsave();
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
 				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
+			}
+			irqrestore(flags);
 
 			return OK;
 		}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
 		/* XXX implement this */
@@ -281,7 +265,8 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
 ssize_t
 Airspeed::read(struct file *filp, char *buffer, size_t buflen)
 {
-	unsigned count = buflen / sizeof(struct differential_pressure_s);
+	unsigned count = buflen / sizeof(differential_pressure_s);
+	differential_pressure_s *abuf = reinterpret_cast<differential_pressure_s *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -297,10 +282,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(abuf)) {
+				ret += sizeof(*abuf);
+				abuf++;
 			}
 		}
 
@@ -309,9 +293,8 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement - run one conversion */
-	/* XXX really it'd be nice to lock against other readers here */
 	do {
-		_oldest_report = _next_report = 0;
+		_reports->flush();
 
 		/* trigger a measurement */
 		if (OK != measure()) {
@@ -329,8 +312,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
-		ret = sizeof(*_reports);
+		if (_reports->get(abuf)) {
+			ret = sizeof(*abuf);
+		}
 
 	} while (0);
 
@@ -342,7 +326,7 @@ Airspeed::start()
 {
 	/* reset the report ring and state machine */
 	_collect_phase = false;
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1);
@@ -385,6 +369,12 @@ Airspeed::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	warnx("poll interval:  %u ticks", _measure_ticks);
-	warnx("report queue:   %u (%u/%u @ %p)",
-	       _num_reports, _oldest_report, _next_report, _reports);
+	_reports->print_info("report queue");
+}
+
+void
+Airspeed::new_report(const differential_pressure_s &report)
+{
+	if (!_reports->force(&report))
+		perf_count(_buffer_overflows);
 }
diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
index b87494b40..048784813 100644
--- a/src/drivers/airspeed/airspeed.h
+++ b/src/drivers/airspeed/airspeed.h
@@ -68,6 +68,7 @@
 
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
@@ -102,6 +103,10 @@ public:
 	 */
 	virtual void	print_info();
 
+private:
+	RingBuffer		*_reports;
+	perf_counter_t		_buffer_overflows;
+
 protected:
 	virtual int	probe();
 
@@ -114,10 +119,7 @@ protected:
 	virtual int	collect() = 0;
 
 	work_s			_work;
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	differential_pressure_s	*_reports;
+	uint16_t		_max_differential_pressure_pa;
 	bool			_sensor_ok;
 	int			_measure_ticks;
 	bool			_collect_phase;
@@ -129,7 +131,6 @@ protected:
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
-	perf_counter_t		_buffer_overflows;
 
 
 	/**
@@ -162,8 +163,12 @@ protected:
 	*/
 	static void	cycle_trampoline(void *arg);
 
+	/**
+	* add a new report to the reports queue
+	*
+	* @param report		differential_pressure_s report
+	*/
+	void	new_report(const differential_pressure_s &report);
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
 
diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
index 079b5d21c..f0044d36f 100644
--- a/src/drivers/bma180/bma180.cpp
+++ b/src/drivers/bma180/bma180.cpp
@@ -63,6 +63,7 @@
 
 #include <drivers/device/spi.h>
 #include <drivers/drv_accel.h>
+#include <drivers/device/ringbuffer.h>
 
 
 /* oddly, ERROR is not defined for c++ */
@@ -146,10 +147,7 @@ private:
 	struct hrt_call		_call;
 	unsigned		_call_interval;
 
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	struct accel_report	*_reports;
+	RingBuffer		*_reports;
 
 	struct accel_scale	_accel_scale;
 	float			_accel_range_scale;
@@ -233,16 +231,9 @@ private:
 	int			set_lowpass(unsigned frequency);
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
-
-
 BMA180::BMA180(int bus, spi_dev_e device) :
 	SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
 	_call_interval(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
@@ -270,7 +261,7 @@ BMA180::~BMA180()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 
 	/* delete the perf counter */
 	perf_free(_sample_perf);
@@ -286,16 +277,15 @@ BMA180::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_oldest_report = _next_report = 0;
-	_reports = new struct accel_report[_num_reports];
+	_reports = new RingBuffer(2, sizeof(accel_report));
 
 	if (_reports == nullptr)
 		goto out;
 
 	/* advertise sensor topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]);
+	struct accel_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
 
 	/* perform soft reset (p48) */
 	write_reg(ADDR_RESET, SOFT_RESET);
@@ -352,6 +342,7 @@ ssize_t
 BMA180::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct accel_report);
+	struct accel_report *arp = reinterpret_cast<struct accel_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -367,10 +358,9 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with it.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(arp)) {
+				ret += sizeof(*arp);
+				arp++;
 			}
 		}
 
@@ -379,12 +369,12 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 	measure();
 
 	/* measurement will have generated a report, copy it out */
-	memcpy(buffer, _reports, sizeof(*_reports));
-	ret = sizeof(*_reports);
+	if (_reports->get(arp))
+		ret = sizeof(*arp);
 
 	return ret;
 }
@@ -449,31 +439,22 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* account for sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct accel_report *buf = new struct accel_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
-
-			return OK;
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 2) || (arg > 100))
+			return -EINVAL;
+		
+		irqstate_t flags = irqsave();
+		if (!_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
 		}
+		irqrestore(flags);
+		
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
 		/* XXX implement */
@@ -654,7 +635,7 @@ BMA180::start()
 	stop();
 
 	/* reset the report ring */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* start polling at the specified rate */
 	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this);
@@ -688,7 +669,7 @@ BMA180::measure()
 // 	} raw_report;
 // #pragma pack(pop)
 
-	accel_report		*report = &_reports[_next_report];
+	struct accel_report report;
 
 	/* start the performance counter */
 	perf_begin(_sample_perf);
@@ -708,45 +689,40 @@ BMA180::measure()
 	 * them before.  There is no good way to synchronise with the internal
 	 * measurement flow without using the external interrupt.
 	 */
-	_reports[_next_report].timestamp = hrt_absolute_time();
+	report.timestamp = hrt_absolute_time();
 	/*
 	 * y of board is x of sensor and x of board is -y of sensor
 	 * perform only the axis assignment here.
 	 * Two non-value bits are discarded directly
 	 */
-	report->y_raw  = read_reg(ADDR_ACC_X_LSB + 0);
-	report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
-	report->x_raw  = read_reg(ADDR_ACC_X_LSB + 2);
-	report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
-	report->z_raw  = read_reg(ADDR_ACC_X_LSB + 4);
-	report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
+	report.y_raw  = read_reg(ADDR_ACC_X_LSB + 0);
+	report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
+	report.x_raw  = read_reg(ADDR_ACC_X_LSB + 2);
+	report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
+	report.z_raw  = read_reg(ADDR_ACC_X_LSB + 4);
+	report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
 
 	/* discard two non-value bits in the 16 bit measurement */
-	report->x_raw = (report->x_raw / 4);
-	report->y_raw = (report->y_raw / 4);
-	report->z_raw = (report->z_raw / 4);
+	report.x_raw = (report.x_raw / 4);
+	report.y_raw = (report.y_raw / 4);
+	report.z_raw = (report.z_raw / 4);
 
 	/* invert y axis, due to 14 bit data no overflow can occur in the negation */
-	report->y_raw = -report->y_raw;
-
-	report->x = ((report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	report->y = ((report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	report->z = ((report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-	report->scaling = _accel_range_scale;
-	report->range_m_s2 = _accel_range_m_s2;
+	report.y_raw = -report.y_raw;
 
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
+	report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	report.scaling = _accel_range_scale;
+	report.range_m_s2 = _accel_range_m_s2;
 
-	/* if we are running up against the oldest report, fix it */
-	if (_next_report == _oldest_report)
-		INCREMENT(_oldest_report, _num_reports);
+	_reports->force(&report);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, report);
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
 
 	/* stop the perf counter */
 	perf_end(_sample_perf);
@@ -756,8 +732,7 @@ void
 BMA180::print_info()
 {
 	perf_print_counter(_sample_perf);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
+	_reports->print_info("report queue");
 }
 
 /**
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
index 135767b26..ae2a645f7 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -58,6 +58,7 @@
 #include <nuttx/sdio.h>
 #include <nuttx/mmcsd.h>
 #include <nuttx/analog/adc.h>
+#include <nuttx/gran.h>
 
 #include <stm32.h>
 #include "board_config.h"
@@ -69,6 +70,7 @@
 #include <drivers/drv_led.h>
 
 #include <systemlib/cpuload.h>
+#include <systemlib/perf_counter.h>
 
 /****************************************************************************
  * Pre-Processor Definitions
@@ -96,10 +98,70 @@
  * Protected Functions
  ****************************************************************************/
 
+#if defined(CONFIG_FAT_DMAMEMORY)
+# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
+#  error microSD DMA support requires CONFIG_GRAN
+# endif
+
+static GRAN_HANDLE dma_allocator;
+
+/* 
+ * The DMA heap size constrains the total number of things that can be 
+ * ready to do DMA at a time.
+ *
+ * For example, FAT DMA depends on one sector-sized buffer per filesystem plus
+ * one sector-sized buffer per file. 
+ *
+ * We use a fundamental alignment / granule size of 64B; this is sufficient
+ * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
+ */
+static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
+static perf_counter_t g_dma_perf;
+
+static void
+dma_alloc_init(void)
+{
+	dma_allocator = gran_initialize(g_dma_heap,
+					sizeof(g_dma_heap),
+					7,  /* 128B granule - must be > alignment (XXX bug?) */
+					6); /* 64B alignment */
+	if (dma_allocator == NULL) {
+		message("[boot] DMA allocator setup FAILED");
+	} else {
+		g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
+	}
+}
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
 
+/*
+ * DMA-aware allocator stubs for the FAT filesystem.
+ */
+
+__EXPORT void *fat_dma_alloc(size_t size);
+__EXPORT void fat_dma_free(FAR void *memory, size_t size);
+
+void *
+fat_dma_alloc(size_t size)
+{
+	perf_count(g_dma_perf);
+	return gran_alloc(dma_allocator, size);
+}
+
+void
+fat_dma_free(FAR void *memory, size_t size)
+{
+	gran_free(dma_allocator, memory, size);
+}
+
+#else
+
+# define dma_alloc_init()
+
+#endif
+
 /************************************************************************************
  * Name: stm32_boardinitialize
  *
@@ -110,7 +172,8 @@
  *
  ************************************************************************************/
 
-__EXPORT void stm32_boardinitialize(void)
+__EXPORT void
+stm32_boardinitialize(void)
 {
 	/* configure SPI interfaces */
 	stm32_spiinitialize();
@@ -170,6 +233,9 @@ __EXPORT int nsh_archinitialize(void)
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
 
+	/* configure the DMA allocator */
+	dma_alloc_init();
+
 	/* configure CPU load estimation */
 #ifdef CONFIG_SCHED_INSTRUMENTATION
 	cpuload_initialize_once();
diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h
index 818b64436..4d41d0d07 100644
--- a/src/drivers/boards/px4io-v2/board_config.h
+++ b/src/drivers/boards/px4io-v2/board_config.h
@@ -84,7 +84,7 @@
 
 /* Power switch controls ******************************************************/
 
-#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
 
 #define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)
 
diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c
index 0ea95bded..ccd01edf5 100644
--- a/src/drivers/boards/px4io-v2/px4iov2_init.c
+++ b/src/drivers/boards/px4io-v2/px4iov2_init.c
@@ -111,9 +111,7 @@ __EXPORT void stm32_boardinitialize(void)
 
 	stm32_configgpio(GPIO_BTN_SAFETY);
 
-	/* spektrum power enable is active high - disable it by default */
-	/* XXX might not want to do this on warm restart? */
-	stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false);
+	/* spektrum power enable is active high - enable it by default */
 	stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
 
 	stm32_configgpio(GPIO_SERVO_FAULT_DETECT);
diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h
index dc0c84052..a9e22eaa6 100644
--- a/src/drivers/device/ringbuffer.h
+++ b/src/drivers/device/ringbuffer.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2013 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
@@ -35,15 +34,14 @@
 /**
  * @file ringbuffer.h
  *
- * A simple ringbuffer template.
+ * A flexible ringbuffer class.
  */
 
 #pragma once
 
-template<typename T>
 class RingBuffer {
 public:
-	RingBuffer(unsigned size);
+	RingBuffer(unsigned ring_size, size_t entry_size);
 	virtual ~RingBuffer();
 
 	/**
@@ -52,15 +50,37 @@ public:
 	 * @param val		Item to put
 	 * @return		true if the item was put, false if the buffer is full
 	 */
-	bool			put(T &val);
+	bool			put(const void *val, size_t val_size = 0);
+
+	bool			put(int8_t val);
+	bool			put(uint8_t val);
+	bool			put(int16_t val);
+	bool			put(uint16_t val);
+	bool			put(int32_t val);
+	bool			put(uint32_t val);
+	bool			put(int64_t val);
+	bool			put(uint64_t val);
+	bool			put(float val);
+	bool			put(double val);
 
 	/**
-	 * Put an item into the buffer.
+	 * Force an item into the buffer, discarding an older item if there is not space.
 	 *
 	 * @param val		Item to put
-	 * @return		true if the item was put, false if the buffer is full
+	 * @return		true if an item was discarded to make space
 	 */
-	bool			put(const T &val);
+	bool			force(const void *val, size_t val_size = 0);
+
+	bool			force(int8_t val);
+	bool			force(uint8_t val);
+	bool			force(int16_t val);
+	bool			force(uint16_t val);
+	bool			force(int32_t val);
+	bool			force(uint32_t val);
+	bool			force(int64_t val);
+	bool			force(uint64_t val);
+	bool			force(float val);
+	bool			force(double val);
 
 	/**
 	 * Get an item from the buffer.
@@ -68,15 +88,18 @@ public:
 	 * @param val		Item that was gotten
 	 * @return		true if an item was got, false if the buffer was empty.
 	 */
-	bool			get(T &val);
+	bool			get(void *val, size_t val_size = 0);
 
-	/**
-	 * Get an item from the buffer (scalars only).
-	 *
-	 * @return		The value that was fetched, or zero if the buffer was
-	 *			empty.
-	 */
-	T			get(void);
+	bool			get(int8_t &val);
+	bool			get(uint8_t &val);
+	bool			get(int16_t &val);
+	bool			get(uint16_t &val);
+	bool			get(int32_t &val);
+	bool			get(uint32_t &val);
+	bool			get(int64_t &val);
+	bool			get(uint64_t &val);
+	bool			get(float &val);
+	bool			get(double &val);
 
 	/*
 	 * Get the number of slots free in the buffer.
@@ -97,54 +120,103 @@ public:
 	/*
 	 * Returns true if the buffer is empty.
 	 */
-	bool			empty() { return _tail == _head; }
+	bool			empty();
 
 	/*
 	 * Returns true if the buffer is full.
 	 */
-	bool			full() { return _next(_head) == _tail; }
+	bool			full();
 
 	/*
 	 * Returns the capacity of the buffer, or zero if the buffer could
 	 * not be allocated.
 	 */
-	unsigned		size() { return (_buf != nullptr) ? _size : 0; }
+	unsigned		size();
 
 	/*
 	 * Empties the buffer.
 	 */
-	void			flush() { _head = _tail = _size; }
+	void			flush();
+
+	/*
+	 * resize the buffer. This is unsafe to be called while
+	 * a producer or consuming is running. Caller is responsible
+	 * for any locking needed
+	 *
+	 * @param new_size	new size for buffer
+	 * @return		true if the resize succeeds, false if
+	 * 			not (allocation error)
+	 */
+	bool			resize(unsigned new_size);
+
+	/*
+	 * printf() some info on the buffer
+	 */
+	void			print_info(const char *name);
 
 private:
-	T			*const _buf;	
-	const unsigned		_size;
-	volatile unsigned	_head;	/**< insertion point */
-	volatile unsigned	_tail;	/**< removal point */
+	unsigned		_num_items;
+	const size_t		_item_size;
+	char			*_buf;	
+	volatile unsigned	_head;	/**< insertion point in _item_size units */
+	volatile unsigned	_tail;	/**< removal point in _item_size units */
 
 	unsigned		_next(unsigned index);
 };
 
-template <typename T>
-RingBuffer<T>::RingBuffer(unsigned with_size) :
-	_buf(new T[with_size + 1]),
-	_size(with_size),
- 	_head(with_size),
- 	_tail(with_size)
+RingBuffer::RingBuffer(unsigned num_items, size_t item_size) :
+	_num_items(num_items),
+	_item_size(item_size),
+	_buf(new char[(_num_items+1) * item_size]),
+ 	_head(_num_items),
+ 	_tail(_num_items)
 {}
 
-template <typename T>
-RingBuffer<T>::~RingBuffer()
+RingBuffer::~RingBuffer()
 {
 	if (_buf != nullptr)
 		delete[] _buf;
 }
 
-template <typename T>
-bool RingBuffer<T>::put(T &val) 
+unsigned
+RingBuffer::_next(unsigned index)
+{
+	return (0 == index) ? _num_items : (index - 1); 
+}
+
+bool
+RingBuffer::empty()
+{
+	return _tail == _head; 
+}
+
+bool
+RingBuffer::full()
+{
+	return _next(_head) == _tail; 
+}
+
+unsigned
+RingBuffer::size()
+{
+	return (_buf != nullptr) ? _num_items : 0; 
+}
+
+void
+RingBuffer::flush()
+{
+	while (!empty())
+		get(NULL);
+}
+
+bool
+RingBuffer::put(const void *val, size_t val_size) 
 {
 	unsigned next = _next(_head);
 	if (next != _tail) {
-		_buf[_head] = val;
+		if ((val_size == 0) || (val_size > _item_size))
+			val_size = _item_size;
+		memcpy(&_buf[_head * _item_size], val, val_size);
 		_head = next;
 		return true;
 	} else {
@@ -152,52 +224,286 @@ bool RingBuffer<T>::put(T &val)
 	}
 }
 
-template <typename T>
-bool RingBuffer<T>::put(const T &val) 
+bool
+RingBuffer::put(int8_t val)
 {
-	unsigned next = _next(_head);
-	if (next != _tail) {
-		_buf[_head] = val;
-		_head = next;
-		return true;
-	} else {
-		return false;
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint8_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(int16_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint16_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(int32_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint32_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(int64_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint64_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(float val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(double val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(const void *val, size_t val_size)
+{
+	bool overwrote = false;
+
+	for (;;) {
+		if (put(val, val_size))
+			break;
+		get(NULL);
+		overwrote = true;
 	}
+	return overwrote;
+}
+
+bool
+RingBuffer::force(int8_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint8_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(int16_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint16_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(int32_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint32_t val)
+{
+	return force(&val, sizeof(val));
 }
 
-template <typename T>
-bool RingBuffer<T>::get(T &val) 
+bool
+RingBuffer::force(int64_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint64_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(float val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(double val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(void *val, size_t val_size) 
 {
 	if (_tail != _head) {
-		val = _buf[_tail];
-		_tail = _next(_tail);
+		unsigned candidate;
+		unsigned next;
+
+		if ((val_size == 0) || (val_size > _item_size))
+			val_size = _item_size;
+
+		do {
+			/* decide which element we think we're going to read */
+			candidate = _tail;
+
+			/* and what the corresponding next index will be */
+			next = _next(candidate);
+
+			/* go ahead and read from this index */
+			if (val != NULL)
+				memcpy(val, &_buf[candidate * _item_size], val_size);
+
+			/* if the tail pointer didn't change, we got our item */
+		} while (!__sync_bool_compare_and_swap(&_tail, candidate, next));
+
 		return true;
 	} else {
 		return false;
 	}
 }
 
-template <typename T>
-T RingBuffer<T>::get(void) 
+bool
+RingBuffer::get(int8_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint8_t &val)
 {
-	T val;
-	return get(val) ? val : 0;
+	return get(&val, sizeof(val));
 }
 
-template <typename T>
-unsigned RingBuffer<T>::space(void) 
+bool
+RingBuffer::get(int16_t &val)
 {
-	return (_tail >= _head) ? (_size - (_tail - _head)) : (_head - _tail - 1);
+	return get(&val, sizeof(val));
 }
 
-template <typename T>
-unsigned RingBuffer<T>::count(void) 
+bool
+RingBuffer::get(uint16_t &val)
 {
-	return _size - space();
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(int32_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint32_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(int64_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint64_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(float &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(double &val)
+{
+	return get(&val, sizeof(val));
+}
+
+unsigned
+RingBuffer::space(void) 
+{
+	unsigned tail, head;
+
+	/*
+	 * Make a copy of the head/tail pointers in a fashion that
+	 * may err on the side of under-estimating the free space
+	 * in the buffer in the case that the buffer is being updated
+	 * asynchronously with our check.
+	 * If the head pointer changes (reducing space) while copying,
+	 * re-try the copy.
+	 */
+	do {
+		head = _head;
+		tail = _tail;
+	} while (head != _head);
+
+	return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1);
+}
+
+unsigned
+RingBuffer::count(void) 
+{
+	/*
+	 * Note that due to the conservative nature of space(), this may
+	 * over-estimate the number of items in the buffer.
+	 */
+	return _num_items - space();
+}
+
+bool
+RingBuffer::resize(unsigned new_size) 
+{
+	char *old_buffer;
+	char *new_buffer = new char [(new_size+1) * _item_size];
+	if (new_buffer == nullptr) {
+		return false;
+	}
+	old_buffer = _buf;
+	_buf = new_buffer;
+	_num_items = new_size;
+	_head = new_size;
+	_tail = new_size;
+	delete[] old_buffer;
+	return true;
 }
 
-template <typename T>
-unsigned RingBuffer<T>::_next(unsigned index) 
+void
+RingBuffer::print_info(const char *name) 
 {
-	return (0 == index) ? _size : (index - 1);
+	printf("%s	%u/%u (%u/%u @ %p)\n",
+	       name, 
+	       _num_items, 
+	       _num_items * _item_size, 
+	       _head, 
+	       _tail, 
+	       _buf);
 }
diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp
index 8fffd60cb..fa6b78d64 100644
--- a/src/drivers/device/spi.cpp
+++ b/src/drivers/device/spi.cpp
@@ -67,6 +67,7 @@ SPI::SPI(const char *name,
 	CDev(name, devname, irq),
 	// public
 	// protected
+	locking_mode(LOCK_PREEMPTION),
 	// private
 	_bus(bus),
 	_device(device),
@@ -132,13 +133,25 @@ SPI::probe()
 int
 SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 {
+	irqstate_t	state;
 
 	if ((send == nullptr) && (recv == nullptr))
 		return -EINVAL;
 
-	/* do common setup */
-	if (!up_interrupt_context())
-		SPI_LOCK(_dev, true);
+	/* lock the bus as required */
+	if (!up_interrupt_context()) {
+		switch (locking_mode) {
+		default:
+		case LOCK_PREEMPTION:
+			state = irqsave();
+			break;
+		case LOCK_THREADS:
+			SPI_LOCK(_dev, true);
+			break;
+		case LOCK_NONE:
+			break;
+		}
+	}
 
 	SPI_SETFREQUENCY(_dev, _frequency);
 	SPI_SETMODE(_dev, _mode);
@@ -151,8 +164,19 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 	/* and clean up */
 	SPI_SELECT(_dev, _device, false);
 
-	if (!up_interrupt_context())
-		SPI_LOCK(_dev, false);
+	if (!up_interrupt_context()) {
+		switch (locking_mode) {
+		default:
+		case LOCK_PREEMPTION:
+			irqrestore(state);
+			break;
+		case LOCK_THREADS:
+			SPI_LOCK(_dev, false);
+			break;
+		case LOCK_NONE:
+			break;
+		}
+	}
 
 	return OK;
 }
diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h
index e0122372a..9103dca2e 100644
--- a/src/drivers/device/spi.h
+++ b/src/drivers/device/spi.h
@@ -101,6 +101,17 @@ protected:
 	 */
 	int		transfer(uint8_t *send, uint8_t *recv, unsigned len);
 
+	/**
+	 * Locking modes supported by the driver.
+	 */
+	enum LockMode {
+		LOCK_PREEMPTION,	/**< the default; lock against all forms of preemption. */
+		LOCK_THREADS,		/**< lock only against other threads, using SPI_LOCK */
+		LOCK_NONE		/**< perform no locking, only safe if the bus is entirely private */
+	};
+
+	LockMode	locking_mode;	/**< selected locking mode */
+
 private:
 	int			_bus;
 	enum spi_dev_e		_device;
diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h
index 1dda8ef0b..398cf4870 100644
--- a/src/drivers/drv_gps.h
+++ b/src/drivers/drv_gps.h
@@ -51,8 +51,7 @@
 typedef enum {
 	GPS_DRIVER_MODE_NONE = 0,
 	GPS_DRIVER_MODE_UBX,
-	GPS_DRIVER_MODE_MTK,
-	GPS_DRIVER_MODE_NMEA,
+	GPS_DRIVER_MODE_MTK
 } gps_driver_mode_t;
 
 
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 6ed9320cb..fc916b522 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -119,6 +119,9 @@ ORB_DECLARE(output_pwm);
 /** start DSM bind */
 #define DSM_BIND_START	_IOC(_PWM_SERVO_BASE, 7)
 
+#define DSM2_BIND_PULSES 3	/* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
+#define DSMX_BIND_PULSES 7	/* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
+
 /** power up DSM receiver */
 #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8)
 
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index f0b860620..2fab37dd2 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -60,6 +60,8 @@
 
 #include <sys/ioctl.h>
 
+#define TONEALARM_DEVICE_PATH "/dev/tone_alarm"
+
 #define _TONE_ALARM_BASE	0x7400
 #define TONE_SET_ALARM		_IOC(_TONE_ALARM_BASE, 1)
 
@@ -142,6 +144,7 @@ enum {
 	TONE_ARMING_WARNING_TUNE,
 	TONE_BATTERY_WARNING_SLOW_TUNE,
 	TONE_BATTERY_WARNING_FAST_TUNE,
+	TONE_GPS_WARNING_TUNE,
 	TONE_NUMBER_OF_TUNES
 };
 
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index 257b41935..dd8436b10 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -68,6 +68,7 @@
 
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
@@ -173,27 +174,22 @@ ETSAirspeed::collect()
 		diff_pres_pa -= _diff_pres_offset;
 	}
 
-	// XXX we may want to smooth out the readings to remove noise.
-
-	_reports[_next_report].timestamp = hrt_absolute_time();
-	_reports[_next_report].differential_pressure_pa = diff_pres_pa;
-
 	// Track maximum differential pressure measured (so we can work out top speed).
-	if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
-		_reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
+	if (diff_pres_pa > _max_differential_pressure_pa) {
+	    _max_differential_pressure_pa = diff_pres_pa;
 	}
 
-	/* announce the airspeed if needed, just publish else */
-	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
+	// XXX we may want to smooth out the readings to remove noise.
+	differential_pressure_s report;
+	report.timestamp = hrt_absolute_time();
+	report.differential_pressure_pa = diff_pres_pa;
+	report.voltage = 0;
+	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
+	/* announce the airspeed if needed, just publish else */
+	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
 
-	/* if we are running up against the oldest report, toss it */
-	if (_next_report == _oldest_report) {
-		perf_count(_buffer_overflows);
-		INCREMENT(_oldest_report, _num_reports);
-	}
+	new_report(report);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 38835418b..fc500a9ec 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -85,7 +85,7 @@ static const int ERROR = -1;
 class GPS : public device::CDev
 {
 public:
-	GPS(const char* uart_path);
+	GPS(const char *uart_path);
 	virtual ~GPS();
 
 	virtual int			init();
@@ -156,7 +156,7 @@ GPS	*g_dev;
 }
 
 
-GPS::GPS(const char* uart_path) :
+GPS::GPS(const char *uart_path) :
 	CDev("gps", GPS_DEVICE_PATH),
 	_task_should_exit(false),
 	_healthy(false),
@@ -192,6 +192,7 @@ GPS::~GPS()
 	/* well, kill it anyway, though this will probably crash */
 	if (_task != -1)
 		task_delete(_task);
+
 	g_dev = nullptr;
 
 }
@@ -270,19 +271,20 @@ GPS::task_main()
 		}
 
 		switch (_mode) {
-			case GPS_DRIVER_MODE_UBX:
-				_Helper = new UBX(_serial_fd, &_report);
-				break;
-			case GPS_DRIVER_MODE_MTK:
-				_Helper = new MTK(_serial_fd, &_report);
-				break;
-			case GPS_DRIVER_MODE_NMEA:
-				//_Helper = new NMEA(); //TODO: add NMEA
-				break;
-			default:
-				break;
+		case GPS_DRIVER_MODE_UBX:
+			_Helper = new UBX(_serial_fd, &_report);
+			break;
+
+		case GPS_DRIVER_MODE_MTK:
+			_Helper = new MTK(_serial_fd, &_report);
+			break;
+
+		default:
+			break;
 		}
+
 		unlock();
+
 		if (_Helper->configure(_baudrate) == 0) {
 			unlock();
 
@@ -294,6 +296,7 @@ GPS::task_main()
 				/* opportunistic publishing - else invalid data would end up on the bus */
 				if (_report_pub > 0) {
 					orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+
 				} else {
 					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
 				}
@@ -310,10 +313,26 @@ GPS::task_main()
 				}
 
 				if (!_healthy) {
-					warnx("module found");
+					char *mode_str = "unknown";
+
+					switch (_mode) {
+					case GPS_DRIVER_MODE_UBX:
+						mode_str = "UBX";
+						break;
+
+					case GPS_DRIVER_MODE_MTK:
+						mode_str = "MTK";
+						break;
+
+					default:
+						break;
+					}
+
+					warnx("module found: %s", mode_str);
 					_healthy = true;
 				}
 			}
+
 			if (_healthy) {
 				warnx("module lost");
 				_healthy = false;
@@ -322,25 +341,26 @@ GPS::task_main()
 
 			lock();
 		}
+
 		lock();
 
 		/* select next mode */
 		switch (_mode) {
-			case GPS_DRIVER_MODE_UBX:
-				_mode = GPS_DRIVER_MODE_MTK;
-				break;
-			case GPS_DRIVER_MODE_MTK:
-				_mode = GPS_DRIVER_MODE_UBX;
-				break;
-		//				case GPS_DRIVER_MODE_NMEA:
-		//					_mode = GPS_DRIVER_MODE_UBX;
-		//					break;
-			default:
-				break;
+		case GPS_DRIVER_MODE_UBX:
+			_mode = GPS_DRIVER_MODE_MTK;
+			break;
+
+		case GPS_DRIVER_MODE_MTK:
+			_mode = GPS_DRIVER_MODE_UBX;
+			break;
+
+		default:
+			break;
 		}
 
 	}
-	debug("exiting");
+
+	warnx("exiting");
 
 	::close(_serial_fd);
 
@@ -361,23 +381,25 @@ void
 GPS::print_info()
 {
 	switch (_mode) {
-		case GPS_DRIVER_MODE_UBX:
-			warnx("protocol: UBX");
-			break;
-		case GPS_DRIVER_MODE_MTK:
-			warnx("protocol: MTK");
-			break;
-		case GPS_DRIVER_MODE_NMEA:
-			warnx("protocol: NMEA");
-			break;
-		default:
-			break;
+	case GPS_DRIVER_MODE_UBX:
+		warnx("protocol: UBX");
+		break;
+
+	case GPS_DRIVER_MODE_MTK:
+		warnx("protocol: MTK");
+		break;
+
+	default:
+		break;
 	}
+
 	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
+
 	if (_report.timestamp_position != 0) {
-		warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
-			(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
+		warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type,
+				_report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
 		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
+		warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m);
 		warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
 		warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
 		warnx("rate publication:\t%6.2f Hz", (double)_rate);
@@ -428,6 +450,7 @@ start(const char *uart_path)
 		errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
 		goto fail;
 	}
+
 	exit(0);
 
 fail:
@@ -503,7 +526,7 @@ gps_main(int argc, char *argv[])
 {
 
 	/* set to default */
-	char* device_name = GPS_DEFAULT_UART_PORT;
+	char *device_name = GPS_DEFAULT_UART_PORT;
 
 	/*
 	 * Start/load the driver.
@@ -513,15 +536,18 @@ gps_main(int argc, char *argv[])
 		if (argc > 3) {
 			if (!strcmp(argv[2], "-d")) {
 				device_name = argv[3];
+
 			} else {
 				goto out;
 			}
 		}
+
 		gps::start(device_name);
 	}
 
 	if (!strcmp(argv[1], "stop"))
 		gps::stop();
+
 	/*
 	 * Test the driver/device.
 	 */
diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp
index ba86d370a..2e2cbc8dd 100644
--- a/src/drivers/gps/gps_helper.cpp
+++ b/src/drivers/gps/gps_helper.cpp
@@ -87,13 +87,15 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
 
 	case 115200: speed = B115200; break;
 
-	warnx("try baudrate: %d\n", speed);
+		warnx("try baudrate: %d\n", speed);
 
 	default:
 		warnx("ERROR: Unsupported baudrate: %d\n", baud);
 		return -EINVAL;
 	}
+
 	struct termios uart_config;
+
 	int termios_state;
 
 	/* fill the struct for the new configuration */
@@ -109,14 +111,17 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
 		warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
 		return -1;
 	}
+
 	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
 		warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
 		return -1;
 	}
+
 	if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
 		warnx("ERROR setting baudrate (tcsetattr)\n");
 		return -1;
 	}
+
 	/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
 	return 0;
 }
diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
index defc1a074..73d4b889c 100644
--- a/src/drivers/gps/gps_helper.h
+++ b/src/drivers/gps/gps_helper.h
@@ -33,7 +33,7 @@
  *
  ****************************************************************************/
 
-/** 
+/**
  * @file gps_helper.h
  */
 
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index 62941d74b..56b702ea6 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -48,9 +48,9 @@
 
 
 MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
-_fd(fd),
-_gps_position(gps_position),
-_mtk_revision(0)
+	_fd(fd),
+	_gps_position(gps_position),
+	_mtk_revision(0)
 {
 	decode_init();
 }
@@ -73,24 +73,28 @@ MTK::configure(unsigned &baudrate)
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
@@ -128,12 +132,15 @@ MTK::receive(unsigned timeout)
 					handle_message(packet);
 					return 1;
 				}
+
 				/* in case we keep trying but only get crap from GPS */
-				if (time_started + timeout*1000 < hrt_absolute_time() ) {
+				if (time_started + timeout * 1000 < hrt_absolute_time()) {
 					return -1;
 				}
+
 				j++;
 			}
+
 			/* everything is read */
 			j = count = 0;
 		}
@@ -181,6 +188,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 		if (b == MTK_SYNC1_V16) {
 			_decode_state = MTK_DECODE_GOT_CK_A;
 			_mtk_revision = 16;
+
 		} else if (b == MTK_SYNC1_V19) {
 			_decode_state = MTK_DECODE_GOT_CK_A;
 			_mtk_revision = 19;
@@ -201,7 +209,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 			add_byte_to_checksum(b);
 
 		// Fill packet buffer
-		((uint8_t*)(&packet))[_rx_count] = b;
+		((uint8_t *)(&packet))[_rx_count] = b;
 		_rx_count++;
 
 		/* Packet size minus checksum, XXX ? */
@@ -209,14 +217,17 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 			/* Compare checksum */
 			if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
 				ret = 1;
+
 			} else {
 				warnx("MTK Checksum invalid");
 				ret = -1;
 			}
+
 			// Reset state machine to decode next packet
 			decode_init();
 		}
 	}
+
 	return ret;
 }
 
@@ -226,19 +237,22 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 	if (_mtk_revision == 16) {
 		_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
 		_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
+
 	} else if (_mtk_revision == 19) {
 		_gps_position->lat = packet.latitude; // both degrees*1e7
 		_gps_position->lon = packet.longitude; // both degrees*1e7
+
 	} else {
 		warnx("mtk: unknown revision");
 		_gps_position->lat = 0;
 		_gps_position->lon = 0;
 	}
+
 	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
 	_gps_position->fix_type = packet.fix_type;
 	_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
 	_gps_position->epv_m = 0.0; //unknown in mtk custom mode
-	_gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
+	_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
 	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
 	_gps_position->satellites_visible = packet.satellites;
 
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index ba5d14cc4..86291901c 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -60,13 +60,14 @@
 #define UBX_CONFIG_TIMEOUT		200		// ms, timeout for waiting ACK
 #define UBX_PACKET_TIMEOUT		2		// ms, if now data during this delay assume that full update received
 #define UBX_WAIT_BEFORE_READ	20		// ms, wait before reading to save read() calls
+#define DISABLE_MSG_INTERVAL	1000000	// us, try to disable message with this interval
 
 UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
 	_fd(fd),
 	_gps_position(gps_position),
 	_configured(false),
 	_waiting_for_ack(false),
-	_disable_cmd_counter(0)
+	_disable_cmd_last(0)
 {
 	decode_init();
 }
@@ -191,35 +192,35 @@ UBX::configure(unsigned &baudrate)
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV POSLLH\n");
+		warnx("ubx: msg rate configuration failed: NAV POSLLH");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV TIMEUTC\n");
+		warnx("ubx: msg rate configuration failed: NAV TIMEUTC");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV SOL\n");
+		warnx("ubx: msg rate configuration failed: NAV SOL");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV VELNED\n");
+		warnx("ubx: msg rate configuration failed: NAV VELNED");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV SVINFO\n");
+		warnx("ubx: msg rate configuration failed: NAV SVINFO");
 		return 1;
 	}
 
@@ -271,11 +272,17 @@ UBX::receive(unsigned timeout)
 
 		if (ret < 0) {
 			/* something went wrong when polling */
+			warnx("ubx: poll error");
 			return -1;
 
 		} else if (ret == 0) {
 			/* return success after short delay after receiving a packet or timeout after long delay */
-			return handled ? 1 : -1;
+			if (handled) {
+				return 1;
+
+			} else {
+				return -1;
+			}
 
 		} else if (ret > 0) {
 			/* if we have new data from GPS, go handle it */
@@ -292,8 +299,6 @@ UBX::receive(unsigned timeout)
 				/* pass received bytes to the packet decoder */
 				for (int i = 0; i < count; i++) {
 					if (parse_char(buf[i]) > 0) {
-						/* return to configure during configuration or to the gps driver during normal work
-						 * if a packet has arrived */
 						if (handle_message() > 0)
 							handled = true;
 					}
@@ -303,6 +308,7 @@ UBX::receive(unsigned timeout)
 
 		/* abort after timeout if no useful packets received */
 		if (time_started + timeout * 1000 < hrt_absolute_time()) {
+			warnx("ubx: timeout - no useful messages");
 			return -1;
 		}
 	}
@@ -453,16 +459,16 @@ UBX::handle_message()
 					timeinfo.tm_min = packet->min;
 					timeinfo.tm_sec = packet->sec;
 					time_t epoch = mktime(&timeinfo);
-                
+
 #ifndef CONFIG_RTC
-                    //Since we lack a hardware RTC, set the system time clock based on GPS UTC
-                    //TODO generalize this by moving into gps.cpp? 
-                    timespec ts;
-                    ts.tv_sec = epoch;
-                    ts.tv_nsec = packet->time_nanoseconds;
-                    clock_settime(CLOCK_REALTIME,&ts);
+					//Since we lack a hardware RTC, set the system time clock based on GPS UTC
+					//TODO generalize this by moving into gps.cpp?
+					timespec ts;
+					ts.tv_sec = epoch;
+					ts.tv_nsec = packet->time_nanoseconds;
+					clock_settime(CLOCK_REALTIME, &ts);
 #endif
-                
+
 					_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
 					_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
 					_gps_position->timestamp_time = hrt_absolute_time();
@@ -564,10 +570,13 @@ UBX::handle_message()
 
 		if (ret == 0) {
 			/* message not handled */
-			warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id);
+			warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
+
+			hrt_abstime t = hrt_absolute_time();
 
-			if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) {
+			if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
 				/* don't attempt for every message to disable, some might not be disabled */
+				_disable_cmd_last = t;
 				warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
 				configure_message_rate(_message_class, _message_id, 0);
 			}
@@ -640,7 +649,7 @@ UBX::add_checksum_to_message(uint8_t *message, const unsigned length)
 		ck_b = ck_b + ck_a;
 	}
 
-	/* The checksum is written to the last to bytes of a message */
+	/* the checksum is written to the last to bytes of a message */
 	message[length - 2] = ck_a;
 	message[length - 1] = ck_b;
 }
@@ -669,17 +678,17 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
 {
 	ssize_t ret = 0;
 
-	/* Calculate the checksum now */
+	/* calculate the checksum now */
 	add_checksum_to_message(packet, length);
 
 	const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
 
-	/* Start with the two sync bytes */
+	/* start with the two sync bytes */
 	ret += write(fd, sync_bytes, sizeof(sync_bytes));
 	ret += write(fd, packet, length);
 
 	if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
-		warnx("ubx: config write fail");
+		warnx("ubx: configuration write fail");
 }
 
 void
@@ -696,7 +705,7 @@ UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
 	add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b);
 	add_checksum((uint8_t *)msg, size, ck_a, ck_b);
 
-	// Configure receive check
+	/* configure ACK check */
 	_message_class_needed = msg_class;
 	_message_id_needed = msg_id;
 
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 4fc276975..76ef873a3 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -347,7 +347,7 @@ private:
 	/**
 	 * Add the two checksum bytes to an outgoing message
 	 */
-	void			add_checksum_to_message(uint8_t* message, const unsigned length);
+	void			add_checksum_to_message(uint8_t *message, const unsigned length);
 
 	/**
 	 * Helper to send a config packet
@@ -358,7 +358,7 @@ private:
 
 	void			send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
 
-	void			add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
+	void			add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
 
 	int			wait_for_ack(unsigned timeout);
 
@@ -376,7 +376,7 @@ private:
 	uint8_t			_message_class;
 	uint8_t			_message_id;
 	unsigned		_payload_size;
-	uint8_t			_disable_cmd_counter;
+	uint8_t			_disable_cmd_last;
 };
 
 #endif /* UBX_H_ */
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 3ede90a17..5e891d7bb 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -65,6 +65,7 @@
 
 #include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/subsystem_info.h>
@@ -148,10 +149,7 @@ private:
 	work_s			_work;
 	unsigned		_measure_ticks;
 
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	mag_report		*_reports;
+	RingBuffer		*_reports;
 	mag_scale		_scale;
 	float 			_range_scale;
 	float 			_range_ga;
@@ -310,9 +308,6 @@ private:
 
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
-
 /*
  * Driver 'main' command.
  */
@@ -322,9 +317,6 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
 HMC5883::HMC5883(int bus) :
 	I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
 	_measure_ticks(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_range_scale(0), /* default range scale from counts to gauss */
 	_range_ga(1.3f),
@@ -356,9 +348,8 @@ HMC5883::~HMC5883()
 	/* make sure we are truly inactive */
 	stop();
 
-	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 
 	// free perf counters
 	perf_free(_sample_perf);
@@ -375,21 +366,18 @@ HMC5883::init()
 	if (I2C::init() != OK)
 		goto out;
 
-	/* reset the device configuration */
-	reset();
-
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_reports = new struct mag_report[_num_reports];
-
+	_reports = new RingBuffer(2, sizeof(mag_report));
 	if (_reports == nullptr)
 		goto out;
 
-	_oldest_report = _next_report = 0;
+	/* reset the device configuration */
+	reset();
 
 	/* get a publish handle on the mag topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]);
+	struct mag_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
 
 	if (_mag_topic < 0)
 		debug("failed to create sensor_mag object");
@@ -493,6 +481,7 @@ ssize_t
 HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct mag_report);
+	struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -501,17 +490,15 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 
 	/* if automatic measurement is enabled */
 	if (_measure_ticks > 0) {
-
 		/*
 		 * While there is space in the caller's buffer, and reports, copy them.
 		 * Note that we may be pre-empted by the workq thread while we are doing this;
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(mag_buf)) {
+				ret += sizeof(struct mag_report);
+				mag_buf++;
 			}
 		}
 
@@ -522,7 +509,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 	/* manual measurement - run one conversion */
 	/* XXX really it'd be nice to lock against other readers here */
 	do {
-		_oldest_report = _next_report = 0;
+		_reports->flush();
 
 		/* trigger a measurement */
 		if (OK != measure()) {
@@ -539,10 +526,9 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 			break;
 		}
 
-		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
-		ret = sizeof(*_reports);
-
+		if (_reports->get(mag_buf)) {
+			ret = sizeof(struct mag_report);
+		}
 	} while (0);
 
 	return ret;
@@ -615,31 +601,22 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000/TICK2USEC(_measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* add one to account for the sentinel in the ring */
-			arg++;
-
 			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
+			if ((arg < 1) || (arg > 100))
 				return -EINVAL;
 
-			/* allocate new buffer */
-			struct mag_report *buf = new struct mag_report[arg];
-
-			if (nullptr == buf)
+			irqstate_t flags = irqsave();
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
 				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
+			}
+			irqrestore(flags);
 
 			return OK;
 		}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
 		return reset();
@@ -701,7 +678,7 @@ HMC5883::start()
 {
 	/* reset the report ring and state machine */
 	_collect_phase = false;
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1);
@@ -810,9 +787,10 @@ HMC5883::collect()
 
 
 	perf_begin(_sample_perf);
+	struct mag_report new_report;
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
-	_reports[_next_report].timestamp = hrt_absolute_time();
+	new_report.timestamp = hrt_absolute_time();
 
 	/*
 	 * @note  We could read the status register here, which could tell us that
@@ -842,8 +820,10 @@ HMC5883::collect()
 	 */
 	if ((abs(report.x) > 2048) ||
 	    (abs(report.y) > 2048) ||
-	    (abs(report.z) > 2048))
+	    (abs(report.z) > 2048)) {
+		perf_count(_comms_errors);
 		goto out;
+	}
 
 	/*
 	 * RAW outputs
@@ -851,10 +831,10 @@ HMC5883::collect()
 	 * to align the sensor axes with the board, x and y need to be flipped
 	 * and y needs to be negated
 	 */
-	_reports[_next_report].x_raw = report.y;
-	_reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x);
+	new_report.x_raw = report.y;
+	new_report.y_raw = -report.x;
 	/* z remains z */
-	_reports[_next_report].z_raw = report.z;
+	new_report.z_raw = report.z;
 
 	/* scale values for output */
 
@@ -876,34 +856,30 @@ HMC5883::collect()
 #ifdef PX4_I2C_BUS_ONBOARD
 	if (_bus == PX4_I2C_BUS_ONBOARD) {
 		/* to align the sensor axes with the board, x and y need to be flipped */
-		_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+		new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
 		/* flip axes and negate value for y */
-		_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
+		new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
 		/* z remains z */
-		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
+		new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 	} else {
 #endif
 		/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
 		 * therefore switch x and y and invert y */
-		_reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale;
+		new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
 		/* flip axes and negate value for y */
-		_reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
+		new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
 		/* z remains z */
-		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
+		new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 #ifdef PX4_I2C_BUS_ONBOARD
 	}
 #endif
 
 	/* publish it */
-	orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]);
+	orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
 
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
-
-	/* if we are running up against the oldest report, toss it */
-	if (_next_report == _oldest_report) {
+	/* post a report to the ring */
+	if (_reports->force(&new_report)) {
 		perf_count(_buffer_overflows);
-		INCREMENT(_oldest_report, _num_reports);
 	}
 
 	/* notify anyone waiting for data */
@@ -1222,8 +1198,7 @@ HMC5883::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
+	_reports->print_info("report queue");
 }
 
 /**
@@ -1332,7 +1307,7 @@ test()
 		errx(1, "failed to get if mag is onboard or external");
 	warnx("device active: %s", ret ? "external" : "onboard");
 
-	/* set the queue depth to 10 */
+	/* set the queue depth to 5 */
 	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
 		errx(1, "failed to set queue depth");
 
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index e6d765e13..748809d3f 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -61,6 +61,7 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/device/spi.h>
 #include <drivers/drv_gyro.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
@@ -183,11 +184,8 @@ private:
 
 	struct hrt_call		_call;
 	unsigned		_call_interval;
-
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	struct gyro_report	*_reports;
+	
+	RingBuffer		*_reports;
 
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
@@ -299,16 +297,9 @@ private:
 	 int 			self_test();
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
-
-
 L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
 	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000),
 	_call_interval(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
@@ -340,7 +331,7 @@ L3GD20::~L3GD20()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 
 	/* delete the perf counter */
 	perf_free(_sample_perf);
@@ -356,16 +347,15 @@ L3GD20::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_oldest_report = _next_report = 0;
-	_reports = new struct gyro_report[_num_reports];
+	_reports = new RingBuffer(2, sizeof(gyro_report));
 
 	if (_reports == nullptr)
 		goto out;
 
 	/* advertise sensor topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]);
+	struct gyro_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
 
 	reset();
 
@@ -380,6 +370,8 @@ L3GD20::probe()
 	/* read dummy value to void to clear SPI statemachine on sensor */
 	(void)read_reg(ADDR_WHO_AM_I);
 
+	bool success = false;
+
 	/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
 	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
 
@@ -390,15 +382,19 @@ L3GD20::probe()
 		#else
 			#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
 		#endif
-		return OK;
+
+		success = true;
 	}
 
 
 	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
 		_orientation = SENSOR_BOARD_ROTATION_180_DEG;
-		return OK;
+		success = true;
 	}
 
+	if (success)
+		return OK;
+
 	return -EIO;
 }
 
@@ -406,6 +402,7 @@ ssize_t
 L3GD20::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct gyro_report);
+	struct gyro_report *gbuf = reinterpret_cast<struct gyro_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -421,10 +418,9 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with it.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(gbuf)) {
+				ret += sizeof(*gbuf);
+				gbuf++;
 			}
 		}
 
@@ -433,12 +429,13 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 	measure();
 
 	/* measurement will have generated a report, copy it out */
-	memcpy(buffer, _reports, sizeof(*_reports));
-	ret = sizeof(*_reports);
+	if (_reports->get(gbuf)) {
+		ret = sizeof(*gbuf);
+	}
 
 	return ret;
 }
@@ -506,31 +503,22 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* account for sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct gyro_report *buf = new struct gyro_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
-
-			return OK;
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+
+		irqstate_t flags = irqsave();
+		if (!_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
 		}
+		irqrestore(flags);
+		
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
 		reset();
@@ -699,7 +687,7 @@ L3GD20::start()
 	stop();
 
 	/* reset the report ring */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* start polling at the specified rate */
 	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this);
@@ -759,7 +747,7 @@ L3GD20::measure()
 	} raw_report;
 #pragma pack(pop)
 
-	gyro_report		*report = &_reports[_next_report];
+	gyro_report report;
 
 	/* start the performance counter */
 	perf_begin(_sample_perf);
@@ -782,61 +770,56 @@ L3GD20::measure()
 	 *	 	  the offset is 74 from the origin and subtracting
 	 *		  74 from all measurements centers them around zero.
 	 */
-	report->timestamp = hrt_absolute_time();
+	report.timestamp = hrt_absolute_time();
 	
 	switch (_orientation) {
 
 		case SENSOR_BOARD_ROTATION_000_DEG:
 			/* keep axes in place */
-			report->x_raw = raw_report.x;
-			report->y_raw = raw_report.y;
+			report.x_raw = raw_report.x;
+			report.y_raw = raw_report.y;
 			break;
 
 		case SENSOR_BOARD_ROTATION_090_DEG:
 			/* swap x and y */
-			report->x_raw = raw_report.y;
-			report->y_raw = raw_report.x;
+			report.x_raw = raw_report.y;
+			report.y_raw = raw_report.x;
 			break;
 
 		case SENSOR_BOARD_ROTATION_180_DEG:
 			/* swap x and y and negate both */
-			report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
-			report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
+			report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+			report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
 			break;
 
 		case SENSOR_BOARD_ROTATION_270_DEG:
 			/* swap x and y and negate y */
-			report->x_raw = raw_report.y;
-			report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+			report.x_raw = raw_report.y;
+			report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
 			break;
 	}
 
-	report->z_raw = raw_report.z;
-
-	report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
-	report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
-	report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
+	report.z_raw = raw_report.z;
 
-	report->x = _gyro_filter_x.apply(report->x);
-	report->y = _gyro_filter_y.apply(report->y);
-	report->z = _gyro_filter_z.apply(report->z);
+	report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
+	report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
+	report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
 
-	report->scaling = _gyro_range_scale;
-	report->range_rad_s = _gyro_range_rad_s;
+	report.x = _gyro_filter_x.apply(report.x);
+	report.y = _gyro_filter_y.apply(report.y);
+	report.z = _gyro_filter_z.apply(report.z);
 
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
+	report.scaling = _gyro_range_scale;
+	report.range_rad_s = _gyro_range_rad_s;
 
-	/* if we are running up against the oldest report, fix it */
-	if (_next_report == _oldest_report)
-		INCREMENT(_oldest_report, _num_reports);
+	_reports->force(&report);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
 	if (_gyro_topic > 0)
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
+		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
 
 	_read++;
 
@@ -849,8 +832,7 @@ L3GD20::print_info()
 {
 	printf("gyro reads:          %u\n", _read);
 	perf_print_counter(_sample_perf);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
+	_reports->print_info("report queue");
 }
 
 int
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 05d6f1881..2c56b6035 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -62,6 +62,7 @@
 #include <drivers/device/spi.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_mag.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
@@ -218,15 +219,8 @@ private:
 	unsigned		_call_accel_interval;
 	unsigned		_call_mag_interval;
 
-	unsigned		_num_accel_reports;
-	volatile unsigned	_next_accel_report;
-	volatile unsigned	_oldest_accel_report;
-	struct accel_report	*_accel_reports;
-
-	unsigned		_num_mag_reports;
-	volatile unsigned	_next_mag_report;
-	volatile unsigned	_oldest_mag_report;
-	struct mag_report	*_mag_reports;
+	RingBuffer		*_accel_reports;
+	RingBuffer		*_mag_reports;
 
 	struct accel_scale	_accel_scale;
 	unsigned		_accel_range_m_s2;
@@ -404,7 +398,7 @@ public:
 	LSM303D_mag(LSM303D *parent);
 	~LSM303D_mag();
 
-	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual ssize_t			read(struct file *filp, char *buffer, size_t buflen);
 	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
 
 protected:
@@ -420,22 +414,12 @@ private:
 };
 
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
-
-
 LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
 	_mag(new LSM303D_mag(this)),
 	_call_accel_interval(0),
 	_call_mag_interval(0),
-	_num_accel_reports(0),
-	_next_accel_report(0),
-	_oldest_accel_report(0),
 	_accel_reports(nullptr),
-	_num_mag_reports(0),
-	_next_mag_report(0),
-	_oldest_mag_report(0),
 	_mag_reports(nullptr),
 	_accel_range_m_s2(0.0f),
 	_accel_range_scale(0.0f),
@@ -480,9 +464,9 @@ LSM303D::~LSM303D()
 
 	/* free any existing reports */
 	if (_accel_reports != nullptr)
-		delete[] _accel_reports;
+		delete _accel_reports;
 	if (_mag_reports != nullptr)
-		delete[] _mag_reports;
+		delete _mag_reports;
 
 	delete _mag;
 
@@ -498,24 +482,23 @@ LSM303D::init()
 	int mag_ret;
 
 	/* do SPI init (and probe) first */
-	if (SPI::init() != OK)
+	if (SPI::init() != OK) {
+		warnx("SPI init failed");
 		goto out;
+	}
 
 	/* allocate basic report buffers */
-	_num_accel_reports = 2;
-	_oldest_accel_report = _next_accel_report = 0;
-	_accel_reports = new struct accel_report[_num_accel_reports];
+	_accel_reports = new RingBuffer(2, sizeof(accel_report));
 
 	if (_accel_reports == nullptr)
 		goto out;
 
 	/* advertise accel topic */
-	memset(&_accel_reports[0], 0, sizeof(_accel_reports[0]));
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]);
+	struct accel_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
 
-	_num_mag_reports = 2;
-	_oldest_mag_report = _next_mag_report = 0;
-	_mag_reports = new struct mag_report[_num_mag_reports];
+	_mag_reports = new RingBuffer(2, sizeof(mag_report));
 
 	if (_mag_reports == nullptr)
 		goto out;
@@ -523,8 +506,9 @@ LSM303D::init()
 	reset();
 
 	/* advertise mag topic */
-	memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
-	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
+	struct mag_report zero_mag_report;
+	memset(&zero_mag_report, 0, sizeof(zero_mag_report));
+	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report);
 
 	/* do CDev init for the mag device node, keep it optional */
 	mag_ret = _mag->init();
@@ -567,7 +551,9 @@ LSM303D::probe()
 	(void)read_reg(ADDR_WHO_AM_I);
 
 	/* verify that the device is attached and functioning */
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM)
+	bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
+	
+	if (success)
 		return OK;
 
 	return -EIO;
@@ -577,6 +563,7 @@ ssize_t
 LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct accel_report);
+	accel_report *arb = reinterpret_cast<accel_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -585,17 +572,13 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 
 	/* if automatic measurement is enabled */
 	if (_call_accel_interval > 0) {
-
 		/*
 		 * While there is space in the caller's buffer, and reports, copy them.
-		 * Note that we may be pre-empted by the measurement code while we are doing this;
-		 * we are careful to avoid racing with it.
 		 */
 		while (count--) {
-			if (_oldest_accel_report != _next_accel_report) {
-				memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports));
-				ret += sizeof(_accel_reports[0]);
-				INCREMENT(_oldest_accel_report, _num_accel_reports);
+			if (_accel_reports->get(arb)) {
+				ret += sizeof(*arb);
+				arb++;
 			}
 		}
 
@@ -604,12 +587,11 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement */
-	_oldest_accel_report = _next_accel_report = 0;
 	measure();
 
 	/* measurement will have generated a report, copy it out */
-	memcpy(buffer, _accel_reports, sizeof(*_accel_reports));
-	ret = sizeof(*_accel_reports);
+	if (_accel_reports->get(arb))
+		ret = sizeof(*arb);
 
 	return ret;
 }
@@ -618,6 +600,7 @@ ssize_t
 LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct mag_report);
+	mag_report *mrb = reinterpret_cast<mag_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -629,14 +612,11 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
 
 		/*
 		 * While there is space in the caller's buffer, and reports, copy them.
-		 * Note that we may be pre-empted by the measurement code while we are doing this;
-		 * we are careful to avoid racing with it.
 		 */
 		while (count--) {
-			if (_oldest_mag_report != _next_mag_report) {
-				memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports));
-				ret += sizeof(_mag_reports[0]);
-				INCREMENT(_oldest_mag_report, _num_mag_reports);
+			if (_mag_reports->get(mrb)) {
+				ret += sizeof(*mrb);
+				mrb++;
 			}
 		}
 
@@ -645,12 +625,12 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement */
-	_oldest_mag_report = _next_mag_report = 0;
+	_mag_reports->flush();
 	measure();
 
 	/* measurement will have generated a report, copy it out */
-	memcpy(buffer, _mag_reports, sizeof(*_mag_reports));
-	ret = sizeof(*_mag_reports);
+	if (_mag_reports->get(mrb))
+		ret = sizeof(*mrb);
 
 	return ret;
 }
@@ -718,31 +698,22 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_accel_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
-		/* account for sentinel in the ring */
-		arg++;
-
 		/* lower bound is mandatory, upper bound is a sanity check */
-		if ((arg < 2) || (arg > 100))
+		if ((arg < 1) || (arg > 100))
 			return -EINVAL;
 
-		/* allocate new buffer */
-		struct accel_report *buf = new struct accel_report[arg];
-
-		if (nullptr == buf)
+		irqstate_t flags = irqsave();
+		if (!_accel_reports->resize(arg)) {
+			irqrestore(flags);
 			return -ENOMEM;
-
-		/* reset the measurement state machine with the new buffer, free the old */
-		stop();
-		delete[] _accel_reports;
-		_num_accel_reports = arg;
-		_accel_reports = buf;
-		start();
+		}
+		irqrestore(flags);
 
 		return OK;
 	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_accel_reports - 1;
+		return _accel_reports->size();
 
 	case SENSORIOCRESET:
 		reset();
@@ -854,31 +825,22 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_mag_interval;
 	
 	case SENSORIOCSQUEUEDEPTH: {
-			/* account for sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct mag_report *buf = new struct mag_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _mag_reports;
-			_num_mag_reports = arg;
-			_mag_reports = buf;
-			start();
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
 
-			return OK;
+		irqstate_t flags = irqsave();
+		if (!_mag_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
 		}
+		irqrestore(flags);
+
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_mag_reports - 1;
+		return _mag_reports->size();
 
 	case SENSORIOCRESET:
 		reset();
@@ -1046,6 +1008,7 @@ LSM303D::accel_set_range(unsigned max_g)
 
 	_accel_range_scale = new_scale_g_digit * LSM303D_ONE_G;
 
+
 	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
 
 	return OK;
@@ -1211,8 +1174,8 @@ LSM303D::start()
 	stop();
 
 	/* reset the report ring */
-	_oldest_accel_report = _next_accel_report = 0;
-	_oldest_mag_report = _next_mag_report = 0;
+	_accel_reports->flush();
+	_mag_reports->flush();
 
 	/* start polling at the specified rate */
 	hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this);
@@ -1259,7 +1222,7 @@ LSM303D::measure()
 	} raw_accel_report;
 #pragma pack(pop)
 
-	accel_report		*accel_report = &_accel_reports[_next_accel_report];
+	accel_report accel_report;
 
 	/* start the performance counter */
 	perf_begin(_accel_sample_perf);
@@ -1284,35 +1247,30 @@ LSM303D::measure()
 	 */
 
 
-	accel_report->timestamp = hrt_absolute_time();
-
-	accel_report->x_raw = raw_accel_report.x;
-	accel_report->y_raw = raw_accel_report.y;
-	accel_report->z_raw = raw_accel_report.z;
+	accel_report.timestamp = hrt_absolute_time();
 
-	float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	accel_report.x_raw = raw_accel_report.x;
+	accel_report.y_raw = raw_accel_report.y;
+	accel_report.z_raw = raw_accel_report.z;
 
-	accel_report->x = _accel_filter_x.apply(x_in_new);
-	accel_report->y = _accel_filter_y.apply(y_in_new);
-	accel_report->z = _accel_filter_z.apply(z_in_new);
+	float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
 
-	accel_report->scaling = _accel_range_scale;
-	accel_report->range_m_s2 = _accel_range_m_s2;
+	accel_report.x = _accel_filter_x.apply(x_in_new);
+	accel_report.y = _accel_filter_y.apply(y_in_new);
+	accel_report.z = _accel_filter_z.apply(z_in_new);
 
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_accel_report, _num_accel_reports);
+	accel_report.scaling = _accel_range_scale;
+	accel_report.range_m_s2 = _accel_range_m_s2;
 
-	/* if we are running up against the oldest report, fix it */
-	if (_next_accel_report == _oldest_accel_report)
-		INCREMENT(_oldest_accel_report, _num_accel_reports);
+	_accel_reports->force(&accel_report);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report);
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
 
 	_accel_read++;
 
@@ -1334,7 +1292,7 @@ LSM303D::mag_measure()
 	} raw_mag_report;
 #pragma pack(pop)
 
-	mag_report		*mag_report = &_mag_reports[_next_mag_report];
+	mag_report mag_report;
 
 	/* start the performance counter */
 	perf_begin(_mag_sample_perf);
@@ -1359,30 +1317,25 @@ LSM303D::mag_measure()
 	 */
 
 
-	mag_report->timestamp = hrt_absolute_time();
+	mag_report.timestamp = hrt_absolute_time();
 
-	mag_report->x_raw = raw_mag_report.x;
-	mag_report->y_raw = raw_mag_report.y;
-	mag_report->z_raw = raw_mag_report.z;
-	mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
-	mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
-	mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
-	mag_report->scaling = _mag_range_scale;
-	mag_report->range_ga = (float)_mag_range_ga;
+	mag_report.x_raw = raw_mag_report.x;
+	mag_report.y_raw = raw_mag_report.y;
+	mag_report.z_raw = raw_mag_report.z;
+	mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
+	mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
+	mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
+	mag_report.scaling = _mag_range_scale;
+	mag_report.range_ga = (float)_mag_range_ga;
 
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_mag_report, _num_mag_reports);
-
-	/* if we are running up against the oldest report, fix it */
-	if (_next_mag_report == _oldest_mag_report)
-		INCREMENT(_oldest_mag_report, _num_mag_reports);
+	_mag_reports->force(&mag_report);
 
 	/* XXX please check this poll_notify, is it the right one? */
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report);
+	orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
 
 	_mag_read++;
 
@@ -1396,11 +1349,8 @@ LSM303D::print_info()
 	printf("accel reads:          %u\n", _accel_read);
 	printf("mag reads:            %u\n", _mag_read);
 	perf_print_counter(_accel_sample_perf);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports);
-	perf_print_counter(_mag_sample_perf);
-		printf("report queue:   %u (%u/%u @ %p)\n",
-		   _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports);
+	_accel_reports->print_info("accel reports");
+	_mag_reports->print_info("mag reports");
 }
 
 LSM303D_mag::LSM303D_mag(LSM303D *parent) :
@@ -1470,8 +1420,10 @@ start()
 	/* create the driver */
 	g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
 
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
+		warnx("failed instantiating LSM303D obj");
 		goto fail;
+	}
 
 	if (OK != g_dev->init())
 		goto fail;
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index f83416993..ccc5bc15e 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -64,6 +64,7 @@
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_range_finder.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/subsystem_info.h>
@@ -119,10 +120,7 @@ private:
 	float				_min_distance;
 	float				_max_distance;
 	work_s				_work;
-	unsigned			_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	range_finder_report	*_reports;
+	RingBuffer		*_reports;
 	bool				_sensor_ok;
 	int					_measure_ticks;
 	bool				_collect_phase;
@@ -183,9 +181,6 @@ private:
 	
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
-
 /*
  * Driver 'main' command.
  */
@@ -195,9 +190,6 @@ MB12XX::MB12XX(int bus, int address) :
 	I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000),
 	_min_distance(MB12XX_MIN_DISTANCE),
 	_max_distance(MB12XX_MAX_DISTANCE),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_sensor_ok(false),
 	_measure_ticks(0),
@@ -221,7 +213,7 @@ MB12XX::~MB12XX()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 }
 
 int
@@ -234,17 +226,15 @@ MB12XX::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_reports = new struct range_finder_report[_num_reports];
+	_reports = new RingBuffer(2, sizeof(range_finder_report));
 
 	if (_reports == nullptr)
 		goto out;
 
-	_oldest_report = _next_report = 0;
-
 	/* get a publish handle on the range finder topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]);
+	struct range_finder_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
 
 	if (_range_finder_topic < 0)
 		debug("failed to create sensor_range_finder object. Did you start uOrb?");
@@ -354,31 +344,22 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* add one to account for the sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct range_finder_report *buf = new struct range_finder_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start();
-
-			return OK;
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+		
+		irqstate_t flags = irqsave();
+		if (!_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
 		}
+		irqrestore(flags);
+		
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 		
 	case SENSORIOCRESET:
 		/* XXX implement this */
@@ -406,6 +387,7 @@ ssize_t
 MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct range_finder_report);
+	struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -421,10 +403,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(rbuf)) {
+				ret += sizeof(*rbuf);
+				rbuf++;
 			}
 		}
 
@@ -433,9 +414,8 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement - run one conversion */
-	/* XXX really it'd be nice to lock against other readers here */
 	do {
-		_oldest_report = _next_report = 0;
+		_reports->flush();
 
 		/* trigger a measurement */
 		if (OK != measure()) {
@@ -453,8 +433,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
-		ret = sizeof(*_reports);
+		if (_reports->get(rbuf)) {
+			ret = sizeof(*rbuf);
+		}
 
 	} while (0);
 
@@ -498,26 +479,25 @@ MB12XX::collect()
 	if (ret < 0)
 	{
 		log("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
 		return ret;
 	}
 	
 	uint16_t distance = val[0] << 8 | val[1];
 	float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */
+	struct range_finder_report report;
+
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
-	_reports[_next_report].timestamp = hrt_absolute_time();
-	_reports[_next_report].distance = si_units;
-	_reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
+	report.timestamp = hrt_absolute_time();
+	report.distance = si_units;
+	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
 	
 	/* publish it */
-	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]);
-
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
+	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
 
-	/* if we are running up against the oldest report, toss it */
-	if (_next_report == _oldest_report) {
+	if (_reports->force(&report)) {
 		perf_count(_buffer_overflows);
-		INCREMENT(_oldest_report, _num_reports);
 	}
 
 	/* notify anyone waiting for data */
@@ -525,11 +505,8 @@ MB12XX::collect()
 
 	ret = OK;
 
-out:
 	perf_end(_sample_perf);
 	return ret;
-	
-	return ret;
 }
 
 void
@@ -537,7 +514,7 @@ MB12XX::start()
 {
 	/* reset the report ring and state machine */
 	_collect_phase = false;
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
@@ -626,8 +603,7 @@ MB12XX::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
+	_reports->print_info("report queue");
 }
 
 /**
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index b1cb2b3d8..03d7bbfb9 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -199,27 +199,23 @@ MEASAirspeed::collect()
 	// Calculate differential pressure. As its centered around 8000
 	// and can go positive or negative, enforce absolute value
 	uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
-
-	_reports[_next_report].timestamp = hrt_absolute_time();
-	_reports[_next_report].temperature = temperature;
-	_reports[_next_report].differential_pressure_pa = diff_press_pa;
+	struct differential_pressure_s report;
 
 	// Track maximum differential pressure measured (so we can work out top speed).
-	if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) {
-		_reports[_next_report].max_differential_pressure_pa = diff_press_pa;
+	if (diff_press_pa > _max_differential_pressure_pa) {
+	    _max_differential_pressure_pa = diff_press_pa;
 	}
 
-	/* announce the airspeed if needed, just publish else */
-	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
+	report.timestamp = hrt_absolute_time();
+	report.temperature = temperature;
+	report.differential_pressure_pa = diff_press_pa;
+	report.voltage = 0;
+	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
-	/* post a report to the ring - note, not locked */
-	INCREMENT(_next_report, _num_reports);
+	/* announce the airspeed if needed, just publish else */
+	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
 
-	/* if we are running up against the oldest report, toss it */
-	if (_next_report == _oldest_report) {
-		perf_count(_buffer_overflows);
-		INCREMENT(_oldest_report, _num_reports);
-	}
+	new_report(report);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 1bc3e97a4..d0de26a1a 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -96,9 +96,10 @@ class MK : public device::I2C
 {
 public:
 	enum Mode {
+		MODE_NONE,
 		MODE_2PWM,
 		MODE_4PWM,
-		MODE_NONE
+		MODE_6PWM,
 	};
 
 	enum MappingMode {
@@ -1023,9 +1024,11 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
 		return ret;
 
 	/* if we are in valid PWM mode, try it as a PWM ioctl as well */
+	/*
 	switch (_mode) {
 	case MODE_2PWM:
 	case MODE_4PWM:
+	case MODE_6PWM:
 		ret = pwm_ioctl(filp, cmd, arg);
 		break;
 
@@ -1033,6 +1036,8 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
 		debug("not in a PWM mode");
 		break;
 	}
+	*/
+	ret = pwm_ioctl(filp, cmd, arg);
 
 	/* if nobody wants it, let CDev have it */
 	if (ret == -ENOTTY)
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 14f8f44b8..14a3571de 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -194,16 +194,14 @@ private:
 	struct hrt_call		_call;
 	unsigned		_call_interval;
 
-	typedef RingBuffer<accel_report> AccelReportBuffer;
-	AccelReportBuffer	*_accel_reports;
+	RingBuffer		*_accel_reports;
 
 	struct accel_scale	_accel_scale;
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
 
-	typedef RingBuffer<gyro_report> GyroReportBuffer;
-	GyroReportBuffer	*_gyro_reports;
+	RingBuffer		*_gyro_reports;
 
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
@@ -431,11 +429,11 @@ MPU6000::init()
 	}
 
 	/* allocate basic report buffers */
-	_accel_reports = new AccelReportBuffer(2);
+	_accel_reports = new RingBuffer(2, sizeof(accel_report));
 	if (_accel_reports == nullptr)
 		goto out;
 
-	_gyro_reports = new GyroReportBuffer(2);
+	_gyro_reports = new RingBuffer(2, sizeof(gyro_report));
 	if (_gyro_reports == nullptr)
 		goto out;
 
@@ -466,14 +464,14 @@ MPU6000::init()
 		_gyro_topic = -1;
 	} else {
 		gyro_report gr;
-		_gyro_reports->get(gr);
+		_gyro_reports->get(&gr);
 
 		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
 	}
 
 	/* advertise accel topic */
 	accel_report ar;
-	_accel_reports->get(ar);
+	_accel_reports->get(&ar);
 	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
 
 out:
@@ -658,9 +656,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
 	accel_report *arp = reinterpret_cast<accel_report *>(buffer);
 	int transferred = 0;
 	while (count--) {
-		if (!_accel_reports->get(*arp++))
+		if (!_accel_reports->get(arp))
 			break;
 		transferred++;
+		arp++;
 	}
 
 	/* return the number of bytes transferred */
@@ -748,12 +747,13 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
 		return -EAGAIN;
 
 	/* copy reports out of our buffer to the caller */
-	gyro_report *arp = reinterpret_cast<gyro_report *>(buffer);
+	gyro_report *grp = reinterpret_cast<gyro_report *>(buffer);
 	int transferred = 0;
 	while (count--) {
-		if (!_gyro_reports->get(*arp++))
+		if (!_gyro_reports->get(grp))
 			break;
 		transferred++;
+		grp++;
 	}
 
 	/* return the number of bytes transferred */
@@ -837,28 +837,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 1) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			AccelReportBuffer *buf = new AccelReportBuffer(arg);
-
-			if (nullptr == buf)
-				return -ENOMEM;
-			if (buf->size() == 0) {
-				delete buf;
-				return -ENOMEM;
-			}
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete _accel_reports;
-			_accel_reports = buf;
-			start();
-
-			return OK;
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+		
+		irqstate_t flags = irqsave();
+		if (!_accel_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
 		}
+		irqrestore(flags);
+		
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _accel_reports->size();
@@ -935,21 +926,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		if ((arg < 1) || (arg > 100))
 			return -EINVAL;
 
-		/* allocate new buffer */
-		GyroReportBuffer *buf = new GyroReportBuffer(arg);
-
-		if (nullptr == buf)
-			return -ENOMEM;
-		if (buf->size() == 0) {
-			delete buf;
+		irqstate_t flags = irqsave();
+		if (!_gyro_reports->resize(arg)) {
+			irqrestore(flags);
 			return -ENOMEM;
 		}
-
-		/* reset the measurement state machine with the new buffer, free the old */
-		stop();
-		delete _gyro_reports;
-		_gyro_reports = buf;
-		start();
+		irqrestore(flags);
 
 		return OK;
 	}
@@ -1260,8 +1242,8 @@ MPU6000::measure()
 	grb.temperature_raw = report.temp;
 	grb.temperature = (report.temp) / 361.0f + 35.0f;
 
-	_accel_reports->put(arb);
-	_gyro_reports->put(grb);
+	_accel_reports->force(&arb);
+	_gyro_reports->force(&grb);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
@@ -1280,7 +1262,10 @@ MPU6000::measure()
 void
 MPU6000::print_info()
 {
+	perf_print_counter(_sample_perf);
 	printf("reads:          %u\n", _reads);
+	_accel_reports->print_info("accel queue");
+	_gyro_reports->print_info("gyro queue");
 }
 
 MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 4e43f19c5..1c8a4d776 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -60,6 +60,7 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_baro.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -114,10 +115,7 @@ protected:
 	struct work_s		_work;
 	unsigned		_measure_ticks;
 
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	struct baro_report	*_reports;
+	RingBuffer		*_reports;
 
 	bool			_collect_phase;
 	unsigned		_measure_phase;
@@ -196,9 +194,6 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
 	_interface(interface),
 	_prom(prom_buf.s),
 	_measure_ticks(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_collect_phase(false),
 	_measure_phase(0),
@@ -223,7 +218,7 @@ MS5611::~MS5611()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 
 	// free perf counters
 	perf_free(_sample_perf);
@@ -246,8 +241,7 @@ MS5611::init()
 	}
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
-	_reports = new struct baro_report[_num_reports];
+	_reports = new RingBuffer(2, sizeof(baro_report));
 
 	if (_reports == nullptr) {
 		debug("can't get memory for reports");
@@ -255,11 +249,10 @@ MS5611::init()
 		goto out;
 	}
 
-	_oldest_report = _next_report = 0;
-
 	/* get a publish handle on the baro topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
-	_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
+	struct baro_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report);
 
 	if (_baro_topic < 0) {
 		debug("failed to create sensor_baro object");
@@ -276,6 +269,7 @@ ssize_t
 MS5611::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct baro_report);
+	struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -291,10 +285,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
-				ret += sizeof(_reports[0]);
-				INCREMENT(_oldest_report, _num_reports);
+			if (_reports->get(brp)) {
+				ret += sizeof(*brp);
+				brp++;
 			}
 		}
 
@@ -303,10 +296,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement - run one conversion */
-	/* XXX really it'd be nice to lock against other readers here */
 	do {
 		_measure_phase = 0;
-		_oldest_report = _next_report = 0;
+		_reports->flush();
 
 		/* do temperature first */
 		if (OK != measure()) {
@@ -335,8 +327,8 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
-		ret = sizeof(*_reports);
+		if (_reports->get(brp))
+			ret = sizeof(*brp);
 
 	} while (0);
 
@@ -411,31 +403,21 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* add one to account for the sentinel in the ring */
-			arg++;
-
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			struct baro_report *buf = new struct baro_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop_cycle();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start_cycle();
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
 
-			return OK;
+		irqstate_t flags = irqsave();
+		if (!_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
 		}
+		irqrestore(flags);		
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
 		/* XXX implement this */
@@ -469,7 +451,7 @@ MS5611::start_cycle()
 	/* reset the report ring and state machine */
 	_collect_phase = false;
 	_measure_phase = 0;
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
@@ -588,8 +570,9 @@ MS5611::collect()
 
 	perf_begin(_sample_perf);
 
+	struct baro_report report;
 	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
-	_reports[_next_report].timestamp = hrt_absolute_time();
+	report.timestamp = hrt_absolute_time();
 
 	/* read the most recent measurement - read offset/size are hardcoded in the interface */
 	ret = _interface->read(0, (void *)&raw, 0);
@@ -638,8 +621,8 @@ MS5611::collect()
 		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
 
 		/* generate a new report */
-		_reports[_next_report].temperature = _TEMP / 100.0f;
-		_reports[_next_report].pressure = P / 100.0f;		/* convert to millibar */
+		report.temperature = _TEMP / 100.0f;
+		report.pressure = P / 100.0f;		/* convert to millibar */
 
 		/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
 
@@ -676,18 +659,13 @@ MS5611::collect()
 		 * h = -------------------------------  + h1
 		 *                   a
 		 */
-		_reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
+		report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
 
 		/* publish it */
-		orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]);
-
-		/* post a report to the ring - note, not locked */
-		INCREMENT(_next_report, _num_reports);
+		orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
 
-		/* if we are running up against the oldest report, toss it */
-		if (_next_report == _oldest_report) {
+		if (_reports->force(&report)) {
 			perf_count(_buffer_overflows);
-			INCREMENT(_oldest_report, _num_reports);
 		}
 
 		/* notify anyone waiting for data */
@@ -709,8 +687,7 @@ MS5611::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
+	_reports->print_info("report queue");
 	printf("TEMP:           %d\n", _TEMP);
 	printf("SENS:           %lld\n", _SENS);
 	printf("OFF:            %lld\n", _OFF);
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
index f6c624340..e547c913b 100644
--- a/src/drivers/ms5611/ms5611_spi.cpp
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -134,6 +134,7 @@ int
 MS5611_SPI::init()
 {
 	int ret;
+	irqstate_t flags;
 
 	ret = SPI::init();
 	if (ret != OK) {
@@ -261,13 +262,7 @@ MS5611_SPI::_reg16(unsigned reg)
 int
 MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len)
 {
-	irqstate_t flags = irqsave();
-
-	int ret = transfer(send, recv, len);
-
-	irqrestore(flags);
-
-	return ret;
+	return transfer(send, recv, len);
 }
 
 #endif /* PX4_SPIDEV_BARO */
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 6d4019f24..b1dd55dd7 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1096,10 +1096,11 @@ fmu_start(void)
 void
 test(void)
 {
-	int	fd;
+	int	 fd;
         unsigned servo_count = 0;
 	unsigned pwm_value = 1000;
 	int	 direction = 1;
+	int	 ret;
         
 	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
 
@@ -1114,9 +1115,9 @@ test(void)
 
 	warnx("Testing %u servos", (unsigned)servo_count);
 
-	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
-	if (!console)
-		err(1, "failed opening console");
+	struct pollfd fds;
+	fds.fd = 0; /* stdin */
+	fds.events = POLLIN;
 
 	warnx("Press CTRL-C or 'c' to abort.");
 
@@ -1166,15 +1167,17 @@ test(void)
 
 		/* Check if user wants to quit */
 		char c;
-		if (read(console, &c, 1) == 1) {
-			if (c == 0x03 || c == 0x63) {
+		ret = poll(&fds, 1, 0);
+		if (ret > 0) {
+
+			read(0, &c, 1);
+			if (c == 0x03 || c == 0x63 || c == 'q') {
 				warnx("User abort\n");
                                 break;
 			}
 		}
 	}
 
-        close(console);
 	close(fd);
 
 	exit(0);
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index bd5f33043..0fed99692 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -189,6 +189,7 @@ public:
 	 */
 	int			disable_rc_handling();
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	/**
 	 * Set the DSM VCC is controlled by relay one flag
 	 *
@@ -208,6 +209,9 @@ public:
 	{
 		return _dsm_vcc_ctl;
 	};
+#endif
+
+	inline uint16_t		system_status() const {return _status;}
 
 private:
 	device::Device		*_interface;
@@ -226,7 +230,8 @@ private:
 	volatile int		_task;			///<worker task id
 	volatile bool		_task_should_exit;	///<worker terminate flag
 
-	int			_mavlink_fd;		///<mavlink file descriptor
+	int			_mavlink_fd;		///<mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
+	int			_thread_mavlink_fd;	///<mavlink file descriptor for thread.
 
 	perf_counter_t		_perf_update;		///<local performance counter
 
@@ -239,6 +244,7 @@ private:
 	int			_t_actuator_armed;	///< system armed control topic
 	int 			_t_vehicle_control_mode;///< vehicle control mode topic
 	int			_t_param;		///< parameter update topic
+	int			_t_vehicle_command;	///< vehicle command topic
 
 	/* advertised topics */
 	orb_advert_t 		_to_input_rc;		///< rc inputs from io
@@ -257,7 +263,9 @@ private:
 	float			_battery_mamphour_total;///<amp hours consumed so far
 	uint64_t		_battery_last_timestamp;///<last amp hour calculation timestamp
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	bool			_dsm_vcc_ctl;		///<true if relay 1 controls DSM satellite RX power
+#endif
 
 	/**
 	 * Trampoline to the worker task
@@ -374,7 +382,7 @@ private:
 	/**
 	 * Send mixer definition text to IO
 	 */
-	int			mixer_send(const char *buf, unsigned buflen);
+	int			mixer_send(const char *buf, unsigned buflen, unsigned retries=3);
 
 	/**
 	 * Set the minimum PWM signals when armed
@@ -409,6 +417,13 @@ private:
 	 */
 	int			io_handle_alarms(uint16_t alarms);
 
+	/**
+	 * Handle issuing dsm bind ioctl to px4io.
+	 *
+	 * @param dsmMode	0:dsm2, 1:dsmx
+	 */
+	void			dsm_bind_ioctl(int dsmMode);
+
 };
 
 
@@ -433,6 +448,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_task(-1),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
+	_thread_mavlink_fd(-1),
 	_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
 	_status(0),
 	_alarms(0),
@@ -440,6 +456,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_t_actuator_armed(-1),
 	_t_vehicle_control_mode(-1),
 	_t_param(-1),
+	_t_vehicle_command(-1),
 	_to_input_rc(0),
 	_to_actuators_effective(0),
 	_to_outputs(0),
@@ -449,8 +466,11 @@ PX4IO::PX4IO(device::Device *interface) :
 	_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
-	_battery_last_timestamp(0),
-	_dsm_vcc_ctl(false)
+	_battery_last_timestamp(0)
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+	,_dsm_vcc_ctl(false)
+#endif
+
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -487,25 +507,27 @@ PX4IO::detect()
 {
 	int ret;
 
-	ASSERT(_task == -1);
+	if (_task == -1) {
 
-	/* do regular cdev init */
-	ret = CDev::init();
-	if (ret != OK)
-		return ret;
+		/* do regular cdev init */
+		ret = CDev::init();
+		if (ret != OK)
+			return ret;
 
-	/* get some parameters */
-	unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
-	if (protocol != PX4IO_PROTOCOL_VERSION) {
-		if (protocol == _io_reg_get_error) {
-			log("IO not installed");
-		} else {
-			log("IO version error");
-			mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!");
+		/* get some parameters */
+		unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
+		if (protocol != PX4IO_PROTOCOL_VERSION) {
+			if (protocol == _io_reg_get_error) {
+				log("IO not installed");
+			} else {
+				log("IO version error");
+				mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!");
+			}
+			
+			return -1;
 		}
-		
-		return -1;
 	}
+
 	log("IO found");
 
 	return 0;
@@ -569,6 +591,9 @@ PX4IO::init()
 	if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
 	    (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 
+		/* get a status update from IO */
+		io_get_status();
+
 	    	mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
 	    	log("INAIR RESTART RECOVERY (needs commander app running)");
 
@@ -708,10 +733,10 @@ void
 PX4IO::task_main()
 {
 	hrt_abstime last_poll_time = 0;
-	int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
 
 	log("starting");
 
+	_thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
 
 	/*
 	 * Subscribe to the appropriate PWM output topic based on whether we are the
@@ -730,16 +755,20 @@ PX4IO::task_main()
 	_t_param = orb_subscribe(ORB_ID(parameter_update));
 	orb_set_interval(_t_param, 500);		/* 2Hz update rate max. */
 
+	_t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
+	orb_set_interval(_t_param, 1000);		/* 1Hz update rate max. */
+
 	if ((_t_actuators < 0) ||
 		(_t_actuator_armed < 0) ||
 		(_t_vehicle_control_mode < 0) ||
-		(_t_param < 0)) {
+		(_t_param < 0) ||
+		(_t_vehicle_command < 0)) {
 		log("subscription(s) failed");
 		goto out;
 	}
 
 	/* poll descriptor */
-	pollfd fds[4];
+	pollfd fds[5];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
 	fds[1].fd = _t_actuator_armed;
@@ -748,8 +777,10 @@ PX4IO::task_main()
 	fds[2].events = POLLIN;
 	fds[3].fd = _t_param;
 	fds[3].events = POLLIN;
+	fds[4].fd = _t_vehicle_command;
+	fds[4].events = POLLIN;
 
-	debug("ready");
+	log("ready");
 
 	/* lock against the ioctl handler */
 	lock();
@@ -789,6 +820,16 @@ PX4IO::task_main()
 		if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN))
 			io_set_arming_state();
 
+		/* if we have a vehicle command, handle it */
+		if (fds[4].revents & POLLIN) {
+			struct vehicle_command_s cmd;
+			orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
+			// Check for a DSM pairing command
+			if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) {
+				dsm_bind_ioctl((int)cmd.param2);
+			}
+		}
+
 		/*
 		 * If it's time for another tick of the polling status machine,
 		 * try it now.
@@ -823,20 +864,11 @@ PX4IO::task_main()
 				int32_t dsm_bind_val;
 				param_t dsm_bind_param;
 
-				// See if bind parameter has been set, and reset it to 0
+				// See if bind parameter has been set, and reset it to -1
 				param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
-				if (dsm_bind_val) {
-					if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
-						if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) {
-							mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x');
-							ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7);
-						} else {
-							mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected");
-						}
-					} else {
-						mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected"); 
-					}
-					dsm_bind_val = 0;
+				if (dsm_bind_val > -1) {
+					dsm_bind_ioctl(dsm_bind_val);
+					dsm_bind_val = -1;
 					param_set(dsm_bind_param, &dsm_bind_val);
 				}
 
@@ -1143,6 +1175,23 @@ PX4IO::io_handle_status(uint16_t status)
 	return ret;
 }
 
+void
+PX4IO::dsm_bind_ioctl(int dsmMode)
+{
+	if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
+		/* 0: dsm2, 1:dsmx */
+		if ((dsmMode == 0) || (dsmMode == 1)) {
+			mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x');
+			ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES);
+		} else {
+			mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected");
+		}
+	} else {
+		mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); 
+	}
+}
+
+
 int
 PX4IO::io_handle_alarms(uint16_t alarms)
 {
@@ -1430,61 +1479,70 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
 }
 
 int
-PX4IO::mixer_send(const char *buf, unsigned buflen)
+PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 {
-	uint8_t	frame[_max_transfer];
-	px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
-	unsigned max_len = _max_transfer - sizeof(px4io_mixdata);
 
-	msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
-	msg->action = F2I_MIXER_ACTION_RESET;
+	uint8_t	frame[_max_transfer];
 
 	do {
-		unsigned count = buflen;
 
-		if (count > max_len)
-			count = max_len;
+		px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
+		unsigned max_len = _max_transfer - sizeof(px4io_mixdata);
 
-		if (count > 0) {
-			memcpy(&msg->text[0], buf, count);
-			buf += count;
-			buflen -= count;
-		}
+		msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
+		msg->action = F2I_MIXER_ACTION_RESET;
 
-		/*
-		 * We have to send an even number of bytes.  This
-		 * will only happen on the very last transfer of a
-		 * mixer, and we are guaranteed that there will be
-		 * space left to round up as _max_transfer will be
-		 * even.
-		 */
-		unsigned total_len = sizeof(px4io_mixdata) + count;
-		if (total_len % 1) {
-			msg->text[count] = '\0';
-			total_len++;
-		}
+		do {
+			unsigned count = buflen;
 
-		int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+			if (count > max_len)
+				count = max_len;
 
-		if (ret) {
-			log("mixer send error %d", ret);
-			return ret;
-		}
+			if (count > 0) {
+				memcpy(&msg->text[0], buf, count);
+				buf += count;
+				buflen -= count;
+			}
+
+			/*
+			 * We have to send an even number of bytes.  This
+			 * will only happen on the very last transfer of a
+			 * mixer, and we are guaranteed that there will be
+			 * space left to round up as _max_transfer will be
+			 * even.
+			 */
+			unsigned total_len = sizeof(px4io_mixdata) + count;
+			if (total_len % 2) {
+				msg->text[count] = '\0';
+				total_len++;
+			}
+
+			int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
 
-		msg->action = F2I_MIXER_ACTION_APPEND;
+			if (ret) {
+				log("mixer send error %d", ret);
+				return ret;
+			}
+
+			msg->action = F2I_MIXER_ACTION_APPEND;
+
+		} while (buflen > 0);
+
+		retries--;
+
+		log("mixer sent");
 
-	} while (buflen > 0);
+	} while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK)));
 
 	/* check for the mixer-OK flag */
 	if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) {
-		debug("mixer upload OK");
 		mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok");
 		return 0;
-	} else {
-		debug("mixer rejected by IO");
-		mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
 	}
 
+	log("mixer rejected by IO");
+	mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
+
 	/* load must have failed for some reason */
 	return -EINVAL;
 }
@@ -1591,11 +1649,19 @@ PX4IO::print_status()
 		((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)   ? " FAILSAFE_CUSTOM" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)  ? " ALWAYS_PWM_ENABLE" : ""));
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	printf("rates 0x%04x default %u alt %u\n",
+		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
+		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
+		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE));
+#endif
 	printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
 	printf("controls");
 	for (unsigned i = 0; i < _max_controls; i++)
@@ -1754,36 +1820,58 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 	}
 
 	case GPIO_RESET: {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		uint32_t bits = (1 << _max_relays) - 1;
 		/* don't touch relay1 if it's controlling RX vcc */
 		if (_dsm_vcc_ctl)
 			bits &= ~PX4IO_P_SETUP_RELAYS_POWER1;
 		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 	}
 
 	case GPIO_SET:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		arg &= ((1 << _max_relays) - 1);
 		/* don't touch relay1 if it's controlling RX vcc */
-		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
+		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
 			ret = -EINVAL;
-		else
-			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
+			break;
+		}
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 
 	case GPIO_CLEAR:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		arg &= ((1 << _max_relays) - 1);
 		/* don't touch relay1 if it's controlling RX vcc */
-		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
+		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
 			ret = -EINVAL;
-		else
+			break;
+		}
 			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 
 	case GPIO_GET:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS);
 		if (*(uint32_t *)arg == _io_reg_get_error)
 			ret = -EIO;
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 
 	case MIXERIOCGETOUTPUTCOUNT:
@@ -1961,6 +2049,7 @@ start(int argc, char *argv[])
 		}
 	}
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	int dsm_vcc_ctl;
 
 	if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
@@ -1969,6 +2058,7 @@ start(int argc, char *argv[])
 			g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
 		}
 	}
+#endif
 	exit(0);
 }
 
@@ -2008,21 +2098,26 @@ bind(int argc, char *argv[])
 	if (g_dev == nullptr)
 		errx(1, "px4io must be started first");
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	if (!g_dev->get_dsm_vcc_ctl())
 		errx(1, "DSM bind feature not enabled");
+#endif
 
 	if (argc < 3)
 		errx(0, "needs argument, use dsm2 or dsmx");
 
 	if (!strcmp(argv[2], "dsm2"))
-		pulses = 3;
+		pulses = DSM2_BIND_PULSES;
 	else if (!strcmp(argv[2], "dsmx"))
-		pulses = 7;
+		pulses = DSMX_BIND_PULSES;
 	else 
 		errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
+	if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) 
+		errx(1, "system must not be armed");
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
-
+#endif
 	g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
 
 	exit(0);
@@ -2054,10 +2149,9 @@ test(void)
 	if (ioctl(fd, PWM_SERVO_ARM, 0))
 		err(1, "failed to arm servos");
 
-	/* Open console directly to grab CTRL-C signal */
-	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
-	if (!console)
-		err(1, "failed opening console");
+	struct pollfd fds;
+	fds.fd = 0; /* stdin */
+	fds.events = POLLIN;
 
 	warnx("Press CTRL-C or 'c' to abort.");
 
@@ -2098,10 +2192,12 @@ test(void)
 
 		/* Check if user wants to quit */
 		char c;
-		if (read(console, &c, 1) == 1) {
+		ret = poll(&fds, 1, 0);
+		if (ret > 0) {
+
+			read(0, &c, 1);
 			if (c == 0x03 || c == 0x63 || c == 'q') {
 				warnx("User abort\n");
-				close(console);
 				exit(0);
 			}
 		}
@@ -2171,7 +2267,7 @@ px4io_main(int argc, char *argv[])
 		}
 
 		PX4IO_Uploader *up;
-		const char *fn[5];
+		const char *fn[3];
 
 		/* work out what we're uploading... */
 		if (argc > 2) {
@@ -2179,11 +2275,19 @@ px4io_main(int argc, char *argv[])
 			fn[1] = nullptr;
 
 		} else {
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+			fn[0] = "/etc/extras/px4io-v1_default.bin";
+			fn[1] =	"/fs/microsd/px4io1.bin";
+			fn[2] =	"/fs/microsd/px4io.bin";
+			fn[3] =	nullptr;
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
 			fn[0] = "/etc/extras/px4io-v2_default.bin";
-			fn[1] = "/etc/extras/px4io-v1_default.bin";
+			fn[1] =	"/fs/microsd/px4io2.bin";
 			fn[2] =	"/fs/microsd/px4io.bin";
-			fn[3] =	"/fs/microsd/px4io2.bin";
-			fn[4] =	nullptr;
+			fn[3] =	nullptr;
+#else
+#error "unknown board"
+#endif
 		}
 
 		up = new PX4IO_Uploader;
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index feb8f1c6c..fedff769b 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -1,6 +1,8 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Julian Oes <joes@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,7 +38,6 @@
  *
  * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
  *
- *
  */
 
 #include <nuttx/config.h>
@@ -92,16 +93,14 @@ public:
 private:
 	work_s			_work;
 
-	rgbled_color_t		_color;
 	rgbled_mode_t		_mode;
 	rgbled_pattern_t	_pattern;
 
-	float			_brightness;
 	uint8_t			_r;
 	uint8_t			_g;
 	uint8_t			_b;
+	float			_brightness;
 
-	bool			_should_run;
 	bool			_running;
 	int			_led_interval;
 	int			_counter;
@@ -109,35 +108,33 @@ private:
 	void 			set_color(rgbled_color_t ledcolor);
 	void			set_mode(rgbled_mode_t mode);
 	void			set_pattern(rgbled_pattern_t *pattern);
-	void			set_brightness(float brightness);
 
 	static void		led_trampoline(void *arg);
 	void			led();
 
-	int			set(bool on, uint8_t r, uint8_t g, uint8_t b);
-	int			set_on(bool on);
-	int			set_rgb(uint8_t r, uint8_t g, uint8_t b);
-	int			get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+	int			send_led_enable(bool enable);
+	int			send_led_rgb();
+	int			get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
 };
 
 /* for now, we only support one RGBLED */
 namespace
 {
-	RGBLED *g_rgbled;
+RGBLED *g_rgbled;
 }
 
+void rgbled_usage();
 
 extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
 
 RGBLED::RGBLED(int bus, int rgbled) :
 	I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000),
-	_color(RGBLED_COLOR_OFF),
 	_mode(RGBLED_MODE_OFF),
-	_running(false),
-	_brightness(1.0f),
 	_r(0),
 	_g(0),
 	_b(0),
+	_brightness(1.0f),
+	_running(false),
 	_led_interval(0),
 	_counter(0)
 {
@@ -159,8 +156,9 @@ RGBLED::init()
 		return ret;
 	}
 
-	/* start off */
-	set(false, 0, 0, 0);
+	/* switch off LED on start */
+	send_led_enable(false);
+	send_led_rgb();
 
 	return OK;
 }
@@ -169,10 +167,10 @@ int
 RGBLED::probe()
 {
 	int ret;
-	bool on, not_powersave;
+	bool on, powersave;
 	uint8_t r, g, b;
 
-	ret = get(on, not_powersave, r, g, b);
+	ret = get(on, powersave, r, g, b);
 
 	return ret;
 }
@@ -181,15 +179,16 @@ int
 RGBLED::info()
 {
 	int ret;
-	bool on, not_powersave;
+	bool on, powersave;
 	uint8_t r, g, b;
 
-	ret = get(on, not_powersave, r, g, b);
+	ret = get(on, powersave, r, g, b);
 
 	if (ret == OK) {
 		/* we don't care about power-save mode */
 		log("state: %s", on ? "ON" : "OFF");
 		log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+
 	} else {
 		warnx("failed to read led");
 	}
@@ -201,28 +200,30 @@ int
 RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	int ret = ENOTTY;
+
 	switch (cmd) {
 	case RGBLED_SET_RGB:
-		/* set the specified RGB values */
-		rgbled_rgbset_t rgbset;
-		memcpy(&rgbset, (rgbled_rgbset_t*)arg, sizeof(rgbset));
-		set_rgb(rgbset.red, rgbset.green, rgbset.blue);
-		set_mode(RGBLED_MODE_ON);
+		/* set the specified color */
+		_r = ((rgbled_rgbset_t *) arg)->red;
+		_g = ((rgbled_rgbset_t *) arg)->green;
+		_b = ((rgbled_rgbset_t *) arg)->blue;
+		send_led_rgb();
 		return OK;
 
 	case RGBLED_SET_COLOR:
 		/* set the specified color name */
 		set_color((rgbled_color_t)arg);
+		send_led_rgb();
 		return OK;
 
 	case RGBLED_SET_MODE:
-		/* set the specified blink speed */
+		/* set the specified mode */
 		set_mode((rgbled_mode_t)arg);
 		return OK;
 
 	case RGBLED_SET_PATTERN:
 		/* set a special pattern */
-		set_pattern((rgbled_pattern_t*)arg);
+		set_pattern((rgbled_pattern_t *)arg);
 		return OK;
 
 	default:
@@ -241,39 +242,54 @@ RGBLED::led_trampoline(void *arg)
 	rgbl->led();
 }
 
-
-
+/**
+ * Main loop function
+ */
 void
 RGBLED::led()
 {
 	switch (_mode) {
-		case RGBLED_MODE_BLINK_SLOW:
-		case RGBLED_MODE_BLINK_NORMAL:
-		case RGBLED_MODE_BLINK_FAST:
-			if(_counter % 2 == 0)
-				set_on(true);
-			else
-				set_on(false);
-			break;
-		case RGBLED_MODE_BREATHE:
-			if (_counter >= 30)
-				_counter = 0;
-			if (_counter <= 15) {
-				set_brightness(((float)_counter)*((float)_counter)/(15.0f*15.0f));
-			} else {
-				set_brightness(((float)(30-_counter))*((float)(30-_counter))/(15.0f*15.0f));
-			}
-			break;
-		case RGBLED_MODE_PATTERN:
-			/* don't run out of the pattern array and stop if the next frame is 0 */
-			if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
-				_counter = 0;
+	case RGBLED_MODE_BLINK_SLOW:
+	case RGBLED_MODE_BLINK_NORMAL:
+	case RGBLED_MODE_BLINK_FAST:
+		if (_counter >= 2)
+			_counter = 0;
 
-			set_color(_pattern.color[_counter]);
-			_led_interval = _pattern.duration[_counter];
-			break;
-		default:
-			break;
+		send_led_enable(_counter == 0);
+
+		break;
+
+	case RGBLED_MODE_BREATHE:
+
+		if (_counter >= 62)
+			_counter = 0;
+
+		int n;
+
+		if (_counter < 32) {
+			n = _counter;
+
+		} else {
+			n = 62 - _counter;
+		}
+
+		_brightness = n * n / (31.0f * 31.0f);
+		send_led_rgb();
+		break;
+
+	case RGBLED_MODE_PATTERN:
+
+		/* don't run out of the pattern array and stop if the next frame is 0 */
+		if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
+			_counter = 0;
+
+		set_color(_pattern.color[_counter]);
+		send_led_rgb();
+		_led_interval = _pattern.duration[_counter];
+		break;
+
+	default:
+		break;
 	}
 
 	_counter++;
@@ -282,181 +298,231 @@ RGBLED::led()
 	work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval);
 }
 
+/**
+ * Parse color constant and set _r _g _b values
+ */
 void
-RGBLED::set_color(rgbled_color_t color) {
+RGBLED::set_color(rgbled_color_t color)
+{
+	switch (color) {
+	case RGBLED_COLOR_OFF:
+		_r = 0;
+		_g = 0;
+		_b = 0;
+		break;
 
-	_color = color;
+	case RGBLED_COLOR_RED:
+		_r = 255;
+		_g = 0;
+		_b = 0;
+		break;
 
-	switch (color) {
-		case RGBLED_COLOR_OFF:		// off
-			set_rgb(0,0,0);
-			break;
-		case RGBLED_COLOR_RED:		// red
-			set_rgb(255,0,0);
-			break;
-		case RGBLED_COLOR_YELLOW:	// yellow
-			set_rgb(255,70,0);
-			break;
-		case RGBLED_COLOR_PURPLE:	// purple
-			set_rgb(255,0,255);
-			break;
-		case RGBLED_COLOR_GREEN:	// green
-			set_rgb(0,255,0);
-			break;
-		case RGBLED_COLOR_BLUE:		// blue
-			set_rgb(0,0,255);
-			break;
-		case RGBLED_COLOR_WHITE:	// white
-			set_rgb(255,255,255);
-			break;
-		case RGBLED_COLOR_AMBER:	// amber
-			set_rgb(255,20,0);
-			break;
-		case RGBLED_COLOR_DIM_RED:	// red
-			set_rgb(90,0,0);
-			break;
-		case RGBLED_COLOR_DIM_YELLOW:	// yellow
-			set_rgb(80,30,0);
-			break;
-		case RGBLED_COLOR_DIM_PURPLE:	// purple
-			set_rgb(45,0,45);
-			break;
-		case RGBLED_COLOR_DIM_GREEN:	// green
-			set_rgb(0,90,0);
-			break;
-		case RGBLED_COLOR_DIM_BLUE:	// blue
-			set_rgb(0,0,90);
-			break;
-		case RGBLED_COLOR_DIM_WHITE:	// white
-			set_rgb(30,30,30);
-			break;
-		case RGBLED_COLOR_DIM_AMBER:	// amber
-			set_rgb(80,20,0);
-			break;
-		default:
-			warnx("color unknown");
-			break;
+	case RGBLED_COLOR_YELLOW:
+		_r = 255;
+		_g = 200;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_PURPLE:
+		_r = 255;
+		_g = 0;
+		_b = 255;
+		break;
+
+	case RGBLED_COLOR_GREEN:
+		_r = 0;
+		_g = 255;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_BLUE:
+		_r = 0;
+		_g = 0;
+		_b = 255;
+		break;
+
+	case RGBLED_COLOR_WHITE:
+		_r = 255;
+		_g = 255;
+		_b = 255;
+		break;
+
+	case RGBLED_COLOR_AMBER:
+		_r = 255;
+		_g = 80;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_RED:
+		_r = 90;
+		_g = 0;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_YELLOW:
+		_r = 80;
+		_g = 30;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_PURPLE:
+		_r = 45;
+		_g = 0;
+		_b = 45;
+		break;
+
+	case RGBLED_COLOR_DIM_GREEN:
+		_r = 0;
+		_g = 90;
+		_b = 0;
+		break;
+
+	case RGBLED_COLOR_DIM_BLUE:
+		_r = 0;
+		_g = 0;
+		_b = 90;
+		break;
+
+	case RGBLED_COLOR_DIM_WHITE:
+		_r = 30;
+		_g = 30;
+		_b = 30;
+		break;
+
+	case RGBLED_COLOR_DIM_AMBER:
+		_r = 80;
+		_g = 20;
+		_b = 0;
+		break;
+
+	default:
+		warnx("color unknown");
+		break;
 	}
 }
 
+/**
+ * Set mode, if mode not changed has no any effect (doesn't reset blinks phase)
+ */
 void
 RGBLED::set_mode(rgbled_mode_t mode)
 {
-	_mode = mode;
+	if (mode != _mode) {
+		_mode = mode;
+		bool should_run = false;
 
-	switch (mode) {
+		switch (mode) {
 		case RGBLED_MODE_OFF:
-			_should_run = false;
-			set_on(false);
+			send_led_enable(false);
 			break;
+
 		case RGBLED_MODE_ON:
-			_should_run = false;
-			set_on(true);
+			_brightness = 1.0f;
+			send_led_rgb();
+			send_led_enable(true);
 			break;
+
 		case RGBLED_MODE_BLINK_SLOW:
-			_should_run = true;
+			should_run = true;
+			_counter = 0;
 			_led_interval = 2000;
+			_brightness = 1.0f;
+			send_led_rgb();
 			break;
+
 		case RGBLED_MODE_BLINK_NORMAL:
-			_should_run = true;
+			should_run = true;
+			_counter = 0;
 			_led_interval = 500;
+			_brightness = 1.0f;
+			send_led_rgb();
 			break;
+
 		case RGBLED_MODE_BLINK_FAST:
-			_should_run = true;
+			should_run = true;
+			_counter = 0;
 			_led_interval = 100;
+			_brightness = 1.0f;
+			send_led_rgb();
 			break;
+
 		case RGBLED_MODE_BREATHE:
-			_should_run = true;
-			set_on(true);
+			should_run = true;
 			_counter = 0;
-			_led_interval = 1000/15;
+			_led_interval = 25;
+			send_led_enable(true);
 			break;
+
 		case RGBLED_MODE_PATTERN:
-			_should_run = true;
-			set_on(true);
+			should_run = true;
 			_counter = 0;
+			_brightness = 1.0f;
+			send_led_enable(true);
 			break;
+
 		default:
 			warnx("mode unknown");
 			break;
-	}
+		}
 
-	/* if it should run now, start the workq */
-	if (_should_run && !_running) {
-		_running = true;
-		work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
-	}
-	/* if it should stop, then cancel the workq */
-	if (!_should_run && _running) {
-		_running = false;
-		work_cancel(LPWORK, &_work);
+		/* if it should run now, start the workq */
+		if (should_run && !_running) {
+			_running = true;
+			work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
+		}
+
+		/* if it should stop, then cancel the workq */
+		if (!should_run && _running) {
+			_running = false;
+			work_cancel(LPWORK, &_work);
+		}
 	}
 }
 
+/**
+ * Set pattern for PATTERN mode, but don't change current mode
+ */
 void
 RGBLED::set_pattern(rgbled_pattern_t *pattern)
 {
 	memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t));
-
-	set_mode(RGBLED_MODE_PATTERN);
-}
-
-void
-RGBLED::set_brightness(float brightness) {
-
-	_brightness = brightness;
-	set_rgb(_r, _g, _b);
-}
-
-int
-RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b)
-{
-	uint8_t settings_byte = 0;
-
-	if (on)
-		settings_byte |= SETTING_ENABLE;
-/* powersave not used */
-//	if (not_powersave)
-		settings_byte |= SETTING_NOT_POWERSAVE;
-
-	const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte};
-
-	return transfer(msg, sizeof(msg), nullptr, 0);
 }
 
+/**
+ * Sent ENABLE flag to LED driver
+ */
 int
-RGBLED::set_on(bool on)
+RGBLED::send_led_enable(bool enable)
 {
 	uint8_t settings_byte = 0;
 
-	if (on)
+	if (enable)
 		settings_byte |= SETTING_ENABLE;
 
-/* powersave not used */
-//	if (not_powersave)
-		settings_byte |= SETTING_NOT_POWERSAVE;
+	settings_byte |= SETTING_NOT_POWERSAVE;
 
 	const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte};
 
 	return transfer(msg, sizeof(msg), nullptr, 0);
 }
 
+/**
+ * Send RGB PWM settings to LED driver according to current color and brightness
+ */
 int
-RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b)
+RGBLED::send_led_rgb()
 {
-	/* save the RGB values in case we want to change the brightness later */
-	_r = r;
-	_g = g;
-	_b = b;
-
-	const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)((float)b/255.0f*15.0f*_brightness), SUB_ADDR_PWM1, (uint8_t)((float)g/255.0f*15.0f*_brightness), SUB_ADDR_PWM2, (uint8_t)((float)r/255.0f*15.0f*_brightness)};
-
+	/* To scale from 0..255 -> 0..15 shift right by 4 bits */
+	const uint8_t msg[6] = {
+		SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4),
+		SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4),
+		SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4)
+	};
 	return transfer(msg, sizeof(msg), nullptr, 0);
 }
 
-
 int
-RGBLED::get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
+RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
 {
 	uint8_t result[2];
 	int ret;
@@ -465,24 +531,23 @@ RGBLED::get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
 
 	if (ret == OK) {
 		on = result[0] & SETTING_ENABLE;
-		not_powersave = result[0] & SETTING_NOT_POWERSAVE;
+		powersave = !(result[0] & SETTING_NOT_POWERSAVE);
 		/* XXX check, looks wrong */
-		r = (result[0] & 0x0f)*255/15;
-		g = (result[1] & 0xf0)*255/15;
-		b = (result[1] & 0x0f)*255/15;
+		r = (result[0] & 0x0f) << 4;
+		g = (result[1] & 0xf0);
+		b = (result[1] & 0x0f) << 4;
 	}
 
 	return ret;
 }
 
-void rgbled_usage();
-
-
-void rgbled_usage() {
-	warnx("missing command: try 'start', 'test', 'info', 'stop'/'off', 'rgb 30 40 50'");
+void
+rgbled_usage()
+{
+	warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'");
 	warnx("options:");
 	warnx("    -b i2cbus (%d)", PX4_I2C_BUS_LED);
-	errx(0, "    -a addr (0x%x)", ADDR);
+	warnx("    -a addr (0x%x)", ADDR);
 }
 
 int
@@ -492,17 +557,21 @@ rgbled_main(int argc, char *argv[])
 	int rgbledadr = ADDR; /* 7bit */
 
 	int ch;
+
 	/* jump over start/off/etc and look at options first */
-	while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) {
+	while ((ch = getopt(argc - 1, &argv[1], "a:b:")) != EOF) {
 		switch (ch) {
 		case 'a':
 			rgbledadr = strtol(optarg, NULL, 0);
 			break;
+
 		case 'b':
 			i2cdevice = strtol(optarg, NULL, 0);
 			break;
+
 		default:
 			rgbled_usage();
+			exit(0);
 		}
 	}
 
@@ -519,17 +588,21 @@ rgbled_main(int argc, char *argv[])
 			// try the external bus first
 			i2cdevice = PX4_I2C_BUS_EXPANSION;
 			g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr);
+
 			if (g_rgbled != nullptr && OK != g_rgbled->init()) {
 				delete g_rgbled;
 				g_rgbled = nullptr;
 			}
+
 			if (g_rgbled == nullptr) {
 				// fall back to default bus
 				i2cdevice = PX4_I2C_BUS_LED;
 			}
 		}
+
 		if (g_rgbled == nullptr) {
 			g_rgbled = new RGBLED(i2cdevice, rgbledadr);
+
 			if (g_rgbled == nullptr)
 				errx(1, "new failed");
 
@@ -545,21 +618,24 @@ rgbled_main(int argc, char *argv[])
 
 	/* need the driver past this point */
 	if (g_rgbled == nullptr) {
-	    warnx("not started");
-	    rgbled_usage();
-	    exit(0);
+		warnx("not started");
+		rgbled_usage();
+		exit(0);
 	}
 
 	if (!strcmp(verb, "test")) {
 		fd = open(RGBLED_DEVICE_PATH, 0);
+
 		if (fd == -1) {
 			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
 		}
 
-		rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_OFF},
-					     {200,              200,                200,               400             } };
+		rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF},
+			{500, 500, 500, 500, 1000, 0 }	// "0" indicates end of pattern
+		};
 
 		ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern);
+		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN);
 
 		close(fd);
 		exit(ret);
@@ -570,33 +646,39 @@ rgbled_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(verb, "stop") || !strcmp(verb, "off")) {
-		/* although technically it doesn't stop, this is the excepted syntax */
+	if (!strcmp(verb, "off")) {
 		fd = open(RGBLED_DEVICE_PATH, 0);
+
 		if (fd == -1) {
 			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
 		}
+
 		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
 		close(fd);
 		exit(ret);
 	}
 
 	if (!strcmp(verb, "rgb")) {
+		if (argc < 5) {
+			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
+		}
+
 		fd = open(RGBLED_DEVICE_PATH, 0);
+
 		if (fd == -1) {
 			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
 		}
-		if (argc < 5) {
-			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
-		}
+
 		rgbled_rgbset_t v;
-		v.red   = strtol(argv[1], NULL, 0);
-		v.green = strtol(argv[2], NULL, 0);
-		v.blue  = strtol(argv[3], NULL, 0);
+		v.red   = strtol(argv[2], NULL, 0);
+		v.green = strtol(argv[3], NULL, 0);
+		v.blue  = strtol(argv[4], NULL, 0);
 		ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
+		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
 		close(fd);
 		exit(ret);
 	}
 
 	rgbled_usage();
+	exit(0);
 }
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index a582ece17..f36f2091e 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -317,7 +317,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
 
 
 ToneAlarm::ToneAlarm() :
-	CDev("tone_alarm", "/dev/tone_alarm"),
+	CDev("tone_alarm", TONEALARM_DEVICE_PATH),
 	_default_tune_number(0),
 	_user_tune(nullptr),
 	_tune(nullptr),
@@ -333,6 +333,7 @@ ToneAlarm::ToneAlarm() :
 	_default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G";					//arming warning
 	_default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8";					//battery warning slow
 	_default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8";	//battery warning fast
+	_default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#";					//gps warning slow
 
 	_tune_names[TONE_STARTUP_TUNE] = "startup";			// startup tune
 	_tune_names[TONE_ERROR_TUNE] = "error";				// ERROR tone
@@ -342,6 +343,7 @@ ToneAlarm::ToneAlarm() :
 	_tune_names[TONE_ARMING_WARNING_TUNE] = "arming";		// arming warning
 	_tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat";	// battery warning slow
 	_tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat";	// battery warning fast
+	_tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning";	            // gps warning
 }
 
 ToneAlarm::~ToneAlarm()
@@ -820,10 +822,10 @@ play_tune(unsigned tune)
 {
 	int	fd, ret;
 
-	fd = open("/dev/tone_alarm", 0);
+	fd = open(TONEALARM_DEVICE_PATH, 0);
 
 	if (fd < 0)
-		err(1, "/dev/tone_alarm");
+		err(1, TONEALARM_DEVICE_PATH);
 
 	ret = ioctl(fd, TONE_SET_ALARM, tune);
 	close(fd);
@@ -839,10 +841,10 @@ play_string(const char *str, bool free_buffer)
 {
 	int	fd, ret;
 
-	fd = open("/dev/tone_alarm", O_WRONLY);
+	fd = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 
 	if (fd < 0)
-		err(1, "/dev/tone_alarm");
+		err(1, TONEALARM_DEVICE_PATH);
 
 	ret = write(fd, str, strlen(str) + 1);
 	close(fd);
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
new file mode 100644
index 000000000..d876f1a39
--- /dev/null
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -0,0 +1,150 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.cpp
+ * Implementation of a simple orthogonal pitch PID controller.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "ecl_pitch_controller.h"
+#include <math.h>
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+
+ECL_PitchController::ECL_PitchController() :
+	_last_run(0),
+	_last_output(0.0f),
+	_integrator(0.0f),
+	_rate_error(0.0f),
+	_rate_setpoint(0.0f),
+	_max_deflection_rad(math::radians(45.0f))
+{
+}
+
+float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
+				   bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
+{
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+
+	float dt = dt_micros / 1000000;
+
+	/* lock integral for long intervals */
+	if (dt_micros > 500000)
+		lock_integrator = true;
+
+	float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+	float k_i_rate = _k_i * _tc;
+
+	/* input conditioning */
+	if (!isfinite(airspeed)) {
+		/* airspeed is NaN, +- INF or not available, pick center of band */
+		airspeed = 0.5f * (airspeed_min + airspeed_max);
+	} else if (airspeed < airspeed_min) {
+		airspeed = airspeed_min;
+	}
+
+	/* flying inverted (wings upside down) ? */
+	bool inverted = false;
+
+	/* roll is used as feedforward term and inverted flight needs to be considered */
+	if (fabsf(roll) < math::radians(90.0f)) {
+		/* not inverted, but numerically still potentially close to infinity */
+		roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f));
+	} else {
+		/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */
+
+		/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
+		if (roll > 0.0f) {
+			/* right hemisphere */
+			roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f));
+		} else {
+			/* left hemisphere */
+			roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f));
+		}
+	}
+
+	/* calculate the offset in the rate resulting from rolling  */
+	float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
+				tanf(roll) * sinf(roll)) * _roll_ff;
+	if (inverted)
+		turn_offset = -turn_offset;
+
+	float pitch_error = pitch_setpoint - pitch;
+	/* rate setpoint from current error and time constant */
+	_rate_setpoint = pitch_error / _tc;
+
+	/* add turn offset */
+	_rate_setpoint += turn_offset;
+
+	_rate_error = _rate_setpoint - pitch_rate;
+
+	float ilimit_scaled = 0.0f;
+
+	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+		float id = _rate_error * k_i_rate * dt * scaler;
+
+		/*
+		 * anti-windup: do not allow integrator to increase into the
+		 * wrong direction if actuator is at limit
+		 */
+		if (_last_output < -_max_deflection_rad) {
+			/* only allow motion to center: increase value */
+			id = math::max(id, 0.0f);
+		} else if (_last_output > _max_deflection_rad) {
+			/* only allow motion to center: decrease value */
+			id = math::min(id, 0.0f);
+		}
+
+		_integrator += id;
+	}
+
+	/* integrator limit */
+	_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
+	/* store non-limited output */
+	_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler;
+
+	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+}
+
+void ECL_PitchController::reset_integrator()
+{
+	_integrator = 0.0f;
+}
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
new file mode 100644
index 000000000..1e6cec6a1
--- /dev/null
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.h
+ * Definition of a simple orthogonal pitch PID controller.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Acknowledgements:
+ *
+ *   The control design is based on a design
+ *   by Paul Riseborough and Andrew Tridgell, 2013,
+ *   which in turn is based on initial work of
+ *   Jonathan Challinger, 2012.
+ */
+
+#ifndef ECL_PITCH_CONTROLLER_H
+#define ECL_PITCH_CONTROLLER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+class __EXPORT ECL_PitchController
+{
+public:
+	ECL_PitchController();
+
+	float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f,
+		      bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
+
+	void reset_integrator();
+
+	void set_time_constant(float time_constant) {
+		_tc = time_constant;
+	}
+	void set_k_p(float k_p) {
+		_k_p = k_p;
+	}
+	void set_k_i(float k_i) {
+		_k_i = k_i;
+	}
+	void set_k_d(float k_d) {
+		_k_d = k_d;
+	}
+	void set_integrator_max(float max) {
+		_integrator_max = max;
+	}
+	void set_max_rate_pos(float max_rate_pos) {
+		_max_rate_pos = max_rate_pos;
+	}
+	void set_max_rate_neg(float max_rate_neg) {
+		_max_rate_neg = max_rate_neg;
+	}
+	void set_roll_ff(float roll_ff) {
+		_roll_ff = roll_ff;
+	}
+
+	float get_rate_error() {
+		return _rate_error;
+	}
+
+	float get_desired_rate() {
+		return _rate_setpoint;
+	}
+
+private:
+
+	uint64_t _last_run;
+	float _tc;
+	float _k_p;
+	float _k_i;
+	float _k_d;
+	float _integrator_max;
+	float _max_rate_pos;
+	float _max_rate_neg;
+	float _roll_ff;
+	float _last_output;
+	float _integrator;
+	float _rate_error;
+	float _rate_setpoint;
+	float _max_deflection_rad;
+};
+
+#endif // ECL_PITCH_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
new file mode 100644
index 000000000..b9a73fc02
--- /dev/null
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_roll_controller.cpp
+ * Implementation of a simple orthogonal roll PID controller.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "../ecl.h"
+#include "ecl_roll_controller.h"
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+
+ECL_RollController::ECL_RollController() :
+	_last_run(0),
+	_tc(0.1f),
+	_last_output(0.0f),
+	_integrator(0.0f),
+	_rate_error(0.0f),
+	_rate_setpoint(0.0f),
+	_max_deflection_rad(math::radians(45.0f))
+{
+
+}
+
+float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate,
+				  float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
+{
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+
+	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
+
+	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+	float k_i_rate = _k_i * _tc;
+
+	/* input conditioning */
+	if (!isfinite(airspeed)) {
+		/* airspeed is NaN, +- INF or not available, pick center of band */
+		airspeed = 0.5f * (airspeed_min + airspeed_max);
+	} else if (airspeed < airspeed_min) {
+		airspeed = airspeed_min;
+	}
+
+	float roll_error = roll_setpoint - roll;
+	_rate_setpoint = roll_error / _tc;
+
+	/* limit the rate */
+	if (_max_rate > 0.01f) {
+		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+	}
+
+	_rate_error = _rate_setpoint - roll_rate;
+
+
+	float ilimit_scaled = 0.0f;
+
+	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+		float id = _rate_error * k_i_rate * dt * scaler;
+
+		/*
+		 * anti-windup: do not allow integrator to increase into the
+		 * wrong direction if actuator is at limit
+		 */
+		if (_last_output < -_max_deflection_rad) {
+			/* only allow motion to center: increase value */
+			id = math::max(id, 0.0f);
+		} else if (_last_output > _max_deflection_rad) {
+			/* only allow motion to center: decrease value */
+			id = math::min(id, 0.0f);
+		}
+
+		_integrator += id;
+	}
+
+	/* integrator limit */
+	_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
+	/* store non-limited output */
+	_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler;
+
+	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+}
+
+void ECL_RollController::reset_integrator()
+{
+	_integrator = 0.0f;
+}
+
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
new file mode 100644
index 000000000..0d4ea9333
--- /dev/null
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_roll_controller.h
+ * Definition of a simple orthogonal roll PID controller.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Acknowledgements:
+ *
+ *   The control design is based on a design
+ *   by Paul Riseborough and Andrew Tridgell, 2013,
+ *   which in turn is based on initial work of
+ *   Jonathan Challinger, 2012.
+ */
+
+#ifndef ECL_ROLL_CONTROLLER_H
+#define ECL_ROLL_CONTROLLER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+class __EXPORT ECL_RollController
+{
+public:
+	ECL_RollController();
+
+	float control(float roll_setpoint, float roll, float roll_rate,
+		      float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
+
+	void reset_integrator();
+
+	void set_time_constant(float time_constant) {
+		if (time_constant > 0.1f && time_constant < 3.0f) {
+			_tc = time_constant;
+		}
+	}
+	void set_k_p(float k_p) {
+		_k_p = k_p;
+	}
+	void set_k_i(float k_i) {
+		_k_i = k_i;
+	}
+	void set_k_d(float k_d) {
+		_k_d = k_d;
+	}
+	void set_integrator_max(float max) {
+		_integrator_max = max;
+	}
+	void set_max_rate(float max_rate) {
+		_max_rate = max_rate;
+	}
+
+	float get_rate_error() {
+		return _rate_error;
+	}
+
+	float get_desired_rate() {
+		return _rate_setpoint;
+	}
+
+private:
+	uint64_t _last_run;
+	float _tc;
+	float _k_p;
+	float _k_i;
+	float _k_d;
+	float _integrator_max;
+	float _max_rate;
+	float _last_output;
+	float _integrator;
+	float _rate_error;
+	float _rate_setpoint;
+	float _max_deflection_rad;
+};
+
+#endif // ECL_ROLL_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
new file mode 100644
index 000000000..b786acf24
--- /dev/null
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.cpp
+ * Implementation of a simple orthogonal coordinated turn yaw PID controller.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "ecl_yaw_controller.h"
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+
+ECL_YawController::ECL_YawController() :
+	_last_run(0),
+	_last_error(0.0f),
+	_last_output(0.0f),
+	_last_rate_hp_out(0.0f),
+	_last_rate_hp_in(0.0f),
+	_k_d_last(0.0f),
+	_integrator(0.0f)
+{
+
+}
+
+float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator,
+				 float airspeed_min, float airspeed_max, float aspeed)
+{
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+
+	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
+
+	return 0.0f;
+}
+
+void ECL_YawController::reset_integrator()
+{
+	_integrator = 0.0f;
+}
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
new file mode 100644
index 000000000..66b227918
--- /dev/null
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.h
+ * Definition of a simple orthogonal coordinated turn yaw PID controller.
+ *
+ */
+#ifndef ECL_YAW_CONTROLLER_H
+#define ECL_YAW_CONTROLLER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+class __EXPORT ECL_YawController
+{
+public:
+	ECL_YawController();
+
+	float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false,
+		      float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f));
+
+	void reset_integrator();
+
+	void set_k_side(float k_a) {
+		_k_side = k_a;
+	}
+	void set_k_i(float k_i) {
+		_k_i = k_i;
+	}
+	void set_k_d(float k_d) {
+		_k_d = k_d;
+	}
+	void set_k_roll_ff(float k_roll_ff) {
+		_k_roll_ff = k_roll_ff;
+	}
+	void set_integrator_max(float max) {
+		_integrator_max = max;
+	}
+
+private:
+	uint64_t _last_run;
+
+	float _k_side;
+	float _k_i;
+	float _k_d;
+	float _k_roll_ff;
+	float _integrator_max;
+
+	float _last_error;
+	float _last_output;
+	float _last_rate_hp_out;
+	float _last_rate_hp_in;
+	float _k_d_last;
+	float _integrator;
+
+};
+
+#endif // ECL_YAW_CONTROLLER_H
diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h
new file mode 100644
index 000000000..e0f207696
--- /dev/null
+++ b/src/lib/ecl/ecl.h
@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 APL 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 ecl.h
+ * Adapter / shim layer for system calls needed by ECL
+ *
+ */
+
+#include <drivers/drv_hrt.h>
+#include <geo/geo.h>
+
+#define ecl_absolute_time hrt_absolute_time
+#define ecl_elapsed_time hrt_elapsed_time
\ No newline at end of file
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
new file mode 100644
index 000000000..87eb99f16
--- /dev/null
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -0,0 +1,332 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_l1_pos_controller.h
+ * Implementation of L1 position control.
+ * Authors and acknowledgements in header.
+ *
+ */
+
+#include "ecl_l1_pos_controller.h"
+
+float ECL_L1_Pos_Controller::nav_roll()
+{
+	float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
+	ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f);
+	return ret;
+}
+
+float ECL_L1_Pos_Controller::nav_lateral_acceleration_demand()
+{
+	return _lateral_accel;
+}
+
+float ECL_L1_Pos_Controller::nav_bearing()
+{
+	return _wrap_pi(_nav_bearing);
+}
+
+float ECL_L1_Pos_Controller::bearing_error()
+{
+	return _bearing_error;
+}
+
+float ECL_L1_Pos_Controller::target_bearing()
+{
+	return _target_bearing;
+}
+
+float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
+{
+	/* following [2], switching on L1 distance */
+	return math::min(wp_radius, _L1_distance);
+}
+
+bool ECL_L1_Pos_Controller::reached_loiter_target(void)
+{
+	return _circle_mode;
+}
+
+float ECL_L1_Pos_Controller::crosstrack_error(void)
+{
+	return _crosstrack_error;
+}
+
+void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
+				       const math::Vector2f &ground_speed_vector)
+{
+	/* this follows the logic presented in [1] */
+
+	float eta;
+	float xtrack_vel;
+	float ltrack_vel;
+
+	/* get the direction between the last (visited) and next waypoint */
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY());
+
+	/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
+	float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
+
+	/* calculate the L1 length required for the desired period */
+	_L1_distance = _L1_ratio * ground_speed;
+
+	/* calculate vector from A to B */
+	math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
+
+	/*
+	 * check if waypoints are on top of each other. If yes,
+	 * skip A and directly continue to B
+	 */
+	if (vector_AB.length() < 1.0e-6f) {
+		vector_AB = get_local_planar_vector(vector_curr_position, vector_B);
+	}
+
+	vector_AB.normalize();
+
+	/* calculate the vector from waypoint A to the aircraft */
+	math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+
+	/* calculate crosstrack error (output only) */
+	_crosstrack_error = vector_AB % vector_A_to_airplane;
+
+	/*
+	 * If the current position is in a +-135 degree angle behind waypoint A
+	 * and further away from A than the L1 distance, then A becomes the L1 point.
+	 * If the aircraft is already between A and B normal L1 logic is applied.
+	 */
+	float distance_A_to_airplane = vector_A_to_airplane.length();
+	float alongTrackDist = vector_A_to_airplane * vector_AB;
+
+	/* extension from [2] */
+	if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) {
+
+		/* calculate eta to fly to waypoint A */
+
+		/* unit vector from waypoint A to current position */
+		math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+		/* velocity across / orthogonal to line */
+		xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
+		/* velocity along line */
+		ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
+		eta = atan2f(xtrack_vel, ltrack_vel);
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+
+	} else {
+
+		/* calculate eta to fly along the line between A and B */
+
+		/* velocity across / orthogonal to line */
+		xtrack_vel = ground_speed_vector % vector_AB;
+		/* velocity along line */
+		ltrack_vel = ground_speed_vector * vector_AB;
+		/* calculate eta2 (angle of velocity vector relative to line) */
+		float eta2 = atan2f(xtrack_vel, ltrack_vel);
+		/* calculate eta1 (angle to L1 point) */
+		float xtrackErr = vector_A_to_airplane % vector_AB;
+		float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f);
+		/* limit output to 45 degrees */
+		sine_eta1 = math::constrain(sine_eta1, -M_PI_F / 4.0f, +M_PI_F / 4.0f);
+		float eta1 = asinf(sine_eta1);
+		eta = eta1 + eta2;
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1;
+	}
+
+	/* limit angle to +-90 degrees */
+	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
+	_lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
+
+	/* flying to waypoints, not circling them */
+	_circle_mode = false;
+
+	/* the bearing angle, in NED frame */
+	_bearing_error = eta;
+}
+
+void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
+				       const math::Vector2f &ground_speed_vector)
+{
+	/* the complete guidance logic in this section was proposed by [2] */
+
+	/* calculate the gains for the PD loop (circle tracking) */
+	float omega = (2.0f * M_PI_F / _L1_period);
+	float K_crosstrack = omega * omega;
+	float K_velocity = 2.0f * _L1_damping * omega;
+
+	/* update bearing to next waypoint */
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY());
+
+	/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
+	float ground_speed = math::max(ground_speed_vector.length() , 0.1f);
+
+	/* calculate the L1 length required for the desired period */
+	_L1_distance = _L1_ratio * ground_speed;
+
+	/* calculate the vector from waypoint A to current position */
+	math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+
+	/* store the normalized vector from waypoint A to current position */
+	math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
+
+	/* calculate eta angle towards the loiter center */
+
+	/* velocity across / orthogonal to line from waypoint to current position */
+	float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector;
+	/* velocity along line from waypoint to current position */
+	float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit);
+	float eta = atan2f(xtrack_vel_center, ltrack_vel_center);
+	/* limit eta to 90 degrees */
+	eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f);
+
+	/* calculate the lateral acceleration to capture the center point */
+	float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
+
+	/* for PD control: Calculate radial position and velocity errors */
+
+	/* radial velocity error */
+	float xtrack_vel_circle = -ltrack_vel_center;
+	/* radial distance from the loiter circle (not center) */
+	float xtrack_err_circle = vector_A_to_airplane.length() - radius;
+
+	/* cross track error for feedback */
+	_crosstrack_error = xtrack_err_circle;
+
+	/* calculate PD update to circle waypoint */
+	float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity);
+
+	/* calculate velocity on circle / along tangent */
+	float tangent_vel = xtrack_vel_center * loiter_direction;
+
+	/* prevent PD output from turning the wrong way */
+	if (tangent_vel < 0.0f) {
+		lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd , 0.0f);
+	}
+
+	/* calculate centripetal acceleration setpoint */
+	float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius) , (radius + xtrack_err_circle));
+
+	/* add PD control on circle and centripetal acceleration for total circle command */
+	float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal);
+
+	/*
+	 * Switch between circle (loiter) and capture (towards waypoint center) mode when
+	 * the commands switch over. Only fly towards waypoint if outside the circle.
+	 */
+
+	// XXX check switch over
+	if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) |
+		(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) {
+		_lateral_accel = lateral_accel_sp_center;
+		_circle_mode = false;
+		/* angle between requested and current velocity vector */
+		_bearing_error = eta;
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+
+	} else {
+		_lateral_accel = lateral_accel_sp_circle;
+		_circle_mode = true;
+		_bearing_error = 0.0f;
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+	}
+}
+
+
+void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector)
+{
+	/* the complete guidance logic in this section was proposed by [2] */
+
+	float eta;
+
+	/* 
+	 * As the commanded heading is the only reference
+	 * (and no crosstrack correction occurs),
+	 * target and navigation bearing become the same
+	 */
+	_target_bearing = _nav_bearing = _wrap_pi(navigation_heading);
+	eta = _target_bearing - _wrap_pi(current_heading);
+	eta = _wrap_pi(eta);
+
+	/* consequently the bearing error is exactly eta: */
+	_bearing_error = eta; 
+
+	/* ground speed is the length of the ground speed vector */
+	float ground_speed = ground_speed_vector.length();
+
+	/* adjust L1 distance to keep constant frequency */
+	_L1_distance = ground_speed / _heading_omega;
+	float omega_vel = ground_speed * _heading_omega;
+
+	/* not circling a waypoint */
+	_circle_mode = false;
+
+	/* navigating heading means by definition no crosstrack error */
+	_crosstrack_error = 0;
+
+	/* limit eta to 90 degrees */
+	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
+	_lateral_accel = 2.0f * sinf(eta) * omega_vel;
+}
+
+void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
+{
+	/* the logic in this section is trivial, but originally proposed by [2] */
+
+	/* reset all heading / error measures resulting in zero roll */
+	_target_bearing = current_heading;
+	_nav_bearing = current_heading;
+	_bearing_error = 0;
+	_crosstrack_error = 0;
+	_lateral_accel = 0;
+
+	/* not circling a waypoint when flying level */
+	_circle_mode = false;
+
+}
+
+
+math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const
+{
+	/* this is an approximation for small angles, proposed by [2] */
+
+	math::Vector2f out;
+
+	out.setX(math::radians((target.getX() - origin.getX())));
+	out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX()))));
+
+	return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH);
+}
+
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h
new file mode 100644
index 000000000..a6a2c0156
--- /dev/null
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h
@@ -0,0 +1,249 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_l1_pos_control.h
+ * Implementation of L1 position control.
+ *
+ *
+ * Acknowledgements and References:
+ *
+ *    This implementation has been built for PX4 based on the original
+ *    publication from [1] and does include a lot of the ideas (not code)
+ *    from [2].
+ *
+ *
+ *    [1] S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
+ *    Proceedings of the AIAA Guidance, Navigation and Control
+ *    Conference, Aug 2004. AIAA-2004-4900.
+ *
+ *    [2] Paul Riseborough, Brandon Jones and Andrew Tridgell, L1 control for APM. Aug 2013.
+ *     - Explicit control over frequency and damping
+ *     - Explicit control over track capture angle
+ *     - Ability to use loiter radius smaller than L1 length
+ *     - Modified to use PD control for circle tracking to enable loiter radius less than L1 length
+ *     - Modified to enable period and damping of guidance loop to be set explicitly
+ *     - Modified to provide explicit control over capture angle
+ *
+ */
+
+#ifndef ECL_L1_POS_CONTROLLER_H
+#define ECL_L1_POS_CONTROLLER_H
+
+#include <mathlib/mathlib.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+
+/**
+ * L1 Nonlinear Guidance Logic
+ */
+class __EXPORT ECL_L1_Pos_Controller
+{
+public:
+	ECL_L1_Pos_Controller() {
+		_L1_period = 25;
+		_L1_damping = 0.75f;
+	}
+
+	/**
+	 * The current target bearing
+	 *
+	 * @return bearing angle (-pi..pi, in NED frame)
+	 */
+	float nav_bearing();
+
+
+	/**
+	 * Get lateral acceleration demand.
+	 *
+	 * @return Lateral acceleration in m/s^2
+	 */
+	float nav_lateral_acceleration_demand();
+
+
+	/**
+	 * Heading error.
+	 *
+	 * The heading error is either compared to the current track
+	 * or to the tangent of the current loiter radius.
+	 */
+	float bearing_error();
+
+
+	/**
+	 * Bearing from aircraft to current target.
+	 *
+	 * @return bearing angle (-pi..pi, in NED frame)
+	 */
+	float target_bearing();
+
+
+	/**
+	 * Get roll angle setpoint for fixed wing.
+	 *
+	 * @return Roll angle (in NED frame)
+	 */
+	float nav_roll();
+
+
+	/**
+	 * Get the current crosstrack error.
+	 *
+	 * @return Crosstrack error in meters.
+	 */
+	float crosstrack_error();
+
+
+	/**
+	 * Returns true if the loiter waypoint has been reached
+	 */
+	bool reached_loiter_target();
+
+
+	/**
+	 * Get the switch distance
+	 * 
+	 * This is the distance at which the system will
+	 * switch to the next waypoint. This depends on the
+	 * period and damping
+	 *
+	 * @param waypoint_switch_radius The switching radius the waypoint has set.
+	 */
+	float switch_distance(float waypoint_switch_radius);
+
+
+	/**
+	 * Navigate between two waypoints
+	 *
+	 * Calling this function with two waypoints results in the
+	 * control outputs to fly to the line segment defined by
+	 * the points and once captured following the line segment.
+	 * This follows the logic in [1].
+	 *
+	 * @return sets _lateral_accel setpoint
+	 */
+	void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
+			   const math::Vector2f &ground_speed);
+
+
+	/**
+	 * Navigate on an orbit around a loiter waypoint.
+	 *
+	 * This allow orbits smaller than the L1 length,
+	 * this modification was introduced in [2].
+	 *
+	 * @return sets _lateral_accel setpoint
+	 */
+	void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
+			   const math::Vector2f &ground_speed_vector);
+
+
+	/**
+	 * Navigate on a fixed bearing.
+	 *
+	 * This only holds a certain direction and does not perform cross
+	 * track correction. Helpful for semi-autonomous modes. Introduced
+	 * by [2].
+	 *
+	 * @return sets _lateral_accel setpoint
+	 */
+	void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed);
+
+
+	/**
+	 * Keep the wings level.
+	 *
+	 * This is typically needed for maximum-lift-demand situations,
+	 * such as takeoff or near stall. Introduced in [2].
+	 */
+	void navigate_level_flight(float current_heading);
+
+
+	/**
+	 * Set the L1 period.
+	 */
+	void set_l1_period(float period) {
+		_L1_period = period;
+		/* calculate the ratio introduced in [2] */
+		_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
+		/* calculate normalized frequency for heading tracking */
+		_heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period;
+	}
+
+
+	/**
+	 * Set the L1 damping factor.
+	 *
+	 * The original publication recommends a default of sqrt(2) / 2 = 0.707
+	 */
+	void set_l1_damping(float damping) {
+		_L1_damping = damping;
+		/* calculate the ratio introduced in [2] */
+		_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
+		/* calculate the L1 gain (following [2]) */
+		_K_L1 = 4.0f * _L1_damping * _L1_damping;
+	}
+
+private:
+
+	float _lateral_accel;		///< Lateral acceleration setpoint in m/s^2
+	float _L1_distance;		///< L1 lead distance, defined by period and damping
+	bool _circle_mode;		///< flag for loiter mode
+	float _nav_bearing;		///< bearing to L1 reference point
+	float _bearing_error;		///< bearing error
+	float _crosstrack_error;	///< crosstrack error in meters
+	float _target_bearing;		///< the heading setpoint
+
+	float _L1_period;		///< L1 tracking period in seconds
+	float _L1_damping;		///< L1 damping ratio
+	float _L1_ratio;		///< L1 ratio for navigation
+	float _K_L1;			///< L1 control gain for _L1_damping
+	float _heading_omega;		///< Normalized frequency
+
+	/**
+	 * Convert a 2D vector from WGS84 to planar coordinates.
+	 *
+	 * This converts from latitude and longitude to planar
+	 * coordinates with (0,0) being at the position of ref and
+	 * returns a vector in meters towards wp.
+	 *
+	 * @param ref The reference position in WGS84 coordinates
+	 * @param wp The point to convert to into the local coordinates, in WGS84 coordinates
+	 * @return The vector in meters pointing from the reference position to the coordinates
+	 */
+	math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const;
+
+};
+
+
+#endif /* ECL_L1_POS_CONTROLLER_H */
diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk
new file mode 100644
index 000000000..f2aa3db6a
--- /dev/null
+++ b/src/lib/ecl/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2013 Estimation and Control Library (ECL). 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.
+#
+############################################################################
+
+#
+# Estimation and Control Library
+#
+
+SRCS		 = 	attitude_fw/ecl_pitch_controller.cpp \
+			attitude_fw/ecl_roll_controller.cpp \
+			attitude_fw/ecl_yaw_controller.cpp \
+			l1/ecl_l1_pos_controller.cpp
diff --git a/src/lib/external_lgpl/module.mk b/src/lib/external_lgpl/module.mk
new file mode 100644
index 000000000..53f1629e3
--- /dev/null
+++ b/src/lib/external_lgpl/module.mk
@@ -0,0 +1,48 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+#  W A R N I N G: The contents of this directory are license-incompatible
+# 			with the rest of the codebase. Do NOT copy any parts of it
+#			into other folders.
+#
+#  Acknowledgements:
+#
+#			The algorithms in this folder have been developed by Paul Riseborough
+#			with support from Andrew Tridgell.
+#			Originally licensed as LGPL for APM. As this is built as library and
+#			linked, use of this code does not change the usability of PX4 in general
+#			or any of the license implications.
+#
+
+SRCS		 = 	tecs/tecs.cpp
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
new file mode 100644
index 000000000..864a9d24d
--- /dev/null
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -0,0 +1,534 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+#include "tecs.h"
+#include <ecl/ecl.h>
+
+using namespace math;
+
+#ifndef CONSTANTS_ONE_G
+#define CONSTANTS_ONE_G GRAVITY
+#endif
+
+/**
+ * @file tecs.cpp
+ *
+ * @author Paul Riseborough
+ *
+ *  Written by Paul Riseborough 2013 to provide:
+ *  - Combined control of speed and height using throttle to control
+ *    total energy and pitch angle to control exchange of energy between
+ *    potential and kinetic.
+ *    Selectable speed or height priority modes when calculating pitch angle
+ *  - Fallback mode when no airspeed measurement is available that
+ *    sets throttle based on height rate demand and switches pitch angle control to
+ *    height priority
+ *  - Underspeed protection that demands maximum throttle and switches pitch angle control
+ *    to speed priority mode
+ *  - Relative ease of tuning through use of intuitive time constant, integrator and damping gains and the use
+ *    of easy to measure aircraft performance data
+ *
+ */
+
+void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth)
+{
+	// Implement third order complementary filter for height and height rate
+	// estimted height rate = _integ2_state
+	// estimated height     = _integ3_state
+	// Reference Paper :
+	// Optimising the Gains of the Baro-Inertial Vertical Channel
+	// Widnall W.S, Sinha P.K,
+	// AIAA Journal of Guidance and Control, 78-1307R
+
+	// Calculate time in seconds since last update
+	uint64_t now = ecl_absolute_time();
+	float DT = max((now - _update_50hz_last_usec), 0ULL) * 1.0e-6f;
+
+	// printf("dt: %10.6f baro alt: %6.2f eas: %6.2f R(0,0): %6.2f, R(1,1): %6.2f\naccel body: %6.2f %6.2f %6.2f\naccel earth: %6.2f %6.2f %6.2f\n",
+	// 	DT, baro_altitude, airspeed, rotMat(0, 0), rotMat(1, 1), accel_body(0), accel_body(1), accel_body(2),
+	// 	accel_earth(0), accel_earth(1), accel_earth(2));
+
+	if (DT > 1.0f) {
+		_integ3_state = baro_altitude;
+		_integ2_state = 0.0f;
+		_integ1_state = 0.0f;
+		DT            = 0.02f; // when first starting TECS, use a
+		// small time constant
+	}
+
+	_update_50hz_last_usec = now;
+	_EAS = airspeed;
+
+	// Get height acceleration
+	float hgt_ddot_mea = -(accel_earth(2) + CONSTANTS_ONE_G);
+	// Perform filter calculation using backwards Euler integration
+	// Coefficients selected to place all three filter poles at omega
+	float omega2 = _hgtCompFiltOmega * _hgtCompFiltOmega;
+	float hgt_err = baro_altitude - _integ3_state;
+	float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega;
+	_integ1_state = _integ1_state + integ1_input * DT;
+	float integ2_input = _integ1_state + hgt_ddot_mea + hgt_err * omega2 * 3.0f;
+	_integ2_state = _integ2_state + integ2_input * DT;
+	float integ3_input = _integ2_state + hgt_err * _hgtCompFiltOmega * 3.0f;
+
+	// If more than 1 second has elapsed since last update then reset the integrator state
+	// to the measured height
+	if (DT > 1.0f) {
+		_integ3_state = baro_altitude;
+
+	} else {
+		_integ3_state = _integ3_state + integ3_input * DT;
+	}
+
+	// Update and average speed rate of change
+	// Only required if airspeed is being measured and controlled
+	float temp = 0;
+
+	if (isfinite(airspeed) && airspeed_sensor_enabled()) {
+		// Get DCM
+		// Calculate speed rate of change
+		// XXX check
+		temp = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0);
+		// take 5 point moving average
+		//_vel_dot = _vdot_filter.apply(temp);
+		// XXX resolve this properly
+		_vel_dot = 0.9f * _vel_dot + 0.1f * temp;
+
+	} else {
+		_vel_dot = 0.0f;
+	}
+
+}
+
+void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
+			 float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS)
+{
+	// Calculate time in seconds since last update
+	uint64_t now = ecl_absolute_time();
+	float DT = max((now - _update_speed_last_usec), 0ULL) * 1.0e-6f;
+	_update_speed_last_usec = now;
+
+	// Convert equivalent airspeeds to true airspeeds
+
+	_EAS_dem = airspeed_demand;
+	_TAS_dem  = _EAS_dem * EAS2TAS;
+	_TASmax   = indicated_airspeed_max * EAS2TAS;
+	_TASmin   = indicated_airspeed_min * EAS2TAS;
+
+	// Reset states of time since last update is too large
+	if (DT > 1.0f) {
+		_integ5_state = (_EAS * EAS2TAS);
+		_integ4_state = 0.0f;
+		DT            = 0.1f; // when first starting TECS, use a
+		// small time constant
+	}
+
+	// Get airspeed or default to halfway between min and max if
+	// airspeed is not being used and set speed rate to zero
+	if (!isfinite(indicated_airspeed) || !airspeed_sensor_enabled()) {
+		// If no airspeed available use average of min and max
+		_EAS = 0.5f * (indicated_airspeed_min + indicated_airspeed_max);
+
+	} else {
+		_EAS = indicated_airspeed;
+	}
+
+	// Implement a second order complementary filter to obtain a
+	// smoothed airspeed estimate
+	// airspeed estimate is held in _integ5_state
+	float aspdErr = (_EAS * EAS2TAS) - _integ5_state;
+	float integ4_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega;
+
+	// Prevent state from winding up
+	if (_integ5_state < 3.1f) {
+		integ4_input = max(integ4_input , 0.0f);
+	}
+
+	_integ4_state = _integ4_state + integ4_input * DT;
+	float integ5_input = _integ4_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f;
+	_integ5_state = _integ5_state + integ5_input * DT;
+	// limit the airspeed to a minimum of 3 m/s
+	_integ5_state = max(_integ5_state, 3.0f);
+
+}
+
+void TECS::_update_speed_demand(void)
+{
+	// Set the airspeed demand to the minimum value if an underspeed condition exists
+	// or a bad descent condition exists
+	// This will minimise the rate of descent resulting from an engine failure,
+	// enable the maximum climb rate to be achieved and prevent continued full power descent
+	// into the ground due to an unachievable airspeed value
+	if ((_badDescent) || (_underspeed)) {
+		_TAS_dem     = _TASmin;
+	}
+
+	// Constrain speed demand
+	_TAS_dem = constrain(_TAS_dem, _TASmin, _TASmax);
+
+	// calculate velocity rate limits based on physical performance limits
+	// provision to use a different rate limit if bad descent or underspeed condition exists
+	// Use 50% of maximum energy rate to allow margin for total energy contgroller
+	float velRateMax;
+	float velRateMin;
+
+	if ((_badDescent) || (_underspeed)) {
+		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+
+	} else {
+		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+	}
+
+	// Apply rate limit
+	if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
+		_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
+		_TAS_rate_dem = velRateMax;
+
+	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
+		_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
+		_TAS_rate_dem = velRateMin;
+
+	} else {
+		_TAS_dem_adj = _TAS_dem;
+		_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
+	}
+
+	// Constrain speed demand again to protect against bad values on initialisation.
+	_TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax);
+	_TAS_dem_last = _TAS_dem;
+}
+
+void TECS::_update_height_demand(float demand)
+{
+	// Apply 2 point moving average to demanded height
+	// This is required because height demand is only updated at 5Hz
+	_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
+	_hgt_dem_in_old = _hgt_dem;
+
+	// printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
+	// 	_maxClimbRate);
+
+	// Limit height rate of change
+	if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
+		_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
+
+	} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
+		_hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
+	}
+
+	_hgt_dem_prev = _hgt_dem;
+
+	// Apply first order lag to height demand
+	_hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
+	_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
+	_hgt_dem_adj_last = _hgt_dem_adj;
+
+	// printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
+	// 	_hgt_rate_dem);
+}
+
+void TECS::_detect_underspeed(void)
+{
+	if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) {
+		_underspeed = true;
+
+	} else {
+		_underspeed = false;
+	}
+}
+
+void TECS::_update_energies(void)
+{
+	// Calculate specific energy demands
+	_SPE_dem = _hgt_dem_adj * CONSTANTS_ONE_G;
+	_SKE_dem = 0.5f * _TAS_dem_adj * _TAS_dem_adj;
+
+	// Calculate specific energy rate demands
+	_SPEdot_dem = _hgt_rate_dem * CONSTANTS_ONE_G;
+	_SKEdot_dem = _integ5_state * _TAS_rate_dem;
+
+	// Calculate specific energy
+	_SPE_est = _integ3_state * CONSTANTS_ONE_G;
+	_SKE_est = 0.5f * _integ5_state * _integ5_state;
+
+	// Calculate specific energy rate
+	_SPEdot = _integ2_state * CONSTANTS_ONE_G;
+	_SKEdot = _integ5_state * _vel_dot;
+}
+
+void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
+{
+	// Calculate total energy values
+	_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
+	float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
+	float STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
+
+	// Apply 0.5 second first order filter to STEdot_error
+	// This is required to remove accelerometer noise from the  measurement
+	STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast;
+	_STEdotErrLast = STEdot_error;
+
+	// Calculate throttle demand
+	// If underspeed condition is set, then demand full throttle
+	if (_underspeed) {
+		_throttle_dem_unc = 1.0f;
+
+	} else {
+		// Calculate gain scaler from specific energy error to throttle
+		float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min));
+
+		// Calculate feed-forward throttle
+		float ff_throttle = 0;
+		float nomThr = throttle_cruise;
+		// Use the demanded rate of change of total energy as the feed-forward demand, but add
+		// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
+		// drag increase during turns.
+		float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1)));
+		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
+
+		if (STEdot_dem >= 0) {
+			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
+
+		} else {
+			ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
+		}
+
+		// Calculate PD + FF throttle
+		_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
+
+		// Rate limit PD + FF throttle
+		// Calculate the throttle increment from the specified slew time
+		if (fabsf(_throttle_slewrate) < 0.01f) {
+			float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
+
+			_throttle_dem = constrain(_throttle_dem,
+						  _last_throttle_dem - thrRateIncr,
+						  _last_throttle_dem + thrRateIncr);
+			_last_throttle_dem = _throttle_dem;
+		}
+
+
+		// Calculate integrator state upper and lower limits
+		// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
+		float integ_max = (_THRmaxf - _throttle_dem + 0.1f);
+		float integ_min = (_THRminf - _throttle_dem - 0.1f);
+
+		// Calculate integrator state, constraining state
+		// Set integrator to a max throttle value dduring climbout
+		_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
+
+		if (_climbOutDem) {
+			_integ6_state = integ_max;
+
+		} else {
+			_integ6_state = constrain(_integ6_state, integ_min, integ_max);
+		}
+
+		// Sum the components.
+		// Only use feed-forward component if airspeed is not being used
+		if (airspeed_sensor_enabled()) {
+			_throttle_dem = _throttle_dem + _integ6_state;
+
+		} else {
+			_throttle_dem = ff_throttle;
+		}
+	}
+
+	// Constrain throttle demand
+	_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
+}
+
+void TECS::_detect_bad_descent(void)
+{
+	// Detect a demanded airspeed too high for the aircraft to achieve. This will be
+	// evident by the the following conditions:
+	// 1) Underspeed protection not active
+	// 2) Specific total energy error > 200 (greater than ~20m height error)
+	// 3) Specific total energy reducing
+	// 4) throttle demand > 90%
+	// If these four conditions exist simultaneously, then the protection
+	// mode will be activated.
+	// Once active, the following condition are required to stay in the mode
+	// 1) Underspeed protection not active
+	// 2) Specific total energy error > 0
+	// This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set
+	float STEdot = _SPEdot + _SKEdot;
+
+	if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
+		_badDescent = true;
+
+	} else {
+		_badDescent = false;
+	}
+}
+
+void TECS::_update_pitch(void)
+{
+	// Calculate Speed/Height Control Weighting
+	// This is used to determine how the pitch control prioritises speed and height control
+	// A weighting of 1 provides equal priority (this is the normal mode of operation)
+	// A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available
+	// A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected
+	// or during takeoff/climbout where a minimum pitch angle is set to ensure height is gained. In this instance, if airspeed
+	// rises above the demanded value, the pitch angle will be increased by the TECS controller.
+	float SKE_weighting = constrain(_spdWeight, 0.0f, 2.0f);
+
+	if ((_underspeed || _climbOutDem) && airspeed_sensor_enabled()) {
+		SKE_weighting = 2.0f;
+
+	} else if (!airspeed_sensor_enabled()) {
+		SKE_weighting = 0.0f;
+	}
+
+	float SPE_weighting = 2.0f - SKE_weighting;
+
+	// Calculate Specific Energy Balance demand, and error
+	float SEB_dem      = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
+	float SEBdot_dem   = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
+	float SEB_error    = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
+	float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
+
+	// Calculate integrator state, constraining input if pitch limits are exceeded
+	float integ7_input = SEB_error * _integGain;
+
+	if (_pitch_dem_unc > _PITCHmaxf) {
+		integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc);
+
+	} else if (_pitch_dem_unc < _PITCHminf) {
+		integ7_input = max(integ7_input, _PITCHminf - _pitch_dem_unc);
+	}
+
+	_integ7_state = _integ7_state + integ7_input * _DT;
+
+	// Apply max and min values for integrator state that will allow for no more than
+	// 5deg of saturation. This allows for some pitch variation due to gusts before the
+	// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
+	float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
+	float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
+	_integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
+
+	// Calculate pitch demand from specific energy balance signals
+	_pitch_dem_unc = (temp + _integ7_state) / gainInv;
+
+	// Constrain pitch demand
+	_pitch_dem = constrain(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
+
+	// Rate limit the pitch demand to comply with specified vertical
+	// acceleration limit
+	float ptchRateIncr = _DT * _vertAccLim / _integ5_state;
+
+	if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr) {
+		_pitch_dem = _last_pitch_dem + ptchRateIncr;
+
+	} else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr) {
+		_pitch_dem = _last_pitch_dem - ptchRateIncr;
+	}
+
+	_last_pitch_dem = _pitch_dem;
+}
+
+void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad)
+{
+	// Initialise states and variables if DT > 1 second or in climbout
+	if (_DT > 1.0f) {
+		_integ6_state      = 0.0f;
+		_integ7_state      = 0.0f;
+		_last_throttle_dem = throttle_cruise;
+		_last_pitch_dem    = pitch;
+		_hgt_dem_adj_last  = baro_altitude;
+		_hgt_dem_adj       = _hgt_dem_adj_last;
+		_hgt_dem_prev      = _hgt_dem_adj_last;
+		_hgt_dem_in_old    = _hgt_dem_adj_last;
+		_TAS_dem_last      = _TAS_dem;
+		_TAS_dem_adj       = _TAS_dem;
+		_underspeed        = false;
+		_badDescent        = false;
+		_DT                = 0.1f; // when first starting TECS, use a
+		// small time constant
+
+	} else if (_climbOutDem) {
+		_PITCHminf          = ptchMinCO_rad;
+		_THRminf            = _THRmaxf - 0.01f;
+		_hgt_dem_adj_last  = baro_altitude;
+		_hgt_dem_adj       = _hgt_dem_adj_last;
+		_hgt_dem_prev      = _hgt_dem_adj_last;
+		_TAS_dem_last      = _TAS_dem;
+		_TAS_dem_adj       = _TAS_dem;
+		_underspeed        = false;
+		_badDescent 	   = false;
+	}
+}
+
+void TECS::_update_STE_rate_lim(void)
+{
+	// Calculate Specific Total Energy Rate Limits
+	// This is a tivial calculation at the moment but will get bigger once we start adding altitude effects
+	_STEdot_max = _maxClimbRate * CONSTANTS_ONE_G;
+	_STEdot_min = - _minSinkRate * CONSTANTS_ONE_G;
+}
+
+void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
+				 float throttle_min, float throttle_max, float throttle_cruise,
+				 float pitch_limit_min, float pitch_limit_max)
+{
+	// Calculate time in seconds since last update
+	uint64_t now = ecl_absolute_time();
+	_DT = max((now - _update_pitch_throttle_last_usec), 0ULL) * 1.0e-6f;
+	_update_pitch_throttle_last_usec = now;
+
+	// printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n",
+	// 	_DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max);
+
+	// Update the speed estimate using a 2nd order complementary filter
+	_update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS);
+
+	// Convert inputs
+	_THRmaxf  = throttle_max;
+	_THRminf  = throttle_min;
+	_PITCHmaxf = pitch_limit_max;
+	_PITCHminf = pitch_limit_min;
+	_climbOutDem = climbOutDem;
+
+	// initialise selected states and variables if DT > 1 second or in climbout
+	_initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO);
+
+	// Calculate Specific Total Energy Rate Limits
+	_update_STE_rate_lim();
+
+	// Calculate the speed demand
+	_update_speed_demand();
+
+	// Calculate the height demand
+	_update_height_demand(hgt_dem);
+
+	// Detect underspeed condition
+	_detect_underspeed();
+
+	// Calculate specific energy quantitiues
+	_update_energies();
+
+	// Calculate throttle demand
+	_update_throttle(throttle_cruise, rotMat);
+
+	// Detect bad descent due to demanded airspeed being too high
+	_detect_bad_descent();
+
+	// Calculate pitch demand
+	_update_pitch();
+
+//    // Write internal variables to the log_tuning structure. This
+//    // structure will be logged in dataflash at 10Hz
+	// log_tuning.hgt_dem  = _hgt_dem_adj;
+	// log_tuning.hgt      = _integ3_state;
+	// log_tuning.dhgt_dem = _hgt_rate_dem;
+	// log_tuning.dhgt     = _integ2_state;
+	// log_tuning.spd_dem  = _TAS_dem_adj;
+	// log_tuning.spd      = _integ5_state;
+	// log_tuning.dspd     = _vel_dot;
+	// log_tuning.ithr     = _integ6_state;
+	// log_tuning.iptch    = _integ7_state;
+	// log_tuning.thr      = _throttle_dem;
+	// log_tuning.ptch     = _pitch_dem;
+	// log_tuning.dspd_dem = _TAS_rate_dem;
+}
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
new file mode 100644
index 000000000..2ae6b28bb
--- /dev/null
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -0,0 +1,350 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file    tecs.h
+/// @brief   Combined Total Energy Speed & Height Control.
+
+/*
+ *  Written by Paul Riseborough 2013 to provide:
+ *  - Combined control of speed and height using throttle to control
+ *    total energy and pitch angle to control exchange of energy between
+ *    potential and kinetic.
+ *    Selectable speed or height priority modes when calculating pitch angle
+ *  - Fallback mode when no airspeed measurement is available that
+ *    sets throttle based on height rate demand and switches pitch angle control to
+ *    height priority
+ *  - Underspeed protection that demands maximum throttle switches pitch angle control
+ *    to speed priority mode
+ *  - Relative ease of tuning through use of intuitive time constant, trim rate and damping parameters and the use
+ *    of easy to measure aircraft performance data
+ */
+
+#ifndef TECS_H
+#define TECS_H
+
+#include <mathlib/mathlib.h>
+#include <stdint.h>
+
+class __EXPORT TECS
+{
+public:
+	TECS() :
+
+		_airspeed_enabled(false),
+		_throttle_slewrate(0.0f),
+		_climbOutDem(false),
+		_hgt_dem_prev(0.0f),
+		_hgt_dem_adj_last(0.0f),
+		_hgt_dem_in_old(0.0f),
+		_TAS_dem_last(0.0f),
+		_TAS_dem_adj(0.0f),
+		_TAS_dem(0.0f),
+		_integ1_state(0.0f),
+		_integ2_state(0.0f),
+		_integ3_state(0.0f),
+		_integ4_state(0.0f),
+		_integ5_state(0.0f),
+		_integ6_state(0.0f),
+		_integ7_state(0.0f),
+		_pitch_dem(0.0f),
+		_last_pitch_dem(0.0f),
+		_SPE_dem(0.0f),
+		_SKE_dem(0.0f),
+		_SPEdot_dem(0.0f),
+		_SKEdot_dem(0.0f),
+		_SPE_est(0.0f),
+		_SKE_est(0.0f),
+		_SPEdot(0.0f),
+		_SKEdot(0.0f) {
+
+	}
+
+	bool airspeed_sensor_enabled() {
+		return _airspeed_enabled;
+	}
+
+	void enable_airspeed(bool enabled) {
+		_airspeed_enabled = enabled;
+	}
+
+	// Update of the estimated height and height rate internal state
+	// Update of the inertial speed rate internal state
+	// Should be called at 50Hz or greater
+	void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth);
+
+	// Update the control loop calculations
+	void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
+				   float throttle_min, float throttle_max, float throttle_cruise,
+				   float pitch_limit_min, float pitch_limit_max);
+	// demanded throttle in percentage
+	// should return 0 to 100
+	float get_throttle_demand(void) {
+		return _throttle_dem;
+	}
+	int32_t get_throttle_demand_percent(void) {
+		return get_throttle_demand();
+	}
+
+
+	float get_pitch_demand() { return _pitch_dem; }
+
+	// demanded pitch angle in centi-degrees
+	// should return between -9000 to +9000
+	int32_t get_pitch_demand_cd() { return int32_t(get_pitch_demand() * 5729.5781f);}
+
+	// Rate of change of velocity along X body axis in m/s^2
+	float get_VXdot(void) { return _vel_dot; }
+
+	// log data on internal state of the controller. Called at 10Hz
+	// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
+
+	// struct PACKED log_TECS_Tuning {
+	// 	LOG_PACKET_HEADER;
+	// 	float hgt;
+	// 	float dhgt;
+	// 	float hgt_dem;
+	// 	float dhgt_dem;
+	// 	float spd_dem;
+	// 	float spd;
+	// 	float dspd;
+	// 	float ithr;
+	// 	float iptch;
+	// 	float thr;
+	// 	float ptch;
+	// 	float dspd_dem;
+	// } log_tuning;
+
+	void set_time_const(float time_const) {
+		_timeConst = time_const;
+	}
+
+	void set_min_sink_rate(float rate) {
+		_minSinkRate = rate;
+	}
+
+	void set_max_sink_rate(float sink_rate) {
+		_maxSinkRate = sink_rate;
+	}
+
+	void set_max_climb_rate(float climb_rate) {
+		_maxClimbRate = climb_rate;
+	}
+
+	void set_throttle_damp(float throttle_damp) {
+		_thrDamp = throttle_damp;
+	}
+
+	void set_integrator_gain(float gain) {
+		_integGain = gain;
+	}
+
+	void set_vertical_accel_limit(float limit) {
+		_vertAccLim = limit;
+	}
+
+	void set_height_comp_filter_omega(float omega) {
+		_hgtCompFiltOmega = omega;
+	}
+
+	void set_speed_comp_filter_omega(float omega) {
+		_spdCompFiltOmega = omega;
+	}
+
+	void set_roll_throttle_compensation(float compensation) {
+		_rollComp = compensation;
+	}
+
+	void set_speed_weight(float weight) {
+		_spdWeight = weight;
+	}
+
+	void set_pitch_damping(float damping) {
+		_ptchDamp = damping;
+	}
+
+	void set_throttle_slewrate(float slewrate) {
+		_throttle_slewrate = slewrate;
+	}
+
+	void set_indicated_airspeed_min(float airspeed) {
+		_indicated_airspeed_min = airspeed;
+	}
+
+	void set_indicated_airspeed_max(float airspeed) {
+		_indicated_airspeed_max = airspeed;
+	}
+
+private:
+	// Last time update_50Hz was called
+	uint64_t _update_50hz_last_usec;
+
+	// Last time update_speed was called
+	uint64_t _update_speed_last_usec;
+
+	// Last time update_pitch_throttle was called
+	uint64_t _update_pitch_throttle_last_usec;
+
+	// TECS tuning parameters
+	float _hgtCompFiltOmega;
+	float _spdCompFiltOmega;
+	float _maxClimbRate;
+	float _minSinkRate;
+	float _maxSinkRate;
+	float _timeConst;
+	float _ptchDamp;
+	float _thrDamp;
+	float _integGain;
+	float _vertAccLim;
+	float _rollComp;
+	float _spdWeight;
+
+	// throttle demand in the range from 0.0 to 1.0
+	float _throttle_dem;
+
+	// pitch angle demand in radians
+	float _pitch_dem;
+
+	// Integrator state 1 - height filter second derivative
+	float _integ1_state;
+
+	// Integrator state 2 - height rate
+	float _integ2_state;
+
+	// Integrator state 3 - height
+	float _integ3_state;
+
+	// Integrator state 4 - airspeed filter first derivative
+	float _integ4_state;
+
+	// Integrator state 5 - true airspeed
+	float _integ5_state;
+
+	// Integrator state 6 - throttle integrator
+	float _integ6_state;
+
+	// Integrator state 6 - pitch integrator
+	float _integ7_state;
+
+	// throttle demand rate limiter state
+	float _last_throttle_dem;
+
+	// pitch demand rate limiter state
+	float _last_pitch_dem;
+
+	// Rate of change of speed along X axis
+	float _vel_dot;
+
+	// Equivalent airspeed
+	float _EAS;
+
+	// True airspeed limits
+	float _TASmax;
+	float _TASmin;
+
+	// Current and last true airspeed demand
+	float _TAS_dem;
+	float _TAS_dem_last;
+
+	// Equivalent airspeed demand
+	float _EAS_dem;
+
+	// height demands
+	float _hgt_dem;
+	float _hgt_dem_in_old;
+	float _hgt_dem_adj;
+	float _hgt_dem_adj_last;
+	float _hgt_rate_dem;
+	float _hgt_dem_prev;
+
+	// Speed demand after application of rate limiting
+	// This is the demand tracked by the TECS control loops
+	float _TAS_dem_adj;
+
+	// Speed rate demand after application of rate limiting
+	// This is the demand tracked by the TECS control loops
+	float _TAS_rate_dem;
+
+	// Total energy rate filter state
+	float _STEdotErrLast;
+
+	// Underspeed condition
+	bool _underspeed;
+
+	// Bad descent condition caused by unachievable airspeed demand
+	bool _badDescent;
+
+	// climbout mode
+	bool _climbOutDem;
+
+	// throttle demand before limiting
+	float _throttle_dem_unc;
+
+	// pitch demand before limiting
+	float _pitch_dem_unc;
+
+	// Maximum and minimum specific total energy rate limits
+	float _STEdot_max;
+	float _STEdot_min;
+
+	// Maximum and minimum floating point throttle limits
+	float _THRmaxf;
+	float _THRminf;
+
+	// Maximum and minimum floating point pitch limits
+	float _PITCHmaxf;
+	float _PITCHminf;
+
+	// Specific energy quantities
+	float _SPE_dem;
+	float _SKE_dem;
+	float _SPEdot_dem;
+	float _SKEdot_dem;
+	float _SPE_est;
+	float _SKE_est;
+	float _SPEdot;
+	float _SKEdot;
+
+	// Specific energy error quantities
+	float _STE_error;
+
+	// Time since last update of main TECS loop (seconds)
+	float _DT;
+
+	bool _airspeed_enabled;
+	float _throttle_slewrate;
+	float _indicated_airspeed_min;
+	float _indicated_airspeed_max;
+
+	// Update the airspeed internal state using a second order complementary filter
+	void _update_speed(float airspeed_demand, float indicated_airspeed,
+			   float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS);
+
+	// Update the demanded airspeed
+	void _update_speed_demand(void);
+
+	// Update the demanded height
+	void _update_height_demand(float demand);
+
+	// Detect an underspeed condition
+	void _detect_underspeed(void);
+
+	// Update Specific Energy Quantities
+	void _update_energies(void);
+
+	// Update Demanded Throttle
+	void _update_throttle(float throttle_cruise, const math::Dcm &rotMat);
+
+	// Detect Bad Descent
+	void _detect_bad_descent(void);
+
+	// Update Demanded Pitch Angle
+	void _update_pitch(void);
+
+	// Initialise states and variables
+	void _initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad);
+
+	// Calculate specific total energy rate limits
+	void _update_STE_rate_lim(void);
+
+};
+
+#endif //TECS_H
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 63792dda5..43105fdba 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -210,6 +210,36 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
 	return theta;
 }
 
+__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
+{
+	double lat_now_rad = lat_now * M_DEG_TO_RAD;
+	double lon_now_rad = lon_now * M_DEG_TO_RAD;
+	double lat_next_rad = lat_next * M_DEG_TO_RAD;
+	double lon_next_rad = lon_next * M_DEG_TO_RAD;
+
+	double d_lat = lat_next_rad - lat_now_rad;
+	double d_lon = lon_next_rad - lon_now_rad;
+
+	/* conscious mix of double and float trig function to maximize speed and efficiency */
+	*vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
+	*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon);
+}
+
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
+{
+	double lat_now_rad = lat_now * M_DEG_TO_RAD;
+	double lon_now_rad = lon_now * M_DEG_TO_RAD;
+	double lat_next_rad = lat_next * M_DEG_TO_RAD;
+	double lon_next_rad = lon_next * M_DEG_TO_RAD;
+
+	double d_lat = lat_next_rad - lat_now_rad;
+	double d_lon = lon_next_rad - lon_now_rad;
+
+	/* conscious mix of double and float trig function to maximize speed and efficiency */
+	*vy = CONSTANTS_RADIUS_OF_EARTH * d_lon;
+	*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad);
+}
+
 // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
 
 __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index dadec51ec..123ff80f1 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -57,10 +57,6 @@ __BEGIN_DECLS
 #define CONSTANTS_ABSOLUTE_NULL_CELSIUS			-273.15f		/* °C			*/
 #define CONSTANTS_RADIUS_OF_EARTH			6371000			/* meters (m)		*/
 
-/* compatibility aliases */
-#define RADIUS_OF_EARTH		CONSTANTS_RADIUS_OF_EARTH
-#define GRAVITY_MSS		CONSTANTS_ONE_G
-
 // XXX remove
 struct crosstrack_error_s {
 	bool past_end;		// Flag indicating we are past the end of the line/arc segment
@@ -116,6 +112,10 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
  */
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
+__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
+
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
+
 __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
 
 __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
diff --git a/src/lib/mathlib/math/filter/module.mk b/src/lib/mathlib/math/filter/module.mk
index fe92c8c70..f5c0647c8 100644
--- a/src/lib/mathlib/math/filter/module.mk
+++ b/src/lib/mathlib/math/filter/module.mk
@@ -41,4 +41,3 @@ SRCS		 = LowPassFilter2p.cpp
 # current makefile name, since app.mk needs it.
 #
 APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
--include $(TOPDIR)/.config
diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk
index 2146a1413..72bc7db8a 100644
--- a/src/lib/mathlib/module.mk
+++ b/src/lib/mathlib/module.mk
@@ -49,7 +49,6 @@ SRCS		 = math/test/test.cpp \
 # current makefile name, since app.mk needs it.
 #
 APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
--include $(TOPDIR)/.config
 
 ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy)
 INCLUDE_DIRS	+= math/arm
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 333fe30ae..01b7b84d0 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -5,6 +5,7 @@
  *           Lorenz Meier <lm@inf.ethz.ch>
  *           Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -144,8 +145,8 @@ static int mavlink_fd;
 
 /* flags */
 static bool commander_initialized = false;
-static bool thread_should_exit = false;		/**< daemon exit flag */
-static bool thread_running = false;		/**< daemon status flag */
+static volatile bool thread_should_exit = false;		/**< daemon exit flag */
+static volatile bool thread_running = false;		/**< daemon status flag */
 static int daemon_task;				/**< Handle of daemon task / thread */
 
 static unsigned int leds_counter;
@@ -198,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
  */
 int commander_thread_main(int argc, char *argv[]);
 
-void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
+void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
 
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
@@ -230,7 +231,7 @@ int commander_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			warnx("commander already running\n");
+			warnx("commander already running");
 			/* this is not an error */
 			exit(0);
 		}
@@ -242,21 +243,38 @@ int commander_main(int argc, char *argv[])
 					     3000,
 					     commander_thread_main,
 					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
+
+		while (!thread_running) {
+			usleep(200);
+		}
+
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
+
+		if (!thread_running)
+			errx(0, "commander already stopped");
+
 		thread_should_exit = true;
+
+		while (thread_running) {
+			usleep(200000);
+			warnx(".");
+		}
+
+		warnx("terminated.");
+
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			warnx("\tcommander is running\n");
+			warnx("\tcommander is running");
 			print_status();
 
 		} else {
-			warnx("\tcommander not started\n");
+			warnx("\tcommander not started");
 		}
 
 		exit(0);
@@ -326,6 +344,9 @@ void print_status()
 	warnx("arming: %s", armed_str);
 }
 
+static orb_advert_t control_mode_pub;
+static orb_advert_t status_pub;
+
 void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
 {
 	/* result of the command */
@@ -339,13 +360,23 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			uint8_t base_mode = (uint8_t) cmd->param1;
 			uint8_t custom_main_mode = (uint8_t) cmd->param2;
 
+			/* set HIL state */
+			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
+			hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
+
 			// TODO remove debug code
 			//mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
 			/* set arming state */
 			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
 			if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
-				arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+				if (safety->safety_switch_available && !safety->safety_off) {
+					print_reject_arm("NOT ARMING: Press safety switch first.");
+					arming_res = TRANSITION_DENIED;
+
+				} else {
+					arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+				}
 
 				if (arming_res == TRANSITION_CHANGED) {
 					mavlink_log_info(mavlink_fd, "[cmd] ARMED by command");
@@ -439,8 +470,26 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM:
-		// XXX implement later
-		result = VEHICLE_CMD_RESULT_DENIED;
+	{
+		transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+		if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
+			if (safety->safety_switch_available && !safety->safety_off) {
+				print_reject_arm("NOT ARMING: Press safety switch first.");
+				arming_res = TRANSITION_DENIED;
+
+			} else {
+				arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+			}
+
+			if (arming_res == TRANSITION_CHANGED) {
+				mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+			} else {
+				mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+		}
+	}
 		break;
 
 	default:
@@ -526,7 +575,6 @@ int commander_thread_main(int argc, char *argv[])
 	}
 
 	/* Main state machine */
-	orb_advert_t status_pub;
 	/* make sure we are in preflight state */
 	memset(&status, 0, sizeof(status));
 	status.condition_landed = true;	// initialize to safe value
@@ -538,7 +586,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* flags for control apps */
 	struct vehicle_control_mode_s control_mode;
-	orb_advert_t control_mode_pub;
+
 
 	/* Initialize all flags to false */
 	memset(&control_mode, 0, sizeof(control_mode));
@@ -595,16 +643,20 @@ int commander_thread_main(int argc, char *argv[])
 
 	mavlink_log_info(mavlink_fd, "[cmd] started");
 
+	int ret;
+
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
 	pthread_attr_setstacksize(&commander_low_prio_attr, 2992);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
+
 	/* low priority */
 	param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
 	(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
 	pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
+	pthread_attr_destroy(&commander_low_prio_attr);
 
 	/* Start monitoring loop */
 	unsigned counter = 0;
@@ -617,7 +669,6 @@ int commander_thread_main(int argc, char *argv[])
 	bool critical_battery_voltage_actions_done = false;
 
 	uint64_t last_idle_time = 0;
-
 	uint64_t start_time = 0;
 
 	bool status_changed = true;
@@ -695,7 +746,7 @@ int commander_thread_main(int argc, char *argv[])
 	struct subsystem_info_s info;
 	memset(&info, 0, sizeof(info));
 
-	toggle_status_leds(&status, &armed, true);
+	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
 	commander_initialized = true;
@@ -773,16 +824,6 @@ int commander_thread_main(int argc, char *argv[])
 
 		check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
 
-		orb_check(cmd_sub, &updated);
-
-		if (updated) {
-			/* got command */
-			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
-
-			/* handle it */
-			handle_command(&status, &safety, &control_mode, &cmd, &armed);
-		}
-
 		/* update safety topic */
 		orb_check(safety_sub, &updated);
 
@@ -917,11 +958,9 @@ int commander_thread_main(int argc, char *argv[])
 				battery_tune_played = false;
 
 				if (armed.armed) {
-					// XXX not sure what should happen when voltage is low in flight
-					//arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+					arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
 				} else {
-					// XXX should we still allow to arm with critical battery?
-					//arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
+					arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
 				}
 
 				status_changed = true;
@@ -1056,10 +1095,18 @@ int commander_thread_main(int argc, char *argv[])
 
 				/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
 				if (status.arming_state == ARMING_STATE_STANDBY &&
-				    status.main_state == MAIN_STATE_MANUAL &&
 				    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
 					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						if (safety.safety_switch_available && !safety.safety_off) {
+							print_reject_arm("NOT ARMING: Press safety switch first.");
+
+						} else if (status.main_state != MAIN_STATE_MANUAL) {
+							print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
+
+						} else {
+							res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						}
+
 						stick_on_counter = 0;
 
 					} else {
@@ -1079,15 +1126,8 @@ int commander_thread_main(int argc, char *argv[])
 					}
 
 				} else if (res == TRANSITION_DENIED) {
-					/* DENIED here indicates safety switch not pressed */
-
-					if (!(!safety.safety_switch_available || safety.safety_off)) {
-						print_reject_arm("NOT ARMING: Press safety switch first.");
-
-					} else {
-						warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-						mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-					}
+					warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+					mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
 				}
 
 				/* fill current_status according to mode switches */
@@ -1115,6 +1155,18 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
+
+		/* handle commands last, as the system needs to be updated to handle them */
+		orb_check(cmd_sub, &updated);
+
+		if (updated) {
+			/* got command */
+			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+			/* handle it */
+			handle_command(&status, &safety, &control_mode, &cmd, &armed);
+		}
+
 		/* evaluate the navigation state machine */
 		transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
 
@@ -1132,9 +1184,6 @@ int commander_thread_main(int argc, char *argv[])
 		if (arming_state_changed || main_state_changed || navigation_state_changed) {
 			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
 			status_changed = true;
-
-		} else {
-			status_changed = false;
 		}
 
 		hrt_abstime t1 = hrt_absolute_time();
@@ -1194,13 +1243,31 @@ int commander_thread_main(int argc, char *argv[])
 		fflush(stdout);
 		counter++;
 
-		toggle_status_leds(&status, &armed, arming_state_changed || status_changed);
+		int blink_state = blink_msg_state();
+		if (blink_state > 0) {
+			/* blinking LED message, don't touch LEDs */
+			if (blink_state == 2) {
+				/* blinking LED message completed, restore normal state */
+				control_status_leds(&status, &armed, true);
+			}
+		} else {
+			/* normal state */
+			control_status_leds(&status, &armed, status_changed);
+		}
+
+		status_changed = false;
 
 		usleep(COMMANDER_MONITORING_INTERVAL);
 	}
 
 	/* wait for threads to complete */
-	pthread_join(commander_low_prio_thread, NULL);
+	ret = pthread_join(commander_low_prio_thread, NULL);
+
+	if (ret) {
+		warn("join failed", ret);
+	}
+
+	rgbled_set_mode(RGBLED_MODE_OFF);
 
 	/* close fds */
 	led_deinit();
@@ -1218,9 +1285,6 @@ int commander_thread_main(int argc, char *argv[])
 	close(param_changed_sub);
 	close(battery_sub);
 
-	warnx("exiting");
-	fflush(stdout);
-
 	thread_running = false;
 
 	return 0;
@@ -1239,8 +1303,48 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
 }
 
 void
-toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
+control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
 {
+	/* driving rgbled */
+	if (changed) {
+		bool set_normal_color = false;
+		/* set mode */
+		if (status->arming_state == ARMING_STATE_ARMED) {
+			rgbled_set_mode(RGBLED_MODE_ON);
+			set_normal_color = true;
+
+		} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
+			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
+			rgbled_set_color(RGBLED_COLOR_RED);
+
+		} else if (status->arming_state == ARMING_STATE_STANDBY) {
+			rgbled_set_mode(RGBLED_MODE_BREATHE);
+			set_normal_color = true;
+
+		} else {	// STANDBY_ERROR and other states
+			rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL);
+			rgbled_set_color(RGBLED_COLOR_RED);
+		}
+
+		if (set_normal_color) {
+			/* set color */
+			if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
+				if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
+					rgbled_set_color(RGBLED_COLOR_AMBER);
+				}
+				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
+
+			} else {
+				if (status->condition_local_position_valid) {
+					rgbled_set_color(RGBLED_COLOR_GREEN);
+
+				} else {
+					rgbled_set_color(RGBLED_COLOR_BLUE);
+				}
+			}
+		}
+	}
+
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 
 	/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
@@ -1261,46 +1365,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 
 #endif
 
-	if (changed) {
-		/* XXX TODO blink fast when armed and serious error occurs */
-
-		if (armed->armed) {
-			rgbled_set_mode(RGBLED_MODE_ON);
-		} else if (armed->ready_to_arm) {
-			rgbled_set_mode(RGBLED_MODE_BREATHE);
-		} else {
-			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-		}
-	}
-
-	if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
-		switch (status->battery_warning) {
-			case VEHICLE_BATTERY_WARNING_LOW:
-				rgbled_set_color(RGBLED_COLOR_YELLOW);
-				break;
-			case VEHICLE_BATTERY_WARNING_CRITICAL:
-				rgbled_set_color(RGBLED_COLOR_AMBER);
-				break;
-			default:
-				break;
-		}
-	} else {
-		switch (status->main_state) {
-			case MAIN_STATE_MANUAL:
-				rgbled_set_color(RGBLED_COLOR_WHITE);
-				break;
-			case MAIN_STATE_SEATBELT:
-			case MAIN_STATE_EASY:
-				rgbled_set_color(RGBLED_COLOR_GREEN);
-				break;
-			case MAIN_STATE_AUTO:
-				rgbled_set_color(RGBLED_COLOR_BLUE);
-				break;
-			default:
-				break;
-		}
-	}
-
 	/* give system warnings on error LED, XXX maybe add memory usage warning too */
 	if (status->load > 0.95f) {
 		if (leds_counter % 2 == 0)
@@ -1468,16 +1532,18 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
 					return TRANSITION_NOT_CHANGED;
 				}
 			}
+
 			if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF &&
-				status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
-				status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
-				status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
+			    status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
+			    status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
+			    status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
 				/* possibly on ground, switch to TAKEOFF if needed */
 				if (local_pos->z > -takeoff_alt || status->condition_landed) {
 					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
 					return res;
 				}
 			}
+
 			/* switch to AUTO mode */
 			if (status->rc_signal_found_once && !status->rc_signal_lost) {
 				/* act depending on switches when manual control enabled */
@@ -1495,18 +1561,20 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
 						res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
 					}
 				}
+
 			} else {
 				/* switch to MISSION when no RC control and first time in some AUTO mode  */
 				if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
-					status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
-					status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
-					status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+				    status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
+				    status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
+				    status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
 					res = TRANSITION_NOT_CHANGED;
 
 				} else {
 					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
 				}
 			}
+
 		} else {
 			/* disarmed, always switch to AUTO_READY */
 			res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
@@ -1628,7 +1696,7 @@ void *commander_low_prio_loop(void *arg)
 	while (!thread_should_exit) {
 
 		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
 
 		/* timed out - periodic check for _task_should_exit, etc. */
 		if (pret == 0)
@@ -1733,35 +1801,41 @@ void *commander_low_prio_loop(void *arg)
 
 				if (((int)(cmd.param1)) == 0) {
 					int ret = param_load_default();
+
 					if (ret == OK) {
 						mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 					} else {
 						mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
+
 						/* convenience as many parts of NuttX use negative errno */
 						if (ret < 0)
 							ret = -ret;
 
 						if (ret < 1000)
 							mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
+
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
 
 				} else if (((int)(cmd.param1)) == 1) {
 					int ret = param_save_default();
+
 					if (ret == OK) {
 						mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 					} else {
 						mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+
 						/* convenience as many parts of NuttX use negative errno */
 						if (ret < 0)
 							ret = -ret;
 
 						if (ret < 1000)
 							mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
+
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
 				}
@@ -1785,5 +1859,5 @@ void *commander_low_prio_loop(void *arg)
 
 	close(cmd_sub);
 
-	return 0;
+	return NULL;
 }
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 7feace2b4..565b4b66a 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -3,6 +3,7 @@
  *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -55,7 +56,6 @@
 #include <drivers/drv_led.h>
 #include <drivers/drv_rgbled.h>
 
-
 #include "commander_helper.h"
 
 /* oddly, ERROR is not defined for c++ */
@@ -64,25 +64,28 @@
 #endif
 static const int ERROR = -1;
 
+#define BLINK_MSG_TIME	700000	// 3 fast blinks
+
 bool is_multirotor(const struct vehicle_status_s *current_status)
 {
 	return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_TRICOPTER));
+		(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
+		(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
+		(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
 }
 
 bool is_rotary_wing(const struct vehicle_status_s *current_status)
 {
 	return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
-	|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
+	       || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
 }
 
 static int buzzer;
+static hrt_abstime blink_msg_end;
 
 int buzzer_init()
 {
-	buzzer = open("/dev/tone_alarm", O_WRONLY);
+	buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 
 	if (buzzer < 0) {
 		warnx("Buzzer: open fail\n");
@@ -104,16 +107,25 @@ void tune_error()
 
 void tune_positive()
 {
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color(RGBLED_COLOR_GREEN);
+	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE);
 }
 
 void tune_neutral()
 {
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color(RGBLED_COLOR_WHITE);
+	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
 }
 
 void tune_negative()
 {
+	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
+	rgbled_set_color(RGBLED_COLOR_RED);
+	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
 }
 
@@ -132,18 +144,31 @@ int tune_critical_bat()
 	return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE);
 }
 
-
-
 void tune_stop()
 {
 	ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
 }
 
+int blink_msg_state()
+{
+	if (blink_msg_end == 0) {
+		return 0;
+
+	} else if (hrt_absolute_time() > blink_msg_end) {
+		return 2;
+
+	} else {
+		return 1;
+	}
+}
+
 static int leds;
 static int rgbleds;
 
 int led_init()
 {
+	blink_msg_end = 0;
+
 	/* first open normal LEDs */
 	leds = open(LED_DEVICE_PATH, 0);
 
@@ -159,6 +184,7 @@ int led_init()
 		warnx("Blue LED: ioctl fail\n");
 		return ERROR;
 	}
+
 #endif
 
 	if (ioctl(leds, LED_ON, LED_AMBER)) {
@@ -168,6 +194,7 @@ int led_init()
 
 	/* then try RGB LEDs, this can fail on FMUv1*/
 	rgbleds = open(RGBLED_DEVICE_PATH, 0);
+
 	if (rgbleds == -1) {
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
 		errx(1, "Unable to open " RGBLED_DEVICE_PATH);
@@ -203,19 +230,22 @@ int led_off(int led)
 	return ioctl(leds, LED_OFF, led);
 }
 
-void rgbled_set_color(rgbled_color_t color) {
+void rgbled_set_color(rgbled_color_t color)
+{
 
 	if (rgbleds != -1)
 		ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
 }
 
-void rgbled_set_mode(rgbled_mode_t mode) {
+void rgbled_set_mode(rgbled_mode_t mode)
+{
 
 	if (rgbleds != -1)
 		ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
 }
 
-void rgbled_set_pattern(rgbled_pattern_t *pattern) {
+void rgbled_set_pattern(rgbled_pattern_t *pattern)
+{
 
 	if (rgbleds != -1)
 		ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index 027d0535f..e9514446c 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -62,6 +62,7 @@ int tune_arm(void);
 int tune_low_bat(void);
 int tune_critical_bat(void);
 void tune_stop(void);
+int blink_msg_state();
 
 int led_init(void);
 void led_deinit(void);
@@ -70,9 +71,7 @@ int led_on(int led);
 int led_off(int led);
 
 void rgbled_set_color(rgbled_color_t color);
-
 void rgbled_set_mode(rgbled_mode_t mode);
-
 void rgbled_set_pattern(rgbled_pattern_t *pattern);
 
 /**
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 82a0349c9..369e6da62 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -163,7 +163,9 @@ int do_gyro_calibration(int mavlink_fd)
 		return ERROR;
 	}
 
+	mavlink_log_info(mavlink_fd, "offset calibration done.");
 
+#if 0
 	/*** --- SCALING --- ***/
 
 	mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
@@ -241,9 +243,14 @@ int do_gyro_calibration(int mavlink_fd)
 	}
 
 	float gyro_scale = baseline_integral / gyro_integral;
-	float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale };
+	
 	warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
 	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
+#else
+	float gyro_scales[] = { 1.0f, 1.0f, 1.0f };
+#endif
+
+	
 
 
 	if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
@@ -277,9 +284,6 @@ int do_gyro_calibration(int mavlink_fd)
 			warn("WARNING: auto-save of params to storage failed");
 		}
 
-		// char buf[50];
-		// sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
-		// mavlink_log_info(mavlink_fd, buf);
 		mavlink_log_info(mavlink_fd, "gyro calibration done");
 
 		/* third beep by cal end routine */
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 3cef10185..8ce31550f 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -228,8 +228,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 
 		case MAIN_STATE_SEATBELT:
 
-			/* need altitude estimate */
-			if (current_state->condition_local_altitude_valid) {
+			/* need at minimum altitude estimate */
+			if (current_state->condition_local_altitude_valid ||
+				current_state->condition_global_position_valid) {
 				ret = TRANSITION_CHANGED;
 			}
 
@@ -237,8 +238,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 
 		case MAIN_STATE_EASY:
 
-			/* need local position estimate */
-			if (current_state->condition_local_position_valid) {
+			/* need at minimum local position estimate */
+			if (current_state->condition_local_position_valid ||
+				current_state->condition_global_position_valid) {
 				ret = TRANSITION_CHANGED;
 			}
 
@@ -502,6 +504,8 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 		current_control_mode->timestamp = hrt_absolute_time();
 		orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
 
+		// XXX also set lockdown here
+
 		ret = OK;
 
 	} else {
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
new file mode 100644
index 000000000..6b98003fd
--- /dev/null
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -0,0 +1,770 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	Lorenz Meier
+ *
+ * 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 fw_att_control_main.c
+ * Implementation of a generic attitude controller based on classic orthogonal PIDs.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/pid/pid.h>
+#include <geo/geo.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+
+#include <ecl/attitude_fw/ecl_pitch_controller.h>
+#include <ecl/attitude_fw/ecl_roll_controller.h>
+#include <ecl/attitude_fw/ecl_yaw_controller.h>
+
+/**
+ * Fixedwing attitude control app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]);
+
+class FixedwingAttitudeControl
+{
+public:
+	/**
+	 * Constructor
+	 */
+	FixedwingAttitudeControl();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~FixedwingAttitudeControl();
+
+	/**
+	 * Start the sensors task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+private:
+
+	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	int		_control_task;			/**< task handle for sensor task */
+
+	int		_att_sub;			/**< vehicle attitude subscription */
+	int		_accel_sub;			/**< accelerometer subscription */
+	int		_att_sp_sub;			/**< vehicle attitude setpoint */
+	int		_attitude_sub;			/**< raw rc channels data subscription */
+	int		_airspeed_sub;			/**< airspeed subscription */
+	int		_vcontrol_mode_sub;			/**< vehicle status subscription */
+	int 		_params_sub;			/**< notification of parameter updates */
+	int 		_manual_sub;			/**< notification of manual control updates */
+
+	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
+	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint point */
+	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
+
+	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct accel_report				_accel;			/**< body frame accelerations */
+	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
+	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
+	struct airspeed_s				_airspeed;		/**< airspeed */
+	struct vehicle_control_mode_s			_vcontrol_mode;		/**< vehicle control mode */
+	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
+	bool		_airspeed_valid;		/**< flag if the airspeed measurement is valid */
+
+	struct {
+		float tconst;
+		float p_p;
+		float p_d;
+		float p_i;
+		float p_rmax_pos;
+		float p_rmax_neg;
+		float p_integrator_max;
+		float p_roll_feedforward;
+		float r_p;
+		float r_d;
+		float r_i;
+		float r_integrator_max;
+		float r_rmax;
+		float y_p;
+		float y_i;
+		float y_d;
+		float y_roll_feedforward;
+		float y_integrator_max;
+
+		float airspeed_min;
+		float airspeed_trim;
+		float airspeed_max;
+	}		_parameters;			/**< local copies of interesting parameters */
+
+	struct {
+
+		param_t tconst;
+		param_t p_p;
+		param_t p_d;
+		param_t p_i;
+		param_t p_rmax_pos;
+		param_t p_rmax_neg;
+		param_t p_integrator_max;
+		param_t p_roll_feedforward;
+		param_t r_p;
+		param_t r_d;
+		param_t r_i;
+		param_t r_integrator_max;
+		param_t r_rmax;
+		param_t y_p;
+		param_t y_i;
+		param_t y_d;
+		param_t y_roll_feedforward;
+		param_t y_integrator_max;
+
+		param_t airspeed_min;
+		param_t airspeed_trim;
+		param_t airspeed_max;
+	}		_parameter_handles;		/**< handles for interesting parameters */
+
+
+	ECL_RollController				_roll_ctrl;
+	ECL_PitchController				_pitch_ctrl;
+	ECL_YawController				_yaw_ctrl;
+
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int		parameters_update();
+
+	/**
+	 * Update control outputs
+	 *
+	 */
+	void		control_update();
+
+	/**
+	 * Check for changes in vehicle control mode.
+	 */
+	void		vehicle_control_mode_poll();
+
+	/**
+	 * Check for changes in manual inputs.
+	 */
+	void		vehicle_manual_poll();
+
+
+	/**
+	 * Check for airspeed updates.
+	 */
+	bool		vehicle_airspeed_poll();
+
+	/**
+	 * Check for accel updates.
+	 */
+	void		vehicle_accel_poll();
+
+	/**
+	 * Check for set triplet updates.
+	 */
+	void		vehicle_setpoint_poll();
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * Main sensor collection task.
+	 */
+	void		task_main() __attribute__((noreturn));
+};
+
+namespace att_control
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+FixedwingAttitudeControl	*g_control;
+}
+
+FixedwingAttitudeControl::FixedwingAttitudeControl() :
+
+	_task_should_exit(false),
+	_control_task(-1),
+
+/* subscriptions */
+	_att_sub(-1),
+	_accel_sub(-1),
+	_airspeed_sub(-1),
+	_vcontrol_mode_sub(-1),
+	_params_sub(-1),
+	_manual_sub(-1),
+
+/* publications */
+	_rate_sp_pub(-1),
+	_actuators_0_pub(-1),
+	_attitude_sp_pub(-1),
+
+/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
+/* states */
+	_setpoint_valid(false),
+	_airspeed_valid(false)
+{
+	_parameter_handles.tconst = param_find("FW_ATT_TC");
+	_parameter_handles.p_p = param_find("FW_P_P");
+	_parameter_handles.p_d = param_find("FW_P_D");
+	_parameter_handles.p_i = param_find("FW_P_I");
+	_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
+	_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
+	_parameter_handles.p_integrator_max = param_find("FW_P_integrator_max");
+	_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
+
+	_parameter_handles.r_p = param_find("FW_R_P");
+	_parameter_handles.r_d = param_find("FW_R_D");
+	_parameter_handles.r_i = param_find("FW_R_I");
+	_parameter_handles.r_integrator_max = param_find("FW_R_integrator_max");
+	_parameter_handles.r_rmax = param_find("FW_R_RMAX");
+
+	_parameter_handles.y_p = param_find("FW_Y_P");
+	_parameter_handles.y_i = param_find("FW_Y_I");
+	_parameter_handles.y_d = param_find("FW_Y_D");
+	_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
+	_parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max");
+
+	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
+	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
+	_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
+
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+FixedwingAttitudeControl::~FixedwingAttitudeControl()
+{
+	if (_control_task != -1) {
+
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_control_task);
+				break;
+			}
+		} while (_control_task != -1);
+	}
+
+	att_control::g_control = nullptr;
+}
+
+int
+FixedwingAttitudeControl::parameters_update()
+{
+
+	param_get(_parameter_handles.tconst, &(_parameters.tconst));
+	param_get(_parameter_handles.p_p, &(_parameters.p_p));
+	param_get(_parameter_handles.p_d, &(_parameters.p_d));
+	param_get(_parameter_handles.p_i, &(_parameters.p_i));
+	param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
+	param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
+	param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
+	param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward));
+
+	param_get(_parameter_handles.r_p, &(_parameters.r_p));
+	param_get(_parameter_handles.r_d, &(_parameters.r_d));
+	param_get(_parameter_handles.r_i, &(_parameters.r_i));
+	param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
+	param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));
+
+	param_get(_parameter_handles.y_p, &(_parameters.y_p));
+	param_get(_parameter_handles.y_i, &(_parameters.y_i));
+	param_get(_parameter_handles.y_d, &(_parameters.y_d));
+	param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward));
+	param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
+
+	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
+	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
+	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
+
+	/* pitch control parameters */
+	_pitch_ctrl.set_time_constant(_parameters.tconst);
+	_pitch_ctrl.set_k_p(math::radians(_parameters.p_p));
+	_pitch_ctrl.set_k_i(math::radians(_parameters.p_i));
+	_pitch_ctrl.set_k_d(math::radians(_parameters.p_d));
+	_pitch_ctrl.set_integrator_max(math::radians(_parameters.p_integrator_max));
+	_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
+	_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
+	_pitch_ctrl.set_roll_ff(math::radians(_parameters.p_roll_feedforward));
+
+	/* roll control parameters */
+	_roll_ctrl.set_time_constant(_parameters.tconst);
+	_roll_ctrl.set_k_p(math::radians(_parameters.r_p));
+	_roll_ctrl.set_k_i(math::radians(_parameters.r_i));
+	_roll_ctrl.set_k_d(math::radians(_parameters.r_d));
+	_roll_ctrl.set_integrator_max(math::radians(_parameters.r_integrator_max));
+	_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
+
+	/* yaw control parameters */
+	_yaw_ctrl.set_k_side(math::radians(_parameters.y_p));
+	_yaw_ctrl.set_k_i(math::radians(_parameters.y_i));
+	_yaw_ctrl.set_k_d(math::radians(_parameters.y_d));
+	_yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward));
+	_yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max));
+
+	return OK;
+}
+
+void
+FixedwingAttitudeControl::vehicle_control_mode_poll()
+{
+	bool vcontrol_mode_updated;
+
+	/* Check HIL state if vehicle status has changed */
+	orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
+
+	if (vcontrol_mode_updated) {
+
+		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
+	}
+}
+
+void
+FixedwingAttitudeControl::vehicle_manual_poll()
+{
+	bool manual_updated;
+
+	/* get pilots inputs */
+	orb_check(_manual_sub, &manual_updated);
+
+	if (manual_updated) {
+
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
+	}
+}
+
+bool
+FixedwingAttitudeControl::vehicle_airspeed_poll()
+{
+	/* check if there is a new position */
+	bool airspeed_updated;
+	orb_check(_airspeed_sub, &airspeed_updated);
+
+	if (airspeed_updated) {
+		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+		return true;
+	}
+
+	return false;
+}
+
+void
+FixedwingAttitudeControl::vehicle_accel_poll()
+{
+	/* check if there is a new position */
+	bool accel_updated;
+	orb_check(_accel_sub, &accel_updated);
+
+	if (accel_updated) {
+		orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+	}
+}
+
+void
+FixedwingAttitudeControl::vehicle_setpoint_poll()
+{
+	/* check if there is a new setpoint */
+	bool att_sp_updated;
+	orb_check(_att_sp_sub, &att_sp_updated);
+
+	if (att_sp_updated) {
+		orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
+		_setpoint_valid = true;
+	}
+}
+
+void
+FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
+{
+	att_control::g_control->task_main();
+}
+
+void
+FixedwingAttitudeControl::task_main()
+{
+
+	/* inform about start */
+	warnx("Initializing..");
+	fflush(stdout);
+
+	/*
+	 * do subscriptions
+	 */
+	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+
+	/* rate limit vehicle status updates to 5Hz */
+	orb_set_interval(_vcontrol_mode_sub, 200);
+	orb_set_interval(_att_sub, 100);
+
+	parameters_update();
+
+	/* get an initial update for all sensor and status data */
+	(void)vehicle_airspeed_poll();
+	vehicle_setpoint_poll();
+	vehicle_accel_poll();
+	vehicle_control_mode_poll();
+	vehicle_manual_poll();
+
+	/* wakeup source(s) */
+	struct pollfd fds[2];
+
+	/* Setup of loop */
+	fds[0].fd = _params_sub;
+	fds[0].events = POLLIN;
+	fds[1].fd = _att_sub;
+	fds[1].events = POLLIN;
+
+	while (!_task_should_exit) {
+
+		/* wait for up to 500ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+		/* timed out - periodic check for _task_should_exit, etc. */
+		if (pret == 0)
+			continue;
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			continue;
+		}
+
+		perf_begin(_loop_perf);
+
+		/* only update parameters if they changed */
+		if (fds[0].revents & POLLIN) {
+			/* read from param to clear updated flag */
+			struct parameter_update_s update;
+			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+
+			/* update parameters from storage */
+			parameters_update();
+		}
+
+		/* only run controller if attitude changed */
+		if (fds[1].revents & POLLIN) {
+
+
+			static uint64_t last_run = 0;
+			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
+			last_run = hrt_absolute_time();
+
+			/* guard against too large deltaT's */
+			if (deltaT > 1.0f)
+				deltaT = 0.01f;
+
+			/* load local copies */
+			orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+
+			_airspeed_valid = vehicle_airspeed_poll();
+
+			vehicle_setpoint_poll();
+
+			vehicle_accel_poll();
+
+			vehicle_control_mode_poll();
+
+			vehicle_manual_poll();
+
+			/* lock integrator until control is started */
+			bool lock_integrator;
+
+			if (_vcontrol_mode.flag_control_attitude_enabled) {
+				lock_integrator = false;
+
+			} else {
+				lock_integrator = true;
+			}
+
+			/* decide if in stabilized or full manual control */
+
+			if (_vcontrol_mode.flag_control_attitude_enabled) {
+
+				/* scale from radians to normalized -1 .. 1 range */
+				const float actuator_scaling = 1.0f / (M_PI_F / 4.0f);
+
+				/* scale around tuning airspeed */
+
+				float airspeed;
+
+				/* if airspeed is smaller than min, the sensor is not giving good readings */
+				if (!_airspeed_valid ||
+				    (_airspeed.indicated_airspeed_m_s < _parameters.airspeed_min) ||
+				    !isfinite(_airspeed.indicated_airspeed_m_s)) {
+					airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
+
+				} else {
+					airspeed = _airspeed.indicated_airspeed_m_s;
+				}
+
+				float airspeed_scaling = _parameters.airspeed_trim / airspeed;
+				//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
+
+				float roll_sp, pitch_sp;
+				float throttle_sp = 0.0f;
+
+				if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
+					roll_sp = _att_sp.roll_body;
+					pitch_sp = _att_sp.pitch_body;
+					throttle_sp = _att_sp.thrust;
+
+				} else {
+					/*
+					 * Scale down roll and pitch as the setpoints are radians
+					 * and a typical remote can only do 45 degrees, the mapping is
+					 * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians.
+					 *
+					 * With this mapping the stick angle is a 1:1 representation of
+					 * the commanded attitude. If more than 45 degrees are desired,
+					 * a scaling parameter can be applied to the remote.
+					 */
+					roll_sp = _manual.roll * 0.75f;
+					pitch_sp = _manual.pitch * 0.75f;
+					throttle_sp = _manual.throttle;
+					_actuators.control[4] = _manual.flaps;
+
+					/*
+					 * in manual mode no external source should / does emit attitude setpoints.
+					 * emit the manual setpoint here to allow attitude controller tuning
+					 * in attitude control mode.
+					 */
+					struct vehicle_attitude_setpoint_s att_sp;
+					att_sp.timestamp = hrt_absolute_time();
+					att_sp.roll_body = roll_sp;
+					att_sp.pitch_body = pitch_sp;
+					att_sp.yaw_body = 0.0f;
+					att_sp.thrust = throttle_sp;
+
+					/* lazily publish the setpoint only once available */
+					if (_attitude_sp_pub > 0) {
+						/* publish the attitude setpoint */
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
+
+					} else {
+						/* advertise and publish */
+						_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+					}
+				}
+
+				float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
+								    airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
+				_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
+
+				float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
+								      lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
+				_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
+
+				float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
+								  _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
+				_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
+
+				/* throttle passed through */
+				_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+
+				// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
+				// 			airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
+				// 			_actuators.control[2], _actuators.control[3]);
+
+				/*
+				 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
+				 * only once available
+				 */
+				vehicle_rates_setpoint_s rates_sp;
+				rates_sp.roll = _roll_ctrl.get_desired_rate();
+				rates_sp.pitch = _pitch_ctrl.get_desired_rate();
+				rates_sp.yaw = 0.0f; // XXX not yet implemented
+
+				rates_sp.timestamp = hrt_absolute_time();
+
+				if (_rate_sp_pub > 0) {
+					/* publish the attitude setpoint */
+					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
+
+				} else {
+					/* advertise and publish */
+					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+				}
+
+			} else {
+				/* manual/direct control */
+				_actuators.control[0] = _manual.roll;
+				_actuators.control[1] = _manual.pitch;
+				_actuators.control[2] = _manual.yaw;
+				_actuators.control[3] = _manual.throttle;
+				_actuators.control[4] = _manual.flaps;
+			}
+
+			/* lazily publish the setpoint only once available */
+			_actuators.timestamp = hrt_absolute_time();
+
+			if (_actuators_0_pub > 0) {
+				/* publish the attitude setpoint */
+				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
+
+			} else {
+				/* advertise and publish */
+				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+			}
+
+		}
+
+		perf_end(_loop_perf);
+	}
+
+	warnx("exiting.\n");
+
+	_control_task = -1;
+	_exit(0);
+}
+
+int
+FixedwingAttitudeControl::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("fw_att_control",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2048,
+				       (main_t)&FixedwingAttitudeControl::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+int fw_att_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		errx(1, "usage: fw_att_control {start|stop|status}");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (att_control::g_control != nullptr)
+			errx(1, "already running");
+
+		att_control::g_control = new FixedwingAttitudeControl;
+
+		if (att_control::g_control == nullptr)
+			errx(1, "alloc failed");
+
+		if (OK != att_control::g_control->start()) {
+			delete att_control::g_control;
+			att_control::g_control = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (att_control::g_control == nullptr)
+			errx(1, "not running");
+
+		delete att_control::g_control;
+		att_control::g_control = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (att_control::g_control) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
new file mode 100644
index 000000000..97aa275de
--- /dev/null
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 fw_pos_control_l1_params.c
+ *
+ * Parameters defined by the L1 position control task
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Controller parameters, accessible via MAVLink
+ *
+ */
+
+// @DisplayName		Attitude Time Constant
+// @Description		This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
+// @Range		0.4 to 1.0 seconds, in tens of seconds
+PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
+
+// @DisplayName		Proportional gain.
+// @Description		This defines how much the elevator input will be commanded dependend on the current pitch error.
+// @Range		10 to 200, 1 increments
+PARAM_DEFINE_FLOAT(FW_P_P, 40.0f);
+
+// @DisplayName		Damping gain.
+// @Description		This gain damps the airframe pitch rate. In particular relevant for flying wings.
+// @Range		0.0 to 10.0, 0.1 increments
+PARAM_DEFINE_FLOAT(FW_P_D, 0.0f);
+
+// @DisplayName		Integrator gain.
+// @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
+// @Range		0 to 50.0
+PARAM_DEFINE_FLOAT(FW_P_I, 0.0f);
+
+// @DisplayName		Maximum positive / up pitch rate.
+// @Description		This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
+// @Range		0 to 90.0 degrees per seconds, in 1 increments
+PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
+
+// @DisplayName		Maximum negative / down pitch rate.
+// @Description		This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
+// @Range		0 to 90.0 degrees per seconds, in 1 increments
+PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
+
+// @DisplayName		Pitch Integrator Anti-Windup
+// @Description		This limits the range in degrees the integrator can wind up to.
+// @Range		0.0 to 45.0
+// @Increment		1.0
+PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f);
+
+// @DisplayName		Roll feedforward gain.
+// @Description		This compensates during turns and ensures the nose stays level.
+// @Range		0.5 2.0
+// @Increment		0.05
+// @User		User
+PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f);
+
+// @DisplayName		Proportional Gain.
+// @Description		This gain controls the roll angle to roll actuator output.
+// @Range		10.0 200.0
+// @Increment		10.0
+// @User		User
+PARAM_DEFINE_FLOAT(FW_R_P, 40.0f);
+
+// @DisplayName		Damping Gain
+// @Description		Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence.
+// @Range		0.0 10.0
+// @Increment		1.0
+// @User		User
+PARAM_DEFINE_FLOAT(FW_R_D, 0.0f);
+
+// @DisplayName		Integrator Gain
+// @Description		This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors.
+// @Range		0.0 100.0
+// @Increment		5.0
+// @User		User
+PARAM_DEFINE_FLOAT(FW_R_I, 0.0f);
+
+// @DisplayName		Roll Integrator Anti-Windup
+// @Description		This limits the range in degrees the integrator can wind up to.
+// @Range		0.0 to 45.0
+// @Increment		1.0
+PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f);
+
+// @DisplayName		Maximum Roll Rate
+// @Description		This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
+// @Range		0 to 90.0 degrees per seconds
+// @Increment		1.0
+PARAM_DEFINE_FLOAT(FW_R_RMAX, 60);
+
+
+PARAM_DEFINE_FLOAT(FW_Y_P, 0);
+PARAM_DEFINE_FLOAT(FW_Y_I, 0);
+PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f);
+PARAM_DEFINE_FLOAT(FW_Y_D, 0);
+PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 1);
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f);
+PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f);
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f);
diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk
new file mode 100644
index 000000000..1e600757e
--- /dev/null
+++ b/src/modules/fw_att_control/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 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.
+#
+############################################################################
+
+#
+# Fixedwing attitude control application
+#
+
+MODULE_COMMAND	= fw_att_control
+
+SRCS		= fw_att_control_main.cpp \
+		  fw_att_control_params.c
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
new file mode 100644
index 000000000..3d5bce134
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -0,0 +1,1049 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	Lorenz Meier
+ *
+ * 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 fw_pos_control_l1_main.c
+ * Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll
+ * angle, equivalent to a lateral motion (for copters and rovers).
+ *
+ * Original publication for horizontal control class:
+ *    S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
+ *    Proceedings of the AIAA Guidance, Navigation and Control
+ *    Conference, Aug 2004. AIAA-2004-4900.
+ *
+ * Original implementation for total energy control class:
+ *    Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl)
+ *
+ * More details and acknowledgements in the referenced library headers.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_global_position_set_triplet.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/pid/pid.h>
+#include <geo/geo.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+
+#include <ecl/l1/ecl_l1_pos_controller.h>
+#include <external_lgpl/tecs/tecs.h>
+
+/**
+ * L1 control app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
+
+class FixedwingPositionControl
+{
+public:
+	/**
+	 * Constructor
+	 */
+	FixedwingPositionControl();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~FixedwingPositionControl();
+
+	/**
+	 * Start the sensors task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+private:
+
+	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	int		_control_task;			/**< task handle for sensor task */
+
+	int		_global_pos_sub;
+	int		_global_set_triplet_sub;
+	int		_att_sub;			/**< vehicle attitude subscription */
+	int		_attitude_sub;			/**< raw rc channels data subscription */
+	int		_airspeed_sub;			/**< airspeed subscription */
+	int		_control_mode_sub;			/**< vehicle status subscription */
+	int 		_params_sub;			/**< notification of parameter updates */
+	int 		_manual_control_sub;		/**< notification of manual control updates */
+	int		_accel_sub;			/**< body frame accelerations */
+
+	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
+
+	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
+	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
+	struct airspeed_s				_airspeed;		/**< airspeed */
+	struct vehicle_control_mode_s				_control_mode;		/**< vehicle status */
+	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
+	struct vehicle_global_position_set_triplet_s	_global_triplet;	/**< triplet of global setpoints */
+	struct accel_report				_accel;			/**< body frame accelerations */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
+
+	/** manual control states */
+	float		_seatbelt_hold_heading;		/**< heading the system should hold in seatbelt mode */
+	float		_loiter_hold_lat;
+	float		_loiter_hold_lon;
+	float		_loiter_hold_alt;
+	bool		_loiter_hold;
+
+	float		_launch_lat;
+	float		_launch_lon;
+	float		_launch_alt;
+	bool		_launch_valid;
+
+	/* throttle and airspeed states */
+	float _airspeed_error;				///< airspeed error to setpoint in m/s
+	bool _airspeed_valid;				///< flag if a valid airspeed estimate exists
+	uint64_t _airspeed_last_valid;			///< last time airspeed was valid. Used to detect sensor failures
+	float _groundspeed_undershoot;			///< ground speed error to min. speed in m/s
+	bool _global_pos_valid;				///< global position is valid
+	math::Dcm _R_nb;				///< current attitude
+
+	ECL_L1_Pos_Controller				_l1_control;
+	TECS						_tecs;
+
+	struct {
+		float l1_period;
+		float l1_damping;
+
+		float time_const;
+		float min_sink_rate;
+		float max_sink_rate;
+		float max_climb_rate;
+		float throttle_damp;
+		float integrator_gain;
+		float vertical_accel_limit;
+		float height_comp_filter_omega;
+		float speed_comp_filter_omega;
+		float roll_throttle_compensation;
+		float speed_weight;
+		float pitch_damping;
+
+		float airspeed_min;
+		float airspeed_trim;
+		float airspeed_max;
+
+		float pitch_limit_min;
+		float pitch_limit_max;
+		float throttle_min;
+		float throttle_max;
+		float throttle_cruise;
+
+		float loiter_hold_radius;
+	}		_parameters;			/**< local copies of interesting parameters */
+
+	struct {
+
+		param_t l1_period;
+		param_t l1_damping;
+
+		param_t time_const;
+		param_t min_sink_rate;
+		param_t max_sink_rate;
+		param_t max_climb_rate;
+		param_t throttle_damp;
+		param_t integrator_gain;
+		param_t vertical_accel_limit;
+		param_t height_comp_filter_omega;
+		param_t speed_comp_filter_omega;
+		param_t roll_throttle_compensation;
+		param_t speed_weight;
+		param_t pitch_damping;
+
+		param_t airspeed_min;
+		param_t airspeed_trim;
+		param_t airspeed_max;
+
+		param_t pitch_limit_min;
+		param_t pitch_limit_max;
+		param_t throttle_min;
+		param_t throttle_max;
+		param_t throttle_cruise;
+
+		param_t loiter_hold_radius;
+	}		_parameter_handles;		/**< handles for interesting parameters */
+
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int		parameters_update();
+
+	/**
+	 * Update control outputs
+	 *
+	 */
+	void		control_update();
+
+	/**
+	 * Check for changes in vehicle status.
+	 */
+	void		vehicle_control_mode_poll();
+
+	/**
+	 * Check for airspeed updates.
+	 */
+	bool		vehicle_airspeed_poll();
+
+	/**
+	 * Check for position updates.
+	 */
+	void		vehicle_attitude_poll();
+
+	/**
+	 * Check for accel updates.
+	 */
+	void		vehicle_accel_poll();
+
+	/**
+	 * Check for set triplet updates.
+	 */
+	void		vehicle_setpoint_poll();
+
+	/**
+	 * Control position.
+	 */
+	bool		control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed,
+					 const struct vehicle_global_position_set_triplet_s &global_triplet);
+
+	float calculate_target_airspeed(float airspeed_demand);
+	void calculate_gndspeed_undershoot();
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * Main sensor collection task.
+	 */
+	void		task_main() __attribute__((noreturn));
+};
+
+namespace l1_control
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+FixedwingPositionControl	*g_control;
+}
+
+FixedwingPositionControl::FixedwingPositionControl() :
+
+	_task_should_exit(false),
+	_control_task(-1),
+
+/* subscriptions */
+	_global_pos_sub(-1),
+	_global_set_triplet_sub(-1),
+	_att_sub(-1),
+	_airspeed_sub(-1),
+	_control_mode_sub(-1),
+	_params_sub(-1),
+	_manual_control_sub(-1),
+
+/* publications */
+	_attitude_sp_pub(-1),
+
+/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
+/* states */
+	_setpoint_valid(false),
+	_loiter_hold(false),
+	_airspeed_error(0.0f),
+	_airspeed_valid(false),
+	_groundspeed_undershoot(0.0f),
+	_global_pos_valid(false)
+{
+	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
+	_parameter_handles.l1_damping = param_find("FW_L1_DAMPING");
+	_parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R");
+
+	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
+	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
+	_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
+
+	_parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN");
+	_parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX");
+	_parameter_handles.throttle_min = param_find("FW_THR_MIN");
+	_parameter_handles.throttle_max = param_find("FW_THR_MAX");
+	_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
+
+	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
+	_parameter_handles.min_sink_rate = 			param_find("FW_T_SINK_MIN");
+	_parameter_handles.max_sink_rate =			param_find("FW_T_SINK_MAX");
+	_parameter_handles.max_climb_rate =			param_find("FW_T_CLMB_MAX");
+	_parameter_handles.throttle_damp = 			param_find("FW_T_THR_DAMP");
+	_parameter_handles.integrator_gain =			param_find("FW_T_INTEG_GAIN");
+	_parameter_handles.vertical_accel_limit =		param_find("FW_T_VERT_ACC");
+	_parameter_handles.height_comp_filter_omega =		param_find("FW_T_HGT_OMEGA");
+	_parameter_handles.speed_comp_filter_omega =		param_find("FW_T_SPD_OMEGA");
+	_parameter_handles.roll_throttle_compensation = 	param_find("FW_T_RLL2THR");
+	_parameter_handles.speed_weight = 			param_find("FW_T_SPDWEIGHT");
+	_parameter_handles.pitch_damping = 			param_find("FW_T_PTCH_DAMP");
+
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+FixedwingPositionControl::~FixedwingPositionControl()
+{
+	if (_control_task != -1) {
+
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_control_task);
+				break;
+			}
+		} while (_control_task != -1);
+	}
+
+	l1_control::g_control = nullptr;
+}
+
+int
+FixedwingPositionControl::parameters_update()
+{
+
+	/* L1 control parameters */
+	param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping));
+	param_get(_parameter_handles.l1_period, &(_parameters.l1_period));
+	param_get(_parameter_handles.loiter_hold_radius, &(_parameters.loiter_hold_radius));
+
+	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
+	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
+	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
+
+	param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min));
+	param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max));
+	param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min));
+	param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
+	param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
+
+	param_get(_parameter_handles.time_const, &(_parameters.time_const));
+	param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
+	param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
+	param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp));
+	param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain));
+	param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit));
+	param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega));
+	param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega));
+	param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation));
+	param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight));
+	param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
+	param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
+
+	_l1_control.set_l1_damping(_parameters.l1_damping);
+	_l1_control.set_l1_period(_parameters.l1_period);
+
+	_tecs.set_time_const(_parameters.time_const);
+	_tecs.set_min_sink_rate(_parameters.min_sink_rate);
+	_tecs.set_max_sink_rate(_parameters.max_sink_rate);
+	_tecs.set_throttle_damp(_parameters.throttle_damp);
+	_tecs.set_integrator_gain(_parameters.integrator_gain);
+	_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
+	_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
+	_tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega);
+	_tecs.set_roll_throttle_compensation(math::radians(_parameters.roll_throttle_compensation));
+	_tecs.set_speed_weight(_parameters.speed_weight);
+	_tecs.set_pitch_damping(_parameters.pitch_damping);
+	_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
+	_tecs.set_indicated_airspeed_max(_parameters.airspeed_min);
+	_tecs.set_max_climb_rate(_parameters.max_climb_rate);
+
+	/* sanity check parameters */
+	if (_parameters.airspeed_max < _parameters.airspeed_min ||
+	    _parameters.airspeed_max < 5.0f ||
+	    _parameters.airspeed_min > 100.0f ||
+	    _parameters.airspeed_trim < _parameters.airspeed_min ||
+	    _parameters.airspeed_trim > _parameters.airspeed_max) {
+		warnx("error: airspeed parameters invalid");
+		return 1;
+	}
+
+	return OK;
+}
+
+void
+FixedwingPositionControl::vehicle_control_mode_poll()
+{
+	bool vstatus_updated;
+
+	/* Check HIL state if vehicle status has changed */
+	orb_check(_control_mode_sub, &vstatus_updated);
+
+	if (vstatus_updated) {
+
+		bool was_armed = _control_mode.flag_armed;
+
+		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
+
+		if (!was_armed && _control_mode.flag_armed) {
+			_launch_lat = _global_pos.lat / 1e7f;
+			_launch_lon = _global_pos.lon / 1e7f;
+			_launch_alt = _global_pos.alt;
+			_launch_valid = true;
+		}
+	}
+}
+
+bool
+FixedwingPositionControl::vehicle_airspeed_poll()
+{
+	/* check if there is an airspeed update or if it timed out */
+	bool airspeed_updated;
+	orb_check(_airspeed_sub, &airspeed_updated);
+
+	if (airspeed_updated) {
+		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+		_airspeed_valid = true;
+		_airspeed_last_valid = hrt_absolute_time();
+		return true;
+
+	} else {
+
+		/* no airspeed updates for one second */
+		if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
+			_airspeed_valid = false;
+		}
+	}
+
+	/* update TECS state */
+	_tecs.enable_airspeed(_airspeed_valid);
+
+	return false;
+}
+
+void
+FixedwingPositionControl::vehicle_attitude_poll()
+{
+	/* check if there is a new position */
+	bool att_updated;
+	orb_check(_att_sub, &att_updated);
+
+	if (att_updated) {
+		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+
+		/* set rotation matrix */
+		for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+				_R_nb(i, j) = _att.R[i][j];
+	}
+}
+
+void
+FixedwingPositionControl::vehicle_accel_poll()
+{
+	/* check if there is a new position */
+	bool accel_updated;
+	orb_check(_accel_sub, &accel_updated);
+
+	if (accel_updated) {
+		orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+	}
+}
+
+void
+FixedwingPositionControl::vehicle_setpoint_poll()
+{
+	/* check if there is a new setpoint */
+	bool global_sp_updated;
+	orb_check(_global_set_triplet_sub, &global_sp_updated);
+
+	if (global_sp_updated) {
+		orb_copy(ORB_ID(vehicle_global_position_set_triplet), _global_set_triplet_sub, &_global_triplet);
+		_setpoint_valid = true;
+	}
+}
+
+void
+FixedwingPositionControl::task_main_trampoline(int argc, char *argv[])
+{
+	l1_control::g_control->task_main();
+}
+
+float
+FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
+{
+	float airspeed;
+
+	if (_airspeed_valid) {
+		airspeed = _airspeed.true_airspeed_m_s;
+
+	} else {
+		airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
+	}
+
+	/* cruise airspeed for all modes unless modified below */
+	float target_airspeed = airspeed_demand;
+
+	/* add minimum ground speed undershoot (only non-zero in presence of sufficient wind) */
+	target_airspeed += _groundspeed_undershoot;
+
+	if (0/* throttle nudging enabled */) {
+		//target_airspeed += nudge term.
+	}
+
+	/* sanity check: limit to range */
+	target_airspeed = math::constrain(target_airspeed, _parameters.airspeed_min, _parameters.airspeed_max);
+
+	/* plain airspeed error */
+	_airspeed_error = target_airspeed - airspeed;
+
+	return target_airspeed;
+}
+
+void
+FixedwingPositionControl::calculate_gndspeed_undershoot()
+{
+
+	if (_global_pos_valid) {
+		/* get ground speed vector */
+		math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy);
+
+		/* rotate with current attitude */
+		math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
+		yaw_vector.normalize();
+		float ground_speed_body = yaw_vector * ground_speed_vector;
+
+		/*
+		 * Ground speed undershoot is the amount of ground velocity not reached
+		 * by the plane. Consequently it is zero if airspeed is >= min ground speed
+		 * and positive if airspeed < min ground speed.
+		 *
+		 * This error value ensures that a plane (as long as its throttle capability is
+		 * not exceeded) travels towards a waypoint (and is not pushed more and more away
+		 * by wind). Not countering this would lead to a fly-away.
+		 */
+		_groundspeed_undershoot = math::max(_parameters.airspeed_min - ground_speed_body, 0.0f);
+
+	} else {
+		_groundspeed_undershoot = 0;
+	}
+}
+
+bool
+FixedwingPositionControl::control_position(const math::Vector2f &current_position, const math::Vector2f &ground_speed,
+		const struct vehicle_global_position_set_triplet_s &global_triplet)
+{
+	bool setpoint = true;
+
+	calculate_gndspeed_undershoot();
+
+	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
+
+	// XXX re-visit
+	float baro_altitude = _global_pos.alt;
+
+	/* filter speed and altitude for controller */
+	math::Vector3 accel_body(_accel.x, _accel.y, _accel.z);
+	math::Vector3 accel_earth = _R_nb.transpose() * accel_body;
+
+	_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
+	float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
+
+	/* AUTONOMOUS FLIGHT */
+
+	// XXX this should only execute if auto AND safety off (actuators active),
+	// else integrators should be constantly reset.
+	if (_control_mode.flag_control_position_enabled) {
+
+		/* execute navigation once we have a setpoint */
+		if (_setpoint_valid) {
+
+			float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
+
+			/* current waypoint (the one currently heading for) */
+			math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
+
+			/* previous waypoint */
+			math::Vector2f prev_wp;
+
+			if (global_triplet.previous_valid) {
+				prev_wp.setX(global_triplet.previous.lat / 1e7f);
+				prev_wp.setY(global_triplet.previous.lon / 1e7f);
+
+			} else {
+				/*
+				 * No valid next waypoint, go for heading hold.
+				 * This is automatically handled by the L1 library.
+				 */
+				prev_wp.setX(global_triplet.current.lat / 1e7f);
+				prev_wp.setY(global_triplet.current.lon / 1e7f);
+
+			}
+
+			// XXX add RTL switch
+			if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) {
+
+				math::Vector2f rtl_pos(_launch_lat, _launch_lon);
+
+				_l1_control.navigate_waypoints(rtl_pos, rtl_pos, current_position, ground_speed);
+				_att_sp.roll_body = _l1_control.nav_roll();
+				_att_sp.yaw_body = _l1_control.nav_bearing();
+
+				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _launch_alt, calculate_target_airspeed(_parameters.airspeed_trim),
+							    _airspeed.indicated_airspeed_m_s, eas2tas,
+							    false, math::radians(_parameters.pitch_limit_min),
+							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+
+				// XXX handle case when having arrived at home (loiter)
+
+			} else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
+				/* waypoint is a plain navigation waypoint */
+				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+				_att_sp.roll_body = _l1_control.nav_roll();
+				_att_sp.yaw_body = _l1_control.nav_bearing();
+
+				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
+							    _airspeed.indicated_airspeed_m_s, eas2tas,
+							    false, math::radians(_parameters.pitch_limit_min),
+							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+
+			} else if (global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+				   global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+				   global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+
+				/* waypoint is a loiter waypoint */
+				_l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius,
+							  global_triplet.current.loiter_direction, ground_speed);
+				_att_sp.roll_body = _l1_control.nav_roll();
+				_att_sp.yaw_body = _l1_control.nav_bearing();
+
+				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
+							    _airspeed.indicated_airspeed_m_s, eas2tas,
+							    false, math::radians(_parameters.pitch_limit_min),
+							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+
+			} else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) {
+
+				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+				_att_sp.roll_body = _l1_control.nav_roll();
+				_att_sp.yaw_body = _l1_control.nav_bearing();
+
+				/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
+				// XXX this could make a great param
+				if (altitude_error > -20.0f) {
+
+					float flare_angle_rad = math::radians(15.0f);//math::radians(global_triplet.current.param1)
+
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
+								    _airspeed.indicated_airspeed_m_s, eas2tas,
+								    true, flare_angle_rad,
+								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+
+					/* limit roll motion to prevent wings from touching the ground first */
+					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f));
+
+				} else {
+
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
+								    _airspeed.indicated_airspeed_m_s, eas2tas,
+								    false, math::radians(_parameters.pitch_limit_min),
+								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+				}
+
+			} else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
+
+				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+				_att_sp.roll_body = _l1_control.nav_roll();
+				_att_sp.yaw_body = _l1_control.nav_bearing();
+
+				/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
+				if (altitude_error > 10.0f) {
+
+					/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
+								    _airspeed.indicated_airspeed_m_s, eas2tas,
+								    true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)),
+								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+
+					/* limit roll motion to ensure enough lift */
+					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
+
+				} else {
+
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
+								    _airspeed.indicated_airspeed_m_s, eas2tas,
+								    false, math::radians(_parameters.pitch_limit_min),
+								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+				}
+			}
+
+			// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
+			//       (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
+			// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(),
+			//       (double)next_wp.getX(), (double)next_wp.getY(), (global_triplet.previous_valid) ? "valid" : "invalid");
+
+			// XXX at this point we always want no loiter hold if a
+			// mission is active
+			_loiter_hold = false;
+
+		} else if (_control_mode.flag_armed) {
+
+			/* hold position, but only if armed, climb 20m in case this is engaged on ground level */
+
+			// XXX rework with smarter state machine
+
+			if (!_loiter_hold) {
+				_loiter_hold_lat = _global_pos.lat / 1e7f;
+				_loiter_hold_lon = _global_pos.lon / 1e7f;
+				_loiter_hold_alt = _global_pos.alt + 25.0f;
+				_loiter_hold = true;
+			}
+
+			float altitude_error = _loiter_hold_alt - _global_pos.alt;
+
+			math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon);
+
+			/* loiter around current position */
+			_l1_control.navigate_loiter(loiter_hold_pos, current_position, _parameters.loiter_hold_radius,
+						  1, ground_speed);
+			_att_sp.roll_body = _l1_control.nav_roll();
+			_att_sp.yaw_body = _l1_control.nav_bearing();
+
+			/* climb with full throttle if the altitude error is bigger than 5 meters */
+			bool climb_out = (altitude_error > 5);
+
+			float min_pitch;
+
+			if (climb_out) {
+				min_pitch = math::radians(20.0f);
+
+			} else {
+				min_pitch = math::radians(_parameters.pitch_limit_min);
+			}
+
+			_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _loiter_hold_alt, calculate_target_airspeed(_parameters.airspeed_trim),
+						    _airspeed.indicated_airspeed_m_s, eas2tas,
+						    climb_out, min_pitch,
+						    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+						    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+
+			if (climb_out) {
+				/* limit roll motion to ensure enough lift */
+				_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
+			}
+		}
+
+	} else if (0/* seatbelt mode enabled */) {
+
+		/** SEATBELT FLIGHT **/
+
+		if (0/* switched from another mode to seatbelt */) {
+			_seatbelt_hold_heading = _att.yaw;
+		}
+
+		if (0/* seatbelt on and manual control yaw non-zero */) {
+			_seatbelt_hold_heading = _att.yaw + _manual.yaw;
+		}
+
+		/* if in seatbelt mode, set airspeed based on manual control */
+
+		// XXX check if ground speed undershoot should be applied here
+		float seatbelt_airspeed = _parameters.airspeed_min +
+					  (_parameters.airspeed_max - _parameters.airspeed_min) *
+					  _manual.throttle;
+
+		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
+		_att_sp.roll_body = _l1_control.nav_roll();
+		_att_sp.yaw_body = _l1_control.nav_bearing();
+		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
+					    seatbelt_airspeed,
+					    _airspeed.indicated_airspeed_m_s, eas2tas,
+					    false, _parameters.pitch_limit_min,
+					    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+					    _parameters.pitch_limit_min, _parameters.pitch_limit_max);
+
+	} else {
+
+		/** MANUAL FLIGHT **/
+
+		/* no flight mode applies, do not publish an attitude setpoint */
+		setpoint = false;
+	}
+
+	_att_sp.pitch_body = _tecs.get_pitch_demand();
+	_att_sp.thrust = _tecs.get_throttle_demand();
+
+	return setpoint;
+}
+
+void
+FixedwingPositionControl::task_main()
+{
+
+	/* inform about start */
+	warnx("Initializing..");
+	fflush(stdout);
+
+	/*
+	 * do subscriptions
+	 */
+	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	_global_set_triplet_sub = orb_subscribe(ORB_ID(vehicle_global_position_set_triplet));
+	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+
+	/* rate limit vehicle status updates to 5Hz */
+	orb_set_interval(_control_mode_sub, 200);
+	/* rate limit position updates to 50 Hz */
+	orb_set_interval(_global_pos_sub, 20);
+
+	/* abort on a nonzero return value from the parameter init */
+	if (parameters_update()) {
+		/* parameter setup went wrong, abort */
+		warnx("aborting startup due to errors.");
+		_task_should_exit = true;
+	}
+
+	/* wakeup source(s) */
+	struct pollfd fds[2];
+
+	/* Setup of loop */
+	fds[0].fd = _params_sub;
+	fds[0].events = POLLIN;
+	fds[1].fd = _global_pos_sub;
+	fds[1].events = POLLIN;
+
+	while (!_task_should_exit) {
+
+		/* wait for up to 500ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+		/* timed out - periodic check for _task_should_exit, etc. */
+		if (pret == 0)
+			continue;
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			continue;
+		}
+
+		perf_begin(_loop_perf);
+
+		/* check vehicle status for changes to publication state */
+		vehicle_control_mode_poll();
+
+		/* only update parameters if they changed */
+		if (fds[0].revents & POLLIN) {
+			/* read from param to clear updated flag */
+			struct parameter_update_s update;
+			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+
+			/* update parameters from storage */
+			parameters_update();
+		}
+
+		/* only run controller if position changed */
+		if (fds[1].revents & POLLIN) {
+
+
+			static uint64_t last_run = 0;
+			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
+			last_run = hrt_absolute_time();
+
+			/* guard against too large deltaT's */
+			if (deltaT > 1.0f)
+				deltaT = 0.01f;
+
+			/* load local copies */
+			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
+
+			// XXX add timestamp check
+			_global_pos_valid = true;
+
+			vehicle_attitude_poll();
+			vehicle_setpoint_poll();
+			vehicle_accel_poll();
+			vehicle_airspeed_poll();
+			// vehicle_baro_poll();
+
+			math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
+			math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
+
+			/*
+			 * Attempt to control position, on success (= sensors present and not in manual mode),
+			 * publish setpoint.
+			 */
+			if (control_position(current_position, ground_speed, _global_triplet)) {
+				_att_sp.timestamp = hrt_absolute_time();
+
+				/* lazily publish the setpoint only once available */
+				if (_attitude_sp_pub > 0) {
+					/* publish the attitude setpoint */
+					orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);
+
+				} else {
+					/* advertise and publish */
+					_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+				}
+
+			}
+
+		}
+
+		perf_end(_loop_perf);
+	}
+
+	warnx("exiting.\n");
+
+	_control_task = -1;
+	_exit(0);
+}
+
+int
+FixedwingPositionControl::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("fw_pos_control_l1",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       4048,
+				       (main_t)&FixedwingPositionControl::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+int fw_pos_control_l1_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		errx(1, "usage: fw_pos_control_l1 {start|stop|status}");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (l1_control::g_control != nullptr)
+			errx(1, "already running");
+
+		l1_control::g_control = new FixedwingPositionControl;
+
+		if (l1_control::g_control == nullptr)
+			errx(1, "alloc failed");
+
+		if (OK != l1_control::g_control->start()) {
+			delete l1_control::g_control;
+			l1_control::g_control = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (l1_control::g_control == nullptr)
+			errx(1, "not running");
+
+		delete l1_control::g_control;
+		l1_control::g_control = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (l1_control::g_control) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
new file mode 100644
index 000000000..d210ec712
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 fw_pos_control_l1_params.c
+ *
+ * Parameters defined by the L1 position control task
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Controller parameters, accessible via MAVLink
+ *
+ */
+
+PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
+
+
+PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f);
+
+
+PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
+
+
+PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk
new file mode 100644
index 000000000..b00b9aa5a
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 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.
+#
+############################################################################
+
+#
+# Fixedwing L1 position control application
+#
+
+MODULE_COMMAND	= fw_pos_control_l1
+
+SRCS		= fw_pos_control_l1_main.cpp \
+		  fw_pos_control_l1_params.c
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 5eb7cba9b..7a5c2dab2 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -531,7 +531,7 @@ int mavlink_thread_main(int argc, char *argv[])
 		case 'b':
 			baudrate = strtoul(optarg, NULL, 10);
 
-			if (baudrate == 0)
+			if (baudrate < 9600 || baudrate > 921600)
 				errx(1, "invalid baud rate '%s'", optarg);
 
 			break;
@@ -743,7 +743,7 @@ int mavlink_thread_main(int argc, char *argv[])
 
 	thread_running = false;
 
-	exit(0);
+	return 0;
 }
 
 static void
@@ -767,7 +767,7 @@ int mavlink_main(int argc, char *argv[])
 
 		/* this is not an error */
 		if (thread_running)
-			errx(0, "mavlink already running\n");
+			errx(0, "mavlink already running");
 
 		thread_should_exit = false;
 		mavlink_task = task_spawn_cmd("mavlink",
@@ -776,15 +776,25 @@ int mavlink_main(int argc, char *argv[])
 					  2048,
 					  mavlink_thread_main,
 					  (const char **)argv);
+
+		while (!thread_running) {
+			usleep(200);
+		}
+
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
+
+		/* this is not an error */
+		if (!thread_running)
+			errx(0, "mavlink already stopped");
+
 		thread_should_exit = true;
 
 		while (thread_running) {
 			usleep(200000);
-			printf(".");
+			warnx(".");
 		}
 
 		warnx("terminated.");
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 4674f7a24..7dd9e321f 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -102,6 +102,7 @@ struct vehicle_global_position_s hil_global_pos;
 struct vehicle_attitude_s hil_attitude;
 struct vehicle_gps_position_s hil_gps;
 struct sensor_combined_s hil_sensors;
+struct battery_status_s	hil_battery_status;
 static orb_advert_t pub_hil_global_pos = -1;
 static orb_advert_t pub_hil_attitude = -1;
 static orb_advert_t pub_hil_gps = -1;
@@ -111,6 +112,12 @@ static orb_advert_t pub_hil_accel = -1;
 static orb_advert_t pub_hil_mag = -1;
 static orb_advert_t pub_hil_baro = -1;
 static orb_advert_t pub_hil_airspeed = -1;
+static orb_advert_t pub_hil_battery = -1;
+
+/* packet counter */
+static int hil_counter = 0;
+static int hil_frames = 0;
+static uint64_t old_timestamp = 0;
 
 static orb_advert_t cmd_pub = -1;
 static orb_advert_t flow_pub = -1;
@@ -149,7 +156,8 @@ handle_message(mavlink_message_t *msg)
 				vcmd.param5 = cmd_mavlink.param5;
 				vcmd.param6 = cmd_mavlink.param6;
 				vcmd.param7 = cmd_mavlink.param7;
-				vcmd.command = cmd_mavlink.command;
+				// XXX do proper translation
+				vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
 				vcmd.target_system = cmd_mavlink.target_system;
 				vcmd.target_component = cmd_mavlink.target_component;
 				vcmd.source_system = msg->sysid;
@@ -207,7 +215,7 @@ handle_message(mavlink_message_t *msg)
 		vcmd.param5 = 0;
 		vcmd.param6 = 0;
 		vcmd.param7 = 0;
-		vcmd.command = MAV_CMD_DO_SET_MODE;
+		vcmd.command = VEHICLE_CMD_DO_SET_MODE;
 		vcmd.target_system = new_mode.target_system;
 		vcmd.target_component = MAV_COMP_ID_ALL;
 		vcmd.source_system = msg->sysid;
@@ -360,11 +368,6 @@ handle_message(mavlink_message_t *msg)
 			mavlink_hil_sensor_t imu;
 			mavlink_msg_hil_sensor_decode(msg, &imu);
 
-			/* packet counter */
-			static uint16_t hil_counter = 0;
-			static uint16_t hil_frames = 0;
-			static uint64_t old_timestamp = 0;
-
 			/* sensors general */
 			hil_sensors.timestamp = hrt_absolute_time();
 
@@ -391,9 +394,9 @@ handle_message(mavlink_message_t *msg)
 			hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
 
 			/* adc */
-			hil_sensors.adc_voltage_v[0] = 0;
-			hil_sensors.adc_voltage_v[1] = 0;
-			hil_sensors.adc_voltage_v[2] = 0;
+			hil_sensors.adc_voltage_v[0] = 0.0f;
+			hil_sensors.adc_voltage_v[1] = 0.0f;
+			hil_sensors.adc_voltage_v[2] = 0.0f;
 
 			/* magnetometer */
 			float mga2ga = 1.0e-3f;
@@ -506,6 +509,18 @@ handle_message(mavlink_message_t *msg)
 				pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
 			}
 
+			/* fill in HIL battery status */
+			hil_battery_status.timestamp = hrt_absolute_time();
+			hil_battery_status.voltage_v = 11.1f;
+			hil_battery_status.current_a = 10.0f;
+
+			/* lazily publish the battery voltage */
+			if (pub_hil_battery > 0) {
+				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+			} else {
+				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			}
+
 			// increment counters
 			hil_counter++;
 			hil_frames++;
@@ -574,6 +589,7 @@ handle_message(mavlink_message_t *msg)
 				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
 
+			hil_global_pos.valid = true;
 			hil_global_pos.lat = hil_state.lat;
 			hil_global_pos.lon = hil_state.lon;
 			hil_global_pos.alt = hil_state.alt / 1000.0f;
@@ -639,6 +655,18 @@ handle_message(mavlink_message_t *msg)
 			} else {
 				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
 			}
+
+			/* fill in HIL battery status */
+			hil_battery_status.timestamp = hrt_absolute_time();
+			hil_battery_status.voltage_v = 11.1f;
+			hil_battery_status.current_a = 10.0f;
+
+			/* lazily publish the battery voltage */
+			if (pub_hil_battery > 0) {
+				orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
+			} else {
+				pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			}
 		}
 
 		if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
@@ -755,5 +783,7 @@ receive_start(int uart)
 
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
+
+	pthread_attr_destroy(&receiveloop_attr);
 	return thread;
 }
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index be88b8794..3175e64ce 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -167,9 +167,9 @@ bool set_special_fields(float param1, float param2, float param3, float param4,
 	sp->loiter_direction = (param3 >= 0) ? 1 : -1;
 
 	sp->param1 = param1;
-	sp->param1 = param2;
-	sp->param1 = param3;
-	sp->param1 = param4;
+	sp->param2 = param2;
+	sp->param3 = param3;
+	sp->param4 = param4;
 }
 
 /**
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 53d86ec00..cec2fdc43 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -99,6 +99,8 @@ struct listener {
 	uintptr_t	arg;
 };
 
+uint16_t cm_uint16_from_m_float(float m);
+
 static void	l_sensor_combined(const struct listener *l);
 static void	l_vehicle_attitude(const struct listener *l);
 static void	l_vehicle_gps_position(const struct listener *l);
@@ -150,6 +152,19 @@ static const struct listener listeners[] = {
 
 static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
 
+uint16_t
+cm_uint16_from_m_float(float m)
+{
+	if (m < 0.0f) {
+		return 0;
+
+	} else if (m > 655.35f) {
+		return 65535;
+	}
+
+	return (uint16_t)(m * 100.0f);
+}
+
 void
 l_sensor_combined(const struct listener *l)
 {
@@ -235,8 +250,10 @@ l_vehicle_gps_position(const struct listener *l)
 
 	/* GPS COG is 0..2PI in degrees * 1e2 */
 	float cog_deg = gps.cog_rad;
+
 	if (cog_deg > M_PI_F)
 		cog_deg -= 2.0f * M_PI_F;
+
 	cog_deg *= M_RAD_TO_DEG_F;
 
 
@@ -247,10 +264,10 @@ l_vehicle_gps_position(const struct listener *l)
 				     gps.lat,
 				     gps.lon,
 				     gps.alt,
-				     gps.eph_m * 1e2f, // from m to cm
-				     gps.epv_m * 1e2f, // from m to cm
+				     cm_uint16_from_m_float(gps.eph_m),
+				     cm_uint16_from_m_float(gps.epv_m),
 				     gps.vel_m_s * 1e2f, // from m/s to cm/s
-				     cog_deg * 1e2f, // from rad to deg * 100
+				     cog_deg * 1e2f, // from deg to deg * 100
 				     gps.satellites_visible);
 
 	/* update SAT info every 10 seconds */
@@ -829,5 +846,7 @@ uorb_receive_start(void)
 
 	pthread_t thread;
 	pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
+
+	pthread_attr_destroy(&uorb_attr);
 	return thread;
 }
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index e2e630046..c5cd0d03e 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -65,6 +65,7 @@
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
 #include <drivers/drv_rc_input.h>
 
 struct mavlink_subscriptions {
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 16a7c2d35..97472c233 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -246,47 +246,6 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
 	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
 }
 
-//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z)
-//{
-//    if (seq < wpm->size)
-//    {
-//        mavlink_mission_item_t *cur = waypoints->at(seq);
-//
-//        const PxVector3 A(cur->x, cur->y, cur->z);
-//        const PxVector3 C(x, y, z);
-//
-//        // seq not the second last waypoint
-//        if ((uint16_t)(seq+1) < wpm->size)
-//        {
-//            mavlink_mission_item_t *next = waypoints->at(seq+1);
-//            const PxVector3 B(next->x, next->y, next->z);
-//            const float r = (B-A).dot(C-A) / (B-A).lengthSquared();
-//            if (r >= 0 && r <= 1)
-//            {
-//                const PxVector3 P(A + r*(B-A));
-//                return (P-C).length();
-//            }
-//            else if (r < 0.f)
-//            {
-//                return (C-A).length();
-//            }
-//            else
-//            {
-//                return (C-B).length();
-//            }
-//        }
-//        else
-//        {
-//            return (C-A).length();
-//        }
-//    }
-//    else
-//    {
-//        // if (verbose) // printf("ERROR: index out of bounds\n");
-//    }
-//    return -1.f;
-//}
-
 /*
  * Calculate distance in global frame.
  *
@@ -337,7 +296,7 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float
 		float dy = (cur->y - y);
 		float dz = (cur->z - z);
 
-		return sqrt(dx * dx + dy * dy + dz * dz);
+		return sqrtf(dx * dx + dy * dy + dz * dz);
 
 	} else {
 		return -1.0f;
@@ -348,6 +307,11 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 {
 	static uint16_t counter;
 
+	if (!global_pos->valid && !local_pos->xy_valid) {
+		/* nothing to check here, return */
+		return;
+	}
+
 	if (wpm->current_active_wp_id < wpm->size) {
 
 		float orbit;
@@ -417,17 +381,57 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 			}
 
 			if (time_elapsed) {
+
 				if (cur_wp->autocontinue) {
 					cur_wp->current = 0;
 
+					float navigation_lat = -1.0f;
+					float navigation_lon = -1.0f;
+					float navigation_alt = -1.0f;
+					int navigation_frame = -1;
+
+					/* initialize to current position in case we don't find a suitable navigation waypoint */
+					if (global_pos->valid) {
+						navigation_lat = global_pos->lat/1e7;
+						navigation_lon = global_pos->lon/1e7;
+						navigation_alt = global_pos->alt;
+						navigation_frame = MAV_FRAME_GLOBAL;
+					} else if (local_pos->xy_valid && local_pos->z_valid) {
+						navigation_lat = local_pos->x;
+						navigation_lon = local_pos->y;
+						navigation_alt = local_pos->z;
+						navigation_frame = MAV_FRAME_LOCAL_NED;
+					}
+
 					/* only accept supported navigation waypoints, skip unknown ones */
 					do {
+						/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
+						if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
+				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
+				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
+				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
+
+							/* this is a navigation waypoint */
+							navigation_frame = cur_wp->frame;
+							navigation_lat = cur_wp->x;
+							navigation_lon = cur_wp->y;
+							navigation_alt = cur_wp->z;
+						}
 
 						if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
 							/* the last waypoint was reached, if auto continue is
-							 * activated restart the waypoint list from the beginning
+							 * activated keep the system loitering there.
 							 */
-							wpm->current_active_wp_id = 0;
+							cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
+							cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
+							cur_wp->frame = navigation_frame;
+							cur_wp->x = navigation_lat;
+							cur_wp->y = navigation_lon;
+							cur_wp->z = navigation_alt;
+							cur_wp->autocontinue = false;
+
+							/* we risk an endless loop for missions without navigation waypoints, abort. */
+							break;
 
 						} else {
 							if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
@@ -482,11 +486,6 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
 		}
 	}
 
-	// Do NOT continously send the current WP, since we're not loosing messages
-	// if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) {
-	// 	mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-	// }
-
 	check_waypoints_reached(now, global_position, local_position);
 
 	return OK;
@@ -498,115 +497,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 	uint64_t now = mavlink_missionlib_get_system_timestamp();
 
 	switch (msg->msgid) {
-//		case MAVLINK_MSG_ID_ATTITUDE:
-//        {
-//            if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size)
-//            {
-//                mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-//                if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED)
-//                {
-//                    mavlink_attitude_t att;
-//                    mavlink_msg_attitude_decode(msg, &att);
-//                    float yaw_tolerance = wpm->accept_range_yaw;
-//                    //compare current yaw
-//                    if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*FM_PI)
-//                    {
-//                        if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
-//                            wpm->yaw_reached = true;
-//                    }
-//                    else if(att.yaw - yaw_tolerance < 0.0f)
-//                    {
-//                        float lowerBound = 360.0f + att.yaw - yaw_tolerance;
-//                        if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
-//                            wpm->yaw_reached = true;
-//                    }
-//                    else
-//                    {
-//                        float upperBound = att.yaw + yaw_tolerance - 2.f*FM_PI;
-//                        if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
-//                            wpm->yaw_reached = true;
-//                    }
-//                }
-//            }
-//            break;
-//        }
-//
-//		case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
-//        {
-//            if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size)
-//            {
-//                mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-//
-//                if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED)
-//                {
-//                    mavlink_local_position_ned_t pos;
-//                    mavlink_msg_local_position_ned_decode(msg, &pos);
-//                    //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
-//
-//                    wpm->pos_reached = false;
-//
-//                    // compare current position (given in message) with current waypoint
-//                    float orbit = wp->param1;
-//
-//                    float dist;
-////                    if (wp->param2 == 0)
-////                    {
-////						// FIXME segment distance
-////                        //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z);
-////                    }
-////                    else
-////                    {
-//                        dist = mavlink_wpm_distance_to_point(wpm->current_active_wp_id, pos.x, pos.y, pos.z);
-////                    }
-//
-//                    if (dist >= 0.f && dist <= orbit && wpm->yaw_reached)
-//                    {
-//                        wpm->pos_reached = true;
-//                    }
-//                }
-//            }
-//            break;
-//        }
-
-//		case MAVLINK_MSG_ID_CMD: // special action from ground station
-//        {
-//            mavlink_cmd_t action;
-//            mavlink_msg_cmd_decode(msg, &action);
-//            if(action.target == mavlink_system.sysid)
-//            {
-//                // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
-//                switch (action.action)
-//                {
-//						//				case MAV_ACTION_LAUNCH:
-//						//					// if (verbose) std::cerr << "Launch received" << std::endl;
-//						//					current_active_wp_id = 0;
-//						//					if (wpm->size>0)
-//						//					{
-//						//						setActive(waypoints[current_active_wp_id]);
-//						//					}
-//						//					else
-//						//						// if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
-//						//					break;
-//
-//						//				case MAV_ACTION_CONTINUE:
-//						//					// if (verbose) std::c
-//						//					err << "Continue received" << std::endl;
-//						//					idle = false;
-//						//					setActive(waypoints[current_active_wp_id]);
-//						//					break;
-//
-//						//				case MAV_ACTION_HALT:
-//						//					// if (verbose) std::cerr << "Halt received" << std::endl;
-//						//					idle = true;
-//						//					break;
-//
-//						//				default:
-//						//					// if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
-//						//					break;
-//                }
-//            }
-//            break;
-//        }
 
 	case MAVLINK_MSG_ID_MISSION_ACK: {
 			mavlink_mission_ack_t wpa;
@@ -617,26 +507,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 				if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
 					if (wpm->current_wp_id == wpm->size - 1) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-						mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
-#else
 
-						if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n");
+						mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
 
-#endif
 						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 						wpm->current_wp_id = 0;
 					}
 				}
 
 			} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
-#else
-
-				if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
-
-#endif
 			}
 
 			break;
@@ -664,13 +544,8 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 							}
 						}
 
-#ifdef MAVLINK_WPM_NO_PRINTF
 						mavlink_missionlib_send_gcs_string("NEW WP SET");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm->current_active_wp_id);
 
-#endif
 						wpm->yaw_reached = false;
 						wpm->pos_reached = false;
 						mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
@@ -678,33 +553,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 						wpm->timestamp_firstinside_orbit = 0;
 
 					} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
 						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
-#else
-
-						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n");
-
-#endif
 					}
 
 				} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
 					mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
-#else
-
-					if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n");
 
-#endif
 				}
 
 			} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-#else
-
-				if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
-
-#endif
 			}
 
 			break;
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
index c78232f11..8245aa560 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.c
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c
@@ -62,15 +62,15 @@
 #include <systemlib/param/param.h>
 #include <drivers/drv_hrt.h>
 
-PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
+PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f);
 PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
 PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
 //PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
 //PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
 
-PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f);
+PARAM_DEFINE_FLOAT(MC_ATT_P, 6.8f);
 PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
-PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f);
+PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f);
 //PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
 //PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
 
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index 0a336be47..adb63186c 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -59,14 +59,14 @@
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
 
-PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */
-PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
-PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f);
 //PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
 //PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f);
 
-PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.0f); /* 0.15 F405 Flamewheel */
-PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f);
+PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */
+PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
 PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
 //PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
 //PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f);	/**< roughly < 500 deg/s limit */
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 336523072..36dd370fb 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -193,7 +193,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	struct vehicle_local_position_setpoint_s local_pos_sp;
 	memset(&local_pos_sp, 0, sizeof(local_pos_sp));
 	struct vehicle_global_position_setpoint_s global_pos_sp;
-	memset(&global_pos_sp, 0, sizeof(local_pos_sp));
+	memset(&global_pos_sp, 0, sizeof(global_pos_sp));
 	struct vehicle_global_velocity_setpoint_s global_vel_sp;
 	memset(&global_vel_sp, 0, sizeof(global_vel_sp));
 
@@ -212,17 +212,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 
-	bool global_pos_sp_reproject = false;
+	bool reset_mission_sp = false;
 	bool global_pos_sp_valid = false;
-	bool local_pos_sp_valid = false;
-	bool reset_sp_z = true;
-	bool reset_sp_xy = true;
+	bool reset_man_sp_z = true;
+	bool reset_man_sp_xy = true;
 	bool reset_int_z = true;
 	bool reset_int_z_manual = false;
 	bool reset_int_xy = true;
 	bool was_armed = false;
-	bool reset_integral = true;
-	bool reset_auto_pos = true;
+	bool reset_auto_sp_xy = true;
+	bool reset_auto_sp_z = true;
+	bool reset_takeoff_sp = true;
 
 	hrt_abstime t_prev = 0;
 	const float alt_ctl_dz = 0.2f;
@@ -270,11 +270,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 				/* use integral_limit_out = tilt_max / 2 */
 				float i_limit;
 
-				if (params.xy_vel_i == 0.0f) {
+				if (params.xy_vel_i > 0.0f) {
 					i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
 
 				} else {
-					i_limit = 1.0f;	// not used really
+					i_limit = 0.0f;	// not used
 				}
 
 				pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
@@ -297,7 +297,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 		if (updated) {
 			orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
 			global_pos_sp_valid = true;
-			global_pos_sp_reproject = true;
+			reset_mission_sp = true;
 		}
 
 		hrt_abstime t = hrt_absolute_time();
@@ -312,8 +312,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 		if (control_mode.flag_armed && !was_armed) {
 			/* reset setpoints and integrals on arming */
-			reset_sp_z = true;
-			reset_sp_xy = true;
+			reset_man_sp_z = true;
+			reset_man_sp_xy = true;
+			reset_auto_sp_z = true;
+			reset_auto_sp_xy = true;
+			reset_takeoff_sp = true;
 			reset_int_z = true;
 			reset_int_xy = true;
 		}
@@ -348,8 +351,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 				/* reset setpoints to current position if needed */
 				if (control_mode.flag_control_altitude_enabled) {
-					if (reset_sp_z) {
-						reset_sp_z = false;
+					if (reset_man_sp_z) {
+						reset_man_sp_z = false;
 						local_pos_sp.z = local_pos.z;
 						mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
 					}
@@ -371,8 +374,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 				}
 
 				if (control_mode.flag_control_position_enabled) {
-					if (reset_sp_xy) {
-						reset_sp_xy = false;
+					if (reset_man_sp_xy) {
+						reset_man_sp_xy = false;
 						local_pos_sp.x = local_pos.x;
 						local_pos_sp.y = local_pos.y;
 						pid_reset_integral(&xy_vel_pids[0]);
@@ -405,41 +408,45 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					}
 				}
 
+				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
 				local_pos_sp.yaw = att_sp.yaw_body;
 
-				/* local position setpoint is valid and can be used for loiter after position controlled mode */
-				local_pos_sp_valid = control_mode.flag_control_position_enabled;
-
-				reset_auto_pos = true;
+				/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
+				reset_auto_sp_xy = !control_mode.flag_control_position_enabled;
+				reset_auto_sp_z = !control_mode.flag_control_altitude_enabled;
+				reset_takeoff_sp = true;
 
 				/* force reprojection of global setpoint after manual mode */
-				global_pos_sp_reproject = true;
+				reset_mission_sp = true;
 
 			} else if (control_mode.flag_control_auto_enabled) {
 				/* AUTO mode, use global setpoint */
 				if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
-					reset_auto_pos = true;
+					reset_auto_sp_xy = true;
+					reset_auto_sp_z = true;
 
 				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
-					if (reset_auto_pos) {
+					if (reset_takeoff_sp) {
+						reset_takeoff_sp = false;
 						local_pos_sp.x = local_pos.x;
 						local_pos_sp.y = local_pos.y;
 						local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
-						local_pos_sp.yaw = att.yaw;
-						local_pos_sp_valid = true;
 						att_sp.yaw_body = att.yaw;
-						reset_auto_pos = false;
 						mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
 					}
 
+					reset_auto_sp_xy = false;
+					reset_auto_sp_z = true;
+
 				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
 					// TODO
-					reset_auto_pos = true;
+					reset_auto_sp_xy = true;
+					reset_auto_sp_z = true;
 
 				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
 					/* init local projection using local position ref */
 					if (local_pos.ref_timestamp != local_ref_timestamp) {
-						global_pos_sp_reproject = true;
+						reset_mission_sp = true;
 						local_ref_timestamp = local_pos.ref_timestamp;
 						double lat_home = local_pos.ref_lat * 1e-7;
 						double lon_home = local_pos.ref_lon * 1e-7;
@@ -447,9 +454,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 						mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
 					}
 
-					if (global_pos_sp_reproject) {
+					if (reset_mission_sp) {
+						reset_mission_sp = false;
 						/* update global setpoint projection */
-						global_pos_sp_reproject = false;
 
 						if (global_pos_sp_valid) {
 							/* global position setpoint valid, use it */
@@ -464,40 +471,50 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 							} else {
 								local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
 							}
-
-							local_pos_sp.yaw = global_pos_sp.yaw;
 							att_sp.yaw_body = global_pos_sp.yaw;
 
 							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
 
 						} else {
-							if (!local_pos_sp_valid) {
+							if (reset_auto_sp_xy) {
+								reset_auto_sp_xy = false;
 								/* local position setpoint is invalid,
 								 * use current position as setpoint for loiter */
 								local_pos_sp.x = local_pos.x;
 								local_pos_sp.y = local_pos.y;
-								local_pos_sp.z = local_pos.z;
 								local_pos_sp.yaw = att.yaw;
-								local_pos_sp_valid = true;
+							}
+
+							if (reset_auto_sp_z) {
+								reset_auto_sp_z = false;
+								local_pos_sp.z = local_pos.z;
 							}
 
 							mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
 						}
 					}
 
-					reset_auto_pos = true;
+					reset_auto_sp_xy = true;
+					reset_auto_sp_z = true;
+				}
+
+				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) {
+					reset_takeoff_sp = true;
 				}
 
 				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
-					global_pos_sp_reproject = true;
+					reset_mission_sp = true;
 				}
 
+				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
+				local_pos_sp.yaw = att_sp.yaw_body;
+
 				/* reset setpoints after AUTO mode */
-				reset_sp_xy = true;
-				reset_sp_z = true;
+				reset_man_sp_xy = true;
+				reset_man_sp_z = true;
 
 			} else {
-				/* no control, loiter or stay on ground */
+				/* no control (failsafe), loiter or stay on ground */
 				if (local_pos.landed) {
 					/* landed: move setpoint down */
 					/* in air: hold altitude */
@@ -508,27 +525,30 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 						mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
 					}
 
-					reset_sp_z = true;
+					reset_man_sp_z = true;
 
 				} else {
 					/* in air: hold altitude */
-					if (reset_sp_z) {
-						reset_sp_z = false;
+					if (reset_man_sp_z) {
+						reset_man_sp_z = false;
 						local_pos_sp.z = local_pos.z;
 						mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
 					}
+
+					reset_auto_sp_z = false;
 				}
 
 				if (control_mode.flag_control_position_enabled) {
-					if (reset_sp_xy) {
-						reset_sp_xy = false;
+					if (reset_man_sp_xy) {
+						reset_man_sp_xy = false;
 						local_pos_sp.x = local_pos.x;
 						local_pos_sp.y = local_pos.y;
 						local_pos_sp.yaw = att.yaw;
-						local_pos_sp_valid = true;
 						att_sp.yaw_body = att.yaw;
 						mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
 					}
+
+					reset_auto_sp_xy = false;
 				}
 			}
 
@@ -540,7 +560,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 				global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
 
 			} else {
-				reset_sp_z = true;
+				reset_man_sp_z = true;
 				global_vel_sp.vz = 0.0f;
 			}
 
@@ -558,7 +578,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 				}
 
 			} else {
-				reset_sp_xy = true;
+				reset_man_sp_xy = true;
 				global_vel_sp.vx = 0.0f;
 				global_vel_sp.vy = 0.0f;
 			}
@@ -640,12 +660,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 		} else {
 			/* position controller disabled, reset setpoints */
-			reset_sp_z = true;
-			reset_sp_xy = true;
+			reset_man_sp_z = true;
+			reset_man_sp_xy = true;
 			reset_int_z = true;
 			reset_int_xy = true;
-			global_pos_sp_reproject = true;
-			reset_auto_pos = true;
+			reset_mission_sp = true;
+			reset_auto_sp_xy = true;
+			reset_auto_sp_z = true;
 		}
 
 		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
index bf9578ea3..b7041e4d5 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -54,7 +54,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f);
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f);
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f);
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f);
-PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
 PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
 
 int parameters_init(struct multirotor_position_control_param_handles *h)
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
new file mode 100644
index 000000000..0404b06c7
--- /dev/null
+++ b/src/modules/navigator/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 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.
+#
+############################################################################
+
+#
+# Main Navigation Controller
+#
+
+MODULE_COMMAND	= navigator
+
+SRCS		= navigator_main.cpp \
+		  navigator_params.c
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
new file mode 100644
index 000000000..f6c44444a
--- /dev/null
+++ b/src/modules/navigator/navigator_main.cpp
@@ -0,0 +1,604 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	Lorenz Meier
+ *
+ * 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 navigator_main.c
+ * Implementation of the main navigation state machine.
+ *
+ * Handles missions, geo fencing and failsafe navigation behavior.
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_global_position_set_triplet.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/mission.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+
+/**
+ * navigator app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
+
+class Navigator
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Navigator();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~Navigator();
+
+	/**
+	 * Start the sensors task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+private:
+
+	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	int		_navigator_task;			/**< task handle for sensor task */
+
+	int		_global_pos_sub;
+	int		_att_sub;			/**< vehicle attitude subscription */
+	int		_attitude_sub;			/**< raw rc channels data subscription */
+	int		_airspeed_sub;			/**< airspeed subscription */
+	int		_vstatus_sub;			/**< vehicle status subscription */
+	int 		_params_sub;			/**< notification of parameter updates */
+	int 		_manual_control_sub;		/**< notification of manual control updates */
+	int		_mission_sub;
+
+	orb_advert_t	_triplet_pub;			/**< position setpoint */
+
+	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
+	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
+	struct airspeed_s				_airspeed;		/**< airspeed */
+	struct vehicle_status_s				_vstatus;		/**< vehicle status */
+	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
+	struct vehicle_global_position_set_triplet_s	_global_triplet;	/**< triplet of global setpoints */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	unsigned	_mission_items_maxcount;				/**< maximum number of mission items supported */
+	struct	mission_item_s 				* _mission_items;	/**< storage for mission items */
+	bool		_mission_valid;						/**< flag if mission is valid */
+
+	/** manual control states */
+	float		_seatbelt_hold_heading;		/**< heading the system should hold in seatbelt mode */
+	float		_loiter_hold_lat;
+	float		_loiter_hold_lon;
+	float		_loiter_hold_alt;
+	bool		_loiter_hold;
+
+	struct {
+		float throttle_cruise;
+	}		_parameters;			/**< local copies of interesting parameters */
+
+	struct {
+		param_t throttle_cruise;
+
+	}		_parameter_handles;		/**< handles for interesting parameters */
+
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int		parameters_update();
+
+	/**
+	 * Update control outputs
+	 *
+	 */
+	void		control_update();
+
+	/**
+	 * Check for changes in vehicle status.
+	 */
+	void		vehicle_status_poll();
+
+	/**
+	 * Check for position updates.
+	 */
+	void		vehicle_attitude_poll();
+
+	/**
+	 * Check for set triplet updates.
+	 */
+	void		mission_poll();
+
+	/**
+	 * Control throttle.
+	 */
+	float		control_throttle(float energy_error);
+
+	/**
+	 * Control pitch.
+	 */
+	float		control_pitch(float altitude_error);
+
+	void calculate_airspeed_errors();
+	void calculate_gndspeed_undershoot();
+	void calculate_altitude_error();
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * Main sensor collection task.
+	 */
+	void		task_main() __attribute__((noreturn));
+};
+
+namespace navigator
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+Navigator	*g_navigator;
+}
+
+Navigator::Navigator() :
+
+	_task_should_exit(false),
+	_navigator_task(-1),
+
+/* subscriptions */
+	_global_pos_sub(-1),
+	_att_sub(-1),
+	_airspeed_sub(-1),
+	_vstatus_sub(-1),
+	_params_sub(-1),
+	_manual_control_sub(-1),
+
+/* publications */
+	_triplet_pub(-1),
+
+/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
+/* states */
+	_mission_items_maxcount(20),
+	_mission_valid(false),
+	_loiter_hold(false)
+{
+	_mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_items_maxcount);
+	if (!_mission_items) {
+		_mission_items_maxcount = 0;
+		warnx("no free RAM to allocate mission, rejecting any waypoints");
+	}
+
+	_parameter_handles.throttle_cruise = param_find("NAV_DUMMY");
+
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+Navigator::~Navigator()
+{
+	if (_navigator_task != -1) {
+
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_navigator_task);
+				break;
+			}
+		} while (_navigator_task != -1);
+	}
+
+	navigator::g_navigator = nullptr;
+}
+
+int
+Navigator::parameters_update()
+{
+
+	//param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
+
+	return OK;
+}
+
+void
+Navigator::vehicle_status_poll()
+{
+	bool vstatus_updated;
+
+	/* Check HIL state if vehicle status has changed */
+	orb_check(_vstatus_sub, &vstatus_updated);
+
+	if (vstatus_updated) {
+
+		orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
+	}
+}
+
+void
+Navigator::vehicle_attitude_poll()
+{
+	/* check if there is a new position */
+	bool att_updated;
+	orb_check(_att_sub, &att_updated);
+
+	if (att_updated) {
+		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+	}
+}
+
+void
+Navigator::mission_poll()
+{
+	/* check if there is a new setpoint */
+	bool mission_updated;
+	orb_check(_mission_sub, &mission_updated);
+
+	if (mission_updated) {
+
+		struct mission_s mission;
+		orb_copy(ORB_ID(mission), _mission_sub, &mission);
+
+		// XXX this is not optimal yet, but a first prototype /
+		// test implementation
+
+		if (mission.count <= _mission_items_maxcount) {
+			/*
+			 * Perform an atomic copy & state update
+			 */
+			irqstate_t flags = irqsave();
+
+			memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s));
+			_mission_valid = true;
+
+			irqrestore(flags);
+		} else {
+			warnx("mission larger than storage space");
+		}
+	}
+}
+
+void
+Navigator::task_main_trampoline(int argc, char *argv[])
+{
+	navigator::g_navigator->task_main();
+}
+
+void
+Navigator::task_main()
+{
+
+	/* inform about start */
+	warnx("Initializing..");
+	fflush(stdout);
+
+	/*
+	 * do subscriptions
+	 */
+	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	_mission_sub = orb_subscribe(ORB_ID(mission));
+	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+
+	/* rate limit vehicle status updates to 5Hz */
+	orb_set_interval(_vstatus_sub, 200);
+	/* rate limit position updates to 50 Hz */
+	orb_set_interval(_global_pos_sub, 20);
+
+	parameters_update();
+
+	/* wakeup source(s) */
+	struct pollfd fds[2];
+
+	/* Setup of loop */
+	fds[0].fd = _params_sub;
+	fds[0].events = POLLIN;
+	fds[1].fd = _global_pos_sub;
+	fds[1].events = POLLIN;
+
+	while (!_task_should_exit) {
+
+		/* wait for up to 500ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+		/* timed out - periodic check for _task_should_exit, etc. */
+		if (pret == 0)
+			continue;
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			continue;
+		}
+
+		perf_begin(_loop_perf);
+
+		/* check vehicle status for changes to publication state */
+		vehicle_status_poll();
+
+		/* only update parameters if they changed */
+		if (fds[0].revents & POLLIN) {
+			/* read from param to clear updated flag */
+			struct parameter_update_s update;
+			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+
+			/* update parameters from storage */
+			parameters_update();
+		}
+
+		/* only run controller if position changed */
+		if (fds[1].revents & POLLIN) {
+
+
+			static uint64_t last_run = 0;
+			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
+			last_run = hrt_absolute_time();
+
+			/* guard against too large deltaT's */
+			if (deltaT > 1.0f)
+				deltaT = 0.01f;
+
+			/* load local copies */
+			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
+
+			vehicle_attitude_poll();
+
+			mission_poll();
+
+			math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
+			// Current waypoint
+			math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
+			// Global position
+			math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
+
+			/* AUTONOMOUS FLIGHT */
+
+			if (1 /* autonomous flight */) {
+
+				/* execute navigation once we have a setpoint */
+				if (_mission_valid) {
+
+					// Next waypoint
+					math::Vector2f prev_wp;
+
+					if (_global_triplet.previous_valid) {
+						prev_wp.setX(_global_triplet.previous.lat / 1e7f);
+						prev_wp.setY(_global_triplet.previous.lon / 1e7f);
+
+					} else {
+						/*
+						 * No valid next waypoint, go for heading hold.
+						 * This is automatically handled by the L1 library.
+						 */
+						prev_wp.setX(_global_triplet.current.lat / 1e7f);
+						prev_wp.setY(_global_triplet.current.lon / 1e7f);
+
+					}
+
+
+
+					/********   MAIN NAVIGATION STATE MACHINE   ********/
+
+					// XXX to be put in its own class
+
+					if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
+						/* waypoint is a plain navigation waypoint */
+						
+
+					} else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+						_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+						_global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+
+						/* waypoint is a loiter waypoint */
+						
+					}
+
+					// XXX at this point we always want no loiter hold if a
+					// mission is active
+					_loiter_hold = false;
+
+				} else {
+
+					if (!_loiter_hold) {
+						_loiter_hold_lat = _global_pos.lat / 1e7f;
+						_loiter_hold_lon = _global_pos.lon / 1e7f;
+						_loiter_hold_alt = _global_pos.alt;
+						_loiter_hold = true;
+					}
+
+					//_parameters.loiter_hold_radius
+				}
+
+			} else if (0/* seatbelt mode enabled */) {
+
+				/** SEATBELT FLIGHT **/
+				continue;
+
+			} else {
+
+				/** MANUAL FLIGHT **/
+
+				/* no flight mode applies, do not publish an attitude setpoint */
+				continue;
+			}
+
+			/********   MAIN NAVIGATION STATE MACHINE   ********/
+
+			if (_global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
+				// XXX define launch position and return
+
+			} else if (_global_triplet.current.nav_cmd == NAV_CMD_LAND) {
+				// XXX flared descent on final approach
+
+			} else if (_global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
+
+				/* apply minimum pitch if altitude has not yet been reached */
+				if (_global_pos.alt < _global_triplet.current.altitude) {
+					_att_sp.pitch_body = math::max(_att_sp.pitch_body, _global_triplet.current.param1);
+				}
+			}
+
+			/* lazily publish the setpoint only once available */
+			if (_triplet_pub > 0) {
+				/* publish the attitude setpoint */
+				orb_publish(ORB_ID(vehicle_global_position_set_triplet), _triplet_pub, &_global_triplet);
+
+			} else {
+				/* advertise and publish */
+				_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &_global_triplet);
+			}
+
+		}
+
+		perf_end(_loop_perf);
+	}
+
+	warnx("exiting.\n");
+
+	_navigator_task = -1;
+	_exit(0);
+}
+
+int
+Navigator::start()
+{
+	ASSERT(_navigator_task == -1);
+
+	/* start the task */
+	_navigator_task = task_spawn_cmd("navigator",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2048,
+				       (main_t)&Navigator::task_main_trampoline,
+				       nullptr);
+
+	if (_navigator_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+int navigator_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		errx(1, "usage: navigator {start|stop|status}");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (navigator::g_navigator != nullptr)
+			errx(1, "already running");
+
+		navigator::g_navigator = new Navigator;
+
+		if (navigator::g_navigator == nullptr)
+			errx(1, "alloc failed");
+
+		if (OK != navigator::g_navigator->start()) {
+			delete navigator::g_navigator;
+			navigator::g_navigator = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (navigator::g_navigator == nullptr)
+			errx(1, "not running");
+
+		delete navigator::g_navigator;
+		navigator::g_navigator = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (navigator::g_navigator) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
new file mode 100644
index 000000000..06df9a452
--- /dev/null
+++ b/src/modules/navigator/navigator_params.c
@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 navigator_params.c
+ *
+ * Parameters defined by the navigator task.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Navigator parameters, accessible via MAVLink
+ *
+ */
+
+PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f);
+
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 932f61088..10007bf96 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -259,7 +259,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						local_pos.ref_timestamp = hrt_absolute_time();
 						local_pos.z_valid = true;
 						local_pos.v_z_valid = true;
-						local_pos.global_z = true;
+						local_pos.z_global = true;
 					}
 				}
 			}
@@ -597,7 +597,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			local_pos.timestamp = t;
 			local_pos.xy_valid = can_estimate_xy && gps_valid;
 			local_pos.v_xy_valid = can_estimate_xy;
-			local_pos.global_xy = local_pos.xy_valid && gps_valid;	// will make sense when local position sources (e.g. vicon) will be implemented
+			local_pos.xy_global = local_pos.xy_valid && gps_valid;	// will make sense when local position sources (e.g. vicon) will be implemented
 			local_pos.x = x_est[0];
 			local_pos.vx = x_est[1];
 			local_pos.y = y_est[0];
@@ -610,9 +610,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
 
 			/* publish global position */
-			global_pos.valid = local_pos.global_xy;
+			global_pos.valid = local_pos.xy_global;
 
-			if (local_pos.global_xy) {
+			if (local_pos.xy_global) {
 				double est_lat, est_lon;
 				map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
 				global_pos.lat = (int32_t)(est_lat * 1e7);
@@ -630,7 +630,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				global_pos.relative_alt = -local_pos.z;
 			}
 
-			if (local_pos.global_z) {
+			if (local_pos.z_global) {
 				global_pos.alt = local_pos.ref_alt - local_pos.z;
 			}
 
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 796c6cd9f..5c621cfb2 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -108,9 +108,11 @@ controls_tick() {
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
-	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count);
-	if (sbus_updated)
+	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */);
+	if (sbus_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
+		r_raw_rc_count = 8;
+	}
 	perf_end(c_gather_sbus);
 
 	/*
@@ -124,8 +126,6 @@ controls_tick() {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
 	perf_end(c_gather_ppm);
 
-	ASSERT(r_raw_rc_count <= PX4IO_CONTROL_CHANNELS);
-
 	/*
 	 * In some cases we may have received a frame, but input has still
 	 * been lost.
@@ -286,7 +286,7 @@ controls_tick() {
 		 * requested override.
 		 *
 		 */
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH))
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH))
 			override = true;
 
 		if (override) {
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index 206e27db5..433976656 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -243,28 +243,35 @@ dsm_init(const char *device)
 void
 dsm_bind(uint16_t cmd, int pulses)
 {
+#if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2)
+	#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM
+#else
 	const uint32_t usart1RxAsOutp =
 		GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
 
 	if (dsm_fd < 0)
 		return;
 
-#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
-	// XXX implement
-	#warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
-#else
 	switch (cmd) {
 
 	case dsm_bind_power_down:
 
 		/*power down DSM satellite*/
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 		POWER_RELAY1(0);
+#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
+		POWER_SPEKTRUM(0);
+#endif
 		break;
 
 	case dsm_bind_power_up:
 
 		/*power up DSM satellite*/
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 		POWER_RELAY1(1);
+#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
+		POWER_SPEKTRUM(1);
+#endif
 		dsm_guess_format(true);
 		break;
 
@@ -387,8 +394,10 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 		values[channel] = value;
 	}
 
-	if (dsm_channel_shift == 11)
+	if (dsm_channel_shift == 11) {
+		/* Set the 11-bit data indicator */
 		*num_values |= 0x8000;
+	}
 
 	/*
 	 * XXX Note that we may be in failsafe here; we need to work out how to detect that.
@@ -412,7 +421,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
  * Upon receiving a full dsm frame we attempt to decode it.
  *
  * @param[out] values pointer to per channel array of decoded values
- * @param[out] num_values pointer to number of raw channel values returned
+ * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data
  * @return true=decoded raw channel values updated, false=no update
  */
 bool
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 0edd91b24..30aff7d20 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -221,7 +221,7 @@ mixer_tick(void)
 		}
 
 		/* do the calculations during the ramp for all at once */
-		if(esc_state == ESC_RAMP) {
+		if (esc_state == ESC_RAMP) {
 			ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
 		}
 
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index f5fa0ba75..0e2cd1689 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -165,11 +165,13 @@
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
 
+#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 #define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
 #define PX4IO_P_SETUP_RELAYS_POWER1		(1<<0)	/* hardware rev [1] power relay 1 */
 #define PX4IO_P_SETUP_RELAYS_POWER2		(1<<1)	/* hardware rev [1] power relay 2 */
 #define PX4IO_P_SETUP_RELAYS_ACC1		(1<<2)	/* hardware rev [1] accessory power 1 */
 #define PX4IO_P_SETUP_RELAYS_ACC2		(1<<3)	/* hardware rev [1] accessory power 2 */
+#endif
 
 #define PX4IO_P_SETUP_VBATT_SCALE		6	/* hardware rev [1] battery voltage correction factor (float) */
 #define PX4IO_P_SETUP_VSERVO_SCALE		6	/* hardware rev [2] servo voltage correction factor (float) */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index dea67043e..66c4ca906 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -51,7 +51,7 @@
  */
 #define PX4IO_SERVO_COUNT		8
 #define PX4IO_CONTROL_CHANNELS		8
-#define PX4IO_INPUT_CHANNELS		12
+#define PX4IO_INPUT_CHANNELS		8 // XXX this should be 18 channels
 
 /*
  * Debug logging
@@ -100,7 +100,9 @@ extern uint16_t			r_page_servo_idle[];	/* PX4IO_PAGE_IDLE_PWM */
 #define r_setup_pwm_rates	r_page_setup[PX4IO_P_SETUP_PWM_RATES]
 #define r_setup_pwm_defaultrate	r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE]
 #define r_setup_pwm_altrate	r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE]
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 #define r_setup_relays		r_page_setup[PX4IO_P_SETUP_RELAYS]
+#endif
 
 #define r_control_values	(&r_page_controls[0])
 
@@ -200,7 +202,7 @@ extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
 extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
-extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
+extern bool	sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels);
 
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 9c95fd1c5..8cb21e54f 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -145,7 +145,9 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_PWM_RATES]		= 0,
 	[PX4IO_P_SETUP_PWM_DEFAULTRATE]		= 50,
 	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 	[PX4IO_P_SETUP_RELAYS]			= 0,
+#endif
 #ifdef ADC_VSERVO
 	[PX4IO_P_SETUP_VSERVO_SCALE]		= 10000,
 #else
@@ -462,22 +464,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
 			break;
 
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 		case PX4IO_P_SETUP_RELAYS:
 			value &= PX4IO_P_SETUP_RELAYS_VALID;
 			r_setup_relays = value;
-#ifdef POWER_RELAY1
 			POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0);
-#endif
-#ifdef POWER_RELAY2
 			POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0);
-#endif
-#ifdef POWER_ACC1
 			POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0);
-#endif
-#ifdef POWER_ACC2
 			POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0);
-#endif
 			break;
+#endif
 
 		case PX4IO_P_SETUP_VBATT_SCALE:
 			r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value;
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 073ddeaba..c523df6ca 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -66,7 +66,7 @@ static unsigned partial_frame_count;
 
 unsigned sbus_frame_drops;
 
-static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values);
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_channels);
 
 int
 sbus_init(const char *device)
@@ -97,7 +97,7 @@ sbus_init(const char *device)
 }
 
 bool
-sbus_input(uint16_t *values, uint16_t *num_values)
+sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -154,7 +154,7 @@ sbus_input(uint16_t *values, uint16_t *num_values)
 	 * decode it.
 	 */
 	partial_frame_count = 0;
-	return sbus_decode(now, values, num_values);
+	return sbus_decode(now, values, num_values, max_channels);
 }
 
 /*
@@ -194,7 +194,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
 };
 
 static bool
-sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_values)
 {
 	/* check frame boundary markers to avoid out-of-sync cases */
 	if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
@@ -214,8 +214,8 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 	/* we have received something we think is a frame */
 	last_frame_time = frame_time;
 
-	unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ?
-			     SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
+	unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ?
+			     SBUS_INPUT_CHANNELS : max_values;
 
 	/* use the decoder matrix to extract channel data */
 	for (unsigned channel = 0; channel < chancount; channel++) {
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index e83fb7dd3..ec8033202 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1046,6 +1046,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
 				log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
 				log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
+				log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
+				log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
+				log_msg.body.log_LPOS.landed = buf.local_pos.landed;
 				LOGBUFFER_WRITE_AND_COUNT(LPOS);
 			}
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 4eeb65a87..0e88da054 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -109,6 +109,9 @@ struct log_LPOS_s {
 	int32_t ref_lat;
 	int32_t ref_lon;
 	float ref_alt;
+	uint8_t xy_flags;
+	uint8_t z_flags;
+	uint8_t landed;
 };
 
 /* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -250,8 +253,26 @@ struct log_GVSP_s {
 	float vz;
 };
 
+/* --- FWRV - FIRMWARE REVISION --- */
+#define LOG_FWRV_MSG 20
+struct log_FWRV_s {
+	char    fw_revision[64];
+};
+
 #pragma pack(pop)
 
+
+/*
+ GIT_VERSION is defined at build time via a Makefile call to the
+ git command line. We create a fake log message format for 
+ the firmware revision "FWRV" that is written to every log
+ header. This makes it easier to determine which version
+ of the firmware was running when a log was created.
+ */
+#define FREEZE_STR(s) #s
+#define STRINGIFY(s) FREEZE_STR(s)
+#define FW_VERSION_STR  STRINGIFY(GIT_VERSION)
+
 /* construct list of all message formats */
 
 static const struct log_format_s log_formats[] = {
@@ -260,7 +281,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
-	LOG_FORMAT(LPOS, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"),
+	LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
 	LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
@@ -274,6 +295,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
+	LOG_FORMAT(FWRV,"Z",FW_VERSION_STR),
 };
 
 static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 992abf2cc..4d719e0e1 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -164,9 +164,10 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC15_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f);
 
-
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
-PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */
+#endif
+PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
@@ -174,7 +175,8 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
 /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
 /* PX4IOAR: 0.00838095238 */
 /* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */
-PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
+/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
 #endif
 
 PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index e98c4d548..085da905c 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1516,8 +1516,7 @@ Sensors::task_main()
 {
 
 	/* inform about start */
-	printf("[sensors] Initializing..\n");
-	fflush(stdout);
+	warnx("Initializing..");
 
 	/* start individual sensors */
 	accel_init();
diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp
index df0dfe838..b1bb1a66d 100644
--- a/src/modules/systemlib/mixer/mixer.cpp
+++ b/src/modules/systemlib/mixer/mixer.cpp
@@ -142,6 +142,22 @@ NullMixer *
 NullMixer::from_text(const char *buf, unsigned &buflen)
 {
 	NullMixer *nm = nullptr;
+	const char *end = buf + buflen;
+
+	/* enforce that the mixer ends with space or a new line */
+	for (int i = buflen - 1; i >= 0; i--) {
+		if (buf[i] == '\0')
+			continue;
+
+		/* require a space or newline at the end of the buffer, fail on printable chars */
+		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
+			/* found a line ending or space, so no split symbols / numbers. good. */
+			break;
+		} else {
+			return nm;
+		}
+
+	}
 
 	if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
 		nm = new NullMixer;
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 8ded0b05c..2446cc3fb 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -181,6 +181,23 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	char geomname[8];
 	int s[4];
 	int used;
+	const char *end = buf + buflen;
+
+	/* enforce that the mixer ends with space or a new line */
+	for (int i = buflen - 1; i >= 0; i--) {
+		if (buf[i] == '\0')
+			continue;
+
+		/* require a space or newline at the end of the buffer, fail on printable chars */
+		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
+			/* found a line ending or space, so no split symbols / numbers. good. */
+			break;
+		} else {
+			debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
+			return nullptr;
+		}
+
+	}
 
 	if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) {
 		debug("multirotor parse failed on '%s'", buf);
diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp
index 07dc5f37f..c8434f991 100644
--- a/src/modules/systemlib/mixer/mixer_simple.cpp
+++ b/src/modules/systemlib/mixer/mixer_simple.cpp
@@ -55,7 +55,7 @@
 #include "mixer.h"
 
 #define debug(fmt, args...)	do { } while(0)
-//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+// #define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
 
 SimpleMixer::SimpleMixer(ControlCallback control_cb,
 			 uintptr_t cb_handle,
@@ -98,14 +98,17 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
 {
 	int ret;
 	int s[5];
+	int n = -1;
 	
 	buf = findtag(buf, buflen, 'O');
-	if ((buf == nullptr) || (buflen < 12))
+	if ((buf == nullptr) || (buflen < 12)) {
+		debug("output parser failed finding tag, ret: '%s'", buf);
 		return -1;
+	}
 
-	if ((ret = sscanf(buf, "O: %d %d %d %d %d",
-			  &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) {
-		debug("scaler parse failed on '%s' (got %d)", buf, ret);
+	if ((ret = sscanf(buf, "O: %d %d %d %d %d %n",
+			  &s[0], &s[1], &s[2], &s[3], &s[4], &n)) != 5) {
+		debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
 		return -1;
 	}
 	skipline(buf, buflen);
@@ -126,8 +129,10 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 	int s[5];
 
 	buf = findtag(buf, buflen, 'S');
-	if ((buf == nullptr) || (buflen < 16))
+	if ((buf == nullptr) || (buflen < 16)) {
+		debug("contorl parser failed finding tag, ret: '%s'", buf);
 		return -1;
+	}
 
 	if (sscanf(buf, "S: %u %u %d %d %d %d %d",
 		   &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) {
@@ -156,6 +161,22 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 	int used;
 	const char *end = buf + buflen;
 
+	/* enforce that the mixer ends with space or a new line */
+	for (int i = buflen - 1; i >= 0; i--) {
+		if (buf[i] == '\0')
+			continue;
+
+		/* require a space or newline at the end of the buffer, fail on printable chars */
+		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
+			/* found a line ending or space, so no split symbols / numbers. good. */
+			break;
+		} else {
+			debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
+			goto out;
+		}
+
+	}
+
 	/* get the base info for the mixer */
 	if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
 		debug("simple parse failed on '%s'", buf);
@@ -173,22 +194,27 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 
 	mixinfo->control_count = inputs;
 
-	if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler))
+	if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) {
+		debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf);
 		goto out;
+	}
 
 	for (unsigned i = 0; i < inputs; i++) {
 		if (parse_control_scaler(end - buflen, buflen,
 					 mixinfo->controls[i].scaler,
 					 mixinfo->controls[i].control_group,
-					 mixinfo->controls[i].control_index))
+					 mixinfo->controls[i].control_index)) {
+			debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf);
 			goto out;
+		}
+
 	}
 
 	sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
 
 	if (sm != nullptr) {
 		mixinfo = nullptr;
-		debug("loaded mixer with %d inputs", inputs);
+		debug("loaded mixer with %d input(s)", inputs);
 
 	} else {
 		debug("could not allocate memory for mixer");
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index c69de52b7..ccdb2ea38 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -48,6 +48,7 @@
 #include <unistd.h>
 #include <systemlib/err.h>
 #include <errno.h>
+#include <semaphore.h>
 
 #include <sys/stat.h>
 
@@ -95,18 +96,20 @@ ORB_DEFINE(parameter_update, struct parameter_update_s);
 /** parameter update topic handle */
 static orb_advert_t param_topic = -1;
 
+static sem_t param_sem = { .semcount = 1 };
+
 /** lock the parameter store */
 static void
 param_lock(void)
 {
-	/* XXX */
+	//do {} while (sem_wait(&param_sem) != 0);
 }
 
 /** unlock the parameter store */
 static void
 param_unlock(void)
 {
-	/* XXX */
+	//sem_post(&param_sem);
 }
 
 /** assert that the parameter store is locked */
@@ -506,29 +509,59 @@ int
 param_save_default(void)
 {
 	int result;
+	unsigned retries = 0;
 
 	/* delete the file in case it exists */
 	struct stat buffer;
 	if (stat(param_get_default_file(), &buffer) == 0) {
-		result = unlink(param_get_default_file());
+
+		do {
+			result = unlink(param_get_default_file());
+			if (result != 0) {
+				retries++;
+				usleep(1000 * retries);
+			}
+		} while (result != OK && retries < 10);
+
 		if (result != OK)
 			warnx("unlinking file %s failed.", param_get_default_file());
 	}
 
 	/* create the file */
-	int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
+	int fd;
+
+	do {
+		/* do another attempt in case the unlink call is not synced yet */
+		fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
+		if (fd < 0) {
+			retries++;
+			usleep(1000 * retries);
+		}
+
+	} while (fd < 0 && retries < 10);
 
 	if (fd < 0) {
+		
 		warn("opening '%s' for writing failed", param_get_default_file());
 		return fd;
 	}
 
-	result = param_export(fd, false);
+	do {
+		result = param_export(fd, false);
+
+		if (result != OK) {
+			retries++;
+			usleep(1000 * retries);
+		}
+
+	} while (result != 0 && retries < 10);
+
+
 	close(fd);
 
-	if (result != 0) {
+	if (result != OK) {
 		warn("error exporting parameters to '%s'", param_get_default_file());
-		unlink(param_get_default_file());
+		(void)unlink(param_get_default_file());
 		return result;
 	}
 
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 739503ed4..77c952f52 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -167,20 +167,26 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		d = 0.0f;
 	}
 
-	// Calculate the error integral and check for saturation
-	i = pid->integral + (error * dt);
-
-	if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) ||
-			fabsf(i) > pid->intmax) {
-		i = pid->integral;		// If saturated then do not update integral value
-		pid->saturated = 1;
+	if (pid->ki > 0.0f) {
+		// Calculate the error integral and check for saturation
+		i = pid->integral + (error * dt);
+
+		if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) ||
+		    fabsf(i) > pid->intmax) {
+			i = pid->integral;		// If saturated then do not update integral value
+			pid->saturated = 1;
+
+		} else {
+			if (!isfinite(i)) {
+				i = 0.0f;
+			}
 
-	} else {
-		if (!isfinite(i)) {
-			i = 0.0f;
+			pid->integral = i;
+			pid->saturated = 0;
 		}
 
-		pid->integral = i;
+	} else {
+		i = 0.0f;
 		pid->saturated = 0;
 	}
 
diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index 31ff014de..e6e4743c6 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -86,7 +86,8 @@ enum VEHICLE_CMD
 	VEHICLE_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position|  */
 	VEHICLE_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|  */
 	VEHICLE_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm|  */
-	VEHICLE_CMD_ENUM_END=401, /*  | */
+	VEHICLE_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|  */
+	VEHICLE_CMD_ENUM_END=501, /*  | */
 };
 
 /**
@@ -119,7 +120,7 @@ struct vehicle_command_s
 	float param5;			/**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param6;			/**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param7;			/**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum.   */
-	uint16_t command;		/**< Command ID, as defined MAVLink by VEHICLE_CMD enum.   */
+	enum VEHICLE_CMD command;		/**< Command ID, as defined MAVLink by VEHICLE_CMD enum.   */
 	uint8_t target_system;		/**< System which should execute the command  */
 	uint8_t target_component;	/**< Component which should execute the command, 0 for all components  */
 	uint8_t source_system;		/**< System sending the command  */
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index 0fc0ed5c9..143734e37 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -62,17 +62,17 @@
 struct vehicle_global_position_s
 {
 	uint64_t timestamp;		/**< time of this estimate, in microseconds since system start */
-	uint64_t time_gps_usec; /**< GPS timestamp in microseconds							   */
-	bool valid;				/**< true if position satisfies validity criteria of estimator */
+	uint64_t time_gps_usec; 	/**< GPS timestamp in microseconds							   */
+	bool valid;			/**< true if position satisfies validity criteria of estimator */
 
 	int32_t lat;			/**< Latitude in 1E7 degrees							 	   */
 	int32_t lon;			/**< Longitude in 1E7 degrees							 	   */
-	float alt;				/**< Altitude in meters									 	   */
+	float alt;			/**< Altitude in meters									 	   */
 	float relative_alt;		/**< Altitude above home position in meters, 				   */
-	float vx; 				/**< Ground X velocity, m/s in NED				 			   */
-	float vy;				/**< Ground Y velocity, m/s in NED							   */
-	float vz;				/**< Ground Z velocity, m/s	in NED 							   */
-	float yaw; 				/**< Compass heading in radians -PI..+PI.					   */
+	float vx; 			/**< Ground X velocity, m/s in NED				 			   */
+	float vy;			/**< Ground Y velocity, m/s in NED							   */
+	float vz;			/**< Ground Z velocity, m/s	in NED 							   */
+	float yaw; 			/**< Compass heading in radians -PI..+PI.					   */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 0a7fb4e9d..1639a08c2 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -56,9 +56,9 @@
 struct vehicle_gps_position_s
 {
 	uint64_t timestamp_position;			/**< Timestamp for position information */
-	int32_t lat;					/**< Latitude in 1E7 degrees */
-	int32_t lon;					/**< Longitude in 1E7 degrees */
-	int32_t alt;					/**< Altitude in 1E3 meters (millimeters) above MSL  */
+	int32_t lat;					/**< Latitude in 1E-7 degrees */
+	int32_t lon;					/**< Longitude in 1E-7 degrees */
+	int32_t alt;					/**< Altitude in 1E-3 meters (millimeters) above MSL  */
 	
 	uint64_t timestamp_variance;
 	float s_variance_m_s;				/**< speed accuracy estimate m/s */
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 31a0e632b..427153782 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -70,8 +70,8 @@ struct vehicle_local_position_s
 	/* Heading */
 	float yaw;
 	/* Reference position in GPS / WGS84 frame */
-	bool global_xy;			/**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
-	bool global_z;			/**< true if z is valid and has valid global reference (ref_alt) */
+	bool xy_global;			/**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
+	bool z_global;			/**< true if z is valid and has valid global reference (ref_alt) */
 	uint64_t ref_timestamp;	/**< Time when reference position was set */
 	int32_t ref_lat;		/**< Reference point latitude in 1E7 degrees */
 	int32_t ref_lon;		/**< Reference point longitude in 1E7 degrees */
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
index 188fa4e37..608c9fff1 100644
--- a/src/systemcmds/esc_calib/esc_calib.c
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -1,6 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   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
@@ -45,6 +46,7 @@
 #include <stdbool.h>
 #include <unistd.h>
 #include <fcntl.h>
+#include <poll.h>
 #include <sys/mount.h>
 #include <sys/ioctl.h>
 #include <sys/stat.h>
@@ -63,7 +65,7 @@
 static void	usage(const char *reason);
 __EXPORT int	esc_calib_main(int argc, char *argv[]);
 
-#define MAX_CHANNELS 8
+#define MAX_CHANNELS 14
 
 static void
 usage(const char *reason)
@@ -82,22 +84,26 @@ usage(const char *reason)
 int
 esc_calib_main(int argc, char *argv[])
 {
-	const char *dev = PWM_OUTPUT_DEVICE_PATH;
+	char *dev = PWM_OUTPUT_DEVICE_PATH;
 	char *ep;
 	bool channels_selected[MAX_CHANNELS] = {false};
 	int ch;
 	int ret;
 	char c;
 
+	struct pollfd fds;
+	fds.fd = 0; /* stdin */
+	fds.events = POLLIN;
+
 	if (argc < 2)
 		usage(NULL);
 
-	while ((ch = getopt(argc, argv, "d:")) != EOF) {
+	while ((ch = getopt(argc-1, argv, "d:")) != EOF) {
 		switch (ch) {
 		
 		case 'd':
 			dev = optarg;
-			argc--;
+			argc-=2;
 			break;
 
 		default:
@@ -105,7 +111,7 @@ esc_calib_main(int argc, char *argv[])
 		}
 	}
 
-	if(argc < 1) {
+	if(argc < 2) {
 		usage("no channels provided");
 	}
 
@@ -123,35 +129,33 @@ esc_calib_main(int argc, char *argv[])
 		}
 	}
 
-	/* Wait for confirmation */
-	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
-	if (!console)
-		err(1, "failed opening console");
-
-	warnx("ATTENTION, please remove or fix props before starting calibration!\n"
+	printf("\nATTENTION, please remove or fix propellers before starting calibration!\n"
 		"\n"
-		"Also press the arming switch first for safety off\n"
+		"Make sure\n"
+		"\t - that the ESCs are not powered\n"
+		"\t - that safety is off (two short blinks)\n"
+		"\t - that the controllers are stopped\n"
 		"\n"
-		"Do you really want to start calibration: y or n?\n");
+		"Do you want to start calibration now: y or n?\n");
 
 	/* wait for user input */
 	while (1) {
 		
-		if (read(console, &c, 1) == 1) {
+		ret = poll(&fds, 1, 0);
+		if (ret > 0) {
+
+			read(0, &c, 1);
 			if (c == 'y' || c == 'Y') {
 				
 				break;
 			} else if (c == 0x03 || c == 0x63 || c == 'q') {
-				warnx("ESC calibration exited");
-				close(console);
+				printf("ESC calibration exited\n");
 				exit(0);
 			} else if (c == 'n' || c == 'N') {
-				warnx("ESC calibration aborted");
-				close(console);
+				printf("ESC calibration aborted\n");
 				exit(0);
 			} else {
-				warnx("Unknown input, ESC calibration aborted");
-				close(console);
+				printf("Unknown input, ESC calibration aborted\n");
 				exit(0);
 			} 
 		}
@@ -163,23 +167,14 @@ esc_calib_main(int argc, char *argv[])
 	int fd = open(dev, 0);
 	if (fd < 0)
 		err(1, "can't open %s", dev);
-
-	// XXX arming not necessaire at the moment
-	// /* Then arm */
-	// ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
-	// 	if (ret != OK)
-	// 		err(1, "PWM_SERVO_SET_ARM_OK");
-
-	// ret = ioctl(fd, PWM_SERVO_ARM, 0);
-	// 	if (ret != OK)
-	// 		err(1, "PWM_SERVO_ARM");
-
-
 	
 
 	/* Wait for user confirmation */
-	warnx("Set high PWM\n"
-	      "Connect battery now and hit ENTER after the ESCs confirm the first calibration step");
+	printf("\nHigh PWM set\n"
+		"\n"
+		"Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n"
+		"\n");
+	fflush(stdout);
 
 	while (1) {
 
@@ -192,13 +187,15 @@ esc_calib_main(int argc, char *argv[])
 			}
 		}
 
-		if (read(console, &c, 1) == 1) {
+		ret = poll(&fds, 1, 0);
+		if (ret > 0) {
+
+			read(0, &c, 1);
 			if (c == 13) {
 				
 				break;
 			} else if (c == 0x03 || c == 0x63 || c == 'q') {
 				warnx("ESC calibration exited");
-				close(console);
 				exit(0);
 			}
 		}
@@ -209,7 +206,8 @@ esc_calib_main(int argc, char *argv[])
 	/* we don't need any more user input */
 	
 
-	warnx("Set low PWM, hit ENTER when finished");
+	printf("Low PWM set, hit ENTER when finished\n"
+		"\n");
 
 	while (1) {
 
@@ -222,13 +220,15 @@ esc_calib_main(int argc, char *argv[])
 			}
 		}
 
-		if (read(console, &c, 1) == 1) {
+		ret = poll(&fds, 1, 0);
+		if (ret > 0) {
+
+			read(0, &c, 1);
 			if (c == 13) {
 				
 				break;
 			} else if (c == 0x03 || c == 0x63 || c == 'q') {
-				warnx("ESC calibration exited");
-				close(console);
+				printf("ESC calibration exited\n");
 				exit(0);
 			}
 		}
@@ -237,14 +237,7 @@ esc_calib_main(int argc, char *argv[])
 	}
 
 	
-	warnx("ESC calibration finished");
-	close(console);
-
-	// XXX disarming not necessaire at the moment
-	/* Now disarm again */
-	// ret = ioctl(fd, PWM_SERVO_DISARM, 0);
-
-	
+	printf("ESC calibration finished\n");
 
 	exit(0);
-}
\ No newline at end of file
+}
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index f5bd01ce8..e9c5f1a2c 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -163,7 +163,7 @@ system_eval:
 		warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM");
 		fflush(stderr);
 
-		int buzzer = open("/dev/tone_alarm", O_WRONLY);
+		int buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 		int leds = open(LED_DEVICE_PATH, 0);
 
 		if (leds < 0) {
diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c
index ba6b86adb..5690997a9 100644
--- a/src/systemcmds/tests/test_hrt.c
+++ b/src/systemcmds/tests/test_hrt.c
@@ -124,10 +124,10 @@ int test_tone(int argc, char *argv[])
 	int fd, result;
 	unsigned long tone;
 
-	fd = open("/dev/tone_alarm", O_WRONLY);
+	fd = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 
 	if (fd < 0) {
-		printf("failed opening /dev/tone_alarm\n");
+		printf("failed opening " TONEALARM_DEVICE_PATH "\n");
 		goto out;
 	}
 
diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c
index 47f480758..f36c28061 100644
--- a/src/systemcmds/tests/tests_file.c
+++ b/src/systemcmds/tests/tests_file.c
@@ -38,7 +38,9 @@
  */
 
 #include <sys/stat.h>
+#include <dirent.h>
 #include <stdio.h>
+#include <stddef.h>
 #include <unistd.h>
 #include <fcntl.h>
 #include <systemlib/err.h>
@@ -52,6 +54,101 @@
 int
 test_file(int argc, char *argv[])
 {
+	const iterations = 200;
+
+	/* check if microSD card is mounted */
+	struct stat buffer;
+	if (stat("/fs/microsd/", &buffer)) {
+		warnx("no microSD card mounted, aborting file test");
+		return 1;
+	}
+
+	uint8_t buf[512];
+	hrt_abstime start, end;
+	perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)");
+
+	int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
+	memset(buf, 0, sizeof(buf));
+
+	warnx("aligned write - please wait..");
+
+	if ((0x3 & (uintptr_t)buf))
+		warnx("memory is unaligned!");
+
+	start = hrt_absolute_time();
+	for (unsigned i = 0; i < iterations; i++) {
+		perf_begin(wperf);
+		write(fd, buf, sizeof(buf));
+		fsync(fd);
+		perf_end(wperf);
+	}
+	end = hrt_absolute_time();
+
+	warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
+
+	perf_print_counter(wperf);
+	perf_free(wperf);
+
+	int ret = unlink("/fs/microsd/testfile");
+
+	if (ret)
+		err(1, "UNLINKING FILE FAILED");
+
+	warnx("unaligned write - please wait..");
+
+	struct {
+		uint8_t byte;
+		uint8_t unaligned[512];
+	} unaligned_buf;
+
+	if ((0x3 & (uintptr_t)unaligned_buf.unaligned) == 0)
+		warnx("creating unaligned memory failed.");
+
+	wperf = perf_alloc(PC_ELAPSED, "SD writes (unaligned)");
+
+	start = hrt_absolute_time();
+	for (unsigned i = 0; i < iterations; i++) {
+		perf_begin(wperf);
+		write(fd, unaligned_buf.unaligned, sizeof(unaligned_buf.unaligned));
+		fsync(fd);
+		perf_end(wperf);
+	}
+	end = hrt_absolute_time();
+
+	close(fd);
+
+	warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
+
+	perf_print_counter(wperf);
+	perf_free(wperf);
+
+	/* list directory */
+	DIR           *d;
+	struct dirent *dir;
+	d = opendir("/fs/microsd");
+	if (d) {
+
+		while ((dir = readdir(d)) != NULL) {
+			//printf("%s\n", dir->d_name);
+		}
+
+		closedir(d);
+
+		warnx("directory listing ok (FS mounted and readable)");
+
+	} else {
+		/* failed opening dir */
+		err(1, "FAILED LISTING MICROSD ROOT DIRECTORY");
+	}
+
+	return 0;
+}
+#if 0
+int
+test_file(int argc, char *argv[])
+{
+	const iterations = 1024;
+
 	/* check if microSD card is mounted */
 	struct stat buffer;
 	if (stat("/fs/microsd/", &buffer)) {
@@ -67,7 +164,7 @@ test_file(int argc, char *argv[])
 	memset(buf, 0, sizeof(buf));
 
 	start = hrt_absolute_time();
-	for (unsigned i = 0; i < 1024; i++) {
+	for (unsigned i = 0; i < iterations; i++) {
 		perf_begin(wperf);
 		write(fd, buf, sizeof(buf));
 		perf_end(wperf);
@@ -78,7 +175,7 @@ test_file(int argc, char *argv[])
 
 	unlink("/fs/microsd/testfile");
 
-	warnx("512KiB in %llu microseconds", end - start);
+	warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
 	perf_print_counter(wperf);
 	perf_free(wperf);
 
@@ -112,3 +209,4 @@ test_file(int argc, char *argv[])
 
 	return 0;
 }
+#endif