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

Conflicts:
	src/drivers/drv_pwm_output.h

32 files changed, 1520 insertions, 1056 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);