Merge branch 'master' into new_state_machine_drton

65 files changed, 3589 insertions, 839 deletions

diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
new file mode 100644
index 000000000..5a8157deb
--- /dev/null
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -0,0 +1,378 @@
+/****************************************************************************
+ *
+ *   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 ets_airspeed.cpp
+ * @author Simon Wilks
+ *
+ * Driver for the Eagle Tree Airspeed V3 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/airspeed.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+
+#include <drivers/drv_airspeed.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/subsystem_info.h>
+
+#include <drivers/airspeed/airspeed.h>
+
+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),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_diff_pres_offset(0.0f),
+	_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"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+Airspeed::~Airspeed()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete[] _reports;
+}
+
+int
+Airspeed::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 differential_pressure_s[_num_reports];
+
+	for (unsigned i = 0; i < _num_reports; i++)
+		_reports[i].max_differential_pressure_pa = 0;
+
+	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]);
+
+	if (_airspeed_pub < 0)
+		warnx("failed to create airspeed sensor 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
+Airspeed::probe()
+{
+	return measure();
+}
+
+int
+Airspeed::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(_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(_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 differential_pressure_s *buf = new struct differential_pressure_s[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 AIRSPEEDIOCSSCALE: {
+		struct airspeed_scale *s = (struct airspeed_scale*)arg;
+		_diff_pres_offset = s->offset_pa;
+		return OK;
+		}
+
+	case AIRSPEEDIOCGSCALE: {
+		struct airspeed_scale *s = (struct airspeed_scale*)arg;
+		s->offset_pa = _diff_pres_offset;
+		s->scale = 1.0f;
+		return OK;
+		}
+
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+Airspeed::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct differential_pressure_s);
+	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(_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;
+}
+
+void
+Airspeed::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)&Airspeed::cycle_trampoline, this, 1);
+
+	/* notify about state change */
+	struct subsystem_info_s info = {
+		true,
+		true,
+		true,
+		SUBSYSTEM_TYPE_DIFFPRESSURE
+	};
+	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
+Airspeed::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+Airspeed::cycle_trampoline(void *arg)
+{
+	Airspeed *dev = (Airspeed *)arg;
+
+	dev->cycle();
+}
+
+void
+Airspeed::print_info()
+{
+	perf_print_counter(_sample_perf);
+	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);
+}
diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
new file mode 100644
index 000000000..89dfb22d7
--- /dev/null
+++ b/src/drivers/airspeed/airspeed.h
@@ -0,0 +1,169 @@
+/****************************************************************************
+ *
+ *   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 airspeed.h
+ * @author Simon Wilks
+ *
+ * Generic driver for airspeed sensors 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/airspeed.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+
+#include <drivers/drv_airspeed.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/subsystem_info.h>
+
+/* Default I2C bus */
+#define PX4_I2C_BUS_DEFAULT		PX4_I2C_BUS_EXPANSION
+
+/* 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 __EXPORT Airspeed : public device::I2C
+{
+public:
+	Airspeed(int bus, int address, unsigned conversion_interval);
+	virtual ~Airspeed();
+
+	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.
+	 */
+	virtual void	print_info();
+
+protected:
+	virtual int	probe();
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	virtual void	cycle() = 0;
+	virtual int	measure() = 0;
+	virtual int	collect() = 0;
+
+	work_s			_work;
+	unsigned		_num_reports;
+	volatile unsigned	_next_report;
+	volatile unsigned	_oldest_report;
+	differential_pressure_s	*_reports;
+	bool			_sensor_ok;
+	int			_measure_ticks;
+	bool			_collect_phase;
+	float			_diff_pres_offset;
+
+	orb_advert_t		_airspeed_pub;
+
+	unsigned		_conversion_interval;
+
+	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();
+
+	/**
+	* 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)
+
diff --git a/src/drivers/airspeed/module.mk b/src/drivers/airspeed/module.mk
new file mode 100644
index 000000000..4eef06161
--- /dev/null
+++ b/src/drivers/airspeed/module.mk
@@ -0,0 +1,38 @@
+############################################################################
+#
+#   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 generic airspeed driver.
+#
+
+SRCS			= airspeed.cpp
diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp
index 422321850..47ebcd40a 100644
--- a/src/drivers/device/cdev.cpp
+++ b/src/drivers/device/cdev.cpp
@@ -111,21 +111,21 @@ CDev::~CDev()
 int
 CDev::init()
 {
-	int ret = OK;
-
 	// base class init first
-	ret = Device::init();
+	int ret = Device::init();
 
 	if (ret != OK)
 		goto out;
 
 	// now register the driver
-	ret = register_driver(_devname, &fops, 0666, (void *)this);
+	if (_devname != nullptr) {
+		ret = register_driver(_devname, &fops, 0666, (void *)this);
 
-	if (ret != OK)
-		goto out;
+		if (ret != OK)
+			goto out;
 
-	_registered = true;
+		_registered = true;
+	}
 
 out:
 	return ret;
@@ -395,4 +395,4 @@ cdev_poll(struct file *filp, struct pollfd *fds, bool setup)
 	return cdev->poll(filp, fds, setup);
 }
 
-} // namespace device
\ No newline at end of file
+} // namespace device
diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp
index 04a5222c3..c3ee77b1c 100644
--- a/src/drivers/device/device.cpp
+++ b/src/drivers/device/device.cpp
@@ -223,5 +223,22 @@ interrupt(int irq, void *context)
 	return OK;
 }
 
+int
+Device::read(unsigned offset, void *data, unsigned count)
+{
+	return -ENODEV;
+}
+
+int
+Device::write(unsigned offset, void *data, unsigned count)
+{
+	return -ENODEV;
+}
+
+int
+Device::ioctl(unsigned operation, unsigned &arg)
+{
+	return -ENODEV;
+}
 
-} // namespace device
\ No newline at end of file
+} // namespace device
diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
index 7d375aab9..a9ed5d77c 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -69,10 +69,61 @@ class __EXPORT Device
 {
 public:
 	/**
+	 * Destructor.
+	 *
+	 * Public so that anonymous devices can be destroyed.
+	 */
+	virtual ~Device();
+
+	/**
 	 * Interrupt handler.
 	 */
 	virtual void	interrupt(void *ctx);	/**< interrupt handler */
 
+	/*
+	 * Direct access methods.
+	 */
+
+	/**
+	 * Initialise the driver and make it ready for use.
+	 *
+	 * @return	OK if the driver initialized OK, negative errno otherwise;
+	 */
+	virtual int	init();
+
+	/**
+	 * Read directly from the device.
+	 *
+	 * The actual size of each unit quantity is device-specific.
+	 *
+	 * @param offset	The device address at which to start reading
+	 * @param data		The buffer into which the read values should be placed.
+	 * @param count		The number of items to read.
+	 * @return		The number of items read on success, negative errno otherwise.
+	 */
+	virtual int	read(unsigned address, void *data, unsigned count);
+
+	/**
+	 * Write directly to the device.
+	 *
+	 * The actual size of each unit quantity is device-specific.
+	 *
+	 * @param address	The device address at which to start writing.
+	 * @param data		The buffer from which values should be read.
+	 * @param count		The number of items to write.
+	 * @return		The number of items written on success, negative errno otherwise.
+	 */	 
+	virtual int	write(unsigned address, void *data, unsigned count);
+
+	/**
+	 * Perform a device-specific operation.
+	 *
+	 * @param operation	The operation to perform.
+	 * @param arg		An argument to the operation.
+	 * @return		Negative errno on error, OK or positive value on success.
+	 */
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
 protected:
 	const char	*_name;			/**< driver name */
 	bool		_debug_enabled;		/**< if true, debug messages are printed */
@@ -85,14 +136,6 @@ protected:
 	 */
 	Device(const char *name,
 	       int irq = 0);
-	~Device();
-
-	/**
-	 * Initialise the driver and make it ready for use.
-	 *
-	 * @return	OK if the driver initialised OK.
-	 */
-	virtual int	init();
 
 	/**
 	 * Enable the device interrupt
@@ -189,7 +232,7 @@ public:
 	/**
 	 * Destructor
 	 */
-	~CDev();
+	virtual ~CDev();
 
 	virtual int	init();
 
@@ -282,6 +325,7 @@ public:
 	 * Test whether the device is currently open.
 	 *
 	 * This can be used to avoid tearing down a device that is still active.
+	 * Note - not virtual, cannot be overridden by a subclass.
 	 *
 	 * @return              True if the device is currently open.
 	 */
@@ -396,9 +440,9 @@ public:
 	    const char *devname,
 	    uint32_t base,
 	    int irq = 0);
-	~PIO();
+	virtual ~PIO();
 
-	int		init();
+	virtual int	init();
 
 protected:
 
@@ -407,7 +451,7 @@ protected:
 	 *
 	 * @param offset	Register offset in bytes from the base address.
 	 */
-	uint32_t reg(uint32_t offset) {
+	uint32_t	reg(uint32_t offset) {
 		return *(volatile uint32_t *)(_base + offset);
 	}
 
@@ -444,4 +488,4 @@ private:
 
 } // namespace device
 
-#endif /* _DEVICE_DEVICE_H */
\ No newline at end of file
+#endif /* _DEVICE_DEVICE_H */
diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h
index b4a9cdd53..549879352 100644
--- a/src/drivers/device/i2c.h
+++ b/src/drivers/device/i2c.h
@@ -55,13 +55,11 @@ class __EXPORT I2C : public CDev
 
 public:
 
-    /**
-     * Get the address
-     */
-    uint16_t get_address() {
-      return _address;
-    }
-
+	/**
+	 * Get the address
+	 */
+	int16_t		get_address() { return _address; }
+	
 protected:
 	/**
 	 * The number of times a read or write operation will be retried on
@@ -90,7 +88,7 @@ protected:
 	    uint16_t address,
 	    uint32_t frequency,
 	    int irq = 0);
-	~I2C();
+	virtual ~I2C();
 
 	virtual int	init();
 
diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h
new file mode 100644
index 000000000..dc0c84052
--- /dev/null
+++ b/src/drivers/device/ringbuffer.h
@@ -0,0 +1,203 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ringbuffer.h
+ *
+ * A simple ringbuffer template.
+ */
+
+#pragma once
+
+template<typename T>
+class RingBuffer {
+public:
+	RingBuffer(unsigned size);
+	virtual ~RingBuffer();
+
+	/**
+	 * Put an item into the buffer.
+	 *
+	 * @param val		Item to put
+	 * @return		true if the item was put, false if the buffer is full
+	 */
+	bool			put(T &val);
+
+	/**
+	 * Put an item into the buffer.
+	 *
+	 * @param val		Item to put
+	 * @return		true if the item was put, false if the buffer is full
+	 */
+	bool			put(const T &val);
+
+	/**
+	 * Get an item from the buffer.
+	 *
+	 * @param val		Item that was gotten
+	 * @return		true if an item was got, false if the buffer was empty.
+	 */
+	bool			get(T &val);
+
+	/**
+	 * Get an item from the buffer (scalars only).
+	 *
+	 * @return		The value that was fetched, or zero if the buffer was
+	 *			empty.
+	 */
+	T			get(void);
+
+	/*
+	 * Get the number of slots free in the buffer.
+	 *
+	 * @return		The number of items that can be put into the buffer before
+	 *			it becomes full.
+	 */
+	unsigned		space(void);
+
+	/*
+	 * Get the number of items in the buffer.
+	 *
+	 * @return		The number of items that can be got from the buffer before
+	 *			it becomes empty.
+	 */
+	unsigned		count(void);
+
+	/*
+	 * Returns true if the buffer is empty.
+	 */
+	bool			empty() { return _tail == _head; }
+
+	/*
+	 * Returns true if the buffer is full.
+	 */
+	bool			full() { return _next(_head) == _tail; }
+
+	/*
+	 * Returns the capacity of the buffer, or zero if the buffer could
+	 * not be allocated.
+	 */
+	unsigned		size() { return (_buf != nullptr) ? _size : 0; }
+
+	/*
+	 * Empties the buffer.
+	 */
+	void			flush() { _head = _tail = _size; }
+
+private:
+	T			*const _buf;	
+	const unsigned		_size;
+	volatile unsigned	_head;	/**< insertion point */
+	volatile unsigned	_tail;	/**< removal point */
+
+	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)
+{}
+
+template <typename T>
+RingBuffer<T>::~RingBuffer()
+{
+	if (_buf != nullptr)
+		delete[] _buf;
+}
+
+template <typename T>
+bool RingBuffer<T>::put(T &val) 
+{
+	unsigned next = _next(_head);
+	if (next != _tail) {
+		_buf[_head] = val;
+		_head = next;
+		return true;
+	} else {
+		return false;
+	}
+}
+
+template <typename T>
+bool RingBuffer<T>::put(const T &val) 
+{
+	unsigned next = _next(_head);
+	if (next != _tail) {
+		_buf[_head] = val;
+		_head = next;
+		return true;
+	} else {
+		return false;
+	}
+}
+
+template <typename T>
+bool RingBuffer<T>::get(T &val) 
+{
+	if (_tail != _head) {
+		val = _buf[_tail];
+		_tail = _next(_tail);
+		return true;
+	} else {
+		return false;
+	}
+}
+
+template <typename T>
+T RingBuffer<T>::get(void) 
+{
+	T val;
+	return get(val) ? val : 0;
+}
+
+template <typename T>
+unsigned RingBuffer<T>::space(void) 
+{
+	return (_tail >= _head) ? (_size - (_tail - _head)) : (_head - _tail - 1);
+}
+
+template <typename T>
+unsigned RingBuffer<T>::count(void) 
+{
+	return _size - space();
+}
+
+template <typename T>
+unsigned RingBuffer<T>::_next(unsigned index) 
+{
+	return (0 == index) ? _size : (index - 1);
+}
diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h
index d2d01efb3..e0122372a 100644
--- a/src/drivers/device/spi.h
+++ b/src/drivers/device/spi.h
@@ -71,7 +71,7 @@ protected:
 	    enum spi_mode_e mode,
 	    uint32_t frequency,
 	    int irq = 0);
-	~SPI();
+	virtual ~SPI();
 
 	virtual int	init();
 
diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h
index bffc35c62..7bb9ee2af 100644
--- a/src/drivers/drv_airspeed.h
+++ b/src/drivers/drv_airspeed.h
@@ -57,5 +57,14 @@
 #define _AIRSPEEDIOCBASE		(0x7700)
 #define __AIRSPEEDIOC(_n)		(_IOC(_AIRSPEEDIOCBASE, _n))
 
+#define AIRSPEEDIOCSSCALE		__AIRSPEEDIOC(0)
+#define AIRSPEEDIOCGSCALE		__AIRSPEEDIOC(1)
+
+
+/** airspeed scaling factors; out = (in * Vscale) + offset */
+struct airspeed_scale {
+	float	offset_pa;
+	float	scale;
+};
 
 #endif /* _DRV_AIRSPEED_H */
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 52a667403..ec9d4ca09 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -118,11 +118,8 @@ ORB_DECLARE(output_pwm);
 /** start DSM bind */
 #define DSM_BIND_START	_IOC(_PWM_SERVO_BASE, 7)
 
-/** stop DSM bind */
-#define DSM_BIND_STOP	_IOC(_PWM_SERVO_BASE, 8)
-
 /** Power up DSM receiver */
-#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 9)
+#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8)
 
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index b34d3fa5d..cd72d9d23 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -72,9 +72,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/subsystem_info.h>
-
-/* Default I2C bus */
-#define PX4_I2C_BUS_DEFAULT		PX4_I2C_BUS_EXPANSION
+#include <drivers/airspeed/airspeed.h>
 
 /* I2C bus address */
 #define I2C_ADDRESS	0x75	/* 7-bit address. 8-bit address is 0xEA */
@@ -91,336 +89,32 @@
 /* Measurement rate is 100Hz */
 #define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
 
-/* 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 ETSAirspeed : public device::I2C
+class ETSAirspeed : public Airspeed
 {
 public:
 	ETSAirspeed(int bus, int address = I2C_ADDRESS);
-	virtual ~ETSAirspeed();
-
-	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:
-	work_s			_work;
-	unsigned		_num_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	differential_pressure_s	*_reports;
-	bool			_sensor_ok;
-	int			_measure_ticks;
-	bool			_collect_phase;
-	int			_diff_pres_offset;
-
-	orb_advert_t		_airspeed_pub;
-
-	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();
 
 	/**
 	* 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);
-
+	virtual void	cycle();
+	virtual int	measure();
+	virtual int	collect();
 
 };
 
-/* 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 ets_airspeed_main(int argc, char *argv[]);
 
-ETSAirspeed::ETSAirspeed(int bus, int address) :
-	I2C("ETSAirspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
-	_reports(nullptr),
-	_sensor_ok(false),
-	_measure_ticks(0),
-	_collect_phase(false),
-	_diff_pres_offset(0),
-	_airspeed_pub(-1),
-	_sample_perf(perf_alloc(PC_ELAPSED, "ets_airspeed_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "ets_airspeed_comms_errors")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "ets_airspeed_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));
-}
-
-ETSAirspeed::~ETSAirspeed()
+ETSAirspeed::ETSAirspeed(int bus, int address) : Airspeed(bus, address,
+	CONVERSION_INTERVAL)
 {
-	/* make sure we are truly inactive */
-	stop();
 
-	/* free any existing reports */
-	if (_reports != nullptr)
-		delete[] _reports;
-}
-
-int
-ETSAirspeed::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 differential_pressure_s[_num_reports];
-
-	for (unsigned i = 0; i < _num_reports; i++)
-		_reports[i].max_differential_pressure_pa = 0;
-
-	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]);
-
-	if (_airspeed_pub < 0)
-		debug("failed to create airspeed sensor 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
-ETSAirspeed::probe()
-{
-	return measure();
-}
-
-int
-ETSAirspeed::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(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(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 differential_pressure_s *buf = new struct differential_pressure_s[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;
-
-	default:
-		/* give it to the superclass */
-		return I2C::ioctl(filp, cmd, arg);
-	}
-}
-
-ssize_t
-ETSAirspeed::read(struct file *filp, char *buffer, size_t buflen)
-{
-	unsigned count = buflen / sizeof(struct differential_pressure_s);
-	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(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
@@ -463,9 +157,15 @@ ETSAirspeed::collect()
 	}
 
 	uint16_t diff_pres_pa = val[1] << 8 | val[0];
+        if (diff_pres_pa == 0) {
+            // a zero value means the pressure sensor cannot give us a
+            // value. We need to return, and not report a value or the
+            // caller could end up using this value as part of an
+            // average
+            log("zero value from sensor"); 
+            return -1;
+        }
 
-	// XXX move the parameter read out of the driver.
-	param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
 	if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
 		diff_pres_pa = 0;
 
@@ -506,47 +206,6 @@ ETSAirspeed::collect()
 }
 
 void
-ETSAirspeed::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)&ETSAirspeed::cycle_trampoline, this, 1);
-
-	/* notify about state change */
-	struct subsystem_info_s info = {
-		true,
-		true,
-		true,
-		SUBSYSTEM_TYPE_DIFFPRESSURE
-	};
-	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
-ETSAirspeed::stop()
-{
-	work_cancel(HPWORK, &_work);
-}
-
-void
-ETSAirspeed::cycle_trampoline(void *arg)
-{
-	ETSAirspeed *dev = (ETSAirspeed *)arg;
-
-	dev->cycle();
-}
-
-void
 ETSAirspeed::cycle()
 {
 	/* collection phase? */
@@ -571,7 +230,7 @@ ETSAirspeed::cycle()
 			/* schedule a fresh cycle call when we are ready to measure again */
 			work_queue(HPWORK,
 				   &_work,
-				   (worker_t)&ETSAirspeed::cycle_trampoline,
+				   (worker_t)&Airspeed::cycle_trampoline,
 				   this,
 				   _measure_ticks - USEC2TICK(CONVERSION_INTERVAL));
 
@@ -589,22 +248,11 @@ ETSAirspeed::cycle()
 	/* schedule a fresh cycle call when the measurement is done */
 	work_queue(HPWORK,
 		   &_work,
-		   (worker_t)&ETSAirspeed::cycle_trampoline,
+		   (worker_t)&Airspeed::cycle_trampoline,
 		   this,
 		   USEC2TICK(CONVERSION_INTERVAL));
 }
 
-void
-ETSAirspeed::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.
  */
@@ -642,7 +290,7 @@ start(int i2c_bus)
 	if (g_dev == nullptr)
 		goto fail;
 
-	if (OK != g_dev->init())
+	if (OK != g_dev->Airspeed::init())
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
@@ -735,6 +383,10 @@ test()
 		warnx("diff pressure: %d pa", report.differential_pressure_pa);
 	}
 
+	/* reset the sensor polling to its default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT))
+		errx(1, "failed to set default rate");
+
 	errx(0, "PASS");
 }
 
@@ -779,11 +431,11 @@ info()
 static void
 ets_airspeed_usage()
 {
-	fprintf(stderr, "usage: ets_airspeed command [options]\n");
-	fprintf(stderr, "options:\n");
-	fprintf(stderr, "\t-b --bus i2cbus (%d)\n", PX4_I2C_BUS_DEFAULT);
-	fprintf(stderr, "command:\n");
-	fprintf(stderr, "\tstart|stop|reset|test|info\n");
+	warnx("usage: ets_airspeed command [options]");
+	warnx("options:");
+	warnx("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
+	warnx("command:");
+	warnx("\tstart|stop|reset|test|info");
 }
 
 int
diff --git a/src/drivers/ets_airspeed/module.mk b/src/drivers/ets_airspeed/module.mk
index cb5d3b1ed..15346c5c5 100644
--- a/src/drivers/ets_airspeed/module.mk
+++ b/src/drivers/ets_airspeed/module.mk
@@ -36,6 +36,6 @@
 #
 
 MODULE_COMMAND		= ets_airspeed
-MODULE_STACKSIZE	= 1024
+MODULE_STACKSIZE	= 2048
 
 SRCS			= ets_airspeed.cpp
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 98098c83b..1ffca2f43 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -333,8 +333,13 @@ L3GD20::init()
 	write_reg(ADDR_CTRL_REG4, REG4_BDU);
 	write_reg(ADDR_CTRL_REG5, 0);
 
-	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);	  /* disable wake-on-interrupt */
-	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */
+
+	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
+
+        /* disable FIFO. This makes things simpler and ensures we
+         * aren't getting stale data. It means we must run the hrt
+         * callback fast enough to not miss data. */
+	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);	
 
 	set_range(500);				/* default to 500dps */
 	set_samplerate(0);			/* max sample rate */
diff --git a/src/drivers/md25/BlockSysIdent.cpp b/src/drivers/md25/BlockSysIdent.cpp
new file mode 100644
index 000000000..23b0724d8
--- /dev/null
+++ b/src/drivers/md25/BlockSysIdent.cpp
@@ -0,0 +1,8 @@
+#include "BlockSysIdent.hpp"
+
+BlockSysIdent::BlockSysIdent() :
+	Block(NULL, "SYSID"),
+	_freq(this, "FREQ"),
+	_ampl(this, "AMPL")
+{
+}
diff --git a/src/drivers/md25/BlockSysIdent.hpp b/src/drivers/md25/BlockSysIdent.hpp
new file mode 100644
index 000000000..270635f40
--- /dev/null
+++ b/src/drivers/md25/BlockSysIdent.hpp
@@ -0,0 +1,10 @@
+#include <controllib/block/Block.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+class BlockSysIdent : public control::Block {
+public:
+	BlockSysIdent();
+private:
+	control::BlockParam<float> _freq;
+	control::BlockParam<float> _ampl;
+};
diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp
index 71932ad65..d43e3aef9 100644
--- a/src/drivers/md25/md25.cpp
+++ b/src/drivers/md25/md25.cpp
@@ -45,9 +45,16 @@
 #include "md25.hpp"
 #include <poll.h>
 #include <stdio.h>
+#include <math.h>
+#include <string.h>
 
 #include <systemlib/err.h>
 #include <arch/board/board.h>
+#include <mavlink/mavlink_log.h>
+
+#include <controllib/uorb/UOrbPublication.hpp>
+#include <uORB/topics/debug_key_value.h>
+#include <drivers/drv_hrt.h>
 
 // registers
 enum {
@@ -72,6 +79,9 @@ enum {
 	REG_COMMAND_RW,
 };
 
+// File descriptors
+static int mavlink_fd;
+
 MD25::MD25(const char *deviceName, int bus,
 	   uint16_t address, uint32_t speed) :
 	I2C("MD25", deviceName, bus, address, speed),
@@ -106,7 +116,8 @@ MD25::MD25(const char *deviceName, int bus,
 	setMotor2Speed(0);
 	resetEncoders();
 	_setMode(MD25::MODE_UNSIGNED_SPEED);
-	setSpeedRegulation(true);
+	setSpeedRegulation(false);
+	setMotorAccel(10);
 	setTimeout(true);
 }
 
@@ -298,6 +309,12 @@ int MD25::setDeviceAddress(uint8_t address)
 	return OK;
 }
 
+int MD25::setMotorAccel(uint8_t accel)
+{
+	return _writeUint8(REG_ACCEL_RATE_RW,
+			   accel);
+}
+
 int MD25::setMotor1Speed(float value)
 {
 	return _writeUint8(REG_SPEED1_RW,
@@ -451,12 +468,12 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
 	MD25 md25("/dev/md25", bus, address);
 
 	// print status
-	char buf[200];
+	char buf[400];
 	md25.status(buf, sizeof(buf));
 	printf("%s\n", buf);
 
 	// setup for test
-	md25.setSpeedRegulation(true);
+	md25.setSpeedRegulation(false);
 	md25.setTimeout(true);
 	float dt = 0.1;
 	float speed = 0.2;
@@ -550,4 +567,68 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
 	return 0;
 }
 
+int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency)
+{
+	printf("md25 sine: starting\n");
+
+	// setup
+	MD25 md25("/dev/md25", bus, address);
+
+	// print status
+	char buf[400];
+	md25.status(buf, sizeof(buf));
+	printf("%s\n", buf);
+
+	// setup for test
+	md25.setSpeedRegulation(false);
+	md25.setTimeout(true);
+	float dt = 0.01;
+	float t_final = 60.0;
+	float prev_revolution = md25.getRevolutions1();
+
+	// debug publication
+	control::UOrbPublication<debug_key_value_s> debug_msg(NULL,
+			ORB_ID(debug_key_value));
+
+	// sine wave for motor 1
+	md25.resetEncoders();
+	while (true) {
+
+		// input
+		uint64_t timestamp = hrt_absolute_time();
+		float t = timestamp/1000000.0f;
+
+		float input_value = amplitude*sinf(2*M_PI*frequency*t);
+		md25.setMotor1Speed(input_value);
+
+		// output
+		md25.readData();
+		float current_revolution = md25.getRevolutions1();
+
+		// send input message
+		//strncpy(debug_msg.key, "md25 in   ", 10);
+		//debug_msg.timestamp_ms = 1000*timestamp;
+		//debug_msg.value = input_value;
+		//debug_msg.update();
+
+		// send output message
+		strncpy(debug_msg.key, "md25 out  ", 10);
+		debug_msg.timestamp_ms = 1000*timestamp;
+		debug_msg.value = current_revolution;
+		debug_msg.update();
+
+		if (t > t_final) break;
+
+		// update for next step
+		prev_revolution = current_revolution;
+
+		// sleep
+		usleep(1000000 * dt);
+	}
+	md25.setMotor1Speed(0);
+
+	printf("md25 sine complete\n");
+	return 0;
+}
+
 // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp
index e77511b16..1661f67f9 100644
--- a/src/drivers/md25/md25.hpp
+++ b/src/drivers/md25/md25.hpp
@@ -46,7 +46,7 @@
 
 #include <poll.h>
 #include <stdio.h>
-#include <controllib/block/UOrbSubscription.hpp>
+#include <controllib/uorb/UOrbSubscription.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <drivers/device/i2c.h>
 
@@ -213,6 +213,19 @@ public:
 	int setDeviceAddress(uint8_t address);
 
 	/**
+	 * set motor acceleration
+	 * @param accel
+	 * controls motor speed change (1-10)
+	 * accel rate  | time for full fwd. to full rev.
+	 *  1          | 6.375 s
+	 *  2          | 1.6 s
+	 *  3          | 0.675 s
+	 *  5(default) | 1.275 s
+	 * 10          | 0.65 s
+	 */
+	int setMotorAccel(uint8_t accel);
+
+	/**
 	 * set motor 1 speed
 	 * @param normSpeed normalize speed between -1 and 1
 	 * @return non-zero -> error
@@ -290,4 +303,7 @@ private:
 // unit testing
 int md25Test(const char *deviceName, uint8_t bus, uint8_t address);
 
+// sine testing
+int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency);
+
 // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp
index e62c46b0d..7e5904d05 100644
--- a/src/drivers/md25/md25_main.cpp
+++ b/src/drivers/md25/md25_main.cpp
@@ -82,7 +82,7 @@ usage(const char *reason)
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
 
-	fprintf(stderr, "usage: md25 {start|stop|status|search|test|change_address}\n\n");
+	fprintf(stderr, "usage: md25 {start|stop|read|status|search|test|change_address}\n\n");
 	exit(1);
 }
 
@@ -136,6 +136,28 @@ int md25_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "sine")) {
+
+		if (argc < 6) {
+			printf("usage: md25 sine bus address amp freq\n");
+			exit(0);
+		}
+
+		const char *deviceName = "/dev/md25";
+
+		uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+		uint8_t address = strtoul(argv[3], nullptr, 0);
+
+		float amplitude = atof(argv[4]);
+
+		float frequency = atof(argv[5]);
+
+		md25Sine(deviceName, bus, address, amplitude, frequency);
+
+		exit(0);
+	}
+
 	if (!strcmp(argv[1], "probe")) {
 		if (argc < 4) {
 			printf("usage: md25 probe bus address\n");
@@ -162,6 +184,29 @@ int md25_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "read")) {
+		if (argc < 4) {
+			printf("usage: md25 read bus address\n");
+			exit(0);
+		}
+
+		const char *deviceName = "/dev/md25";
+
+		uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+		uint8_t address = strtoul(argv[3], nullptr, 0);
+
+		MD25 md25(deviceName, bus, address);
+
+		// print status
+		char buf[400];
+		md25.status(buf, sizeof(buf));
+		printf("%s\n", buf);
+
+		exit(0);
+	}
+	
+
 	if (!strcmp(argv[1], "search")) {
 		if (argc < 3) {
 			printf("usage: md25 search bus\n");
@@ -246,7 +291,7 @@ int md25_thread_main(int argc, char *argv[])
 	uint8_t address = strtoul(argv[4], nullptr, 0);
 
 	// start
-	MD25 md25("/dev/md25", bus, address);
+	MD25 md25(deviceName, bus, address);
 
 	thread_running = true;
 
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
new file mode 100644
index 000000000..68d2c5d65
--- /dev/null
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -0,0 +1,520 @@
+/****************************************************************************
+ *
+ *   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 meas_airspeed.cpp
+ * @author Lorenz Meier
+ * @author Sarthak Kaingade
+ * @author Simon Wilks
+ *
+ * Driver for the MEAS Spec series connected via I2C.
+ *
+ * Supported sensors:
+ *
+ *    - MS4525DO (http://www.meas-spec.com/downloads/MS4525DO.pdf)
+ *    - untested: MS5525DSO (http://www.meas-spec.com/downloads/MS5525DSO.pdf)
+ *
+ * Interface application notes:
+ *
+ *    - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf)
+ */
+
+#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/airspeed.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+
+#include <drivers/drv_airspeed.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/subsystem_info.h>
+
+#include <drivers/airspeed/airspeed.h>
+
+/* I2C bus address is 1010001x */
+#define I2C_ADDRESS_MS4525DO	0x28	//0x51 /* 7-bit address. */
+/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
+#define I2C_ADDRESS_MS5525DSO	0x77	//0x77/* 7-bit address, addr. pin pulled low */
+
+/* Register address */
+#define ADDR_READ_MR			0x00	/* write to this address to start conversion */
+
+/* Measurement rate is 100Hz */
+#define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
+
+class MEASAirspeed : public Airspeed
+{
+public:
+	MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO);
+
+protected:
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	virtual void	cycle();
+	virtual int	measure();
+	virtual int	collect();
+
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
+
+MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address,
+			CONVERSION_INTERVAL)
+{
+
+}
+
+int
+MEASAirspeed::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	uint8_t cmd = 0;
+	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
+MEASAirspeed::collect()
+{
+	int	ret = -EIO;
+
+	/* read from the sensor */
+	uint8_t val[4] = {0, 0, 0, 0};
+
+
+	perf_begin(_sample_perf);
+
+	ret = transfer(nullptr, 0, &val[0], 4);
+
+	if (ret < 0) {
+		log("error reading from sensor: %d", ret);
+		return ret;
+	}
+
+	uint8_t status = val[0] & 0xC0;
+
+	if (status == 2) {
+		log("err: stale data");
+
+	} else if (status == 3) {
+		log("err: fault");
+	}
+
+	//uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8);
+	uint16_t temp = (val[3] & 0xE0) << 8 | val[2];
+
+	// XXX leaving this in until new calculation method has been cross-checked
+	//diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f));
+	//diff_pres_pa -= _diff_pres_offset;
+	int16_t dp_raw = 0, dT_raw = 0;
+	dp_raw = (val[0] << 8) + val[1];
+	dp_raw = 0x3FFF & dp_raw;
+	dT_raw = (val[2] << 8) + val[3];
+	dT_raw = (0xFFE0 & dT_raw) >> 5;
+	float temperature = ((200 * dT_raw) / 2047) - 50;
+
+	// XXX we may want to smooth out the readings to remove noise.
+
+	// 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;
+
+	// 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;
+	}
+
+	/* announce the airspeed if needed, just publish else */
+	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_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;
+
+	perf_end(_sample_perf);
+
+	return ret;
+}
+
+void
+MEASAirspeed::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(CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&Airspeed::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(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)&Airspeed::cycle_trampoline,
+		   this,
+		   USEC2TICK(CONVERSION_INTERVAL));
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace meas_airspeed
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+MEASAirspeed	*g_dev = nullptr;
+
+void	start(int i2c_bus);
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(int i2c_bus)
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver, try the MS4525DO first */
+	g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO);
+
+	/* check if the MS4525DO was instantiated */
+	if (g_dev == nullptr)
+		goto fail;
+
+	/* try the MS5525DSO next if init fails */
+	if (OK != g_dev->Airspeed::init()) {
+		delete g_dev;
+		g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO);
+
+		/* check if the MS5525DSO was instantiated */
+		if (g_dev == nullptr)
+			goto fail;
+
+		/* both versions failed if the init for the MS5525DSO fails, give up */
+		if (OK != g_dev->Airspeed::init())
+			goto fail;
+	}
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(AIRSPEED_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 differential_pressure_s report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'meas_airspeed start' if the driver is not running", AIRSPEED_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("diff pressure: %d pa", report.differential_pressure_pa);
+
+	/* 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("diff pressure: %d pa", report.differential_pressure_pa);
+		warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
+	}
+
+	/* reset the sensor polling to its default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT))
+		errx(1, "failed to set default rate");
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(AIRSPEED_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
+
+
+static void
+meas_airspeed_usage()
+{
+	warnx("usage: meas_airspeed command [options]");
+	warnx("options:");
+	warnx("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
+	warnx("command:");
+	warnx("\tstart|stop|reset|test|info");
+}
+
+int
+meas_airspeed_main(int argc, char *argv[])
+{
+	int i2c_bus = PX4_I2C_BUS_DEFAULT;
+
+	int i;
+
+	for (i = 1; i < argc; i++) {
+		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
+			if (argc > i + 1) {
+				i2c_bus = atoi(argv[i + 1]);
+			}
+		}
+	}
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start"))
+		meas_airspeed::start(i2c_bus);
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[1], "stop"))
+		meas_airspeed::stop();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		meas_airspeed::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		meas_airspeed::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+		meas_airspeed::info();
+
+	meas_airspeed_usage();
+	exit(0);
+}
diff --git a/src/drivers/meas_airspeed/module.mk b/src/drivers/meas_airspeed/module.mk
new file mode 100644
index 000000000..fed4078b6
--- /dev/null
+++ b/src/drivers/meas_airspeed/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build the MEAS Spec airspeed sensor driver.
+#
+
+MODULE_COMMAND		= meas_airspeed
+MODULE_STACKSIZE	= 2048
+
+SRCS			= meas_airspeed.cpp
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 8d9054a38..c4e331a30 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,6 +35,9 @@
  * @file mpu6000.cpp
  *
  * Driver for the Invensense MPU6000 connected via SPI.
+ *
+ * @author Andrew Tridgell
+ * @author Pat Hickey
  */
 
 #include <nuttx/config.h>
@@ -64,8 +67,10 @@
 #include <drivers/drv_hrt.h>
 
 #include <drivers/device/spi.h>
+#include <drivers/device/ringbuffer.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 #define DIR_READ			0x80
 #define DIR_WRITE			0x00
@@ -178,21 +183,33 @@ private:
 	struct hrt_call		_call;
 	unsigned		_call_interval;
 
-	struct accel_report	_accel_report;
+	typedef RingBuffer<accel_report> AccelReportBuffer;
+	AccelReportBuffer	*_accel_reports;
+
 	struct accel_scale	_accel_scale;
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
 
-	struct gyro_report	_gyro_report;
+	typedef RingBuffer<gyro_report> GyroReportBuffer;
+	GyroReportBuffer	*_gyro_reports;
+
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
 	float			_gyro_range_rad_s;
 	orb_advert_t		_gyro_topic;
 
 	unsigned		_reads;
+	unsigned		_sample_rate;
 	perf_counter_t		_sample_perf;
 
+	math::LowPassFilter2p	_accel_filter_x;
+	math::LowPassFilter2p	_accel_filter_y;
+	math::LowPassFilter2p	_accel_filter_z;
+	math::LowPassFilter2p	_gyro_filter_x;
+	math::LowPassFilter2p	_gyro_filter_y;
+	math::LowPassFilter2p	_gyro_filter_z;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -204,6 +221,13 @@ private:
 	void			stop();
 
 	/**
+	 * Reset chip.
+	 *
+	 * Resets the chip and measurements ranges, but not scale and offset.
+	 */
+	void			reset();
+
+	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
 	 *
@@ -215,7 +239,7 @@ private:
 	static void		measure_trampoline(void *arg);
 
 	/**
-	 * Fetch measurements from the sensor and update the report ring.
+	 * Fetch measurements from the sensor and update the report buffers.
 	 */
 	void			measure();
 
@@ -307,14 +331,23 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
 	_gyro(new MPU6000_gyro(this)),
 	_product(0),
 	_call_interval(0),
+	_accel_reports(nullptr),
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
 	_accel_topic(-1),
+	_gyro_reports(nullptr),
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
 	_gyro_topic(-1),
 	_reads(0),
-	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read"))
+	_sample_rate(1000),
+	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
+	_accel_filter_x(1000, 30),
+	_accel_filter_y(1000, 30),
+	_accel_filter_z(1000, 30),
+	_gyro_filter_x(1000, 30),
+        _gyro_filter_y(1000, 30),
+        _gyro_filter_z(1000, 30)
 {
 	// disable debug() calls
 	_debug_enabled = false;
@@ -335,8 +368,6 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
 	_gyro_scale.z_offset = 0;
 	_gyro_scale.z_scale  = 1.0f;
 
-	memset(&_accel_report, 0, sizeof(_accel_report));
-	memset(&_gyro_report, 0, sizeof(_gyro_report));
 	memset(&_call, 0, sizeof(_call));
 }
 
@@ -348,6 +379,12 @@ MPU6000::~MPU6000()
 	/* delete the gyro subdriver */
 	delete _gyro;
 
+	/* free any existing reports */
+	if (_accel_reports != nullptr)
+		delete _accel_reports;
+	if (_gyro_reports != nullptr)
+		delete _gyro_reports;
+
 	/* delete the perf counter */
 	perf_free(_sample_perf);
 }
@@ -356,6 +393,7 @@ int
 MPU6000::init()
 {
 	int ret;
+	int gyro_ret;
 
 	/* do SPI init (and probe) first */
 	ret = SPI::init();
@@ -366,9 +404,58 @@ MPU6000::init()
 		return ret;
 	}
 
-	/* advertise sensor topics */
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report);
-	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report);
+	/* allocate basic report buffers */
+	_accel_reports = new AccelReportBuffer(2);
+	if (_accel_reports == nullptr)
+		goto out;
+
+	_gyro_reports = new GyroReportBuffer(2);
+	if (_gyro_reports == nullptr)
+		goto out;
+
+	reset();
+
+	/* Initialize offsets and scales */
+	_accel_scale.x_offset = 0;
+	_accel_scale.x_scale  = 1.0f;
+	_accel_scale.y_offset = 0;
+	_accel_scale.y_scale  = 1.0f;
+	_accel_scale.z_offset = 0;
+	_accel_scale.z_scale  = 1.0f;
+
+	_gyro_scale.x_offset = 0;
+	_gyro_scale.x_scale  = 1.0f;
+	_gyro_scale.y_offset = 0;
+	_gyro_scale.y_scale  = 1.0f;
+	_gyro_scale.z_offset = 0;
+	_gyro_scale.z_scale  = 1.0f;
+
+	/* do CDev init for the gyro device node, keep it optional */
+	gyro_ret = _gyro->init();
+
+	/* fetch an initial set of measurements for advertisement */
+	measure();
+
+	if (gyro_ret != OK) {
+		_gyro_topic = -1;
+	} else {
+		gyro_report 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_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
+
+out:
+	return ret;
+}
+
+void MPU6000::reset()
+{
 
 	// Chip reset
 	write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
@@ -384,13 +471,13 @@ MPU6000::init()
 
 	// SAMPLE RATE
 	//write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
-	_set_sample_rate(200); // default sample rate = 200Hz
+	_set_sample_rate(_sample_rate); // default sample rate = 200Hz
 	usleep(1000);
 
 	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)
 	// was 90 Hz, but this ruins quality and does not improve the
 	// system response
-	_set_dlpf_filter(20);
+	_set_dlpf_filter(42);
 	usleep(1000);
 	// Gyro scale 2000 deg/s ()
 	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
@@ -401,12 +488,6 @@ MPU6000::init()
 	// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
 	// scaling factor:
 	// 1/(2^15)*(2000/180)*PI
-	_gyro_scale.x_offset = 0;
-	_gyro_scale.x_scale  = 1.0f;
-	_gyro_scale.y_offset = 0;
-	_gyro_scale.y_scale  = 1.0f;
-	_gyro_scale.z_offset = 0;
-	_gyro_scale.z_scale  = 1.0f;
 	_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
 	_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;
 
@@ -439,12 +520,6 @@ MPU6000::init()
 
 	// Correct accel scale factors of 4096 LSB/g
 	// scale to m/s^2 ( 1g = 9.81 m/s^2)
-	_accel_scale.x_offset = 0;
-	_accel_scale.x_scale  = 1.0f;
-	_accel_scale.y_offset = 0;
-	_accel_scale.y_scale  = 1.0f;
-	_accel_scale.z_offset = 0;
-	_accel_scale.z_scale  = 1.0f;
 	_accel_range_scale = (9.81f / 4096.0f);
 	_accel_range_m_s2 = 8.0f * 9.81f;
 
@@ -460,14 +535,6 @@ MPU6000::init()
 	// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
 	usleep(1000);
 
-	/* do CDev init for the gyro device node, keep it optional */
-	int gyro_ret = _gyro->init();
-
-	if (gyro_ret != OK) {
-		_gyro_topic = -1;
-	}
-
-	return ret;
 }
 
 int
@@ -509,6 +576,7 @@ MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
   if(div>200) div=200;
   if(div<1) div=1;
   write_reg(MPUREG_SMPLRT_DIV, div-1);
+  _sample_rate = 1000 / div;
 }
 
 /*
@@ -545,21 +613,33 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
 ssize_t
 MPU6000::read(struct file *filp, char *buffer, size_t buflen)
 {
-	int ret = 0;
+	unsigned count = buflen / sizeof(accel_report);
 
 	/* buffer must be large enough */
-	if (buflen < sizeof(_accel_report))
+	if (count < 1)
 		return -ENOSPC;
 
-	/* if automatic measurement is not enabled */
-	if (_call_interval == 0)
+	/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
+	if (_call_interval == 0) {
+		_accel_reports->flush();
 		measure();
+	}
 
-	/* copy out the latest reports */
-	memcpy(buffer, &_accel_report, sizeof(_accel_report));
-	ret = sizeof(_accel_report);
+	/* if no data, error (we could block here) */
+	if (_accel_reports->empty())
+		return -EAGAIN;
+
+	/* copy reports out of our buffer to the caller */
+	accel_report *arp = reinterpret_cast<accel_report *>(buffer);
+	int transferred = 0;
+	while (count--) {
+		if (!_accel_reports->get(*arp++))
+			break;
+		transferred++;
+	}
 
-	return ret;
+	/* return the number of bytes transferred */
+	return (transferred * sizeof(accel_report));
 }
 
 int
@@ -576,21 +656,33 @@ MPU6000::self_test()
 ssize_t
 MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
 {
-	int ret = 0;
+	unsigned count = buflen / sizeof(gyro_report);
 
 	/* buffer must be large enough */
-	if (buflen < sizeof(_gyro_report))
+	if (count < 1)
 		return -ENOSPC;
 
-	/* if automatic measurement is not enabled */
-	if (_call_interval == 0)
+	/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
+	if (_call_interval == 0) {
+		_gyro_reports->flush();
 		measure();
+	}
 
-	/* copy out the latest report */
-	memcpy(buffer, &_gyro_report, sizeof(_gyro_report));
-	ret = sizeof(_gyro_report);
+	/* if no data, error (we could block here) */
+	if (_gyro_reports->empty())
+		return -EAGAIN;
+
+	/* copy reports out of our buffer to the caller */
+	gyro_report *arp = reinterpret_cast<gyro_report *>(buffer);
+	int transferred = 0;
+	while (count--) {
+		if (!_gyro_reports->get(*arp++))
+			break;
+		transferred++;
+	}
 
-	return ret;
+	/* return the number of bytes transferred */
+	return (transferred * sizeof(gyro_report));
 }
 
 int
@@ -598,6 +690,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
 
+	case SENSORIOCRESET:
+		reset();
+		return OK;
+
 	case SENSORIOCSPOLLRATE: {
 			switch (arg) {
 
@@ -617,8 +713,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
-				/* XXX 500Hz is just a wild guess */
-				return ioctl(filp, SENSORIOCSPOLLRATE, 500);
+				/* set to same as sample rate per default */
+				return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
@@ -632,6 +728,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 					if (ticks < 1000)
 						return -EINVAL;
 
+					// adjust filters
+					float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
+					float sample_rate = 1.0e6f/ticks;
+					_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+					_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+					_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+
+
+					float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
+					_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
+					_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
+					_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
+
 					/* update interval for next measurement */
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_call.period = _call_interval = ticks;
@@ -651,23 +760,51 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 		return 1000000 / _call_interval;
 
-	case SENSORIOCSQUEUEDEPTH:
-		/* XXX not implemented */
-		return -EINVAL;
+	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;
+		}
 
 	case SENSORIOCGQUEUEDEPTH:
-		/* XXX not implemented */
-		return -EINVAL;
+		return _accel_reports->size();
 
+	case ACCELIOCGSAMPLERATE:
+		return _sample_rate;
 
 	case ACCELIOCSSAMPLERATE:
-	case ACCELIOCGSAMPLERATE:
-	  _set_sample_rate(arg);
-	  return OK;
+		_set_sample_rate(arg);
+		return OK;
 
-	case ACCELIOCSLOWPASS:
 	case ACCELIOCGLOWPASS:
-		_set_dlpf_filter((uint16_t)arg);
+		return _accel_filter_x.get_cutoff_freq();
+
+	case ACCELIOCSLOWPASS:
+		
+		// XXX decide on relationship of both filters
+		// i.e. disable the on-chip filter
+		//_set_dlpf_filter((uint16_t)arg);
+                _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+                _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+                _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		return OK;
 
 	case ACCELIOCSSCALE:
@@ -689,12 +826,13 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 
 	case ACCELIOCSRANGE:
-	case ACCELIOCGRANGE:
 		/* XXX not implemented */
 		// XXX change these two values on set:
 		// _accel_range_scale = (9.81f / 4096.0f);
-		// _accel_range_rad_s = 8.0f * 9.81f;
+		// _accel_range_m_s2 = 8.0f * 9.81f;
 		return -EINVAL;
+	case ACCELIOCGRANGE:
+		return _accel_range_m_s2;
 
 	case ACCELIOCSELFTEST:
 		return self_test();
@@ -713,19 +851,51 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* these are shared with the accel side */
 	case SENSORIOCSPOLLRATE:
 	case SENSORIOCGPOLLRATE:
-	case SENSORIOCSQUEUEDEPTH:
-	case SENSORIOCGQUEUEDEPTH:
 	case SENSORIOCRESET:
 		return ioctl(filp, cmd, arg);
 
-	case GYROIOCSSAMPLERATE:
+	case SENSORIOCSQUEUEDEPTH: {
+			/* lower bound is mandatory, upper bound is a sanity check */
+			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;
+				return -ENOMEM;
+			}
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop();
+			delete _gyro_reports;
+			_gyro_reports = buf;
+			start();
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _gyro_reports->size();
+
 	case GYROIOCGSAMPLERATE:
-	  _set_sample_rate(arg);
-	  return OK;
+		return _sample_rate;
+
+	case GYROIOCSSAMPLERATE:
+		_set_sample_rate(arg);
+		return OK;
 
-	case GYROIOCSLOWPASS:
 	case GYROIOCGLOWPASS:
-		_set_dlpf_filter((uint16_t)arg);
+		return _gyro_filter_x.get_cutoff_freq();
+	case GYROIOCSLOWPASS:
+		_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+		_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+		_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+		// XXX check relation to the internal lowpass
+		//_set_dlpf_filter((uint16_t)arg);
 		return OK;
 
 	case GYROIOCSSCALE:
@@ -739,12 +909,13 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 
 	case GYROIOCSRANGE:
-	case GYROIOCGRANGE:
 		/* XXX not implemented */
 		// XXX change these two values on set:
 		// _gyro_range_scale = xx
-		// _gyro_range_m_s2 = xx
+		// _gyro_range_rad_s = xx
 		return -EINVAL;
+	case GYROIOCGRANGE:
+		return _gyro_range_rad_s;
 
 	case GYROIOCSELFTEST:
 		return self_test();
@@ -849,6 +1020,10 @@ MPU6000::start()
 	/* make sure we are stopped first */
 	stop();
 
+	/* discard any stale data in the buffers */
+	_accel_reports->flush();
+	_gyro_reports->flush();
+
 	/* start polling at the specified rate */
 	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this);
 }
@@ -862,7 +1037,7 @@ MPU6000::stop()
 void
 MPU6000::measure_trampoline(void *arg)
 {
-	MPU6000 *dev = (MPU6000 *)arg;
+	MPU6000 *dev = reinterpret_cast<MPU6000 *>(arg);
 
 	/* make another measurement */
 	dev->measure();
@@ -944,9 +1119,15 @@ MPU6000::measure()
 	report.gyro_y = gyro_yt;
 
 	/*
+	 * Report buffers.
+	 */
+	accel_report		arb;
+	gyro_report		grb;
+
+	/*
 	 * Adjust and scale results to m/s^2.
 	 */
-	_gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time();
+	grb.timestamp = arb.timestamp = hrt_absolute_time();
 
 
 	/*
@@ -967,40 +1148,53 @@ MPU6000::measure()
 
 	/* NOTE: Axes have been swapped to match the board a few lines above. */
 
-	_accel_report.x_raw = report.accel_x;
-	_accel_report.y_raw = report.accel_y;
-	_accel_report.z_raw = report.accel_z;
+	arb.x_raw = report.accel_x;
+	arb.y_raw = report.accel_y;
+	arb.z_raw = report.accel_z;
 
-	_accel_report.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	_accel_report.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	_accel_report.z = ((report.accel_z * _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;
+	float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	
+	arb.x = _accel_filter_x.apply(x_in_new);
+	arb.y = _accel_filter_y.apply(y_in_new);
+	arb.z = _accel_filter_z.apply(z_in_new);
 
-	_accel_report.temperature_raw = report.temp;
-	_accel_report.temperature = (report.temp) / 361.0f + 35.0f;
+	arb.scaling = _accel_range_scale;
+	arb.range_m_s2 = _accel_range_m_s2;
 
-	_gyro_report.x_raw = report.gyro_x;
-	_gyro_report.y_raw = report.gyro_y;
-	_gyro_report.z_raw = report.gyro_z;
+	arb.temperature_raw = report.temp;
+	arb.temperature = (report.temp) / 361.0f + 35.0f;
 
-	_gyro_report.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
-	_gyro_report.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
-	_gyro_report.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
-	_gyro_report.scaling = _gyro_range_scale;
-	_gyro_report.range_rad_s = _gyro_range_rad_s;
+	grb.x_raw = report.gyro_x;
+	grb.y_raw = report.gyro_y;
+	grb.z_raw = report.gyro_z;
 
-	_gyro_report.temperature_raw = report.temp;
-	_gyro_report.temperature = (report.temp) / 361.0f + 35.0f;
+	float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
+	float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
+	float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
+	
+	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
+	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
+	grb.z = _gyro_filter_z.apply(z_gyro_in_new);
+
+	grb.scaling = _gyro_range_scale;
+	grb.range_rad_s = _gyro_range_rad_s;
+
+	grb.temperature_raw = report.temp;
+	grb.temperature = (report.temp) / 361.0f + 35.0f;
+
+	_accel_reports->put(arb);
+	_gyro_reports->put(grb);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 	_gyro->parent_poll_notify();
 
 	/* and publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report);
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
 	if (_gyro_topic != -1) {
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report);
+		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
 	}
 
 	/* stop measuring */
@@ -1103,21 +1297,19 @@ fail:
 void
 test()
 {
-	int fd = -1;
-	int fd_gyro = -1;
-	struct accel_report a_report;
-	struct gyro_report g_report;
+	accel_report a_report;
+	gyro_report g_report;
 	ssize_t sz;
 
 	/* get the driver */
-	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
 		    ACCEL_DEVICE_PATH);
 
 	/* get the driver */
-	fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
+	int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
 
 	if (fd_gyro < 0)
 		err(1, "%s open failed", GYRO_DEVICE_PATH);
@@ -1129,8 +1321,10 @@ test()
 	/* do a simple demand read */
 	sz = read(fd, &a_report, sizeof(a_report));
 
-	if (sz != sizeof(a_report))
+	if (sz != sizeof(a_report)) {
+		warnx("ret: %d, expected: %d", sz, sizeof(a_report));
 		err(1, "immediate acc read failed");
+	}
 
 	warnx("single read");
 	warnx("time:     %lld", a_report.timestamp);
@@ -1146,8 +1340,10 @@ test()
 	/* do a simple demand read */
 	sz = read(fd_gyro, &g_report, sizeof(g_report));
 
-	if (sz != sizeof(g_report))
+	if (sz != sizeof(g_report)) {
+		warnx("ret: %d, expected: %d", sz, sizeof(g_report));
 		err(1, "immediate gyro read failed");
+	}
 
 	warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
 	warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index bc3f1dcc6..2953639bf 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -207,6 +207,12 @@ private:
 	bool			_dsm_vcc_ctl;
 
 	/**
+	 * System armed
+	 */
+
+	bool			_system_armed;
+
+	/**
 	 * Trampoline to the worker task
 	 */
 	static void		task_main_trampoline(int argc, char *argv[]);
@@ -374,7 +380,8 @@ PX4IO::PX4IO() :
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
 	_battery_last_timestamp(0),
-	_dsm_vcc_ctl(false)
+	_dsm_vcc_ctl(false),
+	_system_armed(false)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -633,9 +640,11 @@ void
 PX4IO::task_main()
 {
 	hrt_abstime last_poll_time = 0;
+	int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
 
 	log("starting");
 
+
 	/*
 	 * Subscribe to the appropriate PWM output topic based on whether we are the
 	 * primary PWM output or not.
@@ -735,6 +744,25 @@ PX4IO::task_main()
 			 */
 			if (fds[3].revents & POLLIN) {
 				parameter_update_s pupdate;
+				int32_t dsm_bind_val;
+				param_t dsm_bind_param;
+
+				// See if bind parameter has been set, and reset it to 0
+				param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
+				if (dsm_bind_val) {
+					if (!_system_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;
+					param_set(dsm_bind_param, &dsm_bind_val);
+				}
 
 				/* copy to reset the notification */
 				orb_copy(ORB_ID(parameter_update), _t_param, &pupdate);
@@ -842,6 +870,8 @@ PX4IO::io_set_arming_state()
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
+	_system_armed = vstatus.flag_system_armed;
+
 	if (armed.armed && !armed.lockdown) {
 		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	} else {
@@ -1578,16 +1608,11 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); 
 		usleep(500000);
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
-		usleep(1000);
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
-		usleep(100000);
+		usleep(50000);
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
-		break;
-
-	case DSM_BIND_STOP:
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
+		usleep(50000);
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
-		usleep(500000);
 		break;
 
 	case DSM_BIND_POWER_UP:
@@ -1829,30 +1854,12 @@ bind(int argc, char *argv[])
 	else 
 		errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
 
-	/* Open console directly to grab CTRL-C signal */
-	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
-	if (!console)
-		errx(1, "failed opening console");
-
 	warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
-	warnx("Press CTRL-C or 'c' when done.");
 
 	g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
 
-	for (;;) {
-		usleep(500000L);
-		/* Check if user wants to quit */
-		char c;
-		if (read(console, &c, 1) == 1) {
-			if (c == 0x03 || c == 0x63) {
-				warnx("Done\n");
-				g_dev->ioctl(nullptr, DSM_BIND_STOP, 0);
-				g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
-				close(console);
-	            exit(0);
-            }
-		}
-	}
+	exit(0);
+
 }
 
 void
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index 7ef3db970..83a1a1abb 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -70,7 +70,7 @@
 #include "stm32_gpio.h"
 #include "stm32_tim.h"
 
-#ifdef CONFIG_HRT_TIMER
+#ifdef HRT_TIMER
 
 /* HRT configuration */
 #if   HRT_TIMER == 1
@@ -155,7 +155,7 @@
 #  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
 # endif
 #else
-# error HRT_TIMER must be set in board.h if CONFIG_HRT_TIMER=y
+# error HRT_TIMER must be a value between 1 and 11
 #endif
 
 /*
@@ -275,7 +275,7 @@ static void		hrt_call_invoke(void);
 /*
  * Specific registers and bits used by PPM sub-functions
  */
-#ifdef CONFIG_HRT_PPM
+#ifdef HRT_PPM_CHANNEL
 /* 
  * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it.
  *
@@ -326,7 +326,7 @@ static void		hrt_call_invoke(void);
 #  define CCER_PPM	(GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */
 #  define CCER_PPM_FLIP	GTIM_CCER_CC4P
 # else
-#  error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set
+#  error HRT_PPM_CHANNEL must be a value between 1 and 4
 # endif
 
 /*
@@ -377,7 +377,7 @@ static void	hrt_ppm_decode(uint32_t status);
 # define CCMR1_PPM	0
 # define CCMR2_PPM	0
 # define CCER_PPM	0
-#endif /* CONFIG_HRT_PPM */
+#endif /* HRT_PPM_CHANNEL */
 
 /*
  * Initialise the timer we are going to use.
@@ -424,7 +424,7 @@ hrt_tim_init(void)
 	up_enable_irq(HRT_TIMER_VECTOR);
 }
 
-#ifdef CONFIG_HRT_PPM
+#ifdef HRT_PPM_CHANNEL
 /*
  * Handle the PPM decoder state machine.
  */
@@ -526,7 +526,7 @@ error:
 	ppm_decoded_channels = 0;
 
 }
-#endif /* CONFIG_HRT_PPM */
+#endif /* HRT_PPM_CHANNEL */
 
 /*
  * Handle the compare interupt by calling the callout dispatcher
@@ -546,7 +546,7 @@ hrt_tim_isr(int irq, void *context)
 	/* ack the interrupts we just read */
 	rSR = ~status;
 
-#ifdef CONFIG_HRT_PPM
+#ifdef HRT_PPM_CHANNEL
 
 	/* was this a PPM edge? */
 	if (status & (SR_INT_PPM | SR_OVF_PPM)) {
@@ -686,7 +686,7 @@ hrt_init(void)
 	sq_init(&callout_queue);
 	hrt_tim_init();
 
-#ifdef CONFIG_HRT_PPM
+#ifdef HRT_PPM_CHANNEL
 	/* configure the PPM input pin */
 	stm32_configgpio(GPIO_PPM_IN);
 #endif
@@ -907,4 +907,4 @@ hrt_latency_update(void)
 }
 
 
-#endif /* CONFIG_HRT_TIMER */
+#endif /* HRT_TIMER */
diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c
index 7b060412c..dbb45a138 100644
--- a/src/drivers/stm32/drv_pwm_servo.c
+++ b/src/drivers/stm32/drv_pwm_servo.c
@@ -88,6 +88,7 @@
 #define rCCR4(_tmr)   	REG(_tmr, STM32_GTIM_CCR4_OFFSET)
 #define rDCR(_tmr)    	REG(_tmr, STM32_GTIM_DCR_OFFSET)
 #define rDMAR(_tmr)   	REG(_tmr, STM32_GTIM_DMAR_OFFSET)
+#define rBDTR(_tmr)	REG(_tmr, STM32_ATIM_BDTR_OFFSET)
 
 static void		pwm_timer_init(unsigned timer);
 static void		pwm_timer_set_rate(unsigned timer, unsigned rate);
@@ -110,6 +111,11 @@ pwm_timer_init(unsigned timer)
 	rCCER(timer) = 0;
 	rDCR(timer) = 0;
 
+	if ((pwm_timers[timer].base == STM32_TIM1_BASE) || (pwm_timers[timer].base == STM32_TIM8_BASE)) {
+		/* master output enable = on */
+		rBDTR(timer) = ATIM_BDTR_MOE;
+	}
+
 	/* configure the timer to free-run at 1MHz */
 	rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1;
 
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index e1409a815..b36ba306b 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -117,10 +117,6 @@
 
 #include <systemlib/err.h>
 
-#ifndef CONFIG_HRT_TIMER
-# error This driver requires CONFIG_HRT_TIMER
-#endif
-
 /* Tone alarm configuration */
 #if   TONE_ALARM_TIMER == 2
 # define TONE_ALARM_BASE		STM32_TIM2_BASE
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index 49d0d157d..f01ee0355 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -47,8 +47,8 @@
 #include <mathlib/mathlib.h>
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
-#include <controllib/block/UOrbSubscription.hpp>
-#include <controllib/block/UOrbPublication.hpp>
+#include <controllib/uorb/UOrbSubscription.hpp>
+#include <controllib/uorb/UOrbPublication.hpp>
 
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 19c96e9f4..65abcde1e 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -224,8 +224,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 	/* subscribe to raw data */
 	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
-	/* rate-limit raw data updates to 200Hz */
-	orb_set_interval(sub_raw, 4);
+	/* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
+	orb_set_interval(sub_raw, 3);
 
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
@@ -236,7 +236,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
-
 	int loopcounter = 0;
 	int printcounter = 0;
 
@@ -384,7 +383,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					static bool const_initialized = false;
 
 					/* initialize with good values once we have a reasonable dt estimate */
-					if (!const_initialized && dt < 0.05f && dt > 0.005f) {
+					if (!const_initialized && dt < 0.05f && dt > 0.001f) {
 						dt = 0.005f;
 						parameters_update(&ekf_param_handles, &ekf_params);
 
@@ -424,7 +423,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						continue;
 					}
 
-					if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
+					if (last_data > 0 && raw.timestamp - last_data > 12000)
+						printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
 
 					last_data = raw.timestamp;
 
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index df92d51d2..b3a5d012b 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -275,8 +275,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 	float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
 	/* EMA time constant in seconds*/
 	float ema_len = 0.2f;
-	/* set "still" threshold to 0.1 m/s^2 */
-	float still_thr2 = pow(0.1f, 2);
+	/* set "still" threshold to 0.25 m/s^2 */
+	float still_thr2 = pow(0.25f, 2);
 	/* set accel error threshold to 5m/s^2 */
 	float accel_err_thr = 5.0f;
 	/* still time required in us */
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c6e209f0f..204a41450 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1317,6 +1317,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp
 			/* ready to arm, blink at 2.5Hz */
 			if (leds_counter & 8) {
 				led_on(LED_AMBER);
+
 			} else {
 				led_off(LED_AMBER);
 			}
@@ -1552,6 +1553,7 @@ void *commander_low_prio_loop(void *arg)
 				mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
 				tune_error();
 			}
+
 			low_prio_task = LOW_PRIO_TASK_NONE;
 			break;
 
@@ -1564,6 +1566,7 @@ void *commander_low_prio_loop(void *arg)
 				mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
 				tune_error();
 			}
+
 			low_prio_task = LOW_PRIO_TASK_NONE;
 			break;
 
diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp
index 5994d2315..b964d40a3 100644
--- a/src/modules/controllib/block/Block.cpp
+++ b/src/modules/controllib/block/Block.cpp
@@ -43,8 +43,8 @@
 
 #include "Block.hpp"
 #include "BlockParam.hpp"
-#include "UOrbSubscription.hpp"
-#include "UOrbPublication.hpp"
+#include "../uorb/UOrbSubscription.hpp"
+#include "../uorb/UOrbPublication.hpp"
 
 namespace control
 {
diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp
index 7a785d12e..fefe99702 100644
--- a/src/modules/controllib/blocks.hpp
+++ b/src/modules/controllib/blocks.hpp
@@ -42,6 +42,7 @@
 #include <assert.h>
 #include <time.h>
 #include <stdlib.h>
+#include <math.h>
 #include <mathlib/math/test/test.hpp>
 
 #include "block/Block.hpp"
diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk
index 13d1069c7..d815a8feb 100644
--- a/src/modules/controllib/module.mk
+++ b/src/modules/controllib/module.mk
@@ -37,7 +37,7 @@
 SRCS		 = test_params.c \
 		   block/Block.cpp \
 		   block/BlockParam.cpp \
-		   block/UOrbPublication.cpp \
-		   block/UOrbSubscription.cpp \
-		   blocks.cpp \
-		   fixedwing.cpp
+		   uorb/UOrbPublication.cpp \
+		   uorb/UOrbSubscription.cpp \
+		   uorb/blocks.cpp \
+		   blocks.cpp
diff --git a/src/modules/controllib/block/UOrbPublication.cpp b/src/modules/controllib/uorb/UOrbPublication.cpp
index f69b39d90..f69b39d90 100644
--- a/src/modules/controllib/block/UOrbPublication.cpp
+++ b/src/modules/controllib/uorb/UOrbPublication.cpp
diff --git a/src/modules/controllib/block/UOrbPublication.hpp b/src/modules/controllib/uorb/UOrbPublication.hpp
index 0a8ae2ff7..6f1f3fc1c 100644
--- a/src/modules/controllib/block/UOrbPublication.hpp
+++ b/src/modules/controllib/uorb/UOrbPublication.hpp
@@ -39,8 +39,8 @@
 #pragma once
 
 #include <uORB/uORB.h>
-#include "Block.hpp"
-#include "List.hpp"
+#include "../block/Block.hpp"
+#include "../block/List.hpp"
 
 
 namespace control
diff --git a/src/modules/controllib/block/UOrbSubscription.cpp b/src/modules/controllib/uorb/UOrbSubscription.cpp
index 022cadd24..022cadd24 100644
--- a/src/modules/controllib/block/UOrbSubscription.cpp
+++ b/src/modules/controllib/uorb/UOrbSubscription.cpp
diff --git a/src/modules/controllib/block/UOrbSubscription.hpp b/src/modules/controllib/uorb/UOrbSubscription.hpp
index 22cc2e114..d337d89a8 100644
--- a/src/modules/controllib/block/UOrbSubscription.hpp
+++ b/src/modules/controllib/uorb/UOrbSubscription.hpp
@@ -39,8 +39,8 @@
 #pragma once
 
 #include <uORB/uORB.h>
-#include "Block.hpp"
-#include "List.hpp"
+#include "../block/Block.hpp"
+#include "../block/List.hpp"
 
 
 namespace control
diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp
new file mode 100644
index 000000000..448a42a99
--- /dev/null
+++ b/src/modules/controllib/uorb/blocks.cpp
@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file uorb_blocks.cpp
+ *
+ * uorb block library code
+ */
+
+#include "blocks.hpp"
+
+namespace control
+{
+
+BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_xtYawLimit(this, "XT2YAW"),
+	_xt2Yaw(this, "XT2YAW"),
+	_psiCmd(0)
+{
+}
+
+BlockWaypointGuidance::~BlockWaypointGuidance() {};
+
+void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
+				   vehicle_attitude_s &att,
+				   vehicle_global_position_setpoint_s &posCmd,
+				   vehicle_global_position_setpoint_s &lastPosCmd)
+{
+
+	// heading to waypoint
+	float psiTrack = get_bearing_to_next_waypoint(
+				 (double)pos.lat / (double)1e7d,
+				 (double)pos.lon / (double)1e7d,
+				 (double)posCmd.lat / (double)1e7d,
+				 (double)posCmd.lon / (double)1e7d);
+
+	// cross track
+	struct crosstrack_error_s xtrackError;
+	get_distance_to_line(&xtrackError,
+			     (double)pos.lat / (double)1e7d,
+			     (double)pos.lon / (double)1e7d,
+			     (double)lastPosCmd.lat / (double)1e7d,
+			     (double)lastPosCmd.lon / (double)1e7d,
+			     (double)posCmd.lat / (double)1e7d,
+			     (double)posCmd.lon / (double)1e7d);
+
+	_psiCmd = _wrap_2pi(psiTrack -
+			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
+}
+
+BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	// subscriptions
+	_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
+	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
+	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
+	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
+	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
+	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
+	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
+	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
+	// publications
+	_actuators(&getPublications(), ORB_ID(actuator_controls_0))
+{
+}
+
+BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
+
+} // namespace control
+
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
new file mode 100644
index 000000000..9c0720aa5
--- /dev/null
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file uorb_blocks.h
+ *
+ * uorb block library code
+ */
+
+#pragma once
+
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_global_position_set_triplet.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <drivers/drv_hrt.h>
+#include <poll.h>
+
+extern "C" {
+#include <systemlib/geo/geo.h>
+}
+
+#include "../blocks.hpp"
+#include "UOrbSubscription.hpp"
+#include "UOrbPublication.hpp"
+
+namespace control
+{
+
+/**
+ * Waypoint Guidance block
+ */
+class __EXPORT BlockWaypointGuidance : public SuperBlock
+{
+private:
+	BlockLimitSym _xtYawLimit;
+	BlockP _xt2Yaw;
+	float _psiCmd;
+public:
+	BlockWaypointGuidance(SuperBlock *parent, const char *name);
+	virtual ~BlockWaypointGuidance();
+	void update(vehicle_global_position_s &pos,
+		    vehicle_attitude_s &att,
+		    vehicle_global_position_setpoint_s &posCmd,
+		    vehicle_global_position_setpoint_s &lastPosCmd);
+	float getPsiCmd() { return _psiCmd; }
+};
+
+/**
+ * UorbEnabledAutopilot
+ */
+class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
+{
+protected:
+	// subscriptions
+	UOrbSubscription<vehicle_attitude_s> _att;
+	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
+	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
+	UOrbSubscription<vehicle_global_position_s> _pos;
+	UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
+	UOrbSubscription<manual_control_setpoint_s> _manual;
+	UOrbSubscription<vehicle_status_s> _status;
+	UOrbSubscription<parameter_update_s> _param_update;
+	// publications
+	UOrbPublication<actuator_controls_s> _actuators;
+public:
+	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
+	virtual ~BlockUorbEnabledAutopilot();
+};
+
+} // namespace control
+
diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index 0cfcfd51d..16fcbd864 100644
--- a/src/modules/controllib/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -88,61 +88,6 @@ void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
 	_yawDamper.update(rCmd, r, outputScale);
 }
 
-BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	_xtYawLimit(this, "XT2YAW"),
-	_xt2Yaw(this, "XT2YAW"),
-	_psiCmd(0)
-{
-}
-
-BlockWaypointGuidance::~BlockWaypointGuidance() {};
-
-void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
-				   vehicle_attitude_s &att,
-				   vehicle_global_position_setpoint_s &posCmd,
-				   vehicle_global_position_setpoint_s &lastPosCmd)
-{
-
-	// heading to waypoint
-	float psiTrack = get_bearing_to_next_waypoint(
-				 (double)pos.lat / (double)1e7d,
-				 (double)pos.lon / (double)1e7d,
-				 (double)posCmd.lat / (double)1e7d,
-				 (double)posCmd.lon / (double)1e7d);
-
-	// cross track
-	struct crosstrack_error_s xtrackError;
-	get_distance_to_line(&xtrackError,
-			     (double)pos.lat / (double)1e7d,
-			     (double)pos.lon / (double)1e7d,
-			     (double)lastPosCmd.lat / (double)1e7d,
-			     (double)lastPosCmd.lon / (double)1e7d,
-			     (double)posCmd.lat / (double)1e7d,
-			     (double)posCmd.lon / (double)1e7d);
-
-	_psiCmd = _wrap_2pi(psiTrack -
-			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
-}
-
-BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	// subscriptions
-	_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
-	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
-	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
-	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
-	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
-	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
-	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
-	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
-	// publications
-	_actuators(&getPublications(), ORB_ID(actuator_controls_0))
-{
-}
-
-BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
-
 BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
 	BlockUorbEnabledAutopilot(parent, name),
 	_stabilization(this, ""), // no name needed, already unique
@@ -385,4 +330,4 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
 
 } // namespace control
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/modules/controllib/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp
index e4028c40d..3876e4630 100644
--- a/src/modules/controllib/fixedwing.hpp
+++ b/src/modules/fixedwing_backside/fixedwing.hpp
@@ -39,31 +39,8 @@
 
 #pragma once
 
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/parameter_update.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-#include <drivers/drv_hrt.h>
-#include <poll.h>
-
-#include "blocks.hpp"
-#include "block/UOrbSubscription.hpp"
-#include "block/UOrbPublication.hpp"
-
-extern "C" {
-#include <systemlib/geo/geo.h>
-}
+#include <controllib/blocks.hpp>
+#include <controllib/uorb/blocks.hpp>
 
 namespace control
 {
@@ -251,47 +228,6 @@ public:
  */
 
 /**
- * Waypoint Guidance block
- */
-class __EXPORT BlockWaypointGuidance : public SuperBlock
-{
-private:
-	BlockLimitSym _xtYawLimit;
-	BlockP _xt2Yaw;
-	float _psiCmd;
-public:
-	BlockWaypointGuidance(SuperBlock *parent, const char *name);
-	virtual ~BlockWaypointGuidance();
-	void update(vehicle_global_position_s &pos,
-		    vehicle_attitude_s &att,
-		    vehicle_global_position_setpoint_s &posCmd,
-		    vehicle_global_position_setpoint_s &lastPosCmd);
-	float getPsiCmd() { return _psiCmd; }
-};
-
-/**
- * UorbEnabledAutopilot
- */
-class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
-{
-protected:
-	// subscriptions
-	UOrbSubscription<vehicle_attitude_s> _att;
-	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
-	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
-	UOrbSubscription<vehicle_global_position_s> _pos;
-	UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
-	UOrbSubscription<manual_control_setpoint_s> _manual;
-	UOrbSubscription<vehicle_status_s> _status;
-	UOrbSubscription<parameter_update_s> _param_update;
-	// publications
-	UOrbPublication<actuator_controls_s> _actuators;
-public:
-	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
-	virtual ~BlockUorbEnabledAutopilot();
-};
-
-/**
  * Multi-mode Autopilot
  */
 class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
index 677a86771..b0de69f55 100644
--- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
@@ -45,12 +45,13 @@
 #include <stdlib.h>
 #include <string.h>
 #include <systemlib/systemlib.h>
-#include <controllib/fixedwing.hpp>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
 #include <math.h>
 
+#include "fixedwing.hpp"
+
 static bool thread_should_exit = false;     /**< Deamon exit flag */
 static bool thread_running = false;     /**< Deamon status flag */
 static int deamon_task;             /**< Handle of deamon task / thread */
diff --git a/src/modules/fixedwing_backside/module.mk b/src/modules/fixedwing_backside/module.mk
index ec958d7cb..133728781 100644
--- a/src/modules/fixedwing_backside/module.mk
+++ b/src/modules/fixedwing_backside/module.mk
@@ -38,4 +38,5 @@
 MODULE_COMMAND	= fixedwing_backside
 
 SRCS		= fixedwing_backside_main.cpp \
+		  fixedwing.cpp \
 		  params.c
diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp b/src/modules/mathlib/math/filter/LowPassFilter2p.cpp
new file mode 100644
index 000000000..efb17225d
--- /dev/null
+++ b/src/modules/mathlib/math/filter/LowPassFilter2p.cpp
@@ -0,0 +1,77 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/// @file	LowPassFilter.cpp
+/// @brief	A class to implement a second order low pass filter 
+/// Author: Leonard Hall <LeonardTHall@gmail.com>
+
+#include "LowPassFilter2p.hpp"
+#include "math.h"
+
+namespace math
+{
+
+void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
+{
+    _cutoff_freq = cutoff_freq;
+    float fr = sample_freq/_cutoff_freq;
+    float ohm = tanf(M_PI_F/fr);
+    float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
+    _b0 = ohm*ohm/c;
+    _b1 = 2.0f*_b0;
+    _b2 = _b0;
+    _a1 = 2.0f*(ohm*ohm-1.0f)/c;
+    _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
+}
+
+float LowPassFilter2p::apply(float sample)
+{
+    // do the filtering
+    float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
+    if (isnan(delay_element_0) || isinf(delay_element_0)) {
+        // don't allow bad values to propogate via the filter
+        delay_element_0 = sample;
+    }
+    float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
+    
+    _delay_element_2 = _delay_element_1;
+    _delay_element_1 = delay_element_0;
+
+    // return the value.  Should be no need to check limits
+    return output;
+}
+
+} // namespace math
+
diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp
new file mode 100644
index 000000000..208ec98d4
--- /dev/null
+++ b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp
@@ -0,0 +1,78 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/// @file	LowPassFilter.h
+/// @brief	A class to implement a second order low pass filter 
+/// Author: Leonard Hall <LeonardTHall@gmail.com>
+/// Adapted for PX4 by Andrew Tridgell
+
+#pragma once
+
+namespace math
+{
+class __EXPORT LowPassFilter2p
+{
+public:
+    // constructor
+    LowPassFilter2p(float sample_freq, float cutoff_freq) {
+        // set initial parameters
+        set_cutoff_frequency(sample_freq, cutoff_freq);
+        _delay_element_1 = _delay_element_2 = 0;
+    }
+
+    // change parameters
+    void set_cutoff_frequency(float sample_freq, float cutoff_freq);
+
+    // apply - Add a new raw value to the filter 
+    // and retrieve the filtered result
+    float apply(float sample);
+
+    // return the cutoff frequency
+    float get_cutoff_freq(void) const {
+        return _cutoff_freq;
+    }
+
+private:
+    float           _cutoff_freq; 
+    float           _a1;
+    float           _a2;
+    float           _b0;
+    float           _b1;
+    float           _b2;
+    float           _delay_element_1;        // buffered sample -1
+    float           _delay_element_2;        // buffered sample -2
+};
+
+} // namespace math
diff --git a/src/modules/mathlib/math/filter/module.mk b/src/modules/mathlib/math/filter/module.mk
new file mode 100644
index 000000000..fe92c8c70
--- /dev/null
+++ b/src/modules/mathlib/math/filter/module.mk
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# filter library
+#
+SRCS		 = LowPassFilter2p.cpp
+
+#
+# In order to include .config we first have to save off the
+# current makefile name, since app.mk needs it.
+#
+APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
+-include $(TOPDIR)/.config
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 43d96fb06..fbd82a4c6 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -300,6 +300,8 @@ controls_tick() {
 		} else {
 			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
 		}
+	} else {
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
 	}
 }
 
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index ab6e3fec4..b2c0db425 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -125,9 +125,9 @@ dsm_bind(uint16_t cmd, int pulses)
 	case dsm_bind_send_pulses:
 		for (int i = 0; i < pulses; i++) {
 			stm32_gpiowrite(usart1RxAsOutp, false);
-			up_udelay(50);
+			up_udelay(25);
 			stm32_gpiowrite(usart1RxAsOutp, true);
-			up_udelay(50);
+			up_udelay(25);
 		}
 		break;
 	case dsm_bind_reinit_uart:
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ab3983019..fefa539f9 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -606,6 +606,15 @@ int sdlog2_thread_main(int argc, char *argv[])
 		errx(1, "unable to create logging folder, exiting.");
 	}
 
+	const char *converter_in = "/etc/logging/conv.zip";
+	char* converter_out = malloc(150);
+	sprintf(converter_out, "%s/conv.zip", folder_path);
+
+	if (file_copy(converter_in, converter_out)) {
+		errx(1, "unable to copy conversion scripts, exiting.");
+	}
+	free(converter_out);
+
 	/* only print logging path, important to find log file later */
 	warnx("logging to directory: %s", folder_path);
 
@@ -1318,7 +1327,7 @@ int file_copy(const char *file_old, const char *file_new)
 	fclose(source);
 	fclose(target);
 
-	return ret;
+	return OK;
 }
 
 void handle_command(struct vehicle_command_s *cmd)
diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp
new file mode 100644
index 000000000..b1dc39445
--- /dev/null
+++ b/src/modules/segway/BlockSegwayController.cpp
@@ -0,0 +1,57 @@
+#include "BlockSegwayController.hpp"
+
+void BlockSegwayController::update() {
+	// wait for a sensor update, check for exit condition every 100 ms
+	if (poll(&_attPoll, 1, 100) < 0) return; // poll error
+
+	uint64_t newTimeStamp = hrt_absolute_time();
+	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
+	_timeStamp = newTimeStamp;
+
+	// check for sane values of dt
+	// to prevent large control responses
+	if (dt > 1.0f || dt < 0) return;
+
+	// set dt for all child blocks
+	setDt(dt);
+
+	// check for new updates
+	if (_param_update.updated()) updateParams();
+
+	// get new information from subscriptions
+	updateSubscriptions();
+
+	// default all output to zero unless handled by mode
+	for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++)
+		_actuators.control[i] = 0.0f;
+
+	// only update guidance in auto mode
+	if (_status.state_machine == SYSTEM_STATE_AUTO) {
+		// update guidance
+	}
+
+	// compute speed command
+	float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);
+
+	// handle autopilot modes
+	if (_status.state_machine == SYSTEM_STATE_AUTO ||
+	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
+		_actuators.control[0] = spdCmd;
+		_actuators.control[1] = spdCmd;
+
+	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
+		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+			_actuators.control[CH_LEFT] = _manual.throttle;
+			_actuators.control[CH_RIGHT] = _manual.pitch;
+
+		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+			_actuators.control[0] = spdCmd;
+			_actuators.control[1] = spdCmd;
+		}
+	}
+
+	// update all publications
+	updatePublications();
+
+}
+
diff --git a/src/modules/segway/BlockSegwayController.hpp b/src/modules/segway/BlockSegwayController.hpp
new file mode 100644
index 000000000..4a01f785c
--- /dev/null
+++ b/src/modules/segway/BlockSegwayController.hpp
@@ -0,0 +1,27 @@
+#pragma once
+
+#include <controllib/uorb/blocks.hpp>
+
+using namespace control;
+
+class BlockSegwayController : public control::BlockUorbEnabledAutopilot {
+public:
+	BlockSegwayController() :
+		BlockUorbEnabledAutopilot(NULL,"SEG"),
+		th2v(this, "TH2V"),
+		q2v(this, "Q2V"),
+		_attPoll(),
+		_timeStamp(0)
+	{
+		_attPoll.fd = _att.getHandle();
+		_attPoll.events = POLLIN;
+	}
+	void update();
+private:
+	enum {CH_LEFT, CH_RIGHT};
+	BlockPI th2v;
+	BlockP q2v;
+	struct pollfd _attPoll;
+	uint64_t _timeStamp;
+};
+
diff --git a/src/modules/segway/module.mk b/src/modules/segway/module.mk
new file mode 100644
index 000000000..d5da85601
--- /dev/null
+++ b/src/modules/segway/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# segway controller
+#
+
+MODULE_COMMAND	= segway
+
+SRCS		= segway_main.cpp \
+		  BlockSegwayController.cpp \
+		  params.c
diff --git a/src/modules/segway/params.c b/src/modules/segway/params.c
new file mode 100644
index 000000000..d72923717
--- /dev/null
+++ b/src/modules/segway/params.c
@@ -0,0 +1,8 @@
+#include <systemlib/param/param.h>
+
+// 16 is max name length
+PARAM_DEFINE_FLOAT(SEG_TH2V_P, 10.0f); // pitch to voltage
+PARAM_DEFINE_FLOAT(SEG_TH2V_I, 0.0f); // pitch integral to voltage
+PARAM_DEFINE_FLOAT(SEG_TH2V_I_MAX, 0.0f); // integral limiter
+PARAM_DEFINE_FLOAT(SEG_Q2V, 1.0f); // pitch rate to voltage
+
diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp
new file mode 100644
index 000000000..061fbf9b9
--- /dev/null
+++ b/src/modules/segway/segway_main.cpp
@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: James Goppert
+ *
+ * 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 segway_main.cpp
+ * @author James Goppert
+ *
+ * Segway controller using control library
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <math.h>
+
+#include "BlockSegwayController.hpp"
+
+static bool thread_should_exit = false;     /**< Deamon exit flag */
+static bool thread_running = false;     /**< Deamon status flag */
+static int deamon_task;             /**< Handle of deamon task / thread */
+
+/**
+ * Deamon management function.
+ */
+extern "C" __EXPORT int segway_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int segway_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr, "usage: segway {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int segway_main(int argc, char *argv[])
+{
+
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+
+		deamon_task = task_spawn_cmd("segway",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 10,
+					 5120,
+					 segway_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int segway_thread_main(int argc, char *argv[])
+{
+
+	warnx("starting");
+
+	using namespace control;
+
+	BlockSegwayController autopilot;
+
+	thread_running = true;
+
+	while (!thread_should_exit) {
+		autopilot.update();
+	}
+
+	warnx("exiting.");
+
+	thread_running = false;
+
+	return 0;
+}
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index d0af9e17b..2bd869263 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -68,7 +68,7 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
-PARAM_DEFINE_INT32(SENS_DPRES_OFF, 1667);
+PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667);
 
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
@@ -156,6 +156,7 @@ PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
 
 PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
+PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */
 
 /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index b38dc8d89..22374a1fe 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -60,6 +60,7 @@
 #include <drivers/drv_baro.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_adc.h>
+#include <drivers/drv_airspeed.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -91,8 +92,35 @@
 #define BARO_HEALTH_COUNTER_LIMIT_OK 5
 #define ADC_HEALTH_COUNTER_LIMIT_OK  5
 
-#define ADC_BATTERY_VOLTAGE_CHANNEL	10
-#define ADC_AIRSPEED_VOLTAGE_CHANNEL	11
+/**
+ * Analog layout:
+ * FMU:
+ * IN2 - battery voltage
+ * IN3 - battery current
+ * IN4 - 5V sense
+ * IN10 - spare (we could actually trim these from the set)
+ * IN11 - spare (we could actually trim these from the set)
+ * IN12 - spare (we could actually trim these from the set)
+ * IN13 - aux1
+ * IN14 - aux2
+ * IN15 - pressure sensor
+ * 
+ * IO:
+ * IN4 - servo supply rail
+ * IN5 - analog RSSI
+ */
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+  #define ADC_BATTERY_VOLTAGE_CHANNEL	10
+  #define ADC_AIRSPEED_VOLTAGE_CHANNEL	11
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+  #define ADC_BATTERY_VOLTAGE_CHANNEL	2
+  #define ADC_BATTERY_CURRENT_CHANNEL	3
+  #define ADC_5V_RAIL_SENSE		4
+  #define ADC_AIRSPEED_VOLTAGE_CHANNEL	15
+#endif
 
 #define BAT_VOL_INITIAL 0.f
 #define BAT_VOL_LOWPASS_1 0.99f
@@ -197,7 +225,7 @@ private:
 		float mag_scale[3];
 		float accel_offset[3];
 		float accel_scale[3];
-		int diff_pres_offset_pa;
+		float diff_pres_offset_pa;
 
 		int rc_type;
 
@@ -228,7 +256,6 @@ private:
 
 		float battery_voltage_scaling;
 
-		int   rc_rl1_DSM_VCC_control;
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -277,7 +304,6 @@ private:
 
 		param_t battery_voltage_scaling;
 
-		param_t rc_rl1_DSM_VCC_control;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -389,7 +415,7 @@ namespace sensors
 #endif
 static const int ERROR = -1;
 
-Sensors	*g_sensors;
+Sensors	*g_sensors = nullptr;
 }
 
 Sensors::Sensors() :
@@ -512,9 +538,6 @@ Sensors::Sensors() :
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
-	/* DSM VCC relay control */
-	_parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC");
-
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -552,25 +575,11 @@ Sensors::parameters_update()
 	/* rc values */
 	for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
 
-		if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) {
-			warnx("Failed getting min for chan %d", i);
-		}
-
-		if (param_get(_parameter_handles.trim[i], &(_parameters.trim[i])) != OK) {
-			warnx("Failed getting trim for chan %d", i);
-		}
-
-		if (param_get(_parameter_handles.max[i], &(_parameters.max[i])) != OK) {
-			warnx("Failed getting max for chan %d", i);
-		}
-
-		if (param_get(_parameter_handles.rev[i], &(_parameters.rev[i])) != OK) {
-			warnx("Failed getting rev for chan %d", i);
-		}
-
-		if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) {
-			warnx("Failed getting dead zone for chan %d", i);
-		}
+		param_get(_parameter_handles.min[i], &(_parameters.min[i]));
+		param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
+		param_get(_parameter_handles.max[i], &(_parameters.max[i]));
+		param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
+		param_get(_parameter_handles.dz[i], &(_parameters.dz[i]));
 
 		_parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
 
@@ -656,21 +665,10 @@ Sensors::parameters_update()
 		warnx("Failed getting mode aux 5 index");
 	}
 
-	if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) {
-		warnx("Failed getting rc scaling for roll");
-	}
-
-	if (param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)) != OK) {
-		warnx("Failed getting rc scaling for pitch");
-	}
-
-	if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) {
-		warnx("Failed getting rc scaling for yaw");
-	}
-
-	if (param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)) != OK) {
-		warnx("Failed getting rc scaling for flaps");
-	}
+	param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll));
+	param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
+	param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
+	param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
 
 	/* update RC function mappings */
 	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -726,11 +724,6 @@ Sensors::parameters_update()
 		warnx("Failed updating voltage scaling param");
 	}
 
-	/* relay 1 DSM VCC control */
-	if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) {
-		warnx("Failed updating relay 1 DSM VCC control");
-	}
-
 	return OK;
 }
 
@@ -746,11 +739,26 @@ Sensors::accel_init()
 		errx(1, "FATAL: no accelerometer found");
 
 	} else {
-		/* set the accel internal sampling rate up to at leat 500Hz */
-		ioctl(fd, ACCELIOCSSAMPLERATE, 500);
 
-		/* set the driver to poll at 500Hz */
-		ioctl(fd, SENSORIOCSPOLLRATE, 500);
+		// XXX do the check more elegantly
+
+		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+		/* set the accel internal sampling rate up to at leat 1000Hz */
+		ioctl(fd, ACCELIOCSSAMPLERATE, 1000);
+
+		/* set the driver to poll at 1000Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 1000);
+
+		#else
+
+		/* set the accel internal sampling rate up to at leat 800Hz */
+		ioctl(fd, ACCELIOCSSAMPLERATE, 800);
+
+		/* set the driver to poll at 800Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 800);
+
+		#endif
 
 		warnx("using system accel");
 		close(fd);
@@ -769,11 +777,28 @@ Sensors::gyro_init()
 		errx(1, "FATAL: no gyro found");
 
 	} else {
-		/* set the gyro internal sampling rate up to at leat 500Hz */
-		ioctl(fd, GYROIOCSSAMPLERATE, 500);
 
-		/* set the driver to poll at 500Hz */
-		ioctl(fd, SENSORIOCSPOLLRATE, 500);
+		// XXX do the check more elegantly
+
+		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+		/* set the gyro internal sampling rate up to at least 1000Hz */
+		if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK)
+			ioctl(fd, GYROIOCSSAMPLERATE, 800);
+
+		/* set the driver to poll at 1000Hz */
+		if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK)
+			ioctl(fd, SENSORIOCSPOLLRATE, 800);
+
+		#else
+
+		/* set the gyro internal sampling rate up to at leat 800Hz */
+		ioctl(fd, GYROIOCSSAMPLERATE, 800);
+
+		/* set the driver to poll at 800Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 800);
+
+		#endif
 
 		warnx("using system gyro");
 		close(fd);
@@ -1034,6 +1059,20 @@ Sensors::parameter_update_poll(bool forced)
 
 		close(fd);
 
+		fd = open(AIRSPEED_DEVICE_PATH, 0);
+
+		/* this sensor is optional, abort without error */
+
+		if (fd > 0) {
+			struct airspeed_scale airscale = {
+				_parameters.diff_pres_offset_pa,
+				1.0f,
+			};
+
+			if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
+				warn("WARNING: failed to set scale / offsets for airspeed sensor");
+		}
+
 #if 0
 		printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor * 10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]);
 		printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor * 10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]);
@@ -1364,6 +1403,9 @@ Sensors::task_main()
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vcontrol_mode_sub, 200);
 
+	/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
+	orb_set_interval(_gyro_sub, 4);
+
 	/*
 	 * do advertisements
 	 */
@@ -1399,7 +1441,7 @@ Sensors::task_main()
 
 	while (!_task_should_exit) {
 
-		/* wait for up to 500ms for data */
+		/* wait for up to 100ms for data */
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit, etc. */
diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c
index 8d77f14a8..8e9c2bfcf 100644
--- a/src/modules/systemlib/hx_stream.c
+++ b/src/modules/systemlib/hx_stream.c
@@ -53,14 +53,26 @@
 
 
 struct hx_stream {
-	uint8_t			buf[HX_STREAM_MAX_FRAME + 4];
-	unsigned		frame_bytes;
-	bool			escaped;
-	bool			txerror;
-
+	/* RX state */
+	uint8_t			rx_buf[HX_STREAM_MAX_FRAME + 4];
+	unsigned		rx_frame_bytes;
+	bool			rx_escaped;
+	hx_stream_rx_callback	rx_callback;
+	void			*rx_callback_arg;
+
+	/* TX state */
 	int			fd;
-	hx_stream_rx_callback	callback;
-	void			*callback_arg;
+	bool			tx_error;
+	uint8_t			*tx_buf;
+	unsigned		tx_resid;
+	uint32_t		tx_crc;
+	enum {
+		TX_IDLE = 0,
+		TX_SEND_START,
+		TX_SEND_DATA,
+		TX_SENT_ESCAPE,
+		TX_SEND_END
+	}			tx_state;
 
 	perf_counter_t		pc_tx_frames;
 	perf_counter_t		pc_rx_frames;
@@ -81,21 +93,7 @@ static void
 hx_tx_raw(hx_stream_t stream, uint8_t c)
 {
 	if (write(stream->fd, &c, 1) != 1)
-		stream->txerror = true;
-}
-
-static void
-hx_tx_byte(hx_stream_t stream, uint8_t c)
-{
-	switch (c) {
-	case FBO:
-	case CEO:
-		hx_tx_raw(stream, CEO);
-		c ^= 0x20;
-		break;
-	}
-
-	hx_tx_raw(stream, c);
+		stream->tx_error = true;
 }
 
 static int
@@ -105,11 +103,11 @@ hx_rx_frame(hx_stream_t stream)
 		uint8_t	b[4];
 		uint32_t w;
 	} u;
-	unsigned length = stream->frame_bytes;
+	unsigned length = stream->rx_frame_bytes;
 
 	/* reset the stream */
-	stream->frame_bytes = 0;
-	stream->escaped = false;
+	stream->rx_frame_bytes = 0;
+	stream->rx_escaped = false;
 
 	/* not a real frame - too short */
 	if (length < 4) {
@@ -122,11 +120,11 @@ hx_rx_frame(hx_stream_t stream)
 	length -= 4;
 
 	/* compute expected CRC */
-	u.w = crc32(&stream->buf[0], length);
+	u.w = crc32(&stream->rx_buf[0], length);
 
 	/* compare computed and actual CRC */
 	for (unsigned i = 0; i < 4; i++) {
-		if (u.b[i] != stream->buf[length + i]) {
+		if (u.b[i] != stream->rx_buf[length + i]) {
 			perf_count(stream->pc_rx_errors);
 			return 0;
 		}
@@ -134,7 +132,7 @@ hx_rx_frame(hx_stream_t stream)
 
 	/* frame is good */
 	perf_count(stream->pc_rx_frames);
-	stream->callback(stream->callback_arg, &stream->buf[0], length);
+	stream->rx_callback(stream->rx_callback_arg, &stream->rx_buf[0], length);
 	return 1;
 }
 
@@ -150,8 +148,8 @@ hx_stream_init(int fd,
 	if (stream != NULL) {
 		memset(stream, 0, sizeof(struct hx_stream));
 		stream->fd = fd;
-		stream->callback = callback;
-		stream->callback_arg = arg;
+		stream->rx_callback = callback;
+		stream->rx_callback_arg = arg;
 	}
 
 	return stream;
@@ -179,49 +177,112 @@ hx_stream_set_counters(hx_stream_t stream,
 	stream->pc_rx_errors = rx_errors;
 }
 
+void
+hx_stream_reset(hx_stream_t stream)
+{
+	stream->rx_frame_bytes = 0;
+	stream->rx_escaped = false;
+
+	stream->tx_buf = NULL;
+	stream->tx_resid = 0;
+	stream->tx_state = TX_IDLE;
+}
+
 int
-hx_stream_send(hx_stream_t stream,
+hx_stream_start(hx_stream_t stream,
 	       const void *data,
 	       size_t count)
 {
-	union {
-		uint8_t	b[4];
-		uint32_t w;
-	} u;
-	const uint8_t *p = (const uint8_t *)data;
-	unsigned resid = count;
-
-	if (resid > HX_STREAM_MAX_FRAME)
+	if (count > HX_STREAM_MAX_FRAME)
 		return -EINVAL;
 
-	/* start the frame */
-	hx_tx_raw(stream, FBO);
+	stream->tx_buf = data;
+	stream->tx_resid = count;
+	stream->tx_state = TX_SEND_START;
+	stream->tx_crc = crc32(data, count);
+	return OK;
+}
+
+int
+hx_stream_send_next(hx_stream_t stream)
+{
+	int c;
+
+	/* sort out what we're going to send */
+	switch (stream->tx_state) {
 
-	/* transmit the data */
-	while (resid--)
-		hx_tx_byte(stream, *p++);
+	case TX_SEND_START:
+		stream->tx_state = TX_SEND_DATA;
+		return FBO;
 
-	/* compute the CRC */
-	u.w = crc32(data, count);
+	case TX_SEND_DATA:
+		c = *stream->tx_buf;
 
-	/* send the CRC */
-	p = &u.b[0];
-	resid = 4;
+		switch (c) {
+		case FBO:
+		case CEO:
+			stream->tx_state = TX_SENT_ESCAPE;
+			return CEO;
+		}
+		break;
+
+	case TX_SENT_ESCAPE:
+		c = *stream->tx_buf ^ 0x20;
+		stream->tx_state = TX_SEND_DATA;
+		break;
+
+	case TX_SEND_END:
+		stream->tx_state = TX_IDLE;
+		return FBO;
+
+	case TX_IDLE:
+	default:
+		return -1;
+	}
+
+	/* if we are here, we have consumed a byte from the buffer */
+	stream->tx_resid--;
+	stream->tx_buf++;
+
+	/* buffer exhausted */
+	if (stream->tx_resid == 0) {
+		uint8_t *pcrc = (uint8_t *)&stream->tx_crc;
+
+		/* was the buffer the frame CRC? */
+		if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) {
+			stream->tx_state = TX_SEND_END;
+		} else {
+			/* no, it was the payload - switch to sending the CRC */
+			stream->tx_buf = pcrc;
+			stream->tx_resid = sizeof(stream->tx_crc);
+		}
+	}
+	return c;
+}
+
+int
+hx_stream_send(hx_stream_t stream,
+	       const void *data,
+	       size_t count)
+{
+	int result;
 
-	while (resid--)
-		hx_tx_byte(stream, *p++);
+	result = hx_stream_start(stream, data, count);
+	if (result != OK)
+		return result;
 
-	/* and the trailing frame separator */
-	hx_tx_raw(stream, FBO);
+	int c;
+	while ((c = hx_stream_send_next(stream)) >= 0)
+		hx_tx_raw(stream, c);
 
 	/* check for transmit error */
-	if (stream->txerror) {
-		stream->txerror = false;
+	if (stream->tx_error) {
+		stream->tx_error = false;
 		return -EIO;
 	}
 
 	perf_count(stream->pc_tx_frames);
-	return 0;
+	return OK;
 }
 
 void
@@ -234,17 +295,17 @@ hx_stream_rx(hx_stream_t stream, uint8_t c)
 	}
 
 	/* escaped? */
-	if (stream->escaped) {
-		stream->escaped = false;
+	if (stream->rx_escaped) {
+		stream->rx_escaped = false;
 		c ^= 0x20;
 
 	} else if (c == CEO) {
-		/* now escaped, ignore the byte */
-		stream->escaped = true;
+		/* now rx_escaped, ignore the byte */
+		stream->rx_escaped = true;
 		return;
 	}
 
 	/* save for later */
-	if (stream->frame_bytes < sizeof(stream->buf))
-		stream->buf[stream->frame_bytes++] = c;
+	if (stream->rx_frame_bytes < sizeof(stream->rx_buf))
+		stream->rx_buf[stream->rx_frame_bytes++] = c;
 }
diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h
index 128689953..1f3927222 100644
--- a/src/modules/systemlib/hx_stream.h
+++ b/src/modules/systemlib/hx_stream.h
@@ -58,7 +58,8 @@ __BEGIN_DECLS
  * Allocate a new hx_stream object.
  *
  * @param fd		The file handle over which the protocol will
- *			communicate.
+ *			communicate, or -1 if the protocol will use
+ *			hx_stream_start/hx_stream_send_next.
  * @param callback	Called when a frame is received.
  * @param callback_arg	Passed to the callback.
  * @return		A handle to the stream, or NULL if memory could
@@ -80,6 +81,7 @@ __EXPORT extern void		hx_stream_free(hx_stream_t stream);
  *
  * Any counter may be set to NULL to disable counting that datum.
  *
+ * @param stream	A handle returned from hx_stream_init.
  * @param tx_frames	Counter for transmitted frames.
  * @param rx_frames	Counter for received frames.
  * @param rx_errors	Counter for short and corrupt received frames.
@@ -90,6 +92,44 @@ __EXPORT extern void		hx_stream_set_counters(hx_stream_t stream,
 		perf_counter_t rx_errors);
 
 /**
+ * Reset a stream.
+ *
+ * Forces the local stream state to idle.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ */
+__EXPORT extern void		hx_stream_reset(hx_stream_t stream);
+
+/**
+ * Prepare to send a frame.
+ *
+ * Use this in conjunction with hx_stream_send_next to 
+ * set the frame to be transmitted.
+ *
+ * Use hx_stream_send() to write to the stream fd directly.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @param data		Pointer to the data to send.
+ * @param count		The number of bytes to send.
+ * @return		Zero on success, -errno on error.
+ */
+__EXPORT extern int		hx_stream_start(hx_stream_t stream,
+		const void *data,
+		size_t count);
+
+/**
+ * Get the next byte to send for a stream.
+ *
+ * This requires that the stream be prepared for sending by
+ * calling hx_stream_start first.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @return		The byte to send, or -1 if there is 
+ *			nothing left to send.
+ */
+__EXPORT extern int		hx_stream_send_next(hx_stream_t stream);
+
+/**
  * Send a frame.
  *
  * This function will block until all frame bytes are sent if
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index 879f4715a..3c1e10287 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -201,23 +201,50 @@ perf_end(perf_counter_t handle)
 	switch (handle->type) {
 	case PC_ELAPSED: {
 			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
-			hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
 
-			pce->event_count++;
-			pce->time_total += elapsed;
+			if (pce->time_start != 0) {
+				hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
 
-			if ((pce->time_least > elapsed) || (pce->time_least == 0))
-				pce->time_least = elapsed;
+				pce->event_count++;
+				pce->time_total += elapsed;
 
-			if (pce->time_most < elapsed)
-				pce->time_most = elapsed;
+				if ((pce->time_least > elapsed) || (pce->time_least == 0))
+					pce->time_least = elapsed;
+
+				if (pce->time_most < elapsed)
+					pce->time_most = elapsed;
+
+				pce->time_start = 0;
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+void
+perf_cancel(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+			pce->time_start = 0;
 		}
+		break;
 
 	default:
 		break;
 	}
 }
 
+
+
 void
 perf_reset(perf_counter_t handle)
 {
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 5c2cb15b2..4cd8b67a1 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -92,13 +92,25 @@ __EXPORT extern void		perf_begin(perf_counter_t handle);
  * End a performance event.
  *
  * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * has been called subsequently, no change is made to the counter.
  *
  * @param handle		The handle returned from perf_alloc.
  */
 __EXPORT extern void		perf_end(perf_counter_t handle);
 
 /**
- * Reset a performance event.
+ * Cancel a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * It reverts the effect of a previous perf_begin.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_cancel(perf_counter_t handle);
+
+/**
+ * Reset a performance counter.
  *
  * This call resets performance counter to initial state
  *
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
new file mode 100644
index 000000000..2dad2261b
--- /dev/null
+++ b/src/systemcmds/config/config.c
@@ -0,0 +1,200 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file config.c
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * config tool.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/stat.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_mag.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+
+__EXPORT int config_main(int argc, char *argv[]);
+
+static void	do_gyro(int argc, char *argv[]);
+static void	do_accel(int argc, char *argv[]);
+static void	do_mag(int argc, char *argv[]);
+
+int
+config_main(int argc, char *argv[])
+{
+	if (argc >= 2) {
+		if (!strcmp(argv[1], "gyro")) {
+			if (argc >= 3) {
+				do_gyro(argc - 2, argv + 2);
+			} else {
+				errx(1, "not enough parameters.");
+			}
+		}
+
+		if (!strcmp(argv[1], "accel")) {
+			if (argc >= 3) {
+				do_accel(argc - 2, argv + 2);
+			} else {
+				errx(1, "not enough parameters.");
+			}
+		}
+
+		if (!strcmp(argv[1], "mag")) {
+			if (argc >= 3) {
+				do_mag(argc - 2, argv + 2);
+			} else {
+				errx(1, "not enough parameters.");
+			}
+		}
+	}
+	
+	errx(1, "expected a command, try 'gyro', 'accel', 'mag'");
+}
+
+static void
+do_gyro(int argc, char *argv[])
+{
+	int	fd;
+
+	fd = open(GYRO_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		warn("%s", GYRO_DEVICE_PATH);
+		errx(1, "FATAL: no gyro found");
+
+	} else {
+
+		if (argc >= 2) {
+
+			char* end;
+			int i = strtol(argv[1],&end,10);
+
+			if (!strcmp(argv[0], "sampling")) {
+
+				/* set the accel internal sampling rate up to at leat i Hz */
+				ioctl(fd, GYROIOCSSAMPLERATE, i);
+
+			} else if (!strcmp(argv[0], "rate")) {
+
+				/* set the driver to poll at i Hz */
+				ioctl(fd, SENSORIOCSPOLLRATE, i);
+			} else if (!strcmp(argv[0], "range")) {
+
+				/* set the range to i dps */
+				ioctl(fd, GYROIOCSRANGE, i);
+			}
+
+		} else if (!(argc > 0 && !strcmp(argv[0], "info"))) {
+			warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
+		}
+
+		int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
+		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
+		int range = ioctl(fd, GYROIOCGRANGE, 0);
+
+		warnx("gyro: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", srate, prate, range);
+
+		close(fd);
+	}
+
+	exit(0);
+}
+
+static void
+do_mag(int argc, char *argv[])
+{
+	exit(0);
+}
+
+static void
+do_accel(int argc, char *argv[])
+{
+	int	fd;
+
+	fd = open(ACCEL_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		warn("%s", ACCEL_DEVICE_PATH);
+		errx(1, "FATAL: no accelerometer found");
+
+	} else {
+
+		if (argc >= 2) {
+
+			char* end;
+			int i = strtol(argv[1],&end,10);
+
+			if (!strcmp(argv[0], "sampling")) {
+
+				/* set the accel internal sampling rate up to at leat i Hz */
+				ioctl(fd, ACCELIOCSSAMPLERATE, i);
+
+			} else if (!strcmp(argv[0], "rate")) {
+
+				/* set the driver to poll at i Hz */
+				ioctl(fd, SENSORIOCSPOLLRATE, i);
+			} else if (!strcmp(argv[0], "range")) {
+
+				/* set the range to i dps */
+				ioctl(fd, ACCELIOCSRANGE, i);
+			}
+		} else if (!(argc > 0 && !strcmp(argv[0], "info"))) {
+			warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 2 G\n\t");
+		}
+
+		int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
+		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
+		int range = ioctl(fd, ACCELIOCGRANGE, 0);
+
+		warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d m/s", srate, prate, range);
+
+		close(fd);
+	}
+
+	exit(0);
+}
diff --git a/src/systemcmds/config/module.mk b/src/systemcmds/config/module.mk
new file mode 100644
index 000000000..0a75810b0
--- /dev/null
+++ b/src/systemcmds/config/module.mk
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build the config tool.
+#
+
+MODULE_COMMAND	 = config
+SRCS		 = config.c
+
+MODULE_STACKSIZE = 4096
+
+MAXOPTIMIZATION	 = -Os
+
diff --git a/src/systemcmds/ramtron/module.mk b/src/systemcmds/ramtron/module.mk
new file mode 100644
index 000000000..e4eb1d143
--- /dev/null
+++ b/src/systemcmds/ramtron/module.mk
@@ -0,0 +1,6 @@
+#
+# RAMTRON file system driver
+#
+
+MODULE_COMMAND	 = ramtron
+SRCS		 = ramtron.c
diff --git a/src/systemcmds/ramtron/ramtron.c b/src/systemcmds/ramtron/ramtron.c
new file mode 100644
index 000000000..03c713987
--- /dev/null
+++ b/src/systemcmds/ramtron/ramtron.c
@@ -0,0 +1,279 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ramtron.c
+ *
+ * ramtron service and utility app.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/spi.h>
+#include <nuttx/mtd.h>
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/param/param.h"
+#include "systemlib/err.h"
+
+__EXPORT int ramtron_main(int argc, char *argv[]);
+
+#ifndef CONFIG_MTD_RAMTRON
+
+/* create a fake command with decent message to not confuse users */
+int ramtron_main(int argc, char *argv[])
+{
+	errx(1, "RAMTRON not enabled, skipping.");
+}
+#else
+
+static void	ramtron_attach(void);
+static void	ramtron_start(void);
+static void	ramtron_erase(void);
+static void	ramtron_ioctl(unsigned operation);
+static void	ramtron_save(const char *name);
+static void	ramtron_load(const char *name);
+static void	ramtron_test(void);
+
+static bool attached = false;
+static bool started = false;
+static struct mtd_dev_s *ramtron_mtd;
+
+int ramtron_main(int argc, char *argv[])
+{
+	if (argc >= 2) {
+		if (!strcmp(argv[1], "start"))
+			ramtron_start();
+
+		if (!strcmp(argv[1], "save_param"))
+			ramtron_save(argv[2]);
+
+		if (!strcmp(argv[1], "load_param"))
+			ramtron_load(argv[2]);
+
+		if (!strcmp(argv[1], "erase"))
+			ramtron_erase();
+
+		if (!strcmp(argv[1], "test"))
+			ramtron_test();
+
+		if (0) {	/* these actually require a file on the filesystem... */
+
+			if (!strcmp(argv[1], "reformat"))
+				ramtron_ioctl(FIOC_REFORMAT);
+
+			if (!strcmp(argv[1], "repack"))
+				ramtron_ioctl(FIOC_OPTIMIZE);
+		}
+	}
+
+	errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n");
+}
+
+struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
+
+
+static void
+ramtron_attach(void)
+{
+	/* find the right spi */
+	struct spi_dev_s *spi = up_spiinitialize(2);
+	/* this resets the spi bus, set correct bus speed again */
+    // xxx set in ramtron driver, leave this out
+//	SPI_SETFREQUENCY(spi, 4000000);
+	SPI_SETFREQUENCY(spi, 375000000);
+	SPI_SETBITS(spi, 8);
+	SPI_SETMODE(spi, SPIDEV_MODE3);
+	SPI_SELECT(spi, SPIDEV_FLASH, false);
+
+	if (spi == NULL)
+		errx(1, "failed to locate spi bus");
+
+	/* start the MTD driver, attempt 5 times */
+	for (int i = 0; i < 5; i++) {
+		ramtron_mtd = ramtron_initialize(spi);
+		if (ramtron_mtd) {
+			/* abort on first valid result */
+			if (i > 0) {
+				warnx("warning: ramtron needed %d attempts to attach", i+1);
+			}
+			break;
+		}
+	}
+
+	/* if last attempt is still unsuccessful, abort */
+	if (ramtron_mtd == NULL)
+		errx(1, "failed to initialize ramtron driver");
+
+	attached = true;
+}
+
+static void
+ramtron_start(void)
+{
+	int ret;
+
+	if (started)
+		errx(1, "ramtron already mounted");
+
+	if (!attached)
+		ramtron_attach();
+
+	/* start NXFFS */
+	ret = nxffs_initialize(ramtron_mtd);
+
+	if (ret < 0)
+		errx(1, "failed to initialize NXFFS - erase ramtron to reformat");
+
+	/* mount the ramtron */
+	ret = mount(NULL, "/ramtron", "nxffs", 0, NULL);
+
+	if (ret < 0)
+		errx(1, "failed to mount /ramtron - erase ramtron to reformat");
+
+	started = true;
+	warnx("mounted ramtron at /ramtron");
+	exit(0);
+}
+
+//extern int at24c_nuke(void);
+
+static void
+ramtron_erase(void)
+{
+	if (!attached)
+		ramtron_attach();
+
+//	if (at24c_nuke())
+		errx(1, "erase failed");
+
+	errx(0, "erase done, reboot now");
+}
+
+static void
+ramtron_ioctl(unsigned operation)
+{
+	int fd;
+
+	fd = open("/ramtron/.", 0);
+
+	if (fd < 0)
+		err(1, "open /ramtron");
+
+	if (ioctl(fd, operation, 0) < 0)
+		err(1, "ioctl");
+
+	exit(0);
+}
+
+static void
+ramtron_save(const char *name)
+{
+	if (!started)
+		errx(1, "must be started first");
+
+	if (!name)
+		err(1, "missing argument for device name, try '/ramtron/parameters'");
+
+	warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead");
+
+	/* delete the file in case it exists */
+	unlink(name);
+
+	/* create the file */
+	int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
+
+	if (fd < 0)
+		err(1, "opening '%s' failed", name);
+
+	int result = param_export(fd, false);
+	close(fd);
+
+	if (result < 0) {
+		unlink(name);
+		errx(1, "error exporting to '%s'", name);
+	}
+
+	exit(0);
+}
+
+static void
+ramtron_load(const char *name)
+{
+	if (!started)
+		errx(1, "must be started first");
+
+	if (!name)
+		err(1, "missing argument for device name, try '/ramtron/parameters'");
+
+	warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead");
+
+	int fd = open(name, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "open '%s'", name);
+
+	int result = param_load(fd);
+	close(fd);
+
+	if (result < 0)
+		errx(1, "error importing from '%s'", name);
+
+	exit(0);
+}
+
+//extern void at24c_test(void);
+
+static void
+ramtron_test(void)
+{
+//	at24c_test();
+	exit(0);
+}
+
+#endif
diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c
index f21dd115b..f6e540401 100644
--- a/src/systemcmds/tests/test_hrt.c
+++ b/src/systemcmds/tests/test_hrt.c
@@ -94,7 +94,7 @@ extern uint16_t ppm_pulse_history[];
 
 int test_ppm(int argc, char *argv[])
 {
-#ifdef CONFIG_HRT_PPM
+#ifdef HRT_PPM_CHANNEL
 	unsigned i;
 
 	printf("channels: %u\n", ppm_decoded_channels);