Merge branch 'px4io-i2c' of github.com:PX4/Firmware into px4io-i2c

17 files changed, 1191 insertions, 78 deletions

diff --git a/apps/drivers/drv_range_finder.h b/apps/drivers/drv_range_finder.h
new file mode 100644
index 000000000..03a82ec5d
--- /dev/null
+++ b/apps/drivers/drv_range_finder.h
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Rangefinder driver interface.
+ */
+
+#ifndef _DRV_RANGEFINDER_H
+#define _DRV_RANGEFINDER_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define RANGE_FINDER_DEVICE_PATH	"/dev/range_finder"
+
+/**
+ * range finder report structure.  Reads from the device must be in multiples of this
+ * structure.
+ */
+struct range_finder_report {
+	uint64_t timestamp;
+	float distance; 			/** in meters */
+	uint8_t valid;				/** 1 == within sensor range, 0 = outside sensor range */
+};
+
+/*
+ * ObjDev tag for raw range finder data.
+ */
+ORB_DECLARE(sensor_range_finder);
+
+/*
+ * ioctl() definitions
+ *
+ * Rangefinder drivers also implement the generic sensor driver
+ * interfaces from drv_sensor.h
+ */
+
+#define _RANGEFINDERIOCBASE			(0x7900)
+#define __RANGEFINDERIOC(_n)		(_IOC(_RANGEFINDERIOCBASE, _n))
+
+/** set the minimum effective distance of the device */
+#define RANGEFINDERIOCSETMINIUMDISTANCE	__RANGEFINDERIOC(1)
+
+/** set the maximum effective distance of the device */
+#define RANGEFINDERIOCSETMAXIUMDISTANCE	__RANGEFINDERIOC(2)
+
+
+#endif /* _DRV_RANGEFINDER_H */
diff --git a/apps/drivers/mb12xx/Makefile b/apps/drivers/mb12xx/Makefile
new file mode 100644
index 000000000..0d2405787
--- /dev/null
+++ b/apps/drivers/mb12xx/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build the Maxbotix Sonar driver.
+#
+
+APPNAME		= mb12xx
+PRIORITY	= SCHED_PRIORITY_DEFAULT
+STACKSIZE	= 2048
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/apps/drivers/mb12xx/mb12xx.cpp
new file mode 100644
index 000000000..0e4712f5a
--- /dev/null
+++ b/apps/drivers/mb12xx/mb12xx.cpp
@@ -0,0 +1,842 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mb12xx.cpp
+ * @author Greg Hulands
+ *
+ * Driver for the Maxbotix sonar range finders connected via I2C.
+ */
+	 
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <arch/board/board.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+
+/* Configuration Constants */
+#define MB12XX_BUS 			PX4_I2C_BUS_EXPANSION
+#define MB12XX_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
+
+/* MB12xx Registers addresses */
+
+#define MB12XX_TAKE_RANGE_REG	0x51		/* Measure range Register */
+#define MB12XX_SET_ADDRESS_1	0xAA		/* Change address 1 Register */
+#define MB12XX_SET_ADDRESS_2	0xA5		/* Change address 2 Register */
+
+/* Device limits */
+#define MB12XX_MIN_DISTANCE (0.20f)
+#define MB12XX_MAX_DISTANCE (7.65f)
+	 
+#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class MB12XX : public device::I2C
+{
+public:
+	MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR);
+	~MB12XX();
+	
+	virtual int 		init();
+	
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+	
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+	
+protected:
+	virtual int			probe();
+
+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;
+	bool				_sensor_ok;
+	int					_measure_ticks;
+	bool				_collect_phase;
+	
+	orb_advert_t		_range_finder_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t		_buffer_overflows;
+	
+	/**
+	* Test whether the device supported by the driver is present at a
+	* specific address.
+	*
+	* @param address	The I2C bus address to probe.
+	* @return		True if the device is present.
+	*/
+	int					probe_address(uint8_t address);
+	
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void				start();
+	
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void				stop();
+	
+	/**
+	* Set the min and max distance thresholds if you want the end points of the sensors
+	* range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE
+	* and MB12XX_MAX_DISTANCE
+	*/
+	void				set_minimum_distance(float min);
+	void				set_maximum_distance(float max);
+	float				get_minimum_distance();
+	float				get_maximum_distance();
+	
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void				cycle();
+	int					measure();
+	int					collect();
+	/**
+	* Static trampoline from the workq context; because we don't have a
+	* generic workq wrapper yet.
+	*
+	* @param arg		Instance pointer for the driver that is polling.
+	*/
+	static void		cycle_trampoline(void *arg);
+	
+	
+};
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]);
+
+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),
+	_collect_phase(false),
+	_range_finder_topic(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+	
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+MB12XX::~MB12XX()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete[] _reports;
+}
+
+int
+MB12XX::init()
+{
+	int ret = ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_num_reports = 2;
+	_reports = new struct range_finder_report[_num_reports];
+
+	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]);
+
+	if (_range_finder_topic < 0)
+		debug("failed to create sensor_range_finder object. Did you start uOrb?");
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+out:
+	return ret;
+}
+
+int
+MB12XX::probe()
+{
+	// TODO: take a range reading and see if it is between the min and max
+
+	return OK;
+}
+
+void
+MB12XX::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}	
+
+void
+MB12XX::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}	
+
+float
+MB12XX::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+MB12XX::get_maximum_distance()
+{
+	return _max_distance;
+}
+
+int
+MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+				/* external signalling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+				/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL))
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		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;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _num_reports - 1;
+		
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+	
+	case RANGEFINDERIOCSETMINIUMDISTANCE:
+	{
+		set_minimum_distance(*(float *)arg);
+		return 0;
+	}
+	break;
+	case RANGEFINDERIOCSETMAXIUMDISTANCE:
+	{
+		set_maximum_distance(*(float *)arg);
+		return 0;
+	}
+	break;
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+MB12XX::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct range_finder_report);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* 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 there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	/* XXX really it'd be nice to lock against other readers here */
+	do {
+		_oldest_report = _next_report = 0;
+
+		/* trigger a measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* wait for it to complete */
+		usleep(MB12XX_CONVERSION_INTERVAL);
+
+		/* run the collection phase */
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		memcpy(buffer, _reports, sizeof(*_reports));
+		ret = sizeof(*_reports);
+
+	} while (0);
+
+	return ret;
+}
+
+int
+MB12XX::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	uint8_t cmd = MB12XX_TAKE_RANGE_REG;
+	ret = transfer(&cmd, 1, nullptr, 0);
+
+	if (OK != ret)
+	{
+		perf_count(_comms_errors);
+		log("i2c::transfer returned %d", ret);
+		return ret;
+	}
+	ret = OK;
+	
+	return ret;
+}
+
+int
+MB12XX::collect()
+{
+	int	ret = -EIO;
+	
+	/* read from the sensor */
+	uint8_t val[2] = {0, 0};
+	
+	perf_begin(_sample_perf);
+	
+	ret = transfer(nullptr, 0, &val[0], 2);
+	
+	if (ret < 0)
+	{
+		log("error reading from sensor: %d", ret);
+		return ret;
+	}
+	
+	uint16_t distance = val[0] << 8 | val[1];
+	float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */
+	/* 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;
+	
+	/* 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);
+
+	/* 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);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+out:
+	perf_end(_sample_perf);
+	return ret;
+	
+	return ret;
+}
+
+void
+MB12XX::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_oldest_report = _next_report = 0;
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
+	
+	/* notify about state change */
+	struct subsystem_info_s info = {
+		true,
+		true,
+		true,
+		SUBSYSTEM_TYPE_RANGEFINDER};
+	static orb_advert_t pub = -1;
+
+	if (pub > 0) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+	}
+}
+
+void
+MB12XX::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+MB12XX::cycle_trampoline(void *arg)
+{
+	MB12XX *dev = (MB12XX *)arg;
+
+	dev->cycle();
+}
+
+void
+MB12XX::cycle()
+{
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		if (OK != collect()) {
+			log("collection error");
+			/* restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&MB12XX::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure())
+		log("measure error");
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&MB12XX::cycle_trampoline,
+		   this,
+		   USEC2TICK(MB12XX_CONVERSION_INTERVAL));
+}
+
+void
+MB12XX::print_info()
+{
+	perf_print_counter(_sample_perf);
+	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);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace mb12xx
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+MB12XX	*g_dev;
+
+void	start();
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new MB12XX(MB12XX_BUS);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		goto fail;
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) 
+	{
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver
+ */
+void stop()
+{
+	if (g_dev != nullptr)
+	{
+		delete g_dev;
+		g_dev = nullptr;
+	}
+	else
+	{
+		errx(1, "driver not running");
+	}
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct range_finder_report report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report))
+		err(1, "immediate read failed");
+
+	warnx("single read");
+	warnx("measurement: %0.2f m", (double)report.distance);
+	warnx("time:        %lld", report.timestamp);
+
+	/* start the sensor polling at 2Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+		errx(1, "failed to set 2Hz poll rate");
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1)
+			errx(1, "timed out waiting for sensor data");
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report))
+			err(1, "periodic read failed");
+
+		warnx("periodic read %u", i);
+		warnx("measurement: %0.3f", (double)report.distance);
+		warnx("time:        %lld", report.timestamp);
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+int
+mb12xx_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start"))
+		mb12xx::start();
+	
+	 /*
+	  * Stop the driver
+	  */
+	 if (!strcmp(argv[1], "stop"))
+		 mb12xx::stop();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		mb12xx::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		mb12xx::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+		mb12xx::info();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 2611c4e9c..f9ffa6bcd 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -82,7 +82,9 @@
 #include <uORB/topics/parameter_update.h>
 
 #include <px4io/protocol.h>
+#include <mavlink/mavlink_log.h>
 #include "uploader.h"
+#include <debug.h>
 
 
 class PX4IO : public device::I2C
@@ -111,6 +113,8 @@ private:
 	volatile int		_task;		///< worker task
 	volatile bool		_task_should_exit;
 
+	int			_mavlink_fd;
+
 	perf_counter_t		_perf_update;
 
 	/* cached IO state */
@@ -285,6 +289,7 @@ PX4IO::PX4IO() :
 	_update_interval(0),
 	_task(-1),
 	_task_should_exit(false),
+	_mavlink_fd(-1),
 	_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
 	_status(0),
 	_alarms(0),
@@ -301,6 +306,9 @@ PX4IO::PX4IO() :
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
 
+	/* open MAVLink text channel */
+	_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
+
 	_debug_enabled = true;
 }
 
@@ -354,6 +362,7 @@ PX4IO::init()
 	    (_max_rc_input < 1)  || (_max_rc_input > 255)) {
 
 		log("failed getting parameters from PX4IO");
+		mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort.");
 		return -1;
 	}
 	if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
@@ -380,6 +389,8 @@ PX4IO::init()
 	if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
 	    (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) {
 
+	    	mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
+
 		/* WARNING: COMMANDER app/vehicle status must be initialized.
 		 * If this fails (or the app is not started), worst-case IO
 		 * remains untouched (so manual override is still available).
@@ -469,6 +480,7 @@ PX4IO::init()
 		ret = io_set_rc_config();
 		if (ret != OK) {
 			log("failed to update RC input config");
+			mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
 			return ret;
 		}
 
@@ -490,6 +502,8 @@ PX4IO::init()
 		return -errno;
 	}
 
+	mavlink_log_info(_mavlink_fd, "[IO] init ok");
+
 	return OK;
 }
 
@@ -771,9 +785,16 @@ PX4IO::io_set_rc_config()
 		/* send channel config to IO */
 		ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
 		if (ret != OK) {
-			log("RC config update failed");
+			log("rc config upload failed");
+			break;
+		}
+
+		/* check the IO initialisation flag */
+		if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
+			log("config for RC%d rejected by IO", i + 1);
 			break;
 		}
+
 		offset += PX4IO_P_RC_CONFIG_STRIDE;
 	}
 
@@ -1157,9 +1178,11 @@ PX4IO::mixer_send(const char *buf, unsigned buflen)
 	/* 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");
 	}
 
 	/* load must have failed for some reason */
@@ -1186,7 +1209,7 @@ PX4IO::print_status()
 	printf("%u bytes free\n",
 		io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
 	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
-	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s\n",
+	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n",
 		flags,
 		((flags & PX4IO_P_STATUS_FLAGS_ARMED)    ? " ARMED" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
@@ -1197,7 +1220,8 @@ PX4IO::print_status()
 		((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PPM" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
-		((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"));
+		((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
+		((flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  ? " INIT_OK" : " INIT_FAIL"));
 	uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
 	printf("alarms 0x%04x%s%s%s%s%s%s\n",
 		alarms,
@@ -1475,7 +1499,7 @@ test(void)
 			servos[i] = pwm_value;
 
 		ret = write(fd, servos, sizeof(servos));
-		if (ret != sizeof(servos))
+		if (ret != (int)sizeof(servos))
 			err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
 
 		if (direction > 0) {
@@ -1547,7 +1571,7 @@ px4io_main(int argc, char *argv[])
 			errx(1, "already loaded");
 
 		/* create the driver - it will set g_dev */
-		(void)new PX4IO;
+		(void)new PX4IO();
 
 		if (g_dev == nullptr)
 			errx(1, "driver alloc failed");
@@ -1558,7 +1582,7 @@ px4io_main(int argc, char *argv[])
 		}
 
 		/* look for the optional pwm update rate for the supported modes */
-		if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) {
+		if ((argc > 2) && (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0)) {
 			if (argc > 2 + 1) {
 #warning implement this 
 			} else {
@@ -1570,16 +1594,31 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "recovery")) {
+
+		if (g_dev != nullptr) {
+			/*
+			 * Enable in-air restart support.
+			 * We can cheat and call the driver directly, as it
+		 	 * doesn't reference filp in ioctl()
+			 */
+			g_dev->ioctl(NULL, PWM_SERVO_INAIR_RESTART_ENABLE, 0);
+		} else {
+			errx(1, "not loaded");
+		}
+		exit(0);
+	}
+
 	if (!strcmp(argv[1], "stop")) {
 
-			if (g_dev != nullptr) {
-				/* stop the driver */
-				delete g_dev;
-			} else {
-				errx(1, "not loaded");
-			}
-			exit(0);
+		if (g_dev != nullptr) {
+			/* stop the driver */
+			delete g_dev;
+		} else {
+			errx(1, "not loaded");
 		}
+		exit(0);
+	}
 
 
 	if (!strcmp(argv[1], "status")) {
@@ -1604,8 +1643,9 @@ px4io_main(int argc, char *argv[])
 			exit(1);
 		}
 		uint8_t level = atoi(argv[2]);
-		// we can cheat and call the driver directly, as it
-		// doesn't reference filp in ioctl()
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
 		int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level);
 		if (ret != 0) {
 			printf("SET_DEBUG failed - %d\n", ret);
diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h
index 62e6f7ca0..233a76cb3 100644
--- a/apps/mavlink/mavlink_log.h
+++ b/apps/mavlink/mavlink_log.h
@@ -63,7 +63,11 @@
  * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
  * @param _text		The text to log;
  */
+#ifdef __cplusplus
+#define mavlink_log_emergency(_fd, _text)	::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
+#else
 #define mavlink_log_emergency(_fd, _text)	ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
+#endif
 
 /**
  * Send a mavlink critical message.
@@ -71,7 +75,11 @@
  * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
  * @param _text		The text to log;
  */
+#ifdef __cplusplus
+#define mavlink_log_critical(_fd, _text)	::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
+#else
 #define mavlink_log_critical(_fd, _text)	ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
+#endif
 
 /**
  * Send a mavlink info message.
@@ -79,7 +87,11 @@
  * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
  * @param _text		The text to log;
  */
+#ifdef __cplusplus
+#define mavlink_log_info(_fd, _text)		::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
+#else
 #define mavlink_log_info(_fd, _text)		ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
+#endif
 
 struct mavlink_logmessage {
 	char text[51];
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index dabdde0af..2d99116f8 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -147,32 +147,44 @@ controls_tick() {
 
 				uint16_t raw = r_raw_rc_values[i];
 
-				/* implement the deadzone */
-				if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) {
-					raw += conf[PX4IO_P_RC_CONFIG_DEADZONE];
-					if (raw > conf[PX4IO_P_RC_CONFIG_CENTER])
-						raw = conf[PX4IO_P_RC_CONFIG_CENTER];
-				}
-				if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) {
-					raw -= conf[PX4IO_P_RC_CONFIG_DEADZONE];
-					if (raw < conf[PX4IO_P_RC_CONFIG_CENTER])
-						raw = conf[PX4IO_P_RC_CONFIG_CENTER];
-				}
+				int16_t scaled;
 
-				/* constrain to min/max values */
+				/*
+				 * 1) Constrain to min/max values, as later processing depends on bounds.
+				 */
 				if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
 					raw = conf[PX4IO_P_RC_CONFIG_MIN];
 				if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
 					raw = conf[PX4IO_P_RC_CONFIG_MAX];
 
-				int16_t scaled = raw;
-
-				/* adjust to zero-relative around center (nominal -500..500) */
-				scaled -= conf[PX4IO_P_RC_CONFIG_CENTER];
-
-				/* scale to fixed-point representation (-10000..10000) */
-				scaled *= 20;
+				/*
+				 * 2) Scale around the mid point differently for lower and upper range.
+				 *
+				 * This is necessary as they don't share the same endpoints and slope.
+				 *
+				 * First normalize to 0..1 range with correct sign (below or above center),
+				 * then scale to 20000 range (if center is an actual center, -10000..10000,
+				 * if parameters only support half range, scale to 10000 range, e.g. if
+				 * center == min 0..10000, if center == max -10000..0).
+				 *
+				 * As the min and max bounds were enforced in step 1), division by zero
+				 * cannot occur, as for the case of center == min or center == max the if
+				 * statement is mutually exclusive with the arithmetic NaN case.
+				 *
+				 * DO NOT REMOVE OR ALTER STEP 1!
+				 */
+				if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
+
+				} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
+
+				} else {
+					/* in the configured dead zone, output zero */
+					scaled = 0;
+				}
 
+				/* invert channel if requested */
 				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
 					scaled = -scaled;
 
@@ -256,10 +268,11 @@ controls_tick() {
 		bool override = false;
 
 		/*
-		 * Check mapped channel 5; if the value is 'high' then the pilot has
+		 * Check mapped channel 5 (can be any remote channel,
+		 * depends on RC_MAP_OVER parameter);
+		 * If the value is 'high' then the pilot has
 		 * requested override.
 		 *
-		 * XXX This should be configurable.
 		 */
 		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH))
 			override = true;
diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index 65de864ec..092a35a44 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -109,9 +109,11 @@ mixer_tick(void)
 	/*
 	 * Decide which set of controls we're using.
 	 */
-	if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) {
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ||
+		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 
-		/* don't actually mix anything - we already have raw PWM values */
+		/* don't actually mix anything - we already have raw PWM values or
+		 not a valid mixer. */
 		source = MIX_NONE;
 
 	} else {
@@ -172,9 +174,11 @@ mixer_tick(void)
 	 * XXX correct behaviour for failsafe may require an additional case
 	 * here.
 	 */
-	bool should_arm = (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
-	 		   /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
-	/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)));
+	bool should_arm = (
+	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+		/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
+		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK));
 
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
@@ -239,6 +243,11 @@ static unsigned mixer_text_length = 0;
 void
 mixer_handle_text(const void *buffer, size_t length)
 {
+	/* do not allow a mixer change while fully armed */
+	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+	    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+		return;
+	}
 
 	px4io_mixdata	*msg = (px4io_mixdata *)buffer;
 
@@ -252,9 +261,12 @@ mixer_handle_text(const void *buffer, size_t length)
 	switch (msg->action) {
 	case F2I_MIXER_ACTION_RESET:
 		isr_debug(2, "reset");
+
+		/* FIRST mark the mixer as invalid */
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
+		/* THEN actually delete it */
 		mixer_group.reset();
 		mixer_text_length = 0;
-		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
 
 		/* FALLTHROUGH */
 	case F2I_MIXER_ACTION_APPEND:
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 0ee6d2c4b..14d221b5b 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -103,6 +103,7 @@
 #define PX4IO_P_STATUS_FLAGS_RAW_PWM		(1 << 7) /* raw PWM from FMU is bypassing the mixer */
 #define PX4IO_P_STATUS_FLAGS_MIXER_OK		(1 << 8) /* mixer is OK */
 #define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
+#define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* VBatt is very close to regulator dropout */
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index 589264661..0c4838523 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -215,12 +215,16 @@ user_start(int argc, char *argv[])
 
 		/* post debug state at ~1Hz */
 		if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
-			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u", 
+
+			struct mallinfo minfo = mallinfo();
+
+			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", 
 				  (unsigned)debug_level,
 				  (unsigned)r_status_flags,
 				  (unsigned)r_setup_arming,
 				  (unsigned)r_setup_features,
-				  (unsigned)i2c_loop_resets);
+				  (unsigned)i2c_loop_resets,
+				  (unsigned)minfo.mxordblk);
 			last_debug_time = hrt_absolute_time();
 		}
 	}
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
index 5ec2f7258..511a47f8d 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -361,6 +361,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		break;
 
 	case PX4IO_PAGE_RC_CONFIG: {
+
+		/* do not allow a RC config change while fully armed */
+		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+		    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+			break;
+		}
+
 		unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
 		unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
 		uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
@@ -383,6 +390,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 		case PX4IO_P_RC_CONFIG_OPTIONS:
 			value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 
 			/* set all options except the enabled option */
 			conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
@@ -390,27 +398,44 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			/* should the channel be enabled? */
 			/* this option is normally set last */
 			if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+				uint8_t count = 0;
+
 				/* assert min..center..max ordering */
-				if (conf[PX4IO_P_RC_CONFIG_MIN] < 500)
-					break;
-				if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500)
-					break;
-				if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN])
-					break;
-				if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX])
-					break;
+				if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
+					count++;
+				}
+				if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) {
+					count++;
+				}
+				if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) {
+					count++;
+				}
+				if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
+					count++;
+				}
+
 				/* assert deadzone is sane */
-				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500)
-					break;
-				if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]))
-					break;
-				if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]))
-					break;
-				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS)
-					break;
+				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
+					count++;
+				}
+				// The following check isn't needed as constraint checks in controls.c will catch this.
+				//if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+				//	count++;
+				//}
+				//if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+				//	count++;
+				//}
+				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
+					count++;
+				}
 
 				/* sanity checks pass, enable channel */
-				conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+				if (count) {
+					isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
+					r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
+				} else {
+					conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+				}
 			}
 			break;
 			/* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 540636e19..5495d5ae1 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -176,12 +176,17 @@ heartbeat_blink(void *arg)
 static void
 failsafe_blink(void *arg)
 {
+	/* indicate that a serious initialisation error occured */
+	if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
+		LED_AMBER(true);
+	    return;
+    }
+
 	static bool failsafe = false;
 
 	/* blink the failsafe LED if we don't have FMU input */
 	if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
 		failsafe = !failsafe;
-
 	} else {
 		failsafe = false;
 	}
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 89fd71dc6..d725c7727 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -1129,31 +1129,45 @@ Sensors::ppm_poll()
 		/* Read out values from raw message */
 		for (unsigned int i = 0; i < channel_limit; i++) {
 
-			/* scale around the mid point differently for lower and upper range */
+			/*
+			 * 1) Constrain to min/max values, as later processing depends on bounds.
+			 */
+			if (rc_input.values[i] < _parameters.min[i])
+				rc_input.values[i] = _parameters.min[i];
+			if (rc_input.values[i] > _parameters.max[i])
+				rc_input.values[i] = _parameters.max[i];
+
+			/*
+			 * 2) Scale around the mid point differently for lower and upper range.
+			 *
+			 * This is necessary as they don't share the same endpoints and slope.
+			 *
+			 * First normalize to 0..1 range with correct sign (below or above center),
+			 * the total range is 2 (-1..1).
+			 * If center (trim) == min, scale to 0..1, if center (trim) == max,
+			 * scale to -1..0.
+			 *
+			 * As the min and max bounds were enforced in step 1), division by zero
+			 * cannot occur, as for the case of center == min or center == max the if
+			 * statement is mutually exclusive with the arithmetic NaN case.
+			 *
+			 * DO NOT REMOVE OR ALTER STEP 1!
+			 */
 			if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
-				_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]);
+				_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
 
 			} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
-				/* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */
-				_rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
+				_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
 
 			} else {
 				/* in the configured dead zone, output zero */
 				_rc.chan[i].scaled = 0.0f;
 			}
 
-			/* reverse channel if required */
-			if (i == (int)_rc.function[THROTTLE]) {
-				if ((int)_parameters.rev[i] == -1) {
-					_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
-				}
-
-			} else {
-				_rc.chan[i].scaled *= _parameters.rev[i];
-			}
+			_rc.chan[i].scaled *= _parameters.rev[i];
 
 			/* handle any parameter-induced blowups */
-			if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled))
+			if (!isfinite(_rc.chan[i].scaled))
 				_rc.chan[i].scaled = 0.0f;
 		}
 
diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c
index e2f7b5bd5..55c4f0836 100644
--- a/apps/systemcmds/mixer/mixer.c
+++ b/apps/systemcmds/mixer/mixer.c
@@ -117,7 +117,23 @@ load(const char *devname, const char *fname)
 		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
 			continue;
 
-		/* XXX an optimisation here would be to strip extra whitespace */
+		/* compact whitespace in the buffer */
+		char *t, *f;
+		for (f = buf; *f != '\0'; f++) {
+			/* scan for space characters */
+			if (*f == ' ') {
+				/* look for additional spaces */
+				t = f + 1;
+				while (*t == ' ')
+					t++;
+				if (*t == '\0') {
+					/* strip trailing whitespace */
+					*f = '\0';
+				} else if (t > (f + 1)) {
+					memmove(f + 1, t, strlen(t) + 1);
+				}
+			}
+		}
 
 		/* if the line is too long to fit in the buffer, bail */
 		if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf))
diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp
index 72f765d90..df0dfe838 100644
--- a/apps/systemlib/mixer/mixer.cpp
+++ b/apps/systemlib/mixer/mixer.cpp
@@ -54,6 +54,7 @@
 #include "mixer.h"
 
 Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) :
+	_next(nullptr),
 	_control_cb(control_cb),
 	_cb_handle(cb_handle)
 {
diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp
index 60eeff225..1dbc512cd 100644
--- a/apps/systemlib/mixer/mixer_group.cpp
+++ b/apps/systemlib/mixer/mixer_group.cpp
@@ -93,6 +93,7 @@ MixerGroup::reset()
 		mixer = _first;
 		_first = mixer->_next;
 		delete mixer;
+		mixer = nullptr;
 	}
 }
 
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index 3f3476f62..136375140 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -53,6 +53,9 @@ ORB_DEFINE(sensor_gyro, struct gyro_report);
 #include <drivers/drv_baro.h>
 ORB_DEFINE(sensor_baro, struct baro_report);
 
+#include <drivers/drv_range_finder.h>
+ORB_DEFINE(sensor_range_finder, struct range_finder_report);
+
 #include <drivers/drv_pwm_output.h>
 ORB_DEFINE(output_pwm, struct pwm_output_values);
 
diff --git a/apps/uORB/topics/subsystem_info.h b/apps/uORB/topics/subsystem_info.h
index c3e039d66..c415e832e 100644
--- a/apps/uORB/topics/subsystem_info.h
+++ b/apps/uORB/topics/subsystem_info.h
@@ -71,7 +71,8 @@ enum SUBSYSTEM_TYPE
 	SUBSYSTEM_TYPE_YAWPOSITION = 4096,
 	SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384,
 	SUBSYSTEM_TYPE_POSITIONCONTROL = 32768,
-	SUBSYSTEM_TYPE_MOTORCONTROL = 65536
+	SUBSYSTEM_TYPE_MOTORCONTROL = 65536,
+	SUBSYSTEM_TYPE_RANGEFINDER = 131072
 };
 
 /**