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

Fix px4iov2 build issue by selecting the correct NuttX config.

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]);

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

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 */
 

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)
 	{

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

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 */

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)
 

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);
+}

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

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);

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)

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 */

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;
 }
 

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

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) {
+		if (wait_for_ack(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) {
-			/* 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;
@@ -239,6 +207,15 @@ UBX::configure(unsigned &baudrate)
 	return -1;
 }
 
+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)
 {
@@ -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;
 		}
@@ -736,6 +723,25 @@ UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
 	message[length-1] = ck_b;
 }
 
+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)
 {
@@ -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);
+}

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);
 
-	int 				_fd;
+	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;
 	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_ */

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);
 

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);

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:

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

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

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

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

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

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;

src/drivers/px4io/px4io.cpp

@@ -108,6 +108,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
 	*/
@@ -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'");
 }

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;
 }