Merge branch 'master' of https://github.com/PX4/Firmware into fmuv2_bringup

Fix px4iov2 build issue by selecting the correct NuttX config.

68 files changed, 5468 insertions, 593 deletions

diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c
index f15c74a22..ecd31a073 100644
--- a/src/drivers/ardrone_interface/ardrone_motor_control.c
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.c
@@ -482,10 +482,10 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
 
 	/* Failsafe logic - should never be necessary */
-	motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512;
-	motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512;
-	motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
-	motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
+	motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511;
+	motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511;
+	motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511;
+	motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511;
 
 	/* send motors via UART */
 	ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu/module.mk
index 2cb261d30..66b776917 100644
--- a/src/drivers/boards/px4fmu/module.mk
+++ b/src/drivers/boards/px4fmu/module.mk
@@ -6,4 +6,5 @@ SRCS		 = px4fmu_can.c \
 		   px4fmu_init.c \
 		   px4fmu_pwm_servo.c \
 		   px4fmu_spi.c \
-		   px4fmu_usb.c
+		   px4fmu_usb.c \
+		   px4fmu_led.c
diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c
index 5dd70c3f9..69edc23ab 100644
--- a/src/drivers/boards/px4fmu/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu/px4fmu_init.c
@@ -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
@@ -91,6 +91,19 @@
 #  endif
 #endif
 
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
 /****************************************************************************
  * Protected Functions
  ****************************************************************************/
@@ -114,7 +127,7 @@ __EXPORT void stm32_boardinitialize(void)
 	/* configure SPI interfaces */
 	stm32_spiinitialize();
 
-	/* configure LEDs */
+	/* configure LEDs (empty call to NuttX' ledinit) */
 	up_ledinit();
 }
 
@@ -178,11 +191,11 @@ __EXPORT int nsh_archinitialize(void)
 		       (hrt_callout)stm32_serial_dma_poll,
 		       NULL);
 
-	// initial LED state
-//	drv_led_start();
-	up_ledoff(LED_BLUE);
-	up_ledoff(LED_AMBER);
-	up_ledon(LED_BLUE);
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_AMBER);
+	led_on(LED_BLUE);
+
 
 	/* Configure SPI-based devices */
 
diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu/px4fmu_led.c
index fd1e159aa..34fd194c3 100644
--- a/src/drivers/boards/px4fmu/px4fmu_led.c
+++ b/src/drivers/boards/px4fmu/px4fmu_led.c
@@ -39,19 +39,27 @@
 
 #include <nuttx/config.h>
 
-#include <stdint.h>
 #include <stdbool.h>
-#include <debug.h>
 
-#include <arch/board/board.h>
-
-#include "chip.h"
-#include "up_arch.h"
-#include "up_internal.h"
 #include "stm32_internal.h"
 #include "px4fmu_internal.h"
 
-__EXPORT void up_ledinit()
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
+__EXPORT void led_init()
 {
 	/* Configure LED1-2 GPIOs for output */
 
@@ -59,7 +67,7 @@ __EXPORT void up_ledinit()
 	stm32_configgpio(GPIO_LED2);
 }
 
-__EXPORT void up_ledon(int led)
+__EXPORT void led_on(int led)
 {
 	if (led == 0)
 	{
@@ -73,7 +81,7 @@ __EXPORT void up_ledon(int led)
 	}
 }
 
-__EXPORT void up_ledoff(int led)
+__EXPORT void led_off(int led)
 {
 	if (led == 0)
 	{
diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h
index 7daba31be..b4a9cdd53 100644
--- a/src/drivers/device/i2c.h
+++ b/src/drivers/device/i2c.h
@@ -53,6 +53,15 @@ namespace device __EXPORT
 class __EXPORT I2C : public CDev
 {
 
+public:
+
+    /**
+     * Get the address
+     */
+    uint16_t get_address() {
+      return _address;
+    }
+
 protected:
 	/**
 	 * The number of times a read or write operation will be retried on
diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h
new file mode 100644
index 000000000..bffc35c62
--- /dev/null
+++ b/src/drivers/drv_airspeed.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   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 driver interface.
+ * @author Simon Wilks
+ */
+
+#ifndef _DRV_AIRSPEED_H
+#define _DRV_AIRSPEED_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define AIRSPEED_DEVICE_PATH	"/dev/airspeed"
+
+/*
+ * ioctl() definitions
+ *
+ * Airspeed drivers also implement the generic sensor driver
+ * interfaces from drv_sensor.h
+ */
+
+#define _AIRSPEEDIOCBASE		(0x7700)
+#define __AIRSPEEDIOC(_n)		(_IOC(_AIRSPEEDIOCBASE, _n))
+
+
+#endif /* _DRV_AIRSPEED_H */
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 07831f20c..56af71059 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -109,6 +109,12 @@ ORB_DECLARE(output_pwm);
 /** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
 #define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
 
+/** set the 'ARM ok' bit, which activates the safety switch */
+#define PWM_SERVO_SET_ARM_OK	_IOC(_PWM_SERVO_BASE, 5)
+
+/** clear the 'ARM ok' bit, which deactivates the safety switch */
+#define PWM_SERVO_CLEAR_ARM_OK	_IOC(_PWM_SERVO_BASE, 6)
+
 /** 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
new file mode 100644
index 000000000..e50395e47
--- /dev/null
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -0,0 +1,832 @@
+/****************************************************************************
+ *
+ *   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>
+
+/* Default I2C bus */
+#define PX4_I2C_BUS_DEFAULT		PX4_I2C_BUS_EXPANSION
+
+/* I2C bus address */
+#define I2C_ADDRESS		0x75	/* 7-bit address. 8-bit address is 0xEA */
+
+/* Register address */
+#define READ_CMD		0x07		/* Read the data */
+	 
+/**
+ * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. 
+ */
+#define MIN_ACCURATE_DIFF_PRES_PA 12
+
+/* 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
+{
+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);
+	
+	
+};
+
+/* 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()
+{
+	/* 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
+ETSAirspeed::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	uint8_t cmd = READ_CMD;
+	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
+ETSAirspeed::collect()
+{
+	int	ret = -EIO;
+	
+	/* read from the sensor */
+	uint8_t val[2] = {0, 0};
+	
+	perf_begin(_sample_perf);
+	
+	ret = transfer(nullptr, 0, &val[0], 2);
+	
+	if (ret < 0) {
+		log("error reading from sensor: %d", ret);
+		return ret;
+	}
+	
+	uint16_t diff_pres_pa = val[1] << 8 | val[0];
+
+	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;
+	} else {
+		diff_pres_pa -= _diff_pres_offset;	
+	}
+
+    // XXX we may want to smooth out the readings to remove noise.
+
+	_reports[_next_report].timestamp = hrt_absolute_time();
+	_reports[_next_report].differential_pressure_pa = diff_pres_pa;
+
+	// Track maximum differential pressure measured (so we can work out top speed).
+	if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
+		_reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
+	}
+
+	/* 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
+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? */
+	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)&ETSAirspeed::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)&ETSAirspeed::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.
+ */
+namespace ets_airspeed
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+ETSAirspeed	*g_dev;
+
+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 */
+	g_dev = new ETSAirspeed(i2c_bus);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(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 'ets_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);
+	}
+
+	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 
+ets_airspeed_usage() 
+{
+	fprintf(stderr, "usage: ets_airspeed [options] command\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");
+}
+
+int
+ets_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"))
+		ets_airspeed::start(i2c_bus);
+	
+	 /*
+	  * Stop the driver
+	  */
+	 if (!strcmp(argv[1], "stop"))
+		 ets_airspeed::stop();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		ets_airspeed::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		ets_airspeed::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+		ets_airspeed::info();
+
+	ets_airspeed_usage();
+	exit(0);
+}
diff --git a/src/drivers/ets_airspeed/module.mk b/src/drivers/ets_airspeed/module.mk
new file mode 100644
index 000000000..cb5d3b1ed
--- /dev/null
+++ b/src/drivers/ets_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 Eagle Tree Airspeed V3 driver.
+#
+
+MODULE_COMMAND		= ets_airspeed
+MODULE_STACKSIZE	= 1024
+
+SRCS			= ets_airspeed.cpp
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index e35bdb944..38835418b 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -285,6 +285,10 @@ GPS::task_main()
 		unlock();
 		if (_Helper->configure(_baudrate) == 0) {
 			unlock();
+
+			// GPS is obviously detected successfully, reset statistics
+			_Helper->reset_update_rates();
+
 			while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
 //				lock();
 				/* opportunistic publishing - else invalid data would end up on the bus */
@@ -301,6 +305,8 @@ GPS::task_main()
 					_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
 					last_rate_measurement = hrt_absolute_time();
 					last_rate_count = 0;
+					_Helper->store_update_rates();
+					_Helper->reset_update_rates();
 				}
 
 				if (!_healthy) {
@@ -372,7 +378,10 @@ GPS::print_info()
 		warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
 			(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
 		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
-		warnx("update rate: %6.2f Hz", (double)_rate);
+		warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
+		warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
+		warnx("rate publication:\t%6.2f Hz", (double)_rate);
+
 	}
 
 	usleep(100000);
diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp
index 9c1fad569..ba86d370a 100644
--- a/src/drivers/gps/gps_helper.cpp
+++ b/src/drivers/gps/gps_helper.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
  *
@@ -36,9 +36,39 @@
 #include <termios.h>
 #include <errno.h>
 #include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
 #include "gps_helper.h"
 
-/* @file gps_helper.cpp */
+/**
+ * @file gps_helper.cpp
+ */
+
+float
+GPS_Helper::get_position_update_rate()
+{
+	return _rate_lat_lon;
+}
+
+float
+GPS_Helper::get_velocity_update_rate()
+{
+	return _rate_vel;
+}
+
+float
+GPS_Helper::reset_update_rates()
+{
+	_rate_count_vel = 0;
+	_rate_count_lat_lon = 0;
+	_interval_rate_start = hrt_absolute_time();
+}
+
+float
+GPS_Helper::store_update_rates()
+{
+	_rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
+	_rate_lat_lon = _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
+}
 
 int
 GPS_Helper::set_baudrate(const int &fd, unsigned baud)
diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
index f3d3bc40b..defc1a074 100644
--- a/src/drivers/gps/gps_helper.h
+++ b/src/drivers/gps/gps_helper.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
  *
@@ -33,7 +33,9 @@
  *
  ****************************************************************************/
 
-/* @file gps_helper.h */
+/** 
+ * @file gps_helper.h
+ */
 
 #ifndef GPS_HELPER_H
 #define GPS_HELPER_H
@@ -44,9 +46,22 @@
 class GPS_Helper
 {
 public:
-	virtual int				configure(unsigned &baud) = 0;
+	virtual int			configure(unsigned &baud) = 0;
 	virtual int 			receive(unsigned timeout) = 0;
-	int 					set_baudrate(const int &fd, unsigned baud);
+	int 				set_baudrate(const int &fd, unsigned baud);
+	float				get_position_update_rate();
+	float				get_velocity_update_rate();
+	float				reset_update_rates();
+	float				store_update_rates();
+
+protected:
+	uint8_t _rate_count_lat_lon;
+	uint8_t _rate_count_vel;
+
+	float _rate_lat_lon;
+	float _rate_vel;
+
+	uint64_t _interval_rate_start;
 };
 
 #endif /* GPS_HELPER_H */
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index 4762bd503..62941d74b 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -263,6 +263,10 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 	_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
 	_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
 
+	// Position and velocity update always at the same time
+	_rate_count_vel++;
+	_rate_count_lat_lon++;
+
 	return;
 }
 
diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h
index d4e390b01..b5cfbf0a6 100644
--- a/src/drivers/gps/mtk.h
+++ b/src/drivers/gps/mtk.h
@@ -87,14 +87,14 @@ class MTK : public GPS_Helper
 public:
 	MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
 	~MTK();
-	int					receive(unsigned timeout);
-	int					configure(unsigned &baudrate);
+	int				receive(unsigned timeout);
+	int				configure(unsigned &baudrate);
 
 private:
 	/**
 	 * Parse the binary MTK packet
 	 */
-	int					parse_char(uint8_t b, gps_mtk_packet_t &packet);
+	int				parse_char(uint8_t b, gps_mtk_packet_t &packet);
 
 	/**
 	 * Handle the package once it has arrived
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index c150f5271..b3093b0f6 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -60,7 +60,8 @@
 UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
 _fd(fd),
 _gps_position(gps_position),
-_waiting_for_ack(false)
+_waiting_for_ack(false),
+_disable_cmd_counter(0)
 {
 	decode_init();
 }
@@ -139,12 +140,12 @@ UBX::configure(unsigned &baudrate)
 		cfg_rate_packet.clsID		= UBX_CLASS_CFG;
 		cfg_rate_packet.msgID		= UBX_MESSAGE_CFG_RATE;
 		cfg_rate_packet.length		= UBX_CFG_RATE_LENGTH;
-		cfg_rate_packet.measRate	= UBX_CFG_RATE_PAYLOAD_MEASRATE;
+		cfg_rate_packet.measRate	= UBX_CFG_RATE_PAYLOAD_MEASINTERVAL;
 		cfg_rate_packet.navRate		= UBX_CFG_RATE_PAYLOAD_NAVRATE;
 		cfg_rate_packet.timeRef		= UBX_CFG_RATE_PAYLOAD_TIMEREF;
 
 		send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+		if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
 			/* try next baudrate */
 			continue;
 		}
@@ -164,74 +165,41 @@ UBX::configure(unsigned &baudrate)
 		cfg_nav5_packet.fixMode      = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
 
 		send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-
-		type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
-		memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet));
-
-		_clsID_needed = UBX_CLASS_CFG;
-		_msgID_needed = UBX_MESSAGE_CFG_MSG;
-
-		cfg_msg_packet.clsID        = UBX_CLASS_CFG;
-		cfg_msg_packet.msgID        = UBX_MESSAGE_CFG_MSG;
-		cfg_msg_packet.length       = UBX_CFG_MSG_LENGTH;
-		/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */
-		cfg_msg_packet.rate[1]      = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ;
-
-		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-
-		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-
-		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
-		/* For satelites info 1Hz is enough */
-		cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-
-		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+		if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
 			/* try next baudrate */
 			continue;
 		}
 
-		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-//		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-//		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
-
-//		cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
-//		cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH,
+			UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
+		/* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
+			1);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
+			1);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
+			1);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+		// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP,
+		// 	0);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
+			0);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
 
 		_waiting_for_ack = false;
 		return 0;
@@ -240,6 +208,15 @@ UBX::configure(unsigned &baudrate)
 }
 
 int
+UBX::wait_for_ack(unsigned timeout)
+{
+	_waiting_for_ack = true;
+	int ret = receive(timeout);
+	_waiting_for_ack = false;
+	return ret;
+}
+
+int
 UBX::receive(unsigned timeout)
 {
 	/* poll descriptor */
@@ -498,6 +475,8 @@ UBX::handle_message()
 				_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
 				_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
 
+				_rate_count_lat_lon++;
+
 				/* Add timestamp to finish the report */
 				_gps_position->timestamp_position = hrt_absolute_time();
 				/* only return 1 when new position is available */
@@ -653,6 +632,8 @@ UBX::handle_message()
 				_gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
 				_gps_position->vel_ned_valid = true;
 				_gps_position->timestamp_velocity = hrt_absolute_time();
+
+				_rate_count_vel++;
 			}
 
 			break;
@@ -693,6 +674,12 @@ UBX::handle_message()
 
 		default: //we don't know the message
 			warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
+			if (_disable_cmd_counter++ == 0) {
+				// Don't attempt for every message to disable, some might not be disabled */
+				warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id);
+				configure_message_rate(_message_class, _message_id, 0);
+			}
+		return ret;
 			ret = -1;
 			break;
 		}
@@ -737,6 +724,25 @@ UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
 }
 
 void
+UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b)
+{
+	for (unsigned i = 0; i < length; i++) {
+		ck_a = ck_a + message[i];
+		ck_b = ck_b + ck_a;
+	}
+}
+
+void
+UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
+{
+    struct ubx_cfg_msg_rate msg;
+    msg.msg_class	= msg_class;
+    msg.msg_id		= msg_id;
+    msg.rate		= rate;
+    send_message(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
+}
+
+void
 UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
 {
 	ssize_t ret = 0;
@@ -753,3 +759,27 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
 	if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
 		warnx("ubx: config write fail");
 }
+
+void
+UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
+{
+	struct ubx_header header;
+	uint8_t ck_a=0, ck_b=0;
+	header.sync1 = UBX_SYNC1;
+	header.sync2 = UBX_SYNC2;
+	header.msg_class = msg_class;
+	header.msg_id    = msg_id;
+	header.length    = size;
+
+	add_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b);
+	add_checksum((uint8_t *)msg, size, ck_a, ck_b);
+
+	// Configure receive check
+	_clsID_needed = msg_class;
+	_msgID_needed = msg_id;
+
+	write(_fd, (const char *)&header, sizeof(header));
+	write(_fd, (const char *)msg, size);
+	write(_fd, (const char *)&ck_a, 1);
+	write(_fd, (const char *)&ck_b, 1);
+}
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index e3dd1c7ea..5a433642c 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -65,26 +65,27 @@
 #define UBX_MESSAGE_CFG_RATE 0x08
 
 #define UBX_CFG_PRT_LENGTH 20
-#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01         /**< UART1 */
-#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0     /**< 0b0000100011010000: 8N1 */
-#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400      /**< always choose 38400 as GPS baudrate */
-#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01    /**< UBX in */
-#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01   /**< UBX out */
+#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01			/**< UART1 */
+#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0		/**< 0b0000100011010000: 8N1 */
+#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400		/**< always choose 38400 as GPS baudrate */
+#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01		/**< UBX in */
+#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01		/**< UBX out */
 
 #define UBX_CFG_RATE_LENGTH 6
-#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200		/**< 200ms for 5Hz */
+#define UBX_CFG_RATE_PAYLOAD_MEASINTERVAL 200		/**< 200ms for 5Hz */
 #define UBX_CFG_RATE_PAYLOAD_NAVRATE 1			/**< cannot be changed */
 #define UBX_CFG_RATE_PAYLOAD_TIMEREF 0			/**< 0: UTC, 1: GPS time */
 
 
 #define UBX_CFG_NAV5_LENGTH 36
-#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001		/**< only update dynamic model and fix mode */
+#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005		/**< XXX only update dynamic model and fix mode */
 #define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7			/**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
-#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2          /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
+#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2			/**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
 
 #define UBX_CFG_MSG_LENGTH 8
 #define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 		/**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
 #define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
+#define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10
 
 #define UBX_MAX_PAYLOAD_LENGTH 500
 
@@ -92,6 +93,14 @@
 /** the structures of the binary packets */
 #pragma pack(push, 1)
 
+struct ubx_header {
+	uint8_t sync1;
+	uint8_t sync2;
+	uint8_t msg_class;
+	uint8_t msg_id;
+	uint16_t length;
+};
+
 typedef struct {
 	uint32_t time_milliseconds;		/**<  GPS Millisecond Time of Week */
 	int32_t lon;					/**<  Longitude * 1e-7, deg */
@@ -274,11 +283,17 @@ typedef struct {
 	uint16_t length;
 	uint8_t msgClass_payload;
 	uint8_t msgID_payload;
-	uint8_t rate[6];
+	uint8_t rate;
 	uint8_t ck_a;
 	uint8_t ck_b;
 } type_gps_bin_cfg_msg_packet_t;
 
+struct ubx_cfg_msg_rate {
+	uint8_t msg_class;
+	uint8_t msg_id;
+	uint8_t rate;
+};
+
 
 // END the structures of the binary packets
 // ************
@@ -341,55 +356,64 @@ class UBX : public GPS_Helper
 public:
 	UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
 	~UBX();
-	int					receive(unsigned timeout);
-	int					configure(unsigned &baudrate);
+	int			receive(unsigned timeout);
+	int			configure(unsigned &baudrate);
 
 private:
 
 	/**
 	 * Parse the binary MTK packet
 	 */
-	int					parse_char(uint8_t b);
+	int			parse_char(uint8_t b);
 
 	/**
 	 * Handle the package once it has arrived
 	 */
-	int					handle_message(void);
+	int			handle_message(void);
 
 	/**
 	 * Reset the parse state machine for a fresh start
 	 */
-	void				decode_init(void);
+	void			decode_init(void);
 
 	/**
 	 * While parsing add every byte (except the sync bytes) to the checksum
 	 */
-	void				add_byte_to_checksum(uint8_t);
+	void			add_byte_to_checksum(uint8_t);
 
 	/**
 	 * Add the two checksum bytes to an outgoing message
 	 */
-	void				add_checksum_to_message(uint8_t* message, const unsigned length);
+	void			add_checksum_to_message(uint8_t* message, const unsigned length);
 
 	/**
 	 * Helper to send a config packet
 	 */
-	void				send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
+	void			send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
+
+	void			configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
+
+	void			send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
+
+	void			add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
+
+	int			wait_for_ack(unsigned timeout);
 
-	int 				_fd;
+	int			_fd;
 	struct vehicle_gps_position_s *_gps_position;
 	ubx_config_state_t	_config_state;
-	bool 				_waiting_for_ack;
-	uint8_t				_clsID_needed;
-	uint8_t				_msgID_needed;
+	bool			_waiting_for_ack;
+	uint8_t			_clsID_needed;
+	uint8_t			_msgID_needed;
 	ubx_decode_state_t	_decode_state;
-	uint8_t				_rx_buffer[RECV_BUFFER_SIZE];
-	unsigned			_rx_count;
-	uint8_t 			_rx_ck_a;
-	uint8_t				_rx_ck_b;
-	ubx_message_class_t _message_class;
+	uint8_t			_rx_buffer[RECV_BUFFER_SIZE];
+	unsigned		_rx_count;
+	uint8_t			_rx_ck_a;
+	uint8_t			_rx_ck_b;
+	ubx_message_class_t	_message_class;
 	ubx_message_id_t	_message_id;
-	unsigned			_payload_size;
+	unsigned		_payload_size;
+	uint8_t			_disable_cmd_counter;
 };
 
 #endif /* UBX_H_ */
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index c406a7588..78eda327c 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1225,19 +1225,25 @@ start()
 
 	/* create the driver, attempt expansion bus first */
 	g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION);
+	if (g_dev != nullptr && OK != g_dev->init()) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+			
 
 #ifdef PX4_I2C_BUS_ONBOARD
 	/* if this failed, attempt onboard sensor */
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD);
+		if (g_dev != nullptr && OK != g_dev->init()) {
+			goto fail;
+		}
+	}
 #endif
 
 	if (g_dev == nullptr)
 		goto fail;
 
-	if (OK != g_dev->init())
-		goto fail;
-
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(MAG_DEVICE_PATH, O_RDONLY);
 
diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c
index 5fbee16ce..f0f32d244 100644
--- a/src/drivers/hott_telemetry/messages.c
+++ b/src/drivers/hott_telemetry/messages.c
@@ -42,9 +42,11 @@
 #include <string.h>
 #include <systemlib/systemlib.h>
 #include <unistd.h>
+#include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/sensor_combined.h>
 
+static int airspeed_sub = -1;
 static int battery_sub = -1;
 static int sensor_sub = -1;
 
@@ -52,6 +54,7 @@ void messages_init(void)
 {
 	battery_sub = orb_subscribe(ORB_ID(battery_status));
 	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 }
 
 void build_eam_response(uint8_t *buffer, int *size)
@@ -81,6 +84,15 @@ void build_eam_response(uint8_t *buffer, int *size)
 	msg.altitude_L = (uint8_t)alt & 0xff;
 	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
 
+	/* get a local copy of the current sensor values */
+	struct airspeed_s airspeed;
+	memset(&airspeed, 0, sizeof(airspeed));
+	orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
+
+	uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6);
+	msg.speed_L = (uint8_t)speed & 0xff;
+	msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
+
 	msg.stop = STOP_BYTE;
 
 	memcpy(buffer, &msg, *size);
diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp
index c7c45525e..04b565358 100644
--- a/src/drivers/led/led.cpp
+++ b/src/drivers/led/led.cpp
@@ -41,12 +41,17 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_led.h>
 
-/* Ideally we'd be able to get these from up_internal.h */
-//#include <up_internal.h>
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
 __BEGIN_DECLS
-extern void up_ledinit();
-extern void up_ledon(int led);
-extern void up_ledoff(int led);
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
 __END_DECLS
 
 class LED : device::CDev
@@ -74,7 +79,7 @@ int
 LED::init()
 {
 	CDev::init();
-	up_ledinit();
+	led_init();
 
 	return 0;
 }
@@ -86,11 +91,11 @@ LED::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	switch (cmd) {
 	case LED_ON:
-		up_ledon(arg);
+		led_on(arg);
 		break;
 
 	case LED_OFF:
-		up_ledoff(arg);
+		led_off(arg);
 		break;
 
 	default:
diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp
new file mode 100644
index 000000000..71932ad65
--- /dev/null
+++ b/src/drivers/md25/md25.cpp
@@ -0,0 +1,553 @@
+/****************************************************************************
+ *
+ *   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 md25.cpp
+ *
+ * Driver for MD25 I2C Motor Driver
+ *
+ * references:
+ * http://www.robot-electronics.co.uk/htm/md25tech.htm
+ * http://www.robot-electronics.co.uk/files/rpi_md25.c
+ *
+ */
+
+#include "md25.hpp"
+#include <poll.h>
+#include <stdio.h>
+
+#include <systemlib/err.h>
+#include <arch/board/board.h>
+
+// registers
+enum {
+	// RW: read/write
+	// R: read
+	REG_SPEED1_RW = 0,
+	REG_SPEED2_RW,
+	REG_ENC1A_R,
+	REG_ENC1B_R,
+	REG_ENC1C_R,
+	REG_ENC1D_R,
+	REG_ENC2A_R,
+	REG_ENC2B_R,
+	REG_ENC2C_R,
+	REG_ENC2D_R,
+	REG_BATTERY_VOLTS_R,
+	REG_MOTOR1_CURRENT_R,
+	REG_MOTOR2_CURRENT_R,
+	REG_SW_VERSION_R,
+	REG_ACCEL_RATE_RW,
+	REG_MODE_RW,
+	REG_COMMAND_RW,
+};
+
+MD25::MD25(const char *deviceName, int bus,
+	   uint16_t address, uint32_t speed) :
+	I2C("MD25", deviceName, bus, address, speed),
+	_controlPoll(),
+	_actuators(NULL, ORB_ID(actuator_controls_0), 20),
+	_version(0),
+	_motor1Speed(0),
+	_motor2Speed(0),
+	_revolutions1(0),
+	_revolutions2(0),
+	_batteryVoltage(0),
+	_motor1Current(0),
+	_motor2Current(0),
+	_motorAccel(0),
+	_mode(MODE_UNSIGNED_SPEED),
+	_command(CMD_RESET_ENCODERS)
+{
+	// setup control polling
+	_controlPoll.fd = _actuators.getHandle();
+	_controlPoll.events = POLLIN;
+
+	// if initialization fails raise an error, unless
+	// probing
+	int ret = I2C::init();
+
+	if (ret != OK) {
+		warnc(ret, "I2C::init failed for bus: %d address: %d\n", bus, address);
+	}
+
+	// setup default settings, reset encoders
+	setMotor1Speed(0);
+	setMotor2Speed(0);
+	resetEncoders();
+	_setMode(MD25::MODE_UNSIGNED_SPEED);
+	setSpeedRegulation(true);
+	setTimeout(true);
+}
+
+MD25::~MD25()
+{
+}
+
+int MD25::readData()
+{
+	uint8_t sendBuf[1];
+	sendBuf[0] = REG_SPEED1_RW;
+	uint8_t recvBuf[17];
+	int ret = transfer(sendBuf, sizeof(sendBuf),
+			   recvBuf, sizeof(recvBuf));
+
+	if (ret == OK) {
+		_version = recvBuf[REG_SW_VERSION_R];
+		_motor1Speed = _uint8ToNorm(recvBuf[REG_SPEED1_RW]);
+		_motor2Speed = _uint8ToNorm(recvBuf[REG_SPEED2_RW]);
+		_revolutions1 = -int32_t((recvBuf[REG_ENC1A_R] << 24) +
+					 (recvBuf[REG_ENC1B_R] << 16) +
+					 (recvBuf[REG_ENC1C_R] << 8)  +
+					 recvBuf[REG_ENC1D_R]) / 360.0;
+		_revolutions2 = -int32_t((recvBuf[REG_ENC2A_R] << 24) +
+					 (recvBuf[REG_ENC2B_R] << 16) +
+					 (recvBuf[REG_ENC2C_R] << 8)  +
+					 recvBuf[REG_ENC2D_R]) / 360.0;
+		_batteryVoltage = recvBuf[REG_BATTERY_VOLTS_R] / 10.0;
+		_motor1Current = recvBuf[REG_MOTOR1_CURRENT_R] / 10.0;
+		_motor2Current = recvBuf[REG_MOTOR2_CURRENT_R] / 10.0;
+		_motorAccel = recvBuf[REG_ACCEL_RATE_RW];
+		_mode = e_mode(recvBuf[REG_MODE_RW]);
+		_command = e_cmd(recvBuf[REG_COMMAND_RW]);
+	}
+
+	return ret;
+}
+
+void MD25::status(char *string, size_t n)
+{
+	snprintf(string, n,
+		 "version:\t%10d\n" \
+		 "motor 1 speed:\t%10.2f\n" \
+		 "motor 2 speed:\t%10.2f\n" \
+		 "revolutions 1:\t%10.2f\n" \
+		 "revolutions 2:\t%10.2f\n" \
+		 "battery volts :\t%10.2f\n" \
+		 "motor 1 current :\t%10.2f\n" \
+		 "motor 2 current :\t%10.2f\n" \
+		 "motor accel :\t%10d\n" \
+		 "mode :\t%10d\n" \
+		 "command :\t%10d\n",
+		 getVersion(),
+		 double(getMotor1Speed()),
+		 double(getMotor2Speed()),
+		 double(getRevolutions1()),
+		 double(getRevolutions2()),
+		 double(getBatteryVolts()),
+		 double(getMotor1Current()),
+		 double(getMotor2Current()),
+		 getMotorAccel(),
+		 getMode(),
+		 getCommand());
+}
+
+uint8_t MD25::getVersion()
+{
+	return _version;
+}
+
+float MD25::getMotor1Speed()
+{
+	return _motor1Speed;
+}
+
+float MD25::getMotor2Speed()
+{
+	return _motor2Speed;
+}
+
+float MD25::getRevolutions1()
+{
+	return _revolutions1;
+}
+
+float MD25::getRevolutions2()
+{
+	return _revolutions2;
+}
+
+float MD25::getBatteryVolts()
+{
+	return _batteryVoltage;
+}
+
+float MD25::getMotor1Current()
+{
+	return _motor1Current;
+}
+float MD25::getMotor2Current()
+{
+	return _motor2Current;
+}
+
+uint8_t MD25::getMotorAccel()
+{
+	return _motorAccel;
+}
+
+MD25::e_mode MD25::getMode()
+{
+	return _mode;
+}
+
+MD25::e_cmd MD25::getCommand()
+{
+	return _command;
+}
+
+int MD25::resetEncoders()
+{
+	return _writeUint8(REG_COMMAND_RW,
+			   CMD_RESET_ENCODERS);
+}
+
+int MD25::_setMode(e_mode mode)
+{
+	return _writeUint8(REG_MODE_RW,
+			   mode);
+}
+
+int MD25::setSpeedRegulation(bool enable)
+{
+	if (enable) {
+		return _writeUint8(REG_COMMAND_RW,
+				   CMD_ENABLE_SPEED_REGULATION);
+
+	} else {
+		return _writeUint8(REG_COMMAND_RW,
+				   CMD_DISABLE_SPEED_REGULATION);
+	}
+}
+
+int MD25::setTimeout(bool enable)
+{
+	if (enable) {
+		return _writeUint8(REG_COMMAND_RW,
+				   CMD_ENABLE_TIMEOUT);
+
+	} else {
+		return _writeUint8(REG_COMMAND_RW,
+				   CMD_DISABLE_TIMEOUT);
+	}
+}
+
+int MD25::setDeviceAddress(uint8_t address)
+{
+	uint8_t sendBuf[1];
+	sendBuf[0] = CMD_CHANGE_I2C_SEQ_0;
+	int ret = OK;
+	ret = transfer(sendBuf, sizeof(sendBuf),
+		       nullptr, 0);
+
+	if (ret != OK) {
+		warnc(ret, "MD25::setDeviceAddress");
+		return ret;
+	}
+
+	usleep(5000);
+	sendBuf[0] = CMD_CHANGE_I2C_SEQ_1;
+	ret = transfer(sendBuf, sizeof(sendBuf),
+		       nullptr, 0);
+
+	if (ret != OK) {
+		warnc(ret, "MD25::setDeviceAddress");
+		return ret;
+	}
+
+	usleep(5000);
+	sendBuf[0] = CMD_CHANGE_I2C_SEQ_2;
+	ret = transfer(sendBuf, sizeof(sendBuf),
+		       nullptr, 0);
+
+	if (ret != OK) {
+		warnc(ret, "MD25::setDeviceAddress");
+		return ret;
+	}
+
+	return OK;
+}
+
+int MD25::setMotor1Speed(float value)
+{
+	return _writeUint8(REG_SPEED1_RW,
+			   _normToUint8(value));
+}
+
+int MD25::setMotor2Speed(float value)
+{
+	return _writeUint8(REG_SPEED2_RW,
+			   _normToUint8(value));
+}
+
+void MD25::update()
+{
+	// wait for an actuator publication,
+	// check for exit condition every second
+	// note "::poll" is required to distinguish global
+	// poll from member function for driver
+	if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error
+
+	// if new data, send to motors
+	if (_actuators.updated()) {
+		_actuators.update();
+		setMotor1Speed(_actuators.control[CH_SPEED_LEFT]);
+		setMotor2Speed(_actuators.control[CH_SPEED_RIGHT]);
+	}
+}
+
+int MD25::probe()
+{
+	uint8_t goodAddress = 0;
+	bool found = false;
+	int ret = OK;
+
+	// try initial address first, if good, then done
+	if (readData() == OK) return ret;
+
+	// try all other addresses
+	uint8_t testAddress = 0;
+
+	//printf("searching for MD25 address\n");
+	while (true) {
+		set_address(testAddress);
+		ret = readData();
+
+		if (ret == OK && !found) {
+			//printf("device found at address: 0x%X\n", testAddress);
+			if (!found) {
+				found = true;
+				goodAddress = testAddress;
+			}
+		}
+
+		if (testAddress > 254) {
+			break;
+		}
+
+		testAddress++;
+	}
+
+	if (found) {
+		set_address(goodAddress);
+		return OK;
+
+	} else {
+		set_address(0);
+		return ret;
+	}
+}
+
+int MD25::search()
+{
+	uint8_t goodAddress = 0;
+	bool found = false;
+	int ret = OK;
+	// try all other addresses
+	uint8_t testAddress = 0;
+
+	//printf("searching for MD25 address\n");
+	while (true) {
+		set_address(testAddress);
+		ret = readData();
+
+		if (ret == OK && !found) {
+			printf("device found at address: 0x%X\n", testAddress);
+
+			if (!found) {
+				found = true;
+				goodAddress = testAddress;
+			}
+		}
+
+		if (testAddress > 254) {
+			break;
+		}
+
+		testAddress++;
+	}
+
+	if (found) {
+		set_address(goodAddress);
+		return OK;
+
+	} else {
+		set_address(0);
+		return ret;
+	}
+}
+
+int MD25::_writeUint8(uint8_t reg, uint8_t value)
+{
+	uint8_t sendBuf[2];
+	sendBuf[0] = reg;
+	sendBuf[1] = value;
+	return transfer(sendBuf, sizeof(sendBuf),
+			nullptr, 0);
+}
+
+int MD25::_writeInt8(uint8_t reg, int8_t value)
+{
+	uint8_t sendBuf[2];
+	sendBuf[0] = reg;
+	sendBuf[1] = value;
+	return transfer(sendBuf, sizeof(sendBuf),
+			nullptr, 0);
+}
+
+float MD25::_uint8ToNorm(uint8_t value)
+{
+	// TODO, should go from 0 to 255
+	// possibly should handle this differently
+	return (value - 128) / 127.0;
+}
+
+uint8_t MD25::_normToUint8(float value)
+{
+	if (value > 1) value = 1;
+
+	if (value < -1) value = -1;
+
+	// TODO, should go from 0 to 255
+	// possibly should handle this differently
+	return 127 * value + 128;
+}
+
+int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
+{
+	printf("md25 test: starting\n");
+
+	// setup
+	MD25 md25("/dev/md25", bus, address);
+
+	// print status
+	char buf[200];
+	md25.status(buf, sizeof(buf));
+	printf("%s\n", buf);
+
+	// setup for test
+	md25.setSpeedRegulation(true);
+	md25.setTimeout(true);
+	float dt = 0.1;
+	float speed = 0.2;
+	float t = 0;
+
+	// motor 1 test
+	printf("md25 test: spinning motor 1 forward for 1 rev at 0.1 speed\n");
+	t = 0;
+
+	while (true) {
+		t += dt;
+		md25.setMotor1Speed(speed);
+		md25.readData();
+		usleep(1000000 * dt);
+
+		if (md25.getRevolutions1() > 1) {
+			printf("finished 1 revolution fwd\n");
+			break;
+		}
+
+		if (t > 2.0f) break;
+	}
+
+	md25.setMotor1Speed(0);
+	printf("revolution of wheel 1: %8.4f\n", double(md25.getRevolutions1()));
+	md25.resetEncoders();
+
+	t = 0;
+
+	while (true) {
+		t += dt;
+		md25.setMotor1Speed(-speed);
+		md25.readData();
+		usleep(1000000 * dt);
+
+		if (md25.getRevolutions1() < -1) {
+			printf("finished 1 revolution rev\n");
+			break;
+		}
+
+		if (t > 2.0f) break;
+	}
+
+	md25.setMotor1Speed(0);
+	printf("revolution of wheel 1: %8.4f\n", double(md25.getRevolutions1()));
+	md25.resetEncoders();
+
+	// motor 2 test
+	printf("md25 test: spinning motor 2 forward for 1 rev at 0.1 speed\n");
+	t = 0;
+
+	while (true) {
+		t += dt;
+		md25.setMotor2Speed(speed);
+		md25.readData();
+		usleep(1000000 * dt);
+
+		if (md25.getRevolutions2() > 1) {
+			printf("finished 1 revolution fwd\n");
+			break;
+		}
+
+		if (t > 2.0f) break;
+	}
+
+	md25.setMotor2Speed(0);
+	printf("revolution of wheel 2: %8.4f\n", double(md25.getRevolutions2()));
+	md25.resetEncoders();
+
+	t = 0;
+
+	while (true) {
+		t += dt;
+		md25.setMotor2Speed(-speed);
+		md25.readData();
+		usleep(1000000 * dt);
+
+		if (md25.getRevolutions2() < -1) {
+			printf("finished 1 revolution rev\n");
+			break;
+		}
+
+		if (t > 2.0f) break;
+	}
+
+	md25.setMotor2Speed(0);
+	printf("revolution of wheel 2: %8.4f\n", double(md25.getRevolutions2()));
+	md25.resetEncoders();
+
+	printf("Test 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
new file mode 100644
index 000000000..e77511b16
--- /dev/null
+++ b/src/drivers/md25/md25.hpp
@@ -0,0 +1,293 @@
+/****************************************************************************
+ *
+ *   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 md25.cpp
+ *
+ * Driver for MD25 I2C Motor Driver
+ *
+ * references:
+ * http://www.robot-electronics.co.uk/htm/md25tech.htm
+ * http://www.robot-electronics.co.uk/files/rpi_md25.c
+ *
+ */
+
+#pragma once
+
+#include <poll.h>
+#include <stdio.h>
+#include <controllib/block/UOrbSubscription.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <drivers/device/i2c.h>
+
+/**
+ * This is a driver for the MD25 motor controller utilizing the I2C interface.
+ */
+class MD25 : public device::I2C
+{
+public:
+
+	/**
+	* modes
+	*
+	* NOTE: this driver assumes we are always
+	* in mode 0!
+	*
+	* seprate speed mode:
+	*  motor speed1 = speed1
+	*  motor speed2 = speed2
+	* turn speed mode:
+	*	motor speed1 = speed1 + speed2
+	*  motor speed2 = speed2 - speed2
+	* unsigned:
+	*  full rev (0), stop(128), full fwd (255)
+	* signed:
+	*  full rev (-127), stop(0), full fwd (128)
+	*
+	* modes numbers:
+	* 0 : unsigned separate (default)
+	* 1 : signed separate
+	* 2 : unsigned turn
+	* 3 : signed turn
+	*/
+	enum e_mode {
+		MODE_UNSIGNED_SPEED = 0,
+		MODE_SIGNED_SPEED,
+		MODE_UNSIGNED_SPEED_TURN,
+		MODE_SIGNED_SPEED_TURN,
+	};
+
+	/** commands */
+	enum e_cmd {
+		CMD_RESET_ENCODERS = 32,
+		CMD_DISABLE_SPEED_REGULATION = 48,
+		CMD_ENABLE_SPEED_REGULATION = 49,
+		CMD_DISABLE_TIMEOUT = 50,
+		CMD_ENABLE_TIMEOUT = 51,
+		CMD_CHANGE_I2C_SEQ_0 = 160,
+		CMD_CHANGE_I2C_SEQ_1 = 170,
+		CMD_CHANGE_I2C_SEQ_2 = 165,
+	};
+
+	/** control channels */
+	enum e_channels {
+		CH_SPEED_LEFT = 0,
+		CH_SPEED_RIGHT
+	};
+
+	/**
+	 * constructor
+	 * @param deviceName the name of the device e.g. "/dev/md25"
+	 * @param bus the I2C bus
+	 * @param address the adddress on the I2C bus
+	 * @param speed the speed of the I2C communication
+	 */
+	MD25(const char *deviceName,
+	     int bus,
+	     uint16_t address,
+	     uint32_t speed = 100000);
+
+	/**
+	 * deconstructor
+	 */
+	virtual ~MD25();
+
+	/**
+	 * @return software version
+	 */
+	uint8_t getVersion();
+
+	/**
+	 * @return speed of motor, normalized (-1, 1)
+	 */
+	float getMotor1Speed();
+
+	/**
+	 * @return speed of motor 2, normalized (-1, 1)
+	 */
+	float getMotor2Speed();
+
+	/**
+	 * @return number of rotations since reset
+	 */
+	float getRevolutions1();
+
+	/**
+	 * @return number of rotations since reset
+	 */
+	float getRevolutions2();
+
+	/**
+	 * @return battery voltage, volts
+	 */
+	float getBatteryVolts();
+
+	/**
+	 * @return motor 1 current, amps
+	 */
+	float getMotor1Current();
+
+	/**
+	 * @return motor 2 current, amps
+	 */
+	float getMotor2Current();
+
+	/**
+	 * @return the motor acceleration
+	 * 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
+	 */
+	uint8_t getMotorAccel();
+
+	/**
+	 * @return motor output mode
+	 * */
+	e_mode getMode();
+
+	/**
+	 * @return current command register value
+	 */
+	e_cmd getCommand();
+
+	/**
+	 * resets the encoders
+	 * @return non-zero -> error
+	 * */
+	int resetEncoders();
+
+	/**
+	 * enable/disable speed regulation
+	 * @return non-zero -> error
+	 */
+	int setSpeedRegulation(bool enable);
+
+	/**
+	 * set the timeout for the motors
+	 * enable/disable timeout (motor stop)
+	 * after 2 sec of no i2c messages
+	 * @return non-zero -> error
+	 */
+	int setTimeout(bool enable);
+
+	/**
+	 * sets the device address
+	 * can only be done with one MD25
+	 * on the bus
+	 * @return non-zero -> error
+	 */
+	int setDeviceAddress(uint8_t address);
+
+	/**
+	 * set motor 1 speed
+	 * @param normSpeed normalize speed between -1 and 1
+	 * @return non-zero -> error
+	*/
+	int setMotor1Speed(float normSpeed);
+
+	/**
+	 * set motor 2 speed
+	 * @param normSpeed normalize speed between -1 and 1
+	 * @return non-zero -> error
+	 */
+	int setMotor2Speed(float normSpeed);
+
+	/**
+	 * main update loop that updates MD25 motor
+	 * speeds based on actuator publication
+	 */
+	void update();
+
+	/**
+	 * probe for device
+	 */
+	virtual int probe();
+
+	/**
+	 * search for device
+	 */
+	int search();
+
+	/**
+	 * read data from i2c
+	 */
+	int readData();
+
+	/**
+	 * print status
+	 */
+	void status(char *string, size_t n);
+
+private:
+	/** poll structure for control packets */
+	struct pollfd _controlPoll;
+
+	/** actuator controls subscription */
+	control::UOrbSubscription<actuator_controls_s> _actuators;
+
+	// local copy of data from i2c device
+	uint8_t _version;
+	float _motor1Speed;
+	float _motor2Speed;
+	float _revolutions1;
+	float _revolutions2;
+	float _batteryVoltage;
+	float _motor1Current;
+	float _motor2Current;
+	uint8_t _motorAccel;
+	e_mode _mode;
+	e_cmd _command;
+
+	// private methods
+	int _writeUint8(uint8_t reg, uint8_t value);
+	int _writeInt8(uint8_t reg, int8_t value);
+	float _uint8ToNorm(uint8_t value);
+	uint8_t _normToUint8(float value);
+
+	/**
+	 * set motor control mode,
+	 * this driver assumed we are always in mode 0
+	 * so we don't let the user change the mode
+	 * @return non-zero -> error
+	 */
+	int _setMode(e_mode);
+};
+
+// unit testing
+int md25Test(const char *deviceName, uint8_t bus, uint8_t address);
+
+// 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
new file mode 100644
index 000000000..80850e708
--- /dev/null
+++ b/src/drivers/md25/md25_main.cpp
@@ -0,0 +1,264 @@
+/****************************************************************************
+ *
+ *   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 md25.cpp
+ *
+ * Driver for MD25 I2C Motor Driver
+ *
+ * references:
+ * http://www.robot-electronics.co.uk/htm/md25tech.htm
+ * http://www.robot-electronics.co.uk/files/rpi_md25.c
+ *
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+
+#include <arch/board/board.h>
+#include "md25.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 md25_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int md25_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: md25 {start|stop|status|search|test|change_address}\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 md25_main(int argc, char *argv[])
+{
+
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("md25 already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn("md25",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 10,
+					 2048,
+					 md25_thread_main,
+					 (const char **)argv);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "test")) {
+
+		if (argc < 4) {
+			printf("usage: md25 test 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);
+
+		md25Test(deviceName, bus, address);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "probe")) {
+		if (argc < 4) {
+			printf("usage: md25 probe 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);
+
+		int ret = md25.probe();
+
+		if (ret == OK) {
+			printf("MD25 found on bus %d at address 0x%X\n", bus, md25.get_address());
+
+		} else {
+			printf("MD25 not found on bus %d\n", bus);
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "search")) {
+		if (argc < 3) {
+			printf("usage: md25 search bus\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);
+
+		md25.search();
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "change_address")) {
+		if (argc < 5) {
+			printf("usage: md25 change_address bus old_address new_address\n");
+			exit(0);
+		}
+
+		const char *deviceName = "/dev/md25";
+
+		uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+		uint8_t old_address = strtoul(argv[3], nullptr, 0);
+
+		uint8_t new_address = strtoul(argv[4], nullptr, 0);
+
+		MD25 md25(deviceName, bus, old_address);
+
+		int ret = md25.setDeviceAddress(new_address);
+
+		if (ret == OK)  {
+			printf("MD25 new address set to 0x%X\n", new_address);
+
+		} else {
+			printf("MD25 failed to set address to 0x%X\n", new_address);
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tmd25 app is running\n");
+
+		} else {
+			printf("\tmd25 app not started\n");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int md25_thread_main(int argc, char *argv[])
+{
+	printf("[MD25] starting\n");
+
+	if (argc < 5) {
+		// extra md25 in arg list since this is a thread
+		printf("usage: md25 start bus address\n");
+		exit(0);
+	}
+
+	const char *deviceName = "/dev/md25";
+
+	uint8_t bus = strtoul(argv[3], nullptr, 0);
+
+	uint8_t address = strtoul(argv[4], nullptr, 0);
+
+	// start
+	MD25 md25("/dev/md25", bus, address);
+
+	thread_running = true;
+
+	// loop
+	while (!thread_should_exit) {
+		md25.update();
+	}
+
+	// exit
+	printf("[MD25] exiting.\n");
+	thread_running = false;
+	return 0;
+}
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/md25/module.mk b/src/drivers/md25/module.mk
new file mode 100644
index 000000000..13821a6b5
--- /dev/null
+++ b/src/drivers/md25/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# MD25 I2C Based Motor Controller
+# http://www.robot-electronics.co.uk/htm/md25tech.htm
+#
+
+MODULE_COMMAND		= md25
+
+SRCS			= md25.cpp \
+			  md25_main.cpp
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
new file mode 100644
index 000000000..3a735e26f
--- /dev/null
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -0,0 +1,1442 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mkblctrl.cpp
+ *
+ * Driver/configurator for the Mikrokopter BL-Ctrl.
+ * Marco Bauer
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.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/i2c.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_gpio.h>
+#include <drivers/boards/px4fmu/px4fmu_internal.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_rc_input.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <systemlib/mixer/mixer.h>
+#include <drivers/drv_mixer.h>
+
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/actuator_outputs.h>
+
+#include <systemlib/err.h>
+#include <systemlib/ppm_decode.h>
+
+#define I2C_BUS_SPEED					400000
+#define UPDATE_RATE						400
+#define MAX_MOTORS 						8
+#define BLCTRL_BASE_ADDR 				0x29
+#define BLCTRL_OLD 						0
+#define BLCTRL_NEW 						1
+#define BLCTRL_MIN_VALUE				-0.920F
+#define MOTOR_STATE_PRESENT_MASK		0x80
+#define MOTOR_STATE_ERROR_MASK		0x7F
+#define MOTOR_SPINUP_COUNTER			2500
+
+
+class MK : public device::I2C
+{
+public:
+	enum Mode {
+		MODE_2PWM,
+		MODE_4PWM,
+		MODE_NONE
+	};
+
+	enum MappingMode {
+		MAPPING_MK = 0,
+		MAPPING_PX4,
+	};
+
+	enum FrameType {
+		FRAME_PLUS = 0,
+		FRAME_X,
+	};
+
+	MK(int bus);
+	~MK();
+
+	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
+	virtual int	init(unsigned motors);
+
+	int		set_mode(Mode mode);
+	int		set_pwm_rate(unsigned rate);
+	int		set_motor_count(unsigned count);
+	int		set_motor_test(bool motortest);
+	int		set_px4mode(int px4mode);
+	int		set_frametype(int frametype);
+	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput);
+
+private:
+	static const unsigned _max_actuators = MAX_MOTORS;
+	static const bool	showDebug = false;
+
+	Mode		_mode;
+	int 		_update_rate;
+	int 		_current_update_rate;
+	int		_task;
+	int		_t_actuators;
+	int		_t_armed;
+	unsigned int		_motor;
+	int    _px4mode;
+	int    _frametype;
+	orb_advert_t	_t_outputs;
+	orb_advert_t	_t_actuators_effective;
+	unsigned int	_num_outputs;
+	bool		_primary_pwm_device;
+	bool     _motortest;
+
+	volatile bool	_task_should_exit;
+	bool		_armed;
+
+	unsigned long	debugCounter;
+
+	MixerGroup	*_mixers;
+
+	actuator_controls_s _controls;
+
+	static void	task_main_trampoline(int argc, char *argv[]);
+	void		task_main() __attribute__((noreturn));
+
+	static int	control_callback(uintptr_t handle,
+					 uint8_t control_group,
+					 uint8_t control_index,
+					 float &input);
+
+	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
+
+	struct GPIOConfig {
+		uint32_t	input;
+		uint32_t	output;
+		uint32_t	alt;
+	};
+
+	static const GPIOConfig	_gpio_tab[];
+	static const unsigned	_ngpio;
+
+	void		gpio_reset(void);
+	void		gpio_set_function(uint32_t gpios, int function);
+	void		gpio_write(uint32_t gpios, int function);
+	uint32_t	gpio_read(void);
+	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
+	int			mk_servo_arm(bool status);
+
+	int 		mk_servo_set(unsigned int chan, float val);
+	int 		mk_servo_set_test(unsigned int chan, float val);
+	int 		mk_servo_test(unsigned int chan);
+
+
+};
+
+const MK::GPIOConfig MK::_gpio_tab[] = {
+	{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
+	{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
+	{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
+	{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1},
+	{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1},
+	{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
+	{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
+	{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
+};
+
+const unsigned MK::_ngpio = sizeof(MK::_gpio_tab) / sizeof(MK::_gpio_tab[0]);
+
+const int blctrlAddr_quad_plus[] = { 2, 2, -2, -2, 0, 0, 0, 0 };	// Addresstranslator for Quad + configuration
+const int blctrlAddr_hexa_plus[] = { 0, 2, 2, -2, 1, -3, 0, 0 };	// Addresstranslator for Hexa + configuration
+const int blctrlAddr_octo_plus[] = { 0, 3, -1, 0, 3, 0, 0, -5 };	// Addresstranslator for Octo + configuration
+
+const int blctrlAddr_quad_x[] = { 2, 2, -2, -2, 0, 0, 0, 0 };	// Addresstranslator for Quad X configuration
+const int blctrlAddr_hexa_x[] = { 2, 4, -2, 0, -3, -1, 0, 0 };	// Addresstranslator for Hexa X configuration
+const int blctrlAddr_octo_x[] = { 1, 4, 0, 1, -4, 1, 1, -4 };	// Addresstranslator for Octo X configuration
+
+const int blctrlAddr_px4[]  = { 0, 0, 0, 0, 0, 0, 0, 0};
+
+int addrTranslator[] = {0,0,0,0,0,0,0,0};
+
+struct MotorData_t
+{
+	unsigned int Version;                        // the version of the BL (0 = old)
+  unsigned int SetPoint;                       // written by attitude controller
+  unsigned int SetPointLowerBits;      // for higher Resolution of new BLs
+  unsigned int State;                          // 7 bit for I2C error counter, highest bit indicates if motor is present
+  unsigned int ReadMode;                       // select data to read
+  // the following bytes must be exactly in that order!
+  unsigned int Current;                        // in 0.1 A steps, read back from BL
+  unsigned int MaxPWM;                         // read back from BL is less than 255 if BL is in current limit
+  unsigned int Temperature;            // old BL-Ctrl will return a 255 here, the new version the temp. in
+  unsigned int RoundCount;
+};
+
+MotorData_t Motor[MAX_MOTORS];
+
+
+namespace
+{
+
+MK	*g_mk;
+
+} // namespace
+
+MK::MK(int bus) :
+	I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED),
+	_mode(MODE_NONE),
+	_update_rate(50),
+	_task(-1),
+	_t_actuators(-1),
+	_t_armed(-1),
+	_t_outputs(0),
+	_t_actuators_effective(0),
+	_num_outputs(0),
+	_motortest(false),
+	_motor(-1),
+	_px4mode(MAPPING_MK),
+	_frametype(FRAME_PLUS),
+	_primary_pwm_device(false),
+	_task_should_exit(false),
+	_armed(false),
+	_mixers(nullptr)
+{
+	_debug_enabled = true;
+}
+
+MK::~MK()
+{
+	if (_task != -1) {
+		/* tell the task we want it to go away */
+		_task_should_exit = true;
+
+		unsigned i = 10;
+
+		do {
+			/* wait 50ms - it should wake every 100ms or so worst-case */
+			usleep(50000);
+
+			/* if we have given up, kill it */
+			if (--i == 0) {
+				task_delete(_task);
+				break;
+			}
+
+		} while (_task != -1);
+	}
+
+	/* clean up the alternate device node */
+	if (_primary_pwm_device)
+		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+
+	g_mk = nullptr;
+}
+
+int
+MK::init(unsigned motors)
+{
+	_num_outputs = motors;
+	debugCounter = 0;
+	int ret;
+	ASSERT(_task == -1);
+
+	ret = I2C::init();
+
+	if (ret != OK) {
+		warnx("I2C init failed");
+		return ret;
+	}
+
+	usleep(500000);
+
+	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
+	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+
+	if (ret == OK) {
+		log("default PWM output device");
+		_primary_pwm_device = true;
+	}
+
+	/* reset GPIOs */
+	gpio_reset();
+
+	/* start the IO interface task */
+	_task = task_spawn("mkblctrl",
+			   SCHED_DEFAULT,
+			   SCHED_PRIORITY_MAX -20,
+			   2048,
+			   (main_t)&MK::task_main_trampoline,
+			   nullptr);
+
+
+	if (_task < 0) {
+		debug("task start failed: %d", errno);
+		return -errno;
+	}
+
+	return OK;
+}
+
+void
+MK::task_main_trampoline(int argc, char *argv[])
+{
+	g_mk->task_main();
+}
+
+int
+MK::set_mode(Mode mode)
+{
+	/*
+	 * Configure for PWM output.
+	 *
+	 * Note that regardless of the configured mode, the task is always
+	 * listening and mixing; the mode just selects which of the channels
+	 * are presented on the output pins.
+	 */
+	switch (mode) {
+	case MODE_2PWM:
+		if(_num_outputs == 4) {
+			//debug("MODE_QUAD");
+		} else if(_num_outputs == 6) {
+			//debug("MODE_HEXA");
+		} else if(_num_outputs == 8) {
+			//debug("MODE_OCTO");
+		}
+		//up_pwm_servo_init(0x3);
+		up_pwm_servo_deinit();
+		_update_rate = UPDATE_RATE;	/* default output rate */
+		break;
+
+	case MODE_4PWM:
+		if(_num_outputs == 4) {
+			//debug("MODE_QUADRO");
+		} else if(_num_outputs == 6) {
+			//debug("MODE_HEXA");
+		} else if(_num_outputs == 8) {
+			//debug("MODE_OCTO");
+		}
+		//up_pwm_servo_init(0xf);
+		up_pwm_servo_deinit();
+		_update_rate = UPDATE_RATE;	/* default output rate */
+		break;
+
+	case MODE_NONE:
+		debug("MODE_NONE");
+		/* disable servo outputs and set a very low update rate */
+		up_pwm_servo_deinit();
+		_update_rate = UPDATE_RATE;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	_mode = mode;
+	return OK;
+}
+
+int
+MK::set_pwm_rate(unsigned rate)
+{
+	if ((rate > 500) || (rate < 10))
+		return -EINVAL;
+
+	_update_rate = rate;
+	return OK;
+}
+
+int
+MK::set_px4mode(int px4mode)
+{
+	_px4mode = px4mode;
+}
+
+int
+MK::set_frametype(int frametype)
+{
+	_frametype = frametype;
+}
+
+
+int
+MK::set_motor_count(unsigned count)
+{
+	if(count > 0) {
+
+		_num_outputs = count;
+
+		if(_px4mode == MAPPING_MK) {
+			if(_frametype == FRAME_PLUS) {
+				fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: +\n");
+			} else if(_frametype == FRAME_X) {
+				fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: X\n");
+			}
+			if(_num_outputs == 4) {
+				if(_frametype == FRAME_PLUS) {
+					memcpy(&addrTranslator, &blctrlAddr_quad_plus, sizeof(blctrlAddr_quad_plus));
+				} else if(_frametype == FRAME_X) {
+					memcpy(&addrTranslator, &blctrlAddr_quad_x, sizeof(blctrlAddr_quad_x));
+				}
+			} else if(_num_outputs == 6) {
+				if(_frametype == FRAME_PLUS) {
+					memcpy(&addrTranslator, &blctrlAddr_hexa_plus, sizeof(blctrlAddr_hexa_plus));
+				} else if(_frametype == FRAME_X) {
+					memcpy(&addrTranslator, &blctrlAddr_hexa_x, sizeof(blctrlAddr_hexa_x));
+				}
+			} else if(_num_outputs == 8) {
+				if(_frametype == FRAME_PLUS) {
+					memcpy(&addrTranslator, &blctrlAddr_octo_plus, sizeof(blctrlAddr_octo_plus));
+				} else if(_frametype == FRAME_X) {
+					memcpy(&addrTranslator, &blctrlAddr_octo_x, sizeof(blctrlAddr_octo_x));
+				}
+			}
+		} else {
+			fprintf(stderr, "[mkblctrl] PX4 native addressing used.\n");
+			memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4));
+		}
+
+		if(_num_outputs == 4) {
+			fprintf(stderr, "[mkblctrl] Quadrocopter Mode (4)\n");
+		} else 	if(_num_outputs == 6) {
+			fprintf(stderr, "[mkblctrl] Hexacopter Mode (6)\n");
+		} else 	if(_num_outputs == 8) {
+			fprintf(stderr, "[mkblctrl] Octocopter Mode (8)\n");
+		}
+
+		return OK;
+
+	} else {
+		return -1;
+	}
+
+}
+
+int
+MK::set_motor_test(bool motortest)
+{
+	_motortest = motortest;
+	return OK;
+}
+
+
+void
+MK::task_main()
+{
+	/*
+	 * Subscribe to the appropriate PWM output topic based on whether we are the
+	 * primary PWM output or not.
+	 */
+	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+				     ORB_ID(actuator_controls_1));
+	/* force a reset of the update rate */
+	_current_update_rate = 0;
+
+	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+
+	/* advertise the mixed control outputs */
+	actuator_outputs_s outputs;
+	memset(&outputs, 0, sizeof(outputs));
+	/* advertise the mixed control outputs */
+	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
+				   &outputs);
+
+	/* advertise the effective control inputs */
+	actuator_controls_effective_s controls_effective;
+	memset(&controls_effective, 0, sizeof(controls_effective));
+	/* advertise the effective control inputs */
+	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
+				   &controls_effective);
+
+	pollfd fds[2];
+	fds[0].fd = _t_actuators;
+	fds[0].events = POLLIN;
+	fds[1].fd = _t_armed;
+	fds[1].events = POLLIN;
+
+	// rc input, published to ORB
+	struct rc_input_values rc_in;
+	orb_advert_t to_input_rc = 0;
+
+	memset(&rc_in, 0, sizeof(rc_in));
+	rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
+	
+	log("starting");
+	long update_rate_in_us = 0;
+
+	/* loop until killed */
+	while (!_task_should_exit) {
+
+		/* handle update rate changes */
+		if (_current_update_rate != _update_rate) {
+			int update_rate_in_ms = int(1000 / _update_rate);
+			update_rate_in_us = long(1000000 / _update_rate);
+
+			/* reject faster than 500 Hz updates */
+			if (update_rate_in_ms < 2) {
+				update_rate_in_ms = 2;
+				_update_rate = 500;
+			}
+			/* reject slower than 50 Hz updates */
+			if (update_rate_in_ms > 20) {
+				update_rate_in_ms = 20;
+				_update_rate = 50;
+			}
+
+			orb_set_interval(_t_actuators, update_rate_in_ms);
+			up_pwm_servo_set_rate(_update_rate);
+			_current_update_rate = _update_rate;
+		}
+
+		/* sleep waiting for data, but no more than a second */
+		int ret = ::poll(&fds[0], 2, 1000);
+
+		/* this would be bad... */
+		if (ret < 0) {
+			log("poll error %d", errno);
+			usleep(1000000);
+			continue;
+		}
+
+		/* do we have a control update? */
+		if (fds[0].revents & POLLIN) {
+
+			/* get controls - must always do this to avoid spinning */
+			orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls);
+
+			/* can we mix? */
+			if (_mixers != nullptr) {
+
+				/* do mixing */
+				outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs);
+				outputs.timestamp = hrt_absolute_time();
+
+				// XXX output actual limited values
+				memcpy(&controls_effective, &_controls, sizeof(controls_effective));
+
+				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
+
+				/* iterate actuators */
+				for (unsigned int i = 0; i < _num_outputs; i++) {
+
+					/* last resort: catch NaN, INF and out-of-band errors */
+					if (i < outputs.noutputs &&
+						isfinite(outputs.output[i]) &&
+						outputs.output[i] >= -1.0f &&
+						outputs.output[i] <= 1.0f) {
+						/* scale for PWM output 900 - 2100us */
+						//outputs.output[i] = 1500 + (600 * outputs.output[i]);
+						//outputs.output[i] = 127 + (127 * outputs.output[i]);
+					} else {
+						/*
+						 * Value is NaN, INF or out of band - set to the minimum value.
+						 * This will be clearly visible on the servo status and will limit the risk of accidentally
+						 * spinning motors. It would be deadly in flight.
+						 */
+						if(outputs.output[i] < -1.0f) {
+							outputs.output[i] = -1.0f;
+						} else if(outputs.output[i] > 1.0f) {
+							outputs.output[i] = 1.0f;
+						} else {
+							outputs.output[i] = -1.0f;
+						}
+					}
+
+					/* don't go under BLCTRL_MIN_VALUE */
+					if(outputs.output[i] < BLCTRL_MIN_VALUE) {
+						outputs.output[i] = BLCTRL_MIN_VALUE;
+					}
+					//_motortest = true;
+					/* output to BLCtrl's */
+					if(_motortest == true) {
+						mk_servo_test(i);
+					} else {
+						//mk_servo_set(i, outputs.output[i]);
+						mk_servo_set_test(i, outputs.output[i]);	// 8Bit
+					}
+
+
+				}
+
+				/* and publish for anyone that cares to see */
+				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
+			}
+		}
+
+		/* how about an arming update? */
+		if (fds[1].revents & POLLIN) {
+			actuator_armed_s aa;
+
+			/* get new value */
+			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+
+			/* update PWM servo armed status if armed and not locked down */
+			////up_pwm_servo_arm(aa.armed && !aa.lockdown);
+			mk_servo_arm(aa.armed && !aa.lockdown);
+		}
+
+		// see if we have new PPM input data
+		if (ppm_last_valid_decode != rc_in.timestamp) {
+			// we have a new PPM frame. Publish it.
+			rc_in.channel_count = ppm_decoded_channels;
+			if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
+				rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
+			}
+			for (uint8_t i=0; i<rc_in.channel_count; i++) {
+				rc_in.values[i] = ppm_buffer[i];
+			}
+			rc_in.timestamp = ppm_last_valid_decode;
+
+			/* lazily advertise on first publication */
+			if (to_input_rc == 0) {
+				to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
+			} else {
+				orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
+			}
+		}
+
+	}
+
+	::close(_t_actuators);
+	::close(_t_actuators_effective);
+	::close(_t_armed);
+
+
+	/* make sure servos are off */
+	up_pwm_servo_deinit();
+
+	log("stopping");
+
+	/* note - someone else is responsible for restoring the GPIO config */
+
+	/* tell the dtor that we are exiting */
+	_task = -1;
+	_exit(0);
+}
+
+
+int 
+MK::mk_servo_arm(bool status)
+{
+	_armed = status;
+	return 0;
+}
+
+
+unsigned int
+MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
+{
+	_retries = 50;
+	uint8_t foundMotorCount = 0;
+
+	for(unsigned i=0; i<MAX_MOTORS; i++) {
+		Motor[i].Version = 0;
+		Motor[i].SetPoint = 0;
+		Motor[i].SetPointLowerBits = 0;
+		Motor[i].State = 0;
+		Motor[i].ReadMode = 0;
+		Motor[i].Current = 0;
+		Motor[i].MaxPWM = 0;
+		Motor[i].Temperature = 0;
+		Motor[i].RoundCount = 0;
+	}
+
+	uint8_t msg = 0;
+	uint8_t result[3];
+
+	for(unsigned i=0; i< count; i++) {
+		result[0] = 0;
+		result[1] = 0;
+		result[2] = 0;
+			
+		set_address( BLCTRL_BASE_ADDR + i );
+		
+		if (OK == transfer(&msg, 1, &result[0], 3)) {
+			Motor[i].Current = result[0];
+			Motor[i].MaxPWM = result[1];
+			Motor[i].Temperature = result[2];
+			Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit;
+			foundMotorCount++;
+			if(Motor[i].MaxPWM == 250) {
+				Motor[i].Version = BLCTRL_NEW;
+			} else {
+				Motor[i].Version = BLCTRL_OLD;
+			}
+		}
+	}
+
+	if(showOutput) {
+		fprintf(stderr, "[mkblctrl] MotorsFound: %i\n",foundMotorCount);
+		for(unsigned i=0; i< foundMotorCount; i++) {
+			fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i,Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
+		}
+
+		if(foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
+			_task_should_exit = true;
+		}
+	}
+
+	return foundMotorCount;
+}
+
+
+
+
+int
+MK::mk_servo_set(unsigned int chan, float val)
+{
+	float tmpVal = 0;
+	_retries = 0;
+	uint8_t result[3] = { 0,0,0 };
+	uint8_t msg[2] = { 0,0 };
+	uint8_t rod=0;
+	uint8_t bytesToSendBL2 = 2;
+
+
+	tmpVal = (1023 + (1023 * val));
+	if(tmpVal > 2047) {
+		tmpVal = 2047;
+	}
+
+
+	Motor[chan].SetPoint = (uint8_t) tmpVal / 3;	// divide 8
+	Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 8;	// rest of divide 8
+	//rod = (uint8_t) tmpVal % 8;
+	//Motor[chan].SetPointLowerBits = rod<<1;	// rest of divide 8
+	Motor[chan].SetPointLowerBits = 0;
+
+	if(_armed == false) {
+		Motor[chan].SetPoint = 0;
+		Motor[chan].SetPointLowerBits = 0;
+	}
+
+	//if(Motor[chan].State & MOTOR_STATE_PRESENT_MASK) {
+		set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
+
+		if(Motor[chan].Version == BLCTRL_OLD) {
+			/*
+			*	Old BL-Ctrl 8Bit served. Version < 2.0
+			*/
+			msg[0] = Motor[chan].SetPoint;
+			if(Motor[chan].RoundCount >= 16) {
+				// on each 16th cyle we read out the status messages from the blctrl
+				if (OK == transfer(&msg[0], 1, &result[0], 2)) {
+					Motor[chan].Current = result[0];
+					Motor[chan].MaxPWM = result[1];
+					Motor[chan].Temperature = 255;;
+				} else {
+					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
+				}
+				Motor[chan].RoundCount = 0;
+			} else {
+				if (OK != transfer(&msg[0], 1, nullptr, 0)) {
+					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
+				}
+			}
+
+		} else {
+			/*
+			*	New BL-Ctrl 11Bit served. Version >= 2.0
+			*/
+			msg[0] = Motor[chan].SetPoint;
+			msg[1] = Motor[chan].SetPointLowerBits;
+
+			if(Motor[chan].SetPointLowerBits == 0) {
+				bytesToSendBL2 = 1;	// if setpoint lower bits are zero, we send only the higher bits - this saves time
+			}
+
+			if(Motor[chan].RoundCount >= 16) {
+				// on each 16th cyle we read out the status messages from the blctrl
+				if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) {
+					Motor[chan].Current = result[0];
+					Motor[chan].MaxPWM = result[1];
+					Motor[chan].Temperature = result[2];
+				} else {
+					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
+				}
+				Motor[chan].RoundCount = 0;
+			} else {
+				if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) {
+					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
+				}
+			}
+
+		}
+
+		Motor[chan].RoundCount++;
+	//}
+
+	if(showDebug == true) {
+		debugCounter++;
+		if(debugCounter == 2000) {
+			debugCounter = 0;
+			for(int i=0; i<_num_outputs; i++){
+				if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) {
+					fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State);
+				}
+			}
+			fprintf(stderr, "\n");
+		}
+	}
+	return 0;
+}
+
+int
+MK::mk_servo_set_test(unsigned int chan, float val)
+{
+	_retries = 0;
+	int ret;
+
+	float tmpVal = 0;
+
+	uint8_t msg[2] = { 0,0 };
+
+	tmpVal = (1023 + (1023 * val));
+	if(tmpVal > 2048) {
+		tmpVal = 2048;
+	}
+
+	Motor[chan].SetPoint = (uint8_t) (tmpVal / 8);
+
+	if(_armed == false) {
+		Motor[chan].SetPoint = 0;
+		Motor[chan].SetPointLowerBits = 0;
+	}
+
+	msg[0] = Motor[chan].SetPoint;
+
+	set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
+	ret = transfer(&msg[0], 1, nullptr, 0);
+
+	ret = OK;
+
+	return ret;
+}
+
+
+int
+MK::mk_servo_test(unsigned int chan)
+{
+	int ret=0;
+	float tmpVal = 0;
+	float val = -1;
+	_retries = 0;
+	uint8_t msg[2] = { 0,0 };
+
+	if(debugCounter >= MOTOR_SPINUP_COUNTER) {
+		debugCounter = 0;
+		_motor++;
+
+		if(_motor < _num_outputs) {
+			fprintf(stderr, "[mkblctrl] Motortest - #%i:\tspinup\n", _motor);
+		}
+
+		if(_motor >= _num_outputs) {
+			_motor = -1;
+			_motortest = false;
+		}
+
+	}
+	debugCounter++;
+
+	if(_motor == chan) {
+		val = BLCTRL_MIN_VALUE;
+	} else {
+		val = -1;
+	}
+
+	tmpVal = (1023 + (1023 * val));
+	if(tmpVal > 2048) {
+		tmpVal = 2048;
+	}
+
+	//Motor[chan].SetPoint = (uint8_t) (tmpVal / 8);
+	//Motor[chan].SetPointLowerBits = (uint8_t) (tmpVal % 8) & 0x07;
+	Motor[chan].SetPoint = (uint8_t) tmpVal>>3;
+	Motor[chan].SetPointLowerBits = (uint8_t) tmpVal & 0x07;
+
+	if(_motor != chan) {
+		Motor[chan].SetPoint = 0;
+		Motor[chan].SetPointLowerBits = 0;
+	}
+
+	if(Motor[chan].Version == BLCTRL_OLD) {
+		msg[0] = Motor[chan].SetPoint;
+	} else {
+		msg[0] = Motor[chan].SetPoint;
+		msg[1] = Motor[chan].SetPointLowerBits;
+	}
+
+	set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
+	if(Motor[chan].Version == BLCTRL_OLD) {
+		ret = transfer(&msg[0], 1, nullptr, 0);
+	} else {
+		ret = transfer(&msg[0], 2, nullptr, 0);
+	}
+
+	return ret;
+}
+
+
+int
+MK::control_callback(uintptr_t handle,
+			 uint8_t control_group,
+			 uint8_t control_index,
+			 float &input)
+{
+	const actuator_controls_s *controls = (actuator_controls_s *)handle;
+
+	input = controls->control[control_index];
+	return 0;
+}
+
+int
+MK::ioctl(file *filp, int cmd, unsigned long arg)
+{
+	int ret;
+
+	// XXX disabled, confusing users
+	//debug("ioctl 0x%04x 0x%08x", cmd, arg);
+
+	/* try it as a GPIO ioctl first */
+	ret = gpio_ioctl(filp, cmd, arg);
+
+	if (ret != -ENOTTY)
+		return ret;
+
+	/* if we are in valid PWM mode, try it as a PWM ioctl as well */
+	switch (_mode) {
+	case MODE_2PWM:
+	case MODE_4PWM:
+		ret = pwm_ioctl(filp, cmd, arg);
+		break;
+
+	default:
+		debug("not in a PWM mode");
+		break;
+	}
+
+	/* if nobody wants it, let CDev have it */
+	if (ret == -ENOTTY)
+		ret = CDev::ioctl(filp, cmd, arg);
+
+	return ret;
+}
+
+int
+MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
+{
+	int ret = OK;
+	int channel;
+
+	lock();
+
+	switch (cmd) {
+	case PWM_SERVO_ARM:
+		////up_pwm_servo_arm(true);
+		mk_servo_arm(true);
+		break;
+
+	case PWM_SERVO_DISARM:
+		////up_pwm_servo_arm(false);
+		mk_servo_arm(false);
+		break;
+
+	case PWM_SERVO_SET_UPDATE_RATE:
+		set_pwm_rate(arg);
+		break;
+
+
+	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
+
+		/* fake an update to the selected 'servo' channel */
+		if ((arg >= 0) && (arg <= 255)) {
+			channel = cmd - PWM_SERVO_SET(0);
+			//mk_servo_set(channel, arg);
+		} else {
+			ret = -EINVAL;
+		}
+
+		break;
+
+	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
+		/* copy the current output value from the channel */
+		*(servo_position_t *)arg = cmd - PWM_SERVO_GET(0);
+		break;
+
+	case MIXERIOCGETOUTPUTCOUNT:
+		/*
+		if (_mode == MODE_4PWM) {
+			*(unsigned *)arg = 4;
+		} else {
+			*(unsigned *)arg = 2;
+		}
+		 */
+
+		*(unsigned *)arg = _num_outputs;
+
+		break;
+
+	case MIXERIOCRESET:
+		if (_mixers != nullptr) {
+			delete _mixers;
+			_mixers = nullptr;
+		}
+
+		break;
+
+	case MIXERIOCADDSIMPLE: {
+			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
+
+			SimpleMixer *mixer = new SimpleMixer(control_callback,
+							     (uintptr_t)&_controls, mixinfo);
+
+			if (mixer->check()) {
+				delete mixer;
+				ret = -EINVAL;
+
+			} else {
+				if (_mixers == nullptr)
+					_mixers = new MixerGroup(control_callback,
+								 (uintptr_t)&_controls);
+
+				_mixers->add_mixer(mixer);
+			}
+
+			break;
+		}
+
+	case MIXERIOCLOADBUF: {
+			const char *buf = (const char *)arg;
+			unsigned buflen = strnlen(buf, 1024);
+
+			if (_mixers == nullptr)
+				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+
+			if (_mixers == nullptr) {
+				ret = -ENOMEM;
+
+			} else {
+
+				ret = _mixers->load_from_buf(buf, buflen);
+
+				if (ret != 0) {
+					debug("mixer load failed with %d", ret);
+					delete _mixers;
+					_mixers = nullptr;
+					ret = -EINVAL;
+				}
+			}
+			break;
+		}
+
+	default:
+		ret = -ENOTTY;
+		break;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+void
+MK::gpio_reset(void)
+{
+	/*
+	 * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
+	 * to input mode.
+	 */
+	for (unsigned i = 0; i < _ngpio; i++)
+		stm32_configgpio(_gpio_tab[i].input);
+
+	stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+	stm32_configgpio(GPIO_GPIO_DIR);
+}
+
+void
+MK::gpio_set_function(uint32_t gpios, int function)
+{
+	/*
+	 * GPIOs 0 and 1 must have the same direction as they are buffered
+	 * by a shared 2-port driver.  Any attempt to set either sets both.
+	 */
+	if (gpios & 3) {
+		gpios |= 3;
+
+		/* flip the buffer to output mode if required */
+		if (GPIO_SET_OUTPUT == function)
+			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
+	}
+
+	/* configure selected GPIOs as required */
+	for (unsigned i = 0; i < _ngpio; i++) {
+		if (gpios & (1 << i)) {
+			switch (function) {
+			case GPIO_SET_INPUT:
+				stm32_configgpio(_gpio_tab[i].input);
+				break;
+
+			case GPIO_SET_OUTPUT:
+				stm32_configgpio(_gpio_tab[i].output);
+				break;
+
+			case GPIO_SET_ALT_1:
+				if (_gpio_tab[i].alt != 0)
+					stm32_configgpio(_gpio_tab[i].alt);
+
+				break;
+			}
+		}
+	}
+
+	/* flip buffer to input mode if required */
+	if ((GPIO_SET_INPUT == function) && (gpios & 3))
+		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+}
+
+void
+MK::gpio_write(uint32_t gpios, int function)
+{
+	int value = (function == GPIO_SET) ? 1 : 0;
+
+	for (unsigned i = 0; i < _ngpio; i++)
+		if (gpios & (1 << i))
+			stm32_gpiowrite(_gpio_tab[i].output, value);
+}
+
+uint32_t
+MK::gpio_read(void)
+{
+	uint32_t bits = 0;
+
+	for (unsigned i = 0; i < _ngpio; i++)
+		if (stm32_gpioread(_gpio_tab[i].input))
+			bits |= (1 << i);
+
+	return bits;
+}
+
+int
+MK::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int	ret = OK;
+
+	lock();
+
+	switch (cmd) {
+
+	case GPIO_RESET:
+		gpio_reset();
+		break;
+
+	case GPIO_SET_OUTPUT:
+	case GPIO_SET_INPUT:
+	case GPIO_SET_ALT_1:
+		gpio_set_function(arg, cmd);
+		break;
+
+	case GPIO_SET_ALT_2:
+	case GPIO_SET_ALT_3:
+	case GPIO_SET_ALT_4:
+		ret = -EINVAL;
+		break;
+
+	case GPIO_SET:
+	case GPIO_CLEAR:
+		gpio_write(arg, cmd);
+		break;
+
+	case GPIO_GET:
+		*(uint32_t *)arg = gpio_read();
+		break;
+
+	default:
+		ret = -ENOTTY;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+namespace
+{
+
+enum PortMode {
+	PORT_MODE_UNSET = 0,
+	PORT_FULL_GPIO,
+	PORT_FULL_SERIAL,
+	PORT_FULL_PWM,
+	PORT_GPIO_AND_SERIAL,
+	PORT_PWM_AND_SERIAL,
+	PORT_PWM_AND_GPIO,
+};
+
+enum MappingMode {
+	MAPPING_MK = 0,
+	MAPPING_PX4,
+};
+
+	enum FrameType {
+		FRAME_PLUS = 0,
+		FRAME_X,
+	};
+
+PortMode g_port_mode;
+
+int
+mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype)
+{
+	uint32_t gpio_bits;
+	int shouldStop = 0;
+	MK::Mode servo_mode;
+
+	/* reset to all-inputs */
+	g_mk->ioctl(0, GPIO_RESET, 0);
+
+	gpio_bits = 0;
+	servo_mode = MK::MODE_NONE;
+
+	switch (new_mode) {
+	case PORT_FULL_GPIO:
+	case PORT_MODE_UNSET:
+		/* nothing more to do here */
+		break;
+
+	case PORT_FULL_SERIAL:
+		/* set all multi-GPIOs to serial mode */
+		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
+		break;
+
+	case PORT_FULL_PWM:
+		/* select 4-pin PWM mode */
+		servo_mode = MK::MODE_4PWM;
+		break;
+
+	case PORT_GPIO_AND_SERIAL:
+		/* set RX/TX multi-GPIOs to serial mode */
+		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
+		break;
+
+	case PORT_PWM_AND_SERIAL:
+		/* select 2-pin PWM mode */
+		servo_mode = MK::MODE_2PWM;
+		/* set RX/TX multi-GPIOs to serial mode */
+		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
+		break;
+
+	case PORT_PWM_AND_GPIO:
+		/* select 2-pin PWM mode */
+		servo_mode = MK::MODE_2PWM;
+		break;
+	}
+
+	/* adjust GPIO config for serial mode(s) */
+	if (gpio_bits != 0)
+		g_mk->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
+
+	/* native PX4 addressing) */
+	g_mk->set_px4mode(px4mode);
+
+	/* set frametype (geometry) */
+	g_mk->set_frametype(frametype);
+
+	/* motortest if enabled */
+	g_mk->set_motor_test(motortest);
+
+
+	/* (re)set count of used motors */
+	////g_mk->set_motor_count(motorcount);
+	/* count used motors */
+
+	do {
+		if(g_mk->mk_check_for_blctrl(8, false) != 0) {
+			shouldStop = 4;
+		} else {
+			shouldStop++;
+		}
+		sleep(1);
+	} while ( shouldStop < 3);
+
+	g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true));
+
+	/* (re)set the PWM output mode */
+	g_mk->set_mode(servo_mode);
+
+
+	if ((servo_mode != MK::MODE_NONE) && (update_rate != 0))
+		g_mk->set_pwm_rate(update_rate);
+
+	return OK;
+}
+
+int
+mk_start(unsigned bus, unsigned motors)
+{
+	int ret = OK;
+
+	if (g_mk == nullptr) {
+
+		g_mk = new MK(bus);
+
+		if (g_mk == nullptr) {
+			ret = -ENOMEM;
+
+		} else {
+			ret = g_mk->init(motors);
+
+			if (ret != OK) {
+				delete g_mk;
+				g_mk = nullptr;
+			}
+		}
+	}
+
+	return ret;
+}
+
+
+} // namespace
+
+extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]);
+
+int
+mkblctrl_main(int argc, char *argv[])
+{
+	PortMode port_mode = PORT_FULL_PWM;
+	int pwm_update_rate_in_hz = UPDATE_RATE;
+	int motorcount = 8;
+	int bus = 1;
+	int px4mode = MAPPING_PX4;
+	int frametype = FRAME_PLUS;	// + plus is default
+	bool motortest = false;
+	bool showHelp = false;
+	bool newMode = false;
+
+	/*
+	 * optional parameters
+	 */
+	for (int i = 1; i < argc; i++) {
+
+		/* look for the optional i2c bus parameter */
+		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
+			if (argc > i + 1) {
+				bus = atoi(argv[i + 1]);
+				newMode = true;
+				} else {
+				errx(1, "missing argument for i2c bus (-b)");
+				return 1;
+			}
+		}
+
+		/* look for the optional frame parameter */
+		if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) {
+			if (argc > i + 1) {
+				if(strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) {
+					px4mode = MAPPING_MK;
+					newMode = true;
+					if(strcmp(argv[i + 1], "+") == 0) {
+						frametype = FRAME_PLUS;
+					} else {
+						frametype = FRAME_X;
+					}
+				} else {
+					errx(1, "only + or x for frametype supported !");
+				}
+			} else {
+				errx(1, "missing argument for mkmode (-mkmode)");
+				return 1;
+			}
+		}
+
+		/* look for the optional test parameter */
+		if (strcmp(argv[i], "-t") == 0) {
+			motortest = true;
+			newMode = true;
+		}
+
+		/* look for the optional -h --help parameter */
+		if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
+			showHelp == true;
+		}
+
+	}
+
+	if(showHelp) {
+		fprintf(stderr, "mkblctrl: help:\n");
+		fprintf(stderr, "  [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n");
+		exit(1);
+	}
+
+
+	if (g_mk == nullptr) {
+		if (mk_start(bus, motorcount) != OK) {
+			errx(1, "failed to start the MK-BLCtrl driver");
+		} else {
+			newMode = true;
+		}
+	}
+
+
+	/* parameter set ? */
+	if (newMode) {
+		/* switch parameter */
+		return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype);
+	}
+
+	/* test, etc. here g*/
+
+	exit(1);
+}
diff --git a/src/drivers/mkblctrl/module.mk b/src/drivers/mkblctrl/module.mk
new file mode 100644
index 000000000..3ac263b00
--- /dev/null
+++ b/src/drivers/mkblctrl/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.
+#
+############################################################################
+
+#
+# Interface driver for the Mikrokopter BLCtrl
+#
+
+MODULE_COMMAND		= mkblctrl
+
+SRCS				= mkblctrl.cpp
+
+INCLUDE_DIRS		+= $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index d3865f053..e199a5998 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -574,7 +574,11 @@ PX4FMU::task_main()
 			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
 
 			/* update PWM servo armed status if armed and not locked down */
-			up_pwm_servo_arm(aa.armed && !aa.lockdown);
+			bool set_armed = aa.armed && !aa.lockdown;
+			if (set_armed != _armed) {
+				_armed = set_armed;
+				up_pwm_servo_arm(set_armed);
+			}
 		}
 
 #ifdef FMU_HAVE_PPM
@@ -675,6 +679,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		up_pwm_servo_arm(true);
 		break;
 
+	case PWM_SERVO_SET_ARM_OK:
+	case PWM_SERVO_CLEAR_ARM_OK:
+		// these are no-ops, as no safety switch
+		break;
+
 	case PWM_SERVO_DISARM:
 		up_pwm_servo_arm(false);
 		break;
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 4c2f1736f..a40142792 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -109,6 +109,14 @@ public:
 	int      set_update_rate(int rate);
 
 	/**
+	* Set the battery current scaling and bias
+	*
+	* @param amp_per_volt
+	* @param amp_bias
+	*/
+	void      set_battery_current_scaling(float amp_per_volt, float amp_bias);
+
+	/**
 	* Print the current status of IO
 	*/
 	void			print_status();
@@ -151,6 +159,10 @@ private:
 
 	bool			_primary_pwm_device;	///< true if we are the default PWM output
 
+	float			_battery_amp_per_volt;
+	float			_battery_amp_bias;
+	float			_battery_mamphour_total;
+	uint64_t		_battery_last_timestamp;
 
 	/**
 	 * Trampoline to the worker task
@@ -314,6 +326,10 @@ PX4IO::PX4IO() :
 	_to_actuators_effective(0),
 	_to_outputs(0),
 	_to_battery(0),
+	_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
+	_battery_amp_bias(0),
+	_battery_mamphour_total(0),
+	_battery_last_timestamp(0),
 	_primary_pwm_device(false)
 {
 	/* we need this potentially before it could be set in task_main */
@@ -400,7 +416,7 @@ PX4IO::init()
 	 * already armed, and has been configured for in-air restart
 	 */
 	if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
-	    (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) {
+	    (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 
 	    	mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
 
@@ -484,10 +500,9 @@ PX4IO::init()
 
 		/* dis-arm IO before touching anything */
 		io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 
-			PX4IO_P_SETUP_ARMING_ARM_OK |
+			PX4IO_P_SETUP_ARMING_FMU_ARMED |
 			PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
-			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
-			PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
+			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
 
 		/* publish RC config to IO */
 		ret = io_set_rc_config();
@@ -686,16 +701,18 @@ PX4IO::io_set_arming_state()
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-	if (armed.armed) {
-		set |= PX4IO_P_SETUP_ARMING_ARM_OK;
+	if (armed.armed && !armed.lockdown) {
+		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	} else {
-		clear |= PX4IO_P_SETUP_ARMING_ARM_OK;
+		clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	}
-	if (vstatus.flag_vector_flight_mode_ok) {
-		set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;
+
+	if (armed.ready_to_arm) {
+		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 	} else {
-		clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;					
+		clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 	}
+
 	if (vstatus.flag_external_manual_override_ok) {
 		set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
 	} else {
@@ -884,11 +901,22 @@ PX4IO::io_get_status()
 		/* voltage is scaled to mV */
 		battery_status.voltage_v = regs[2] / 1000.0f;
 
-		/* current scaling should be to cA in order to avoid limiting at 65A */
-		battery_status.current_a = regs[3] / 100.f;
+		/*
+		  regs[3] contains the raw ADC count, as 12 bit ADC
+		  value, with full range being 3.3v
+		 */
+		battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt;
+		battery_status.current_a += _battery_amp_bias;
 
-		/* this requires integration over time - not currently implemented */
-		battery_status.discharged_mah = -1.0f;
+		/*
+		  integrate battery over time to get total mAh used
+		 */
+		if (_battery_last_timestamp != 0) {
+			_battery_mamphour_total += battery_status.current_a * 
+				(battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
+		}
+		battery_status.discharged_mah = _battery_mamphour_total;
+		_battery_last_timestamp = battery_status.timestamp;
 
 		/* lazily publish the battery voltage */
 		if (_to_battery > 0) {
@@ -1245,9 +1273,14 @@ PX4IO::print_status()
 		((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST)      ? " FMU_LOST" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST)       ? " RC_LOST" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR)     ? " PWM_ERROR" : ""));
-	printf("vbatt %u ibatt %u\n",
-		io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
-		io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT));
+	printf("vbatt %u ibatt %u vbatt scale %u\n",
+	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
+	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
+	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
+	printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n",
+	       (double)_battery_amp_per_volt,
+	       (double)_battery_amp_bias,
+	       (double)_battery_mamphour_total);
 	printf("actuators");
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
@@ -1279,19 +1312,15 @@ PX4IO::print_status()
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
 	printf("arming 0x%04x%s%s%s%s\n",
 		arming,
-		((arming & PX4IO_P_SETUP_ARMING_ARM_OK)             ? " ARM_OK" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK)   ? " VECTOR_FLIGHT_OK" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""));
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
-	printf("vbatt scale %u ibatt scale %u ibatt bias %u\n",
-		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE),
-		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE),
-		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS));
 	printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
 	printf("controls");
 	for (unsigned i = 0; i < _max_controls; i++)
@@ -1326,21 +1355,27 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 	switch (cmd) {
 	case PWM_SERVO_ARM:
 		/* set the 'armed' bit */
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_ARM_OK);
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED);
+		break;
+
+	case PWM_SERVO_SET_ARM_OK:
+		/* set the 'OK to arm' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_IO_ARM_OK);
+		break;
+
+	case PWM_SERVO_CLEAR_ARM_OK:
+		/* clear the 'OK to arm' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_IO_ARM_OK, 0);
 		break;
 
 	case PWM_SERVO_DISARM:
 		/* clear the 'armed' bit */
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0);
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
 		break;
 
 	case PWM_SERVO_SET_UPDATE_RATE:
 		/* set the requested alternate rate */
-		if ((arg >= 50) && (arg <= 400)) {	/* TODO: we could go higher for e.g. TurboPWM */
-			ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
-		} else {
-			ret = -EINVAL;
-		}
+		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
 		break;
 
 	case PWM_SERVO_SELECT_UPDATE_RATE: {
@@ -1525,6 +1560,12 @@ PX4IO::set_update_rate(int rate)
 	return 0;
 }
 
+void
+PX4IO::set_battery_current_scaling(float amp_per_volt, float amp_bias)
+{
+	_battery_amp_per_volt = amp_per_volt;
+	_battery_amp_bias = amp_bias;
+}
 
 extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
 
@@ -1662,6 +1703,18 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "current")) {
+		if (g_dev != nullptr) {
+			if ((argc > 3)) {
+				g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
+			} else {
+				errx(1, "missing argument (apm_per_volt, amp_offset)");
+				return 1;
+			}
+		}
+		exit(0);
+	}
+
 	if (!strcmp(argv[1], "recovery")) {
 
 		if (g_dev != nullptr) {
@@ -1789,5 +1842,5 @@ px4io_main(int argc, char *argv[])
 		monitor();
 
 	out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit' or 'update'");
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current' or 'update'");
 }
diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp
index 9a67875e8..15524c3ae 100644
--- a/src/drivers/px4io/uploader.cpp
+++ b/src/drivers/px4io/uploader.cpp
@@ -127,6 +127,8 @@ PX4IO_Uploader::upload(const char *filenames[])
 	if (ret != OK) {
 		/* this is immediately fatal */
 		log("bootloader not responding");
+		close(_io_fd);
+		_io_fd = -1;
 		return -EIO;
 	}
 
@@ -145,18 +147,25 @@ PX4IO_Uploader::upload(const char *filenames[])
 
 	if (filename == NULL) {
 		log("no firmware found");
+		close(_io_fd);
+		_io_fd = -1;
 		return -ENOENT;
 	}
 
 	struct stat st;
 	if (stat(filename, &st) != 0) {
 		log("Failed to stat %s - %d\n", filename, (int)errno);
+		close(_io_fd);
+		_io_fd = -1;
 		return -errno;
 	}
 	fw_size = st.st_size;
 
-	if (_fw_fd == -1)
+	if (_fw_fd == -1) {
+		close(_io_fd);
+		_io_fd = -1;
 		return -ENOENT;
+	}
 
 	/* do the usual program thing - allow for failure */
 	for (unsigned retries = 0; retries < 1; retries++) {
@@ -167,6 +176,8 @@ PX4IO_Uploader::upload(const char *filenames[])
 			if (ret != OK) {
 				/* this is immediately fatal */
 				log("bootloader not responding");
+				close(_io_fd);
+				_io_fd = -1;
 				return -EIO;
 			}
 		}
@@ -178,6 +189,8 @@ PX4IO_Uploader::upload(const char *filenames[])
 				log("found bootloader revision: %d", bl_rev);
 			} else {
 				log("found unsupported bootloader revision %d, exiting", bl_rev);
+				close(_io_fd);
+				_io_fd = -1;
 				return OK;
 			}
 		}
@@ -221,6 +234,8 @@ PX4IO_Uploader::upload(const char *filenames[])
 	}
 
 	close(_fw_fd);
+	close(_io_fd);
+	_io_fd = -1;
 	return ret;
 }
 
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
new file mode 100644
index 000000000..1753070e2
--- /dev/null
+++ b/src/examples/fixedwing_control/main.c
@@ -0,0 +1,474 @@
+/****************************************************************************
+ *
+ *   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 main.c
+ * Implementation of a fixed wing attitude controller. This file is a complete
+ * fixed wing controller flying manual attitude control or auto waypoint control.
+ * There is no need to touch any other system components to extend / modify the
+ * complete control architecture.
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/debug_key_value.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+#include <systemlib/pid/pid.h>
+#include <systemlib/geo/geo.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+
+/* process-specific header files */
+#include "params.h"
+
+/* Prototypes */
+/**
+ * Daemon management function.
+ */
+__EXPORT int ex_fixedwing_control_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of daemon.
+ */
+int fixedwing_control_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
+	float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp, 
+	struct actuator_controls_s *actuators);
+
+void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
+	const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp);
+
+/* Variables */
+static bool thread_should_exit = false;		/**< Daemon exit flag */
+static bool thread_running = false;		/**< Daemon status flag */
+static int deamon_task;				/**< Handle of deamon task / thread */
+static struct params p;
+static struct param_handles ph;
+
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
+	float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp, 
+	struct actuator_controls_s *actuators)
+{
+
+	/* 
+	 * The PX4 architecture provides a mixer outside of the controller.
+	 * The mixer is fed with a default vector of actuator controls, representing
+	 * moments applied to the vehicle frame. This vector
+	 * is structured as:
+	 *
+	 * Control Group 0 (attitude):
+	 * 
+	 *    0  -  roll   (-1..+1)
+	 *    1  -  pitch  (-1..+1)
+	 *    2  -  yaw    (-1..+1)
+	 *    3  -  thrust ( 0..+1)
+	 *    4  -  flaps  (-1..+1)
+	 *    ...
+	 *
+	 * Control Group 1 (payloads / special):
+	 *
+	 *    ...
+	 */
+
+	/*
+	 * Calculate roll error and apply P gain
+	 */
+	float roll_err = att->roll - att_sp->roll_body;
+	actuators->control[0] = roll_err * p.roll_p;
+
+	/*
+	 * Calculate pitch error and apply P gain
+	 */
+	float pitch_err = att->pitch - att_sp->pitch_body;
+	actuators->control[1] = pitch_err * p.pitch_p;
+}
+
+void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
+	const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp)
+{
+
+	/*
+	 * Calculate heading error of current position to desired position
+	 */
+
+	/* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution */
+	float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d);
+
+	/* calculate heading error */
+	float yaw_err = att->yaw - bearing;
+	/* apply control gain */
+	att_sp->roll_body = yaw_err * p.hdng_p;
+}
+
+/* Main Thread */
+int fixedwing_control_thread_main(int argc, char *argv[])
+{
+	/* read arguments */
+	bool verbose = false;
+
+	for (int i = 1; i < argc; i++) {
+		if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
+			verbose = true;
+		}
+	}
+
+	/* welcome user (warnx prints a line, including an appended\n, with variable arguments */
+	warnx("[example fixedwing control] started");
+
+	/* initialize parameters, first the handles, then the values */
+	parameters_init(&ph);
+	parameters_update(&ph, &p);
+
+	/* declare and safely initialize all structs to zero */
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+	struct vehicle_attitude_setpoint_s att_sp;
+	memset(&att_sp, 0, sizeof(att_sp));
+	struct vehicle_rates_setpoint_s rates_sp;
+	memset(&rates_sp, 0, sizeof(rates_sp));
+	struct vehicle_global_position_s global_pos;
+	memset(&global_pos, 0, sizeof(global_pos));
+	struct manual_control_setpoint_s manual_sp;
+	memset(&manual_sp, 0, sizeof(manual_sp));
+	struct vehicle_status_s vstatus;
+	memset(&vstatus, 0, sizeof(vstatus));
+	struct vehicle_global_position_setpoint_s global_sp;
+	memset(&global_sp, 0, sizeof(global_sp));
+
+	/* output structs */
+	struct actuator_controls_s actuators;
+	memset(&actuators, 0, sizeof(actuators));
+
+
+	/* publish actuator controls */
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
+		actuators.control[i] = 0.0f;
+	}
+
+	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
+	orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+
+	/* subscribe */
+	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int global_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
+	int param_sub = orb_subscribe(ORB_ID(parameter_update));
+
+	/* Setup of loop */
+	float gyro[3] = {0.0f, 0.0f, 0.0f};
+	float speed_body[3] = {0.0f, 0.0f, 0.0f};
+	struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
+				{ .fd = att_sub, .events = POLLIN }};
+
+	while (!thread_should_exit) {
+
+		/*
+		 * Wait for a sensor or param update, check for exit condition every 500 ms.
+		 * This means that the execution will block here without consuming any resources,
+		 * but will continue to execute the very moment a new attitude measurement or
+		 * a param update is published. So no latency in contrast to the polling
+		 * design pattern (do not confuse the poll() system call with polling).
+		 *
+		 * This design pattern makes the controller also agnostic of the attitude
+		 * update speed - it runs as fast as the attitude updates with minimal latency.
+		 */
+		int ret = poll(fds, 2, 500);
+
+		if (ret < 0) {
+			/* poll error, this will not really happen in practice */
+			warnx("poll error");
+
+		} else if (ret == 0) {
+			/* no return value = nothing changed for 500 ms, ignore */
+		} else {
+
+			/* only update parameters if they changed */
+			if (fds[0].revents & POLLIN) {
+				/* read from param to clear updated flag (uORB API requirement) */
+				struct parameter_update_s update;
+				orb_copy(ORB_ID(parameter_update), param_sub, &update);
+
+				/* if a param update occured, re-read our parameters */
+				parameters_update(&ph, &p);
+			}
+
+			/* only run controller if attitude changed */
+			if (fds[1].revents & POLLIN) {
+
+
+				/* Check if there is a new position measurement or position setpoint */
+				bool pos_updated;
+				orb_check(global_pos_sub, &pos_updated);
+				bool global_sp_updated;
+				orb_check(global_sp_sub, &global_sp_updated);
+
+				/* get a local copy of attitude */
+				orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+
+				if (global_sp_updated)
+					orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp);
+
+				if (pos_updated) {
+					orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
+
+					if (att.R_valid) {
+						speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
+						speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
+						speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
+
+					} else {
+						speed_body[0] = 0;
+						speed_body[1] = 0;
+						speed_body[2] = 0;
+
+						warnx("Did not get a valid R\n");
+					}
+				}
+
+				orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
+				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
+
+				gyro[0] = att.rollspeed;
+				gyro[1] = att.pitchspeed;
+				gyro[2] = att.yawspeed;
+
+				/* control */
+
+				if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
+				    vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
+
+				    	/* simple heading control */
+				    	control_heading(&global_pos, &global_sp, &att, &att_sp);
+
+				    	/* nail pitch and yaw (rudder) to zero. This example only controls roll (index 0) */
+				    	actuators.control[1] = 0.0f;
+				    	actuators.control[2] = 0.0f;
+					
+					/* simple attitude control */
+					control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
+
+					/* pass through throttle */
+					actuators.control[3] = att_sp.thrust;
+
+					/* set flaps to zero */
+					actuators.control[4] = 0.0f;
+
+				} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
+					if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+
+						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
+						if (vstatus.rc_signal_lost) {
+
+							/* put plane into loiter */
+							att_sp.roll_body = 0.3f;
+							att_sp.pitch_body = 0.0f;
+
+							/* limit throttle to 60 % of last value if sane */
+							if (isfinite(manual_sp.throttle) &&
+							    (manual_sp.throttle >= 0.0f) &&
+							    (manual_sp.throttle <= 1.0f)) {
+								att_sp.thrust = 0.6f * manual_sp.throttle;
+
+							} else {
+								att_sp.thrust = 0.0f;
+							}
+
+							att_sp.yaw_body = 0;
+
+							// XXX disable yaw control, loiter
+
+						} else {
+
+							att_sp.roll_body = manual_sp.roll;
+							att_sp.pitch_body = manual_sp.pitch;
+							att_sp.yaw_body = 0;
+							att_sp.thrust = manual_sp.throttle;
+						}
+
+						att_sp.timestamp = hrt_absolute_time();
+
+						/* attitude control */
+						control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
+
+						/* pass through throttle */
+						actuators.control[3] = att_sp.thrust;
+
+						/* pass through flaps */
+						if (isfinite(manual_sp.flaps)) {
+							actuators.control[4] = manual_sp.flaps;
+
+						} else {
+							actuators.control[4] = 0.0f;
+						}
+
+					} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+						/* directly pass through values */
+						actuators.control[0] = manual_sp.roll;
+						/* positive pitch means negative actuator -> pull up */
+						actuators.control[1] = manual_sp.pitch;
+						actuators.control[2] = manual_sp.yaw;
+						actuators.control[3] = manual_sp.throttle;
+
+						if (isfinite(manual_sp.flaps)) {
+							actuators.control[4] = manual_sp.flaps;
+
+						} else {
+							actuators.control[4] = 0.0f;
+						}
+					}
+				}
+
+				/* publish rates */
+				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
+
+				/* sanity check and publish actuator outputs */
+				if (isfinite(actuators.control[0]) &&
+				    isfinite(actuators.control[1]) &&
+				    isfinite(actuators.control[2]) &&
+				    isfinite(actuators.control[3])) {
+					orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+				}
+			}
+		}
+	}
+
+	printf("[ex_fixedwing_control] exiting, stopping all motors.\n");
+	thread_running = false;
+
+	/* kill all outputs */
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
+		actuators.control[i] = 0.0f;
+
+	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+
+	fflush(stdout);
+
+	return 0;
+}
+
+/* Startup Functions */
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr, "usage: ex_fixedwing_control {start|stop|status}\n\n");
+	exit(1);
+}
+
+/**
+ * The daemon 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 ex_fixedwing_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("ex_fixedwing_control already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn("ex_fixedwing_control",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 20,
+					 2048,
+					 fixedwing_control_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		thread_running = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tex_fixedwing_control is running\n");
+
+		} else {
+			printf("\tex_fixedwing_control not started\n");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+
+
diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk
new file mode 100644
index 000000000..d2c48934f
--- /dev/null
+++ b/src/examples/fixedwing_control/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Fixedwing Attitude Control Demo / Example Application
+#
+
+MODULE_COMMAND	= ex_fixedwing_control
+
+SRCS		= main.c \
+		  params.c
diff --git a/src/examples/fixedwing_control/params.c b/src/examples/fixedwing_control/params.c
new file mode 100644
index 000000000..8042c74f5
--- /dev/null
+++ b/src/examples/fixedwing_control/params.c
@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   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 params.c
+ * 
+ * Parameters for fixedwing demo
+ */
+
+#include "params.h"
+
+/* controller parameters, use max. 15 characters for param name! */
+
+/**
+ *
+ */
+PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.2f);
+
+/**
+ *
+ */
+PARAM_DEFINE_FLOAT(EXFW_ROLL_P, 0.2f);
+
+/**
+ *
+ */
+PARAM_DEFINE_FLOAT(EXFW_PITCH_P, 0.2f);
+
+int parameters_init(struct param_handles *h)
+{
+	/* PID parameters */
+	h->hdng_p 	=	param_find("EXFW_HDNG_P");
+	h->roll_p 	=	param_find("EXFW_ROLL_P");
+	h->pitch_p 	=	param_find("EXFW_PITCH_P");
+
+	return OK;
+}
+
+int parameters_update(const struct param_handles *h, struct params *p)
+{
+	param_get(h->hdng_p, &(p->hdng_p));
+	param_get(h->roll_p, &(p->roll_p));
+	param_get(h->pitch_p, &(p->pitch_p));
+
+	return OK;
+}
diff --git a/src/examples/fixedwing_control/params.h b/src/examples/fixedwing_control/params.h
new file mode 100644
index 000000000..4ae8e90ec
--- /dev/null
+++ b/src/examples/fixedwing_control/params.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   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 params.h
+ * 
+ * Definition of parameters for fixedwing example
+ */
+
+#include <systemlib/param/param.h>
+
+struct params {
+	float hdng_p;
+	float roll_p;
+	float pitch_p;
+};
+
+struct param_handles {
+	param_t hdng_p;
+	param_t roll_p;
+	param_t pitch_p;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct param_handles *h, struct params *p);
diff --git a/src/examples/px4_daemon_app/module.mk b/src/examples/px4_daemon_app/module.mk
index d23c19b75..5f8aa73d5 100644
--- a/src/examples/px4_daemon_app/module.mk
+++ b/src/examples/px4_daemon_app/module.mk
@@ -37,4 +37,4 @@
 
 MODULE_COMMAND		= px4_daemon_app
 
-SRCS				= px4_daemon_app.c
+SRCS			= px4_daemon_app.c
diff --git a/src/examples/px4_mavlink_debug/module.mk b/src/examples/px4_mavlink_debug/module.mk
index 3d400fdbf..fefd61496 100644
--- a/src/examples/px4_mavlink_debug/module.mk
+++ b/src/examples/px4_mavlink_debug/module.mk
@@ -37,4 +37,4 @@
 
 MODULE_COMMAND		= px4_mavlink_debug
 
-SRCS				= px4_mavlink_debug.c
\ No newline at end of file
+SRCS			= px4_mavlink_debug.c
\ No newline at end of file
diff --git a/src/examples/px4_simple_app/module.mk b/src/examples/px4_simple_app/module.mk
index 2c102fa50..32b06c3a5 100644
--- a/src/examples/px4_simple_app/module.mk
+++ b/src/examples/px4_simple_app/module.mk
@@ -37,4 +37,4 @@
 
 MODULE_COMMAND		= px4_simple_app
 
-SRCS				= px4_simple_app.c
+SRCS			= px4_simple_app.c
diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c
new file mode 100644
index 000000000..d79dd93dd
--- /dev/null
+++ b/src/modules/commander/accelerometer_calibration.c
@@ -0,0 +1,414 @@
+/*
+ * accelerometer_calibration.c
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *
+ * Transform acceleration vector to true orientation and scale
+ *
+ * * * * Model * * *
+ * accel_corr = accel_T * (accel_raw - accel_offs)
+ *
+ * accel_corr[3] - fully corrected acceleration vector in body frame
+ * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform
+ * accel_raw[3]  - raw acceleration vector
+ * accel_offs[3] - acceleration offset vector
+ *
+ * * * * Calibration * * *
+ *
+ * Reference vectors
+ * accel_corr_ref[6][3] = [  g  0  0 ]     // nose up
+ *                        | -g  0  0 |     // nose down
+ *                        |  0  g  0 |     // left side down
+ *                        |  0 -g  0 |     // right side down
+ *                        |  0  0  g |     // on back
+ *                        [  0  0 -g ]     // level
+ * accel_raw_ref[6][3]
+ *
+ * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5
+ *
+ * 6 reference vectors * 3 axes = 18 equations
+ * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants
+ *
+ * Find accel_offs
+ *
+ * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2
+ *
+ *
+ * Find accel_T
+ *
+ * 9 unknown constants
+ * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations
+ *
+ * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2
+ *
+ * Solve separate system for each row of accel_T:
+ *
+ * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2
+ *
+ * A * x = b
+ *
+ * x = [ accel_T[0][i] ]
+ *     | accel_T[1][i] |
+ *     [ accel_T[2][i] ]
+ *
+ * b = [ accel_corr_ref[0][i] ]	// One measurement per axis is enough
+ *     | accel_corr_ref[2][i] |
+ *     [ accel_corr_ref[4][i] ]
+ *
+ * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2
+ *
+ * Matrix A is common for all three systems:
+ * A = [ a[0][0]  a[0][1]  a[0][2] ]
+ *     | a[2][0]  a[2][1]  a[2][2] |
+ *     [ a[4][0]  a[4][1]  a[4][2] ]
+ *
+ * x = A^-1 * b
+ *
+ * accel_T = A^-1 * g
+ * g = 9.80665
+ */
+
+#include "accelerometer_calibration.h"
+
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_accel.h>
+#include <systemlib/conversions.h>
+#include <mavlink/mavlink_log.h>
+
+void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
+int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
+int detect_orientation(int mavlink_fd, int sub_sensor_combined);
+int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
+int mat_invert3(float src[3][3], float dst[3][3]);
+int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
+
+void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) {
+	/* announce change */
+	mavlink_log_info(mavlink_fd, "accel calibration started");
+	/* set to accel calibration mode */
+	status->flag_preflight_accel_calibration = true;
+	state_machine_publish(status_pub, status, mavlink_fd);
+
+	/* measure and calculate offsets & scales */
+	float accel_offs[3];
+	float accel_scale[3];
+	int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale);
+
+	if (res == OK) {
+		/* measurements complete successfully, set parameters */
+		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0]))
+			|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1]))
+			|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2]))
+			|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0]))
+			|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1]))
+			|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) {
+			mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
+		}
+
+		int fd = open(ACCEL_DEVICE_PATH, 0);
+		struct accel_scale ascale = {
+			accel_offs[0],
+			accel_scale[0],
+			accel_offs[1],
+			accel_scale[1],
+			accel_offs[2],
+			accel_scale[2],
+		};
+
+		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
+			warn("WARNING: failed to set scale / offsets for accel");
+
+		close(fd);
+
+		/* auto-save to EEPROM */
+		int save_ret = param_save_default();
+
+		if (save_ret != 0) {
+			warn("WARNING: auto-save of params to storage failed");
+		}
+
+		mavlink_log_info(mavlink_fd, "accel calibration done");
+		tune_confirm();
+		sleep(2);
+		tune_confirm();
+		sleep(2);
+		/* third beep by cal end routine */
+	} else {
+		/* measurements error */
+		mavlink_log_info(mavlink_fd, "accel calibration aborted");
+		tune_error();
+		sleep(2);
+	}
+
+	/* exit accel calibration mode */
+	status->flag_preflight_accel_calibration = false;
+	state_machine_publish(status_pub, status, mavlink_fd);
+}
+
+int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
+	const int samples_num = 2500;
+	float accel_ref[6][3];
+	bool data_collected[6] = { false, false, false, false, false, false };
+	const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
+
+	/* reset existing calibration */
+	int fd = open(ACCEL_DEVICE_PATH, 0);
+	struct accel_scale ascale_null = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+	int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null);
+	close(fd);
+
+	if (OK != ioctl_res) {
+		warn("ERROR: failed to set scale / offsets for accel");
+		return ERROR;
+	}
+
+	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+	while (true) {
+		bool done = true;
+		char str[80];
+		int str_ptr;
+		str_ptr = sprintf(str, "keep vehicle still:");
+		for (int i = 0; i < 6; i++) {
+			if (!data_collected[i]) {
+				str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]);
+				done = false;
+			}
+		}
+		if (done)
+			break;
+		mavlink_log_info(mavlink_fd, str);
+
+		int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
+		if (orient < 0)
+			return ERROR;
+
+		sprintf(str, "meas started: %s", orientation_strs[orient]);
+		mavlink_log_info(mavlink_fd, str);
+		read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
+		str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]);
+		mavlink_log_info(mavlink_fd, str);
+		data_collected[orient] = true;
+		tune_confirm();
+	}
+	close(sensor_combined_sub);
+
+	/* calculate offsets and rotation+scale matrix */
+	float accel_T[3][3];
+	int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
+	if (res != 0) {
+		mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error");
+		return ERROR;
+	}
+
+	/* convert accel transform matrix to scales,
+	 * rotation part of transform matrix is not used by now
+	 */
+	for (int i = 0; i < 3; i++) {
+		accel_scale[i] = accel_T[i][i];
+	}
+
+	return OK;
+}
+
+/*
+ * Wait for vehicle become still and detect it's orientation.
+ *
+ * @return 0..5 according to orientation when vehicle is still and ready for measurements,
+ * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
+ */
+int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
+	struct sensor_combined_s sensor;
+	/* exponential moving average of accel */
+	float accel_ema[3] = { 0.0f, 0.0f, 0.0f };
+	/* max-hold dispersion of accel */
+	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 accel error threshold to 5m/s^2 */
+	float accel_err_thr = 5.0f;
+	/* still time required in us */
+	int64_t still_time = 2000000;
+	struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
+
+	hrt_abstime t_start = hrt_absolute_time();
+	/* set timeout to 30s */
+	hrt_abstime timeout = 30000000;
+	hrt_abstime t_timeout = t_start + timeout;
+	hrt_abstime t = t_start;
+	hrt_abstime t_prev = t_start;
+	hrt_abstime t_still = 0;
+	while (true) {
+		/* wait blocking for new data */
+		int poll_ret = poll(fds, 1, 1000);
+		if (poll_ret) {
+			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor);
+			t = hrt_absolute_time();
+			float dt = (t - t_prev) / 1000000.0f;
+			t_prev = t;
+			float w = dt / ema_len;
+			for (int i = 0; i < 3; i++) {
+				accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w;
+				float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i];
+				d = d * d;
+				accel_disp[i] = accel_disp[i] * (1.0f - w);
+				if (d > accel_disp[i])
+					accel_disp[i] = d;
+			}
+			/* still detector with hysteresis */
+			if (  accel_disp[0] < still_thr2 &&
+				  accel_disp[1] < still_thr2 &&
+				  accel_disp[2] < still_thr2 ) {
+				/* is still now */
+				if (t_still == 0) {
+					/* first time */
+					mavlink_log_info(mavlink_fd, "still...");
+					t_still = t;
+					t_timeout = t + timeout;
+				} else {
+					/* still since t_still */
+					if ((int64_t) t - (int64_t) t_still > still_time) {
+						/* vehicle is still, exit from the loop to detection of its orientation */
+						break;
+					}
+				}
+			} else if ( accel_disp[0] > still_thr2 * 2.0f ||
+					    accel_disp[1] > still_thr2 * 2.0f ||
+					    accel_disp[2] > still_thr2 * 2.0f) {
+				/* not still, reset still start time */
+				if (t_still != 0) {
+					mavlink_log_info(mavlink_fd, "moving...");
+					t_still = 0;
+				}
+			}
+		} else if (poll_ret == 0) {
+			/* any poll failure for 1s is a reason to abort */
+			mavlink_log_info(mavlink_fd, "ERROR: poll failure");
+			return -3;
+		}
+		if (t > t_timeout) {
+			mavlink_log_info(mavlink_fd, "ERROR: timeout");
+			return -1;
+		}
+	}
+
+	if (  fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabs(accel_ema[1]) < accel_err_thr &&
+		  fabs(accel_ema[2]) < accel_err_thr  )
+		return 0;	// [ g, 0, 0 ]
+	if (  fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabs(accel_ema[1]) < accel_err_thr &&
+		  fabs(accel_ema[2]) < accel_err_thr  )
+		return 1;	// [ -g, 0, 0 ]
+	if (  fabs(accel_ema[0]) < accel_err_thr &&
+		  fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabs(accel_ema[2]) < accel_err_thr  )
+		return 2;	// [ 0, g, 0 ]
+	if (  fabs(accel_ema[0]) < accel_err_thr &&
+		  fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabs(accel_ema[2]) < accel_err_thr  )
+		return 3;	// [ 0, -g, 0 ]
+	if (  fabs(accel_ema[0]) < accel_err_thr &&
+		  fabs(accel_ema[1]) < accel_err_thr &&
+		  fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr  )
+		return 4;	// [ 0, 0, g ]
+	if (  fabs(accel_ema[0]) < accel_err_thr &&
+		  fabs(accel_ema[1]) < accel_err_thr &&
+		  fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr  )
+		return 5;	// [ 0, 0, -g ]
+
+	mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
+
+	return -2;	// Can't detect orientation
+}
+
+/*
+ * Read specified number of accelerometer samples, calculate average and dispersion.
+ */
+int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
+	struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } };
+	int count = 0;
+	float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
+
+	while (count < samples_num) {
+		int poll_ret = poll(fds, 1, 1000);
+		if (poll_ret == 1) {
+			struct sensor_combined_s sensor;
+			orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+			for (int i = 0; i < 3; i++)
+				accel_sum[i] += sensor.accelerometer_m_s2[i];
+			count++;
+		} else {
+			return ERROR;
+		}
+	}
+
+	for (int i = 0; i < 3; i++) {
+		accel_avg[i] = accel_sum[i] / count;
+	}
+
+	return OK;
+}
+
+int mat_invert3(float src[3][3], float dst[3][3]) {
+	float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
+			    src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
+			    src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
+	if (det == 0.0)
+		return ERROR;	// Singular matrix
+
+	dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
+	dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
+	dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det;
+	dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det;
+	dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det;
+	dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det;
+	dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det;
+	dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det;
+	dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det;
+
+	return OK;
+}
+
+int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) {
+	/* calculate offsets */
+	for (int i = 0; i < 3; i++) {
+		accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2;
+	}
+
+	/* fill matrix A for linear equations system*/
+	float mat_A[3][3];
+	memset(mat_A, 0, sizeof(mat_A));
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			float a = accel_ref[i * 2][j] - accel_offs[j];
+			mat_A[i][j] = a;
+		}
+	}
+
+	/* calculate inverse matrix for A */
+	float mat_A_inv[3][3];
+	if (mat_invert3(mat_A, mat_A_inv) != OK)
+		return ERROR;
+
+	/* copy results to accel_T */
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			/* simplify matrices mult because b has only one non-zero element == g at index i */
+			accel_T[j][i] = mat_A_inv[j][i] * g;
+		}
+	}
+
+	return OK;
+}
diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h
new file mode 100644
index 000000000..a11cf93d3
--- /dev/null
+++ b/src/modules/commander/accelerometer_calibration.h
@@ -0,0 +1,16 @@
+/*
+ * accelerometer_calibration.h
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ */
+
+#ifndef ACCELEROMETER_CALIBRATION_H_
+#define ACCELEROMETER_CALIBRATION_H_
+
+#include <stdint.h>
+#include <uORB/topics/vehicle_status.h>
+
+void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
+
+#endif /* ACCELEROMETER_CALIBRATION_H_ */
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c
index 7c0a25f86..aab8f3e04 100644
--- a/src/modules/commander/commander.c
+++ b/src/modules/commander/commander.c
@@ -94,7 +94,7 @@
 #include <drivers/drv_baro.h>
 
 #include "calibration_routines.h"
-
+#include "accelerometer_calibration.h"
 
 PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0);	/**< Go into low-level failsafe after 0 ms */
 //PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0);	/**< Go into high-level failsafe after 0 ms */
@@ -158,7 +158,6 @@ static int led_off(int led);
 static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
 static void do_mag_calibration(int status_pub, struct vehicle_status_s *status);
 static void do_rc_calibration(int status_pub, struct vehicle_status_s *status);
-static void do_accel_calibration(int status_pub, struct vehicle_status_s *status);
 static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
 
 int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
@@ -666,126 +665,6 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
 	close(sub_sensor_combined);
 }
 
-void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	/* announce change */
-
-	mavlink_log_info(mavlink_fd, "keep it level and still");
-	/* set to accel calibration mode */
-	status->flag_preflight_accel_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	const int calibration_count = 2500;
-
-	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
-	struct sensor_combined_s raw;
-
-	int calibration_counter = 0;
-	float accel_offset[3] = {0.0f, 0.0f, 0.0f};
-
-	int fd = open(ACCEL_DEVICE_PATH, 0);
-	struct accel_scale ascale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null))
-		warn("WARNING: failed to set scale / offsets for accel");
-
-	close(fd);
-
-	while (calibration_counter < calibration_count) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
-			accel_offset[0] += raw.accelerometer_m_s2[0];
-			accel_offset[1] += raw.accelerometer_m_s2[1];
-			accel_offset[2] += raw.accelerometer_m_s2[2];
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "acceleration calibration aborted");
-			return;
-		}
-	}
-
-	accel_offset[0] = accel_offset[0] / calibration_count;
-	accel_offset[1] = accel_offset[1] / calibration_count;
-	accel_offset[2] = accel_offset[2] / calibration_count;
-
-	if (isfinite(accel_offset[0]) && isfinite(accel_offset[1]) && isfinite(accel_offset[2])) {
-
-		/* add the removed length from x / y to z, since we induce a scaling issue else */
-		float total_len = sqrtf(accel_offset[0] * accel_offset[0] + accel_offset[1] * accel_offset[1] + accel_offset[2] * accel_offset[2]);
-
-		/* if length is correct, zero results here */
-		accel_offset[2] = accel_offset[2] + total_len;
-
-		float scale = 9.80665f / total_len;
-
-		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))
-			|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))
-			|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))
-			|| param_set(param_find("SENS_ACC_XSCALE"), &(scale))
-			|| param_set(param_find("SENS_ACC_YSCALE"), &(scale))
-			|| param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) {
-			mavlink_log_critical(mavlink_fd, "Setting offs or scale failed!");
-		}
-
-		fd = open(ACCEL_DEVICE_PATH, 0);
-		struct accel_scale ascale = {
-			accel_offset[0],
-			scale,
-			accel_offset[1],
-			scale,
-			accel_offset[2],
-			scale,
-		};
-
-		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
-			warn("WARNING: failed to set scale / offsets for accel");
-
-		close(fd);
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-		}
-
-		//char buf[50];
-		//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
-		//mavlink_log_info(mavlink_fd, buf);
-		mavlink_log_info(mavlink_fd, "accel calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "accel calibration FAILED (NaN)");
-	}
-
-	/* exit accel calibration mode */
-	status->flag_preflight_accel_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	close(sub_sensor_combined);
-}
-
 void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
 {
 	/* announce change */
@@ -797,22 +676,22 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
 
 	const int calibration_count = 2500;
 
-	int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure));
-	struct differential_pressure_s differential_pressure;
+	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+	struct differential_pressure_s diff_pres;
 
 	int calibration_counter = 0;
-	float airspeed_offset = 0.0f;
+	float diff_pres_offset = 0.0f;
 
 	while (calibration_counter < calibration_count) {
 
 		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } };
+		struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } };
 
 		int poll_ret = poll(fds, 1, 1000);
 
 		if (poll_ret) {
-			orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure);
-			airspeed_offset += differential_pressure.voltage;
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+			diff_pres_offset += diff_pres.differential_pressure_pa;
 			calibration_counter++;
 
 		} else if (poll_ret == 0) {
@@ -822,11 +701,11 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
 		}
 	}
 
-	airspeed_offset = airspeed_offset / calibration_count;
+	diff_pres_offset = diff_pres_offset / calibration_count;
 
-	if (isfinite(airspeed_offset)) {
+	if (isfinite(diff_pres_offset)) {
 
-		if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) {
+		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
 			mavlink_log_critical(mavlink_fd, "Setting offs failed!");
 		}
 
@@ -856,7 +735,7 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
 	status->flag_preflight_airspeed_calibration = false;
 	state_machine_publish(status_pub, status, mavlink_fd);
 
-	close(sub_differential_pressure);
+	close(diff_pres_sub);
 }
 
 
@@ -1040,7 +919,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
 					mavlink_log_info(mavlink_fd, "CMD starting accel cal");
 					tune_confirm();
-					do_accel_calibration(status_pub, &current_status);
+					do_accel_calibration(status_pub, &current_status, mavlink_fd);
 					tune_confirm();
 					mavlink_log_info(mavlink_fd, "CMD finished accel cal");
 					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
@@ -1477,10 +1356,10 @@ int commander_thread_main(int argc, char *argv[])
 	struct sensor_combined_s sensors;
 	memset(&sensors, 0, sizeof(sensors));
 
-	int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
-	struct differential_pressure_s differential_pressure;
-	memset(&differential_pressure, 0, sizeof(differential_pressure));
-	uint64_t last_differential_pressure_time = 0;
+	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+	struct differential_pressure_s diff_pres;
+	memset(&diff_pres, 0, sizeof(diff_pres));
+	uint64_t last_diff_pres_time = 0;
 
 	/* Subscribe to command topic */
 	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -1535,11 +1414,11 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
 		}
 
-		orb_check(differential_pressure_sub, &new_data);
+		orb_check(diff_pres_sub, &new_data);
 
 		if (new_data) {
-			orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure);
-			last_differential_pressure_time = differential_pressure.timestamp;
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+			last_diff_pres_time = diff_pres.timestamp;
 		}
 
 		orb_check(cmd_sub, &new_data);
@@ -1624,21 +1503,39 @@ int commander_thread_main(int argc, char *argv[])
 			if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
 			     current_status.state_machine == SYSTEM_STATE_AUTO  ||
 			     current_status.state_machine == SYSTEM_STATE_MANUAL)) {
-				/* armed */
-				led_toggle(LED_BLUE);
+				/* armed, solid */
+				led_on(LED_AMBER);
 
 			} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
 				/* not armed */
-				led_toggle(LED_BLUE);
+				led_toggle(LED_AMBER);
 			}
 
-			/* toggle error led at 5 Hz in HIL mode */
+			if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) {
+
+				/* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */
+				if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000)
+					&& (gps_position.fix_type == GPS_FIX_TYPE_3D)) {
+					/* GPS lock */
+					led_on(LED_BLUE);
+
+				} else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+					/* no GPS lock, but GPS module is aquiring lock */
+					led_toggle(LED_BLUE);
+				}
+
+			} else {
+				/* no GPS info, don't light the blue led */
+				led_off(LED_BLUE);
+			}
+
+			/* toggle GPS led at 5 Hz in HIL mode */
 			if (current_status.flag_hil_enabled) {
 				/* hil enabled */
-				led_toggle(LED_AMBER);
+				led_toggle(LED_BLUE);
 
 			} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
-				/* toggle error (red) at 5 Hz on low battery or error */
+				/* toggle arming (red) at 5 Hz on low battery or error */
 				led_toggle(LED_AMBER);
 
 			} else {
@@ -1754,7 +1651,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* Check for valid airspeed/differential pressure measurements */
-		if (hrt_absolute_time() - last_differential_pressure_time < 2000000) {
+		if (hrt_absolute_time() - last_diff_pres_time < 2000000) {
 			current_status.flag_airspeed_valid = true;
 
 		} else {
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index 556d5c2df..fe44e955a 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -35,7 +35,9 @@
 # Main system state machine
 #
 
-MODULE_COMMAND	 = commander
-SRCS		 = commander.c \
-			   state_machine_helper.c \
-			   calibration_routines.c
+MODULE_COMMAND	 	= commander
+SRCS		 	= commander.c \
+			state_machine_helper.c \
+			calibration_routines.c \
+			accelerometer_calibration.c
+
diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c
index bea388a10..ab728c7bb 100644
--- a/src/modules/commander/state_machine_helper.c
+++ b/src/modules/commander/state_machine_helper.c
@@ -249,6 +249,11 @@ void publish_armed_status(const struct vehicle_status_s *current_status)
 {
 	struct actuator_armed_s armed;
 	armed.armed = current_status->flag_system_armed;
+
+	/* XXX allow arming by external components on multicopters only if not yet armed by RC */
+	/* XXX allow arming only if core sensors are ok */
+	armed.ready_to_arm = true;
+
 	/* lock down actuators if required, only in HIL */
 	armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
 	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp
index 1cc28b9dd..7be38015c 100644
--- a/src/modules/controllib/fixedwing.cpp
+++ b/src/modules/controllib/fixedwing.cpp
@@ -56,9 +56,9 @@ BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
 
 BlockYawDamper::~BlockYawDamper() {};
 
-void BlockYawDamper::update(float rCmd, float r)
+void BlockYawDamper::update(float rCmd, float r, float outputScale)
 {
-	_rudder = _r2Rdr.update(rCmd -
+	_rudder = outputScale*_r2Rdr.update(rCmd -
 				_rWashout.update(_rLowPass.update(r)));
 }
 
@@ -77,13 +77,13 @@ BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
 BlockStabilization::~BlockStabilization() {};
 
 void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
-				float p, float q, float r)
+				float p, float q, float r, float outputScale)
 {
-	_aileron = _p2Ail.update(
+	_aileron = outputScale*_p2Ail.update(
 			   pCmd - _pLowPass.update(p));
-	_elevator = _q2Elv.update(
+	_elevator = outputScale*_q2Elv.update(
 			    qCmd - _qLowPass.update(q));
-	_yawDamper.update(rCmd, r);
+	_yawDamper.update(rCmd, r, outputScale);
 }
 
 BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
@@ -156,21 +156,21 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
 	_theLimit(this, "THE"),
 	_vLimit(this, "V"),
 
-	// altitude/roc hold
+	// altitude/climb rate hold
 	_h2Thr(this, "H2THR"),
-	_roc2Thr(this, "ROC2THR"),
+	_cr2Thr(this, "CR2THR"),
 
 	// guidance block
 	_guide(this, ""),
 
-	// block params
-	_trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
-	_trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
-	_trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
-	_trimThr(this, "TRIM_THR", true), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
+	_trimAil(this, "TRIM_ROLL", false), 	/* general roll trim (full name: TRIM_ROLL) */
+	_trimElv(this, "TRIM_PITCH", false), 	/* general pitch trim */
+	_trimRdr(this, "TRIM_YAW", false), 	/* general yaw trim */
+	_trimThr(this, "TRIM_THR"), 	/* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
+	_trimV(this, "TRIM_V"), 	/* FWB_ specific trim velocity (full name : FWB_TRIM_V) */
 
 	_vCmd(this, "V_CMD"),
-	_rocMax(this, "ROC_MAX"),
+	_crMax(this, "CR_MAX"),
 	_attPoll(),
 	_lastPosCmd(),
 	_timeStamp(0)
@@ -228,7 +228,15 @@ void BlockMultiModeBacksideAutopilot::update()
 		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
 
 		// calculate velocity, XXX should be airspeed, but using ground speed for now
-		float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
+		// for the purpose of control we will limit the velocity feedback between
+		// the min/max velocity
+		float v = _vLimit.update(sqrtf(
+					_pos.vx * _pos.vx +
+					_pos.vy * _pos.vy +
+					_pos.vz * _pos.vz));
+
+		// limit velocity command between min/max velocity
+		float vCmd = _vLimit.update(_vCmd.get());
 
 		// altitude hold
 		float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
@@ -240,16 +248,19 @@ void BlockMultiModeBacksideAutopilot::update()
 
 		// velocity hold
 		// negative sign because nose over to increase speed
-		float thetaCmd = _theLimit.update(-_v2Theta.update(
-				_vLimit.update(_vCmd.get()) - v));
+		float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
 		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
 
 		// yaw rate cmd
 		float rCmd = 0;
 
 		// stabilization
+		float velocityRatio = _trimV.get()/v;
+		float outputScale = velocityRatio*velocityRatio;
+		// this term scales the output based on the dynamic pressure change from trim
 		_stabilization.update(pCmd, qCmd, rCmd,
-				      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+				      _att.rollspeed, _att.pitchspeed, _att.yawspeed,
+				      outputScale);
 
 		// output
 		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
@@ -280,13 +291,18 @@ void BlockMultiModeBacksideAutopilot::update()
 		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
 
 			// calculate velocity, XXX should be airspeed, but using ground speed for now
-			float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
+			// for the purpose of control we will limit the velocity feedback between
+			// the min/max velocity
+			float v = _vLimit.update(sqrtf(
+						_pos.vx * _pos.vx +
+						_pos.vy * _pos.vy +
+						_pos.vz * _pos.vz));
 
 			// pitch channel -> rate of climb
 			// TODO, might want to put a gain on this, otherwise commanding
 			// from +1 -> -1 m/s for rate of climb
-			//float dThrottle = _roc2Thr.update(
-			//_rocMax.get()*_manual.pitch - _pos.vz);
+			//float dThrottle = _cr2Thr.update(
+			//_crMax.get()*_manual.pitch - _pos.vz);
 
 			// roll channel -> bank angle
 			float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
@@ -294,8 +310,10 @@ void BlockMultiModeBacksideAutopilot::update()
 
 			// throttle channel -> velocity
 			// negative sign because nose over to increase speed
-			float vCmd = _manual.throttle * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin();
-			float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
+			float vCmd = _vLimit.update(_manual.throttle *
+					(_vLimit.getMax() - _vLimit.getMin()) +
+					_vLimit.getMin());
+			float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
 			float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
 
 			// yaw rate cmd
diff --git a/src/modules/controllib/fixedwing.hpp b/src/modules/controllib/fixedwing.hpp
index 281cbb4cb..53d0cf893 100644
--- a/src/modules/controllib/fixedwing.hpp
+++ b/src/modules/controllib/fixedwing.hpp
@@ -193,7 +193,7 @@ public:
 	 * good idea to declare a member to store the temporary
 	 * variable.
 	 */
-	void update(float rCmd, float r);
+	void update(float rCmd, float r, float outputScale = 1.0);
 
 	/**
 	 * Rudder output value accessor
@@ -226,7 +226,8 @@ public:
 	BlockStabilization(SuperBlock *parent, const char *name);
 	virtual ~BlockStabilization();
 	void update(float pCmd, float qCmd, float rCmd,
-		    float p, float q, float r);
+		    float p, float q, float r,
+		    float outputScale = 1.0);
 	float getAileron() { return _aileron; }
 	float getElevator() { return _elevator; }
 	float getRudder() { return _yawDamper.getRudder(); }
@@ -310,9 +311,9 @@ private:
 	BlockLimit _theLimit;
 	BlockLimit _vLimit;
 
-	// altitude/ roc hold
+	// altitude/ climb rate hold
 	BlockPID _h2Thr;
-	BlockPID _roc2Thr;
+	BlockPID _cr2Thr;
 
 	// guidance
 	BlockWaypointGuidance _guide;
@@ -322,8 +323,9 @@ private:
 	BlockParam<float> _trimElv;
 	BlockParam<float> _trimRdr;
 	BlockParam<float> _trimThr;
+	BlockParam<float> _trimV;
 	BlockParam<float> _vCmd;
-	BlockParam<float> _rocMax;
+	BlockParam<float> _crMax;
 
 	struct pollfd _attPoll;
 	vehicle_global_position_setpoint_s _lastPosCmd;
diff --git a/src/modules/fixedwing_backside/params.c b/src/modules/fixedwing_backside/params.c
index 428b779b1..db30af416 100644
--- a/src/modules/fixedwing_backside/params.c
+++ b/src/modules/fixedwing_backside/params.c
@@ -59,13 +59,14 @@ PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f);       // maximum commanded velocity
 // rate of climb
 // this is what rate of climb is commanded (in m/s) 
 // when the pitch stick is fully defelcted in simple mode
-PARAM_DEFINE_FLOAT(FWB_ROC_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f);
 
-//  rate of climb -> thr
-PARAM_DEFINE_FLOAT(FWB_ROC2THR_P, 0.01f);   // rate of climb to throttle PID
-PARAM_DEFINE_FLOAT(FWB_ROC2THR_I, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_ROC2THR_D, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_ROC2THR_D_LP, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_ROC2THR_I_MAX, 0.0f);
+//  climb rate -> thr
+PARAM_DEFINE_FLOAT(FWB_CR2THR_P, 0.01f);   // rate of climb to throttle PID
+PARAM_DEFINE_FLOAT(FWB_CR2THR_I, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_CR2THR_D, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_CR2THR_D_LP, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_CR2THR_I_MAX, 0.0f);
 
-PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f);     // trim throttle (0,1)
+PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f);    // trim throttle (0,1)
+PARAM_DEFINE_FLOAT(FWB_TRIM_V, 12.0f);     // trim velocity, m/s
diff --git a/src/modules/mathlib/CMSIS/module.mk b/src/modules/mathlib/CMSIS/module.mk
index c676f3261..5e1abe642 100644
--- a/src/modules/mathlib/CMSIS/module.mk
+++ b/src/modules/mathlib/CMSIS/module.mk
@@ -38,8 +38,9 @@
 #
 # Find sources
 #
-DSPLIB_SRCDIR		:= $(PX4_MODULE_SRC)/modules/mathlib/CMSIS
-ABS_SRCS		:= $(wildcard $(DSPLIB_SRCDIR)/DSP_Lib/Source/*/*.c)
+DSPLIB_SRCDIR		:= $(dir $(lastword $(MAKEFILE_LIST)))
+SRCLIST			:= $(wildcard $(DSPLIB_SRCDIR)DSP_Lib/Source/*/*.c)
+SRCS			:= $(patsubst $(DSPLIB_SRCDIR)%,%,$(SRCLIST))
 
 INCLUDE_DIRS		+= $(DSPLIB_SRCDIR)/Include \
 			   $(DSPLIB_SRCDIR)/Device/ARM/ARMCM4/Include \
diff --git a/src/modules/mavlink/mavlink_receiver.c b/src/modules/mavlink/mavlink_receiver.c
index e62e4dcc4..a42612f9e 100644
--- a/src/modules/mavlink/mavlink_receiver.c
+++ b/src/modules/mavlink/mavlink_receiver.c
@@ -308,82 +308,6 @@ handle_message(mavlink_message_t *msg)
 
 		uint64_t timestamp = hrt_absolute_time();
 
-		/* TODO, set ground_press/ temp during calib */
-		static const float ground_press = 1013.25f; // mbar
-		static const float ground_tempC = 21.0f;
-		static const float ground_alt = 0.0f;
-		static const float T0 = 273.15f;
-		static const float R = 287.05f;
-		static const float g = 9.806f;
-
-		if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) {
-
-			mavlink_raw_imu_t imu;
-			mavlink_msg_raw_imu_decode(msg, &imu);
-
-			/* packet counter */
-			static uint16_t hil_counter = 0;
-			static uint16_t hil_frames = 0;
-			static uint64_t old_timestamp = 0;
-
-			/* sensors general */
-			hil_sensors.timestamp = imu.time_usec;
-
-			/* hil gyro */
-			static const float mrad2rad = 1.0e-3f;
-			hil_sensors.gyro_counter = hil_counter;
-			hil_sensors.gyro_raw[0] = imu.xgyro;
-			hil_sensors.gyro_raw[1] = imu.ygyro;
-			hil_sensors.gyro_raw[2] = imu.zgyro;
-			hil_sensors.gyro_rad_s[0] = imu.xgyro * mrad2rad;
-			hil_sensors.gyro_rad_s[1] = imu.ygyro * mrad2rad;
-			hil_sensors.gyro_rad_s[2] = imu.zgyro * mrad2rad;
-
-			/* accelerometer */
-			hil_sensors.accelerometer_counter = hil_counter;
-			static const float mg2ms2 = 9.8f / 1000.0f;
-			hil_sensors.accelerometer_raw[0] = imu.xacc;
-			hil_sensors.accelerometer_raw[1] = imu.yacc;
-			hil_sensors.accelerometer_raw[2] = imu.zacc;
-			hil_sensors.accelerometer_m_s2[0] = mg2ms2 * imu.xacc;
-			hil_sensors.accelerometer_m_s2[1] = mg2ms2 * imu.yacc;
-			hil_sensors.accelerometer_m_s2[2] = mg2ms2 * imu.zacc;
-			hil_sensors.accelerometer_mode = 0; // TODO what is this?
-			hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
-
-			/* adc */
-			hil_sensors.adc_voltage_v[0] = 0;
-			hil_sensors.adc_voltage_v[1] = 0;
-			hil_sensors.adc_voltage_v[2] = 0;
-
-			/* magnetometer */
-			float mga2ga = 1.0e-3f;
-			hil_sensors.magnetometer_counter = hil_counter;
-			hil_sensors.magnetometer_raw[0] = imu.xmag;
-			hil_sensors.magnetometer_raw[1] = imu.ymag;
-			hil_sensors.magnetometer_raw[2] = imu.zmag;
-			hil_sensors.magnetometer_ga[0] = imu.xmag * mga2ga;
-			hil_sensors.magnetometer_ga[1] = imu.ymag * mga2ga;
-			hil_sensors.magnetometer_ga[2] = imu.zmag * mga2ga;
-			hil_sensors.magnetometer_range_ga = 32.7f; // int16
-			hil_sensors.magnetometer_mode = 0; // TODO what is this
-			hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
-
-			/* publish */
-			orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
-
-			// increment counters
-			hil_counter += 1 ;
-			hil_frames += 1 ;
-
-			// output
-			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil imu at %d hz\n", hil_frames/10);
-				old_timestamp = timestamp;
-				hil_frames = 0;
-			}
-		}
-
 		if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) {
 
 			mavlink_highres_imu_t imu;
@@ -437,13 +361,9 @@ handle_message(mavlink_message_t *msg)
 			hil_sensors.magnetometer_mode = 0; // TODO what is this
 			hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
 
+			/* baro */
 			hil_sensors.baro_pres_mbar = imu.abs_pressure;
-
-			float tempC =  imu.temperature;
-			float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;
-			float h =  ground_alt + (R / g) * tempAvgK * logf(ground_press / imu.abs_pressure);
-
-			hil_sensors.baro_alt_meter = h;
+			hil_sensors.baro_alt_meter = imu.pressure_alt;
 			hil_sensors.baro_temp_celcius = imu.temperature;
 
 			hil_sensors.gyro_counter = hil_counter;
@@ -516,44 +436,6 @@ handle_message(mavlink_message_t *msg)
 			}
 		}
 
-		if (msg->msgid == MAVLINK_MSG_ID_RAW_PRESSURE) {
-
-			mavlink_raw_pressure_t press;
-			mavlink_msg_raw_pressure_decode(msg, &press);
-
-			/* packet counter */
-			static uint16_t hil_counter = 0;
-			static uint16_t hil_frames = 0;
-			static uint64_t old_timestamp = 0;
-
-			/* sensors general */
-			hil_sensors.timestamp = press.time_usec;
-
-			/* baro */
-
-			float tempC =  press.temperature / 100.0f;
-			float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;
-			float h =  ground_alt + (R / g) * tempAvgK * logf(ground_press / press.press_abs);
-			hil_sensors.baro_counter = hil_counter;
-			hil_sensors.baro_pres_mbar = press.press_abs;
-			hil_sensors.baro_alt_meter = h;
-			hil_sensors.baro_temp_celcius = tempC;
-
-			/* publish */
-			orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
-
-			// increment counters
-			hil_counter += 1 ;
-			hil_frames += 1 ;
-
-			// output
-			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil pressure at %d hz\n", hil_frames/10);
-				old_timestamp = timestamp;
-				hil_frames = 0;
-			}
-		}
-
 		if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {
 
 			mavlink_hil_state_t hil_state;
diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c
index 670b8d635..f744698be 100644
--- a/src/modules/px4iofirmware/adc.c
+++ b/src/modules/px4iofirmware/adc.c
@@ -135,6 +135,9 @@ adc_init(void)
 	return 0;
 }
 
+/*
+  return one measurement, or 0xffff on error
+ */
 uint16_t
 adc_measure(unsigned channel)
 {
@@ -154,9 +157,10 @@ adc_measure(unsigned channel)
 	while (!(rSR & ADC_SR_EOC)) {
 
 		/* never spin forever - this will give a bogus result though */
-		if (hrt_elapsed_time(&now) > 1000) {
+		if (hrt_elapsed_time(&now) > 100) {
 			debug("adc timeout");
-			break;
+			perf_end(adc_perf);
+			return 0xffff;
 		}
 	}
 
@@ -165,4 +169,4 @@ adc_measure(unsigned channel)
 
 	perf_end(adc_perf);
 	return result;
-}
\ No newline at end of file
+}
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 25eeca3be..dc36f6c93 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -288,8 +288,8 @@ controls_tick() {
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
 
 			/* mix new RC input control values to servos */
-			//if (dsm_updated || sbus_updated || ppm_updated)
-			//	mixer_tick();
+			if (dsm_updated || sbus_updated || ppm_updated)
+				mixer_tick();
 
 		} else {
 			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index f38593d2a..5ada1b220 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -174,7 +174,7 @@ mixer_tick(void)
 	 * here.
 	 */
 	bool should_arm = (
-	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
 	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
 		/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
 		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
@@ -246,7 +246,7 @@ void
 mixer_handle_text(const void *buffer, size_t length)
 {
 	/* do not allow a mixer change while fully armed */
-	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
 	    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
 		return;
 	}
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
index 53d1ec2b9..6379366f4 100644
--- a/src/modules/px4iofirmware/module.mk
+++ b/src/modules/px4iofirmware/module.mk
@@ -10,10 +10,10 @@ SRCS		= adc.c \
 		  sbus.c \
 		  ../systemlib/up_cxxinitialize.c \
 		  ../systemlib/hx_stream.c \
-		  ../systemlib/perf_counter.c
-
-#		  mixer.cpp \
-#		  ../systemlib/mixer/mixer.cpp \
-#		  ../systemlib/mixer/mixer_group.cpp \
-#		  ../systemlib/mixer/mixer_multirotor.cpp \
-#		  ../systemlib/mixer/mixer_simple.cpp \
\ No newline at end of file
+		  ../systemlib/perf_counter.c \
+		  mixer.cpp \
+		  ../systemlib/mixer/mixer.cpp \
+		  ../systemlib/mixer/mixer_group.cpp \
+		  ../systemlib/mixer/mixer_multirotor.cpp \
+		  ../systemlib/mixer/mixer_simple.cpp \
+		  
\ No newline at end of file
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 8d8b7e941..e575bd841 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -115,7 +115,7 @@
 #define PX4IO_P_STATUS_ALARMS_PWM_ERROR		(1 << 6) /* PWM configuration or output was bad */
 
 #define PX4IO_P_STATUS_VBATT			4	/* battery voltage in mV */
-#define PX4IO_P_STATUS_IBATT			5	/* battery current in cA */
+#define PX4IO_P_STATUS_IBATT			5	/* battery current (raw ADC) */
 
 /* array of post-mix actuator outputs, -10000..10000 */
 #define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -145,9 +145,9 @@
 #define PX4IO_P_SETUP_FEATURES			0
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
-#define PX4IO_P_SETUP_ARMING_ARM_OK		(1 << 0) /* OK to arm */
+#define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
+#define PX4IO_P_SETUP_ARMING_FMU_ARMED		(1 << 1) /* FMU is already armed */
 #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK	(1 << 2) /* OK to switch to manual override via override RC channel */
-#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK	(1 << 3) /* OK to perform position / vector control (= position lock) */
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
@@ -155,8 +155,6 @@
 #define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
 #define PX4IO_P_SETUP_VBATT_SCALE		6	/* battery voltage correction factor (float) */
-#define PX4IO_P_SETUP_IBATT_SCALE		7	/* battery current scaling factor (float) */
-#define PX4IO_P_SETUP_IBATT_BIAS		8	/* battery current bias value */
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
 /* autopilot control values, -10000..10000 */
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index 6244b1551..bc8dfc116 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -207,7 +207,7 @@ user_start(int argc, char *argv[])
 
 		/* kick the mixer */
 		perf_begin(mixer_perf);
-		// mixer_tick();
+		mixer_tick();
 		perf_end(mixer_perf);
 
 		/* kick the control inputs */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 3647842c0..272cdb7bf 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -148,8 +148,8 @@ extern struct sys_state_s system_state;
 /*
  * Mixer
  */
-//extern void	mixer_tick(void);
-//extern void	mixer_handle_text(const void *buffer, size_t length);
+extern void	mixer_tick(void);
+extern void	mixer_handle_text(const void *buffer, size_t length);
 
 /**
  * Safety switch/LED.
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 64a87a58b..61049c8b6 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -138,15 +138,14 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
 	[PX4IO_P_SETUP_RELAYS]			= 0,
 	[PX4IO_P_SETUP_VBATT_SCALE]		= 10000,
-	[PX4IO_P_SETUP_IBATT_SCALE]		= 0,
-	[PX4IO_P_SETUP_IBATT_BIAS]		= 0,
 	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
 };
 
 #define PX4IO_P_SETUP_FEATURES_VALID	(0)
-#define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_ARM_OK | \
+#define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_FMU_ARMED | \
 					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
-					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)
+					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
+					 PX4IO_P_SETUP_ARMING_IO_ARM_OK)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -245,7 +244,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 
 		/* handle text going to the mixer parser */
 	case PX4IO_PAGE_MIXERLOAD:
-		//mixer_handle_text(values, num_values * sizeof(*values));
+		mixer_handle_text(values, num_values * sizeof(*values));
 		break;
 
 	default:
@@ -313,7 +312,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * so that an in-air reset of FMU can not lead to a
 			 * lockup of the IO arming state.
 			 */
-			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) {
+			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
 			}
 
@@ -364,7 +363,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 	case PX4IO_PAGE_RC_CONFIG: {
 
 		/* do not allow a RC config change while fully armed */
-		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
 		    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
 			break;
 		}
@@ -508,20 +507,27 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 			 * Intercept corrected for best results @ 12V.
 			 */
 			unsigned counts = adc_measure(ADC_VBATT);
-			unsigned mV = (4150 + (counts * 46)) / 10 - 200;
-			unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
+			if (counts != 0xffff) {
+				unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+				unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
 
-			r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
+				r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
+			}
 		}
 
 		/* PX4IO_P_STATUS_IBATT */
 		{
-			unsigned counts = adc_measure(ADC_VBATT);
-			unsigned scaled = (counts * r_page_setup[PX4IO_P_SETUP_IBATT_SCALE]) / 10000;
-			int corrected = scaled + REG_TO_SIGNED(r_page_setup[PX4IO_P_SETUP_IBATT_BIAS]);
-			if (corrected < 0)
-				corrected = 0;
-			r_page_status[PX4IO_P_STATUS_IBATT] = corrected;
+			/*
+			  note that we have no idea what sort of
+			  current sensor is attached, so we just
+			  return the raw 12 bit ADC value and let the
+			  FMU sort it out, with user selectable
+			  configuration for their sensor
+			 */
+			unsigned counts = adc_measure(ADC_IN5);
+			if (counts != 0xffff) {
+				r_page_status[PX4IO_P_STATUS_IBATT] = counts;
+			}
 		}
 
 		SELECT_PAGE(r_page_status);
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c
index 5495d5ae1..4dbecc274 100644
--- a/src/modules/px4iofirmware/safety.c
+++ b/src/modules/px4iofirmware/safety.c
@@ -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
@@ -32,7 +32,8 @@
  ****************************************************************************/
 
 /**
- * @file Safety button logic.
+ * @file safety.c
+ * Safety button logic.
  */
 
 #include <nuttx/config.h>
@@ -56,11 +57,11 @@ static unsigned counter = 0;
 /*
  * Define the various LED flash sequences for each system state.
  */
-#define LED_PATTERN_SAFE 			0xffff		/**< always on 				*/
-#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 	0xFFFE		/**< always on with short break 	*/
-#define LED_PATTERN_FMU_ARMED 			0x4444		/**< slow blinking			*/
-#define LED_PATTERN_IO_ARMED 			0x5555		/**< fast blinking 			*/
-#define LED_PATTERN_IO_FMU_ARMED 		0x5050		/**< long off then double blink 	*/
+#define LED_PATTERN_FMU_OK_TO_ARM 		0x0003		/**< slow blinking			*/
+#define LED_PATTERN_FMU_REFUSE_TO_ARM 		0x5555		/**< fast blinking			*/
+#define LED_PATTERN_IO_ARMED 			0x5050		/**< long off, then double blink 	*/
+#define LED_PATTERN_FMU_ARMED 			0x5500		/**< long off, then quad blink 		*/
+#define LED_PATTERN_IO_FMU_ARMED 		0xffff		/**< constantly on			*/
 
 static unsigned blink_counter = 0;
 
@@ -109,7 +110,8 @@ safety_check_button(void *arg)
 	 * state machine, keep ARM_COUNTER_THRESHOLD the same
 	 * length in all cases of the if/else struct below.
 	 */
-	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+		(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
 			counter++;
@@ -120,8 +122,6 @@ safety_check_button(void *arg)
 			counter++;
 		}
 
-		/* Disarm quickly */
-
 	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
@@ -138,21 +138,21 @@ safety_check_button(void *arg)
 	}
 
 	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
-	uint16_t pattern = LED_PATTERN_SAFE;
+	uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
 
 	if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
-		if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
+		if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
 			pattern = LED_PATTERN_IO_FMU_ARMED;
 
 		} else {
 			pattern = LED_PATTERN_IO_ARMED;
 		}
 
-	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
+	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
 		pattern = LED_PATTERN_FMU_ARMED;
+	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) {
+		pattern = LED_PATTERN_FMU_OK_TO_ARM;
 
-	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) {
-		pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
 	}
 
 	/* Turn the LED on if we have a 1 at the current bit position */
diff --git a/src/modules/sdlog/sdlog.c b/src/modules/sdlog/sdlog.c
index df8745d9f..84a9eb6ac 100644
--- a/src/modules/sdlog/sdlog.c
+++ b/src/modules/sdlog/sdlog.c
@@ -71,6 +71,7 @@
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/airspeed.h>
 
 #include <systemlib/systemlib.h>
 
@@ -443,7 +444,8 @@ int sdlog_thread_main(int argc, char *argv[])
 		struct vehicle_vicon_position_s vicon_pos;
 		struct optical_flow_s flow;
 		struct battery_status_s batt;
-		struct differential_pressure_s diff_pressure;
+		struct differential_pressure_s diff_pres;
+		struct airspeed_s airspeed;
 	} buf;
 	memset(&buf, 0, sizeof(buf));
 
@@ -461,7 +463,8 @@ int sdlog_thread_main(int argc, char *argv[])
 		int vicon_pos_sub;
 		int flow_sub;
 		int batt_sub;
-		int diff_pressure_sub;
+		int diff_pres_sub;
+		int airspeed_sub;
 	} subs;
 
 	/* --- MANAGEMENT - LOGGING COMMAND --- */
@@ -558,11 +561,18 @@ int sdlog_thread_main(int argc, char *argv[])
 
 	/* --- DIFFERENTIAL PRESSURE --- */
 	/* subscribe to ORB for flow measurements */
-	subs.diff_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
-	fds[fdsc_count].fd = subs.diff_pressure_sub;
+	subs.diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+	fds[fdsc_count].fd = subs.diff_pres_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- AIRSPEED --- */
+	/* subscribe to ORB for airspeed */
+	subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	fds[fdsc_count].fd = subs.airspeed_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;	
+
 	/* WARNING: If you get the error message below,
 	 * then the number of registered messages (fdsc)
 	 * differs from the number of messages in the above list.
@@ -654,7 +664,8 @@ int sdlog_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
 				orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
 				orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
-				orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure);
+				orb_copy(ORB_ID(differential_pressure), subs.diff_pres_sub, &buf.diff_pres);
+				orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed);
 				orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt);
 
 				/* if skipping is on or logging is disabled, ignore */
@@ -691,9 +702,9 @@ int sdlog_thread_main(int argc, char *argv[])
 					.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
 					.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
 					.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality},
-					.diff_pressure = buf.diff_pressure.differential_pressure_mbar,
-					.ind_airspeed = buf.diff_pressure.indicated_airspeed_m_s,
-					.true_airspeed = buf.diff_pressure.true_airspeed_m_s
+					.diff_pressure = buf.diff_pres.differential_pressure_pa,
+					.ind_airspeed = buf.airspeed.indicated_airspeed_m_s,
+					.true_airspeed = buf.airspeed.true_airspeed_m_s
 				};
 
 				/* put into buffer for later IO */
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index c850e3a1e..230060148 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -64,7 +64,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_FLOAT(SENS_VAIR_OFF, 2.5f);
+PARAM_DEFINE_INT32(SENS_DPRES_OFF, 1667);
 
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 123bbb120..6b6aeedee 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -77,6 +77,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/airspeed.h>
 
 #define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20   /* 40 ms downtime at 500 Hz update rate   */
 #define ACC_HEALTH_COUNTER_LIMIT_ERROR  20   /* 40 ms downtime at 500 Hz update rate   */
@@ -98,6 +99,12 @@
 #define BAT_VOL_LOWPASS_2 0.01f
 #define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
 
+/**
+ * HACK - true temperature is much less than indicated temperature in baro,
+ * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
+ */
+#define PCB_TEMP_ESTIMATE_DEG 5.0f
+
 #define PPM_INPUT_TIMEOUT_INTERVAL	50000 /**< 50 ms timeout / 20 Hz */
 
 #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
@@ -156,6 +163,8 @@ private:
 	int		_mag_sub;			/**< raw mag data subscription */
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
+	int     _airspeed_sub;		/**< airspeed subscription */
+	int		_diff_pres_sub;		/**< raw differential pressure subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;			/**< notification of manual control updates */
@@ -165,13 +174,15 @@ private:
 	orb_advert_t	_rc_pub;			/**< raw r/c control topic */
 	orb_advert_t	_battery_pub;			/**< battery status */
 	orb_advert_t	_airspeed_pub;			/**< airspeed */
+	orb_advert_t	_diff_pres_pub;			/**< differential_pressure */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	struct rc_channels_s _rc;			/**< r/c channel data */
 	struct battery_status_s _battery_status;	/**< battery status */
 	struct baro_report _barometer;			/**< barometer data */
-	struct differential_pressure_s _differential_pressure;
+	struct differential_pressure_s _diff_pres;
+	struct airspeed_s _airspeed;
 
 	struct {
 		float min[_rc_max_chan_count];
@@ -187,7 +198,7 @@ private:
 		float mag_scale[3];
 		float accel_offset[3];
 		float accel_scale[3];
-		float airspeed_offset;
+		int diff_pres_offset_pa;
 
 		int rc_type;
 
@@ -236,7 +247,7 @@ private:
 		param_t accel_scale[3];
 		param_t mag_offset[3];
 		param_t mag_scale[3];
-		param_t airspeed_offset;
+		param_t diff_pres_offset_pa;
 
 		param_t rc_map_roll;
 		param_t rc_map_pitch;
@@ -331,6 +342,14 @@ private:
 	void		baro_poll(struct sensor_combined_s &raw);
 
 	/**
+	 * Poll the differential pressure sensor for updated data.
+	 *
+	 * @param raw			Combined sensor data structure into which
+	 *				data should be returned.
+	 */
+	void		diff_pres_poll(struct sensor_combined_s &raw);
+
+	/**
 	 * Check for changes in vehicle status.
 	 */
 	void		vehicle_status_poll();
@@ -400,6 +419,7 @@ Sensors::Sensors() :
 	_rc_pub(-1),
 	_battery_pub(-1),
 	_airspeed_pub(-1),
+	_diff_pres_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
@@ -484,8 +504,8 @@ Sensors::Sensors() :
 	_parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE");
 	_parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE");
 
-	/*Airspeed offset */
-	_parameter_handles.airspeed_offset = param_find("SENS_VAIR_OFF");
+	/* Differential pressure offset */
+	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
@@ -695,7 +715,7 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2]));
 
 	/* Airspeed offset */
-	param_get(_parameter_handles.airspeed_offset, &(_parameters.airspeed_offset));
+	param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
 
 	/* scaling of ADC ticks to battery voltage */
 	if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
@@ -890,6 +910,32 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 }
 
 void
+Sensors::diff_pres_poll(struct sensor_combined_s &raw)
+{
+	bool updated;
+	orb_check(_diff_pres_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
+
+		raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
+		raw.differential_pressure_counter++;
+
+		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa);
+		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f, 
+											 			 raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
+
+		/* announce the airspeed if needed, just publish else */
+		if (_airspeed_pub > 0) {
+			orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed);
+
+		} else {
+			_airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed);
+		}
+	}
+}
+
+void
 Sensors::vehicle_status_poll()
 {
 	struct vehicle_status_s vstatus;
@@ -1047,31 +1093,18 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					 */
 					if (voltage > 0.4f) {
 
-						float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP sensor
-
-						float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f,
-							_barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa
-						// XXX HACK - true temperature is much less than indicated temperature in baro,
-						// subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
+						float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
 
-						float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa);
-
-						//printf("voltage: %.4f, diff_pres_pa %.4f, baro press %.4f Pa, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)_barometer.pressure*1e2f, (double)airspeed_indicated, (double)airspeed_true);
-
-						_differential_pressure.timestamp = hrt_absolute_time();
-						_differential_pressure.static_pressure_mbar = _barometer.pressure;
-						_differential_pressure.differential_pressure_mbar = diff_pres_pa*1e-2f;
-						_differential_pressure.temperature_celcius = _barometer.temperature;
-						_differential_pressure.indicated_airspeed_m_s = airspeed_indicated;
-						_differential_pressure.true_airspeed_m_s = airspeed_true;
-						_differential_pressure.voltage = voltage;
+						_diff_pres.timestamp = hrt_absolute_time();
+						_diff_pres.differential_pressure_pa = diff_pres_pa;
+						_diff_pres.voltage = voltage;
 
 						/* announce the airspeed if needed, just publish else */
-						if (_airspeed_pub > 0) {
-							orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_differential_pressure);
+						if (_diff_pres_pub > 0) {
+							orb_publish(ORB_ID(differential_pressure), _diff_pres_pub, &_diff_pres);
 
 						} else {
-							_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_differential_pressure);
+							_diff_pres_pub = orb_advertise(ORB_ID(differential_pressure), &_diff_pres);
 						}
 					}
 				}
@@ -1310,6 +1343,7 @@ Sensors::task_main()
 	_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
 	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
+	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -1336,6 +1370,7 @@ Sensors::task_main()
 	gyro_poll(raw);
 	mag_poll(raw);
 	baro_poll(raw);
+	diff_pres_poll(raw);
 
 	parameter_update_poll(true /* forced */);
 
@@ -1384,6 +1419,8 @@ Sensors::task_main()
 		/* check battery voltage */
 		adc_poll(raw);
 
+		diff_pres_poll(raw);
+
 		/* Inform other processes that new data is available to copy */
 		if (_publishing)
 			orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c
index 264287b10..15bb833a9 100644
--- a/src/modules/systemlib/airspeed.c
+++ b/src/modules/systemlib/airspeed.c
@@ -97,7 +97,7 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp
 	float density = get_air_density(static_pressure, temperature_celsius);
 	if (density < 0.0001f || !isfinite(density)) {
 	 density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
-	 printf("[airspeed] Invalid air density, using density at sea level\n");
+//	 printf("[airspeed] Invalid air density, using density at sea level\n");
 	}
 
 	float pressure_difference = total_pressure - static_pressure;
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index 40d37fce2..bbfa130a9 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -419,6 +419,7 @@ public:
 		QUAD_X = 0,	/**< quad in X configuration */
 		QUAD_PLUS,	/**< quad in + configuration */
 		QUAD_V,		/**< quad in V configuration */
+		QUAD_WIDE,	/**< quad in wide configuration */
 		HEX_X,		/**< hex in X configuration */
 		HEX_PLUS,	/**< hex in + configuration */
 		OCTA_X,
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index d79811c0f..8ded0b05c 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -88,6 +88,12 @@ const MultirotorMixer::Rotor _config_quad_v[] = {
 	{  0.927184,  0.374607, -1.00 },
 	{ -0.694658, -0.719340, -1.00 },
 };
+const MultirotorMixer::Rotor _config_quad_wide[] = {
+	{ -0.927184,  0.374607,  1.00 },
+	{  0.777146, -0.629320,  1.00 },
+	{  0.927184,  0.374607, -1.00 },
+	{ -0.777146, -0.629320, -1.00 },
+};
 const MultirotorMixer::Rotor _config_hex_x[] = {
 	{ -1.000000,  0.000000, -1.00 },
 	{  1.000000,  0.000000,  1.00 },
@@ -128,6 +134,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME
 	&_config_quad_x[0],
 	&_config_quad_plus[0],
 	&_config_quad_v[0],
+	&_config_quad_wide[0],
 	&_config_hex_x[0],
 	&_config_hex_plus[0],
 	&_config_octa_x[0],
@@ -137,6 +144,7 @@ const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
 	4, /* quad_x */
 	4, /* quad_plus */
 	4, /* quad_v */
+	4, /* quad_wide */
 	6, /* hex_x */
 	6, /* hex_plus */
 	8, /* octa_x */
@@ -195,6 +203,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	} else if (!strcmp(geomname, "4v")) {
 		geometry = MultirotorMixer::QUAD_V;
 
+	} else if (!strcmp(geomname, "4w")) {
+		geometry = MultirotorMixer::QUAD_WIDE;
+
 	} else if (!strcmp(geomname, "6+")) {
 		geometry = MultirotorMixer::HEX_PLUS;
 
diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables
index 19a8239a6..683c63040 100755
--- a/src/modules/systemlib/mixer/multi_tables
+++ b/src/modules/systemlib/mixer/multi_tables
@@ -27,6 +27,13 @@ set quad_v {
 	 136 CW
 }
 
+set quad_wide {
+	  68 CCW
+	-129 CCW
+	 -68 CW
+	 129 CW
+}
+
 set hex_x {
 	  90 CW
 	 -90 CCW
@@ -67,7 +74,7 @@ set octa_plus {
 	  90 CW
 }
 
-set tables {quad_x quad_plus quad_v hex_x hex_plus octa_x octa_plus}
+set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus}
 
 proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
 
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 136375140..4197f6fb2 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -122,6 +122,9 @@ ORB_DEFINE(optical_flow, struct optical_flow_s);
 #include "topics/omnidirectional_flow.h"
 ORB_DEFINE(omnidirectional_flow, struct omnidirectional_flow_s);
 
+#include "topics/airspeed.h"
+ORB_DEFINE(airspeed, struct airspeed_s);
+
 #include "topics/differential_pressure.h"
 ORB_DEFINE(differential_pressure, struct differential_pressure_s);
 
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
index 0b50a29ac..b7c4196c0 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/modules/uORB/topics/actuator_controls.h
@@ -69,6 +69,7 @@ ORB_DECLARE(actuator_controls_3);
 /** global 'actuator output is live' control. */
 struct actuator_armed_s {
 	bool	armed;		/**< Set to true if system is armed */
+	bool	ready_to_arm;	/**< Set to true if system is ready to be armed */
 	bool	lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
 };
 
diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h
new file mode 100644
index 000000000..a3da3758f
--- /dev/null
+++ b/src/modules/uORB/topics/airspeed.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file airspeed.h
+ *
+ * Definition of airspeed topic
+ */
+
+#ifndef TOPIC_AIRSPEED_H_
+#define TOPIC_AIRSPEED_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Airspeed
+ */
+struct airspeed_s {
+	uint64_t	timestamp;					/**< microseconds since system boot, needed to integrate */
+	float		indicated_airspeed_m_s;		/**< indicated airspeed in meters per second, -1 if unknown	 */
+	float		true_airspeed_m_s;			/**< true airspeed in meters per second, -1 if unknown */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(airspeed);
+
+#endif
diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index d5e4bf37e..8ce85213b 100644
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -49,16 +49,14 @@
  */
 
 /**
- * Differential pressure and airspeed
+ * Differential pressure.
  */
 struct differential_pressure_s {
-	uint64_t	timestamp;					/**< microseconds since system boot, needed to integrate */
-	float   	static_pressure_mbar;		/**< Static / environment pressure */
-	float		differential_pressure_mbar;	/**< Differential pressure reading */
-	float		temperature_celcius;		/**< ambient temperature in celcius, -1 if unknown */
-	float		indicated_airspeed_m_s;		/**< indicated airspeed in meters per second, -1 if unknown	 */
-	float		true_airspeed_m_s;			/**< true airspeed in meters per second, -1 if unknown */
-	float		voltage;					/**< Voltage from the airspeed sensor (voltage divider already compensated) */
+	uint64_t	timestamp;						/**< microseconds since system boot, needed to integrate */
+	uint16_t	differential_pressure_pa;		/**< Differential pressure reading */
+	uint16_t	max_differential_pressure_pa;	/**< Maximum differential pressure reading */
+	float		voltage;						/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
+
 };
 
 /**
diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h
index 961ee8b4a..9a76b5182 100644
--- a/src/modules/uORB/topics/sensor_combined.h
+++ b/src/modules/uORB/topics/sensor_combined.h
@@ -103,6 +103,8 @@ struct sensor_combined_s {
 	float mcu_temp_celcius;			/**< Internal temperature measurement of MCU */
 	uint32_t baro_counter;			/**< Number of raw baro measurements taken        */
 
+	float differential_pressure_pa;				/**< Airspeed sensor differential pressure                  */ 
+	uint32_t differential_pressure_counter;		/**< Number of raw differential pressure measurements taken */
 };
 
 /**
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 56f5317e3..60e61d07b 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   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
@@ -34,6 +34,7 @@
 
 /**
  * @file param.c
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Parameter tool.
  */
@@ -262,7 +263,7 @@ do_set(const char* name, const char* val)
 	switch (param_type(param)) {
 	case PARAM_TYPE_INT32:
 		if (!param_get(param, &i)) {
-			printf("old: %d", i);
+			printf("curr: %d", i);
 
 			/* convert string */
 			char* end;
@@ -276,14 +277,13 @@ do_set(const char* name, const char* val)
 
 	case PARAM_TYPE_FLOAT:
 		if (!param_get(param, &f)) {
-			printf("float values are not yet supported.");
-			// printf("old: %4.4f", (double)f);
-
-			// /* convert string */
-			// char* end;
-			// f = strtof(val,&end);
-			// param_set(param, &f);
-			// printf(" -> new: %4.4f\n", f);
+			printf("curr: %4.4f", (double)f);
+
+			/* convert string */
+			char* end;
+			f = strtod(val,&end);
+			param_set(param, &f);
+			printf(" -> new: %f\n", f);
 
 		}
 
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index de7a53199..ff733df52 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -71,13 +71,14 @@ usage(const char *reason)
 		warnx("%s", reason);
 	errx(1, 
 		"usage:\n"
-		"pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [arm|disarm] [<channel_value> ...]\n"
+		"pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [-m chanmask ] [arm|disarm] [<channel_value> ...]\n"
 		"  -v                 Print information about the PWM device\n"
 		"  <device>           PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
 		"  <alt_rate>         PWM update rate for channels in <alt_channel_mask>\n"
 		"  <channel_group>    Channel group that should update at the alternate rate (may be specified more than once)\n"
 		"  arm | disarm       Arm or disarm the ouptut\n"
 		"  <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n"
+		"  <chanmask>         Directly supply alt rate channel mask (debug use only)\n"
 		"\n"
 		"When -c is specified, any channel groups not listed with -c will update at the default rate.\n"
 		);
@@ -96,11 +97,12 @@ pwm_main(int argc, char *argv[])
 	int ret;
 	char *ep;
 	unsigned group;
+	int32_t set_mask = -1;
 
 	if (argc < 2)
 		usage(NULL);
 
-	while ((ch = getopt(argc, argv, "c:d:u:v")) != EOF) {
+	while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) {
 		switch (ch) {
 		case 'c':
 			group = strtoul(optarg, &ep, 0);
@@ -120,6 +122,12 @@ pwm_main(int argc, char *argv[])
 				usage("bad alt_rate value");
 			break;
 
+		case 'm':
+			set_mask = strtol(optarg, &ep, 0);
+			if (*ep != '\0')
+				usage("bad set_mask value");
+			break;
+
 		case 'v':
 			print_info = true;
 			break;
@@ -143,6 +151,13 @@ pwm_main(int argc, char *argv[])
 			err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
 	}
 
+	/* directly supplied channel mask */
+	if (set_mask != -1) {
+		ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask);
+		if (ret != OK)
+			err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
+	}
+
 	/* assign alternate rate to channel groups */
 	if (alt_channels_set) {
 		uint32_t mask = 0;