Merge branch 'master' into px4dev_new_param

apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c

@@ -138,7 +138,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 	/* subscribe to raw data */
 	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
 	/* advertise attitude */
-	int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
+	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
 
 	int loopcounter = 0;

apps/commander/commander.c

@@ -96,7 +96,7 @@ static int buzzer;
 static int mavlink_fd;
 static bool commander_initialized = false;
 static struct vehicle_status_s current_status; /**< Main state machine */
-static int stat_pub;
+static orb_advert_t stat_pub;
 
 static uint16_t nofix_counter = 0;
 static uint16_t gotfix_counter = 0;

apps/drivers/drv_mag.h

@@ -48,6 +48,8 @@
 /**
  * mag report structure.  Reads from the device must be in multiples of this
  * structure.
+ *
+ * Output values are in gauss.
  */
 struct mag_report {
 	float x;
@@ -76,7 +78,7 @@ ORB_DECLARE(sensor_mag);
  */
 
 #define _MAGIOCBASE		(0x2300)
-#define _MAGIOC(_n)		(_IOC(_MAGIOBASE, _n))
+#define _MAGIOC(_n)		(_IOC(_MAGIOCBASE, _n))
 
 /** set the driver polling rate to (arg) Hz, or one of the MAG_POLLRATE constants */
 #define MAGIOCSPOLLRATE		_MAGIOC(0)
@@ -99,4 +101,10 @@ ORB_DECLARE(sensor_mag);
 /** set the mag scaling constants to the structure pointed to by (arg) */
 #define MAGIOCSSCALE		_MAGIOC(5)
 
+/** copy the mag scaling constants to the structure pointed to by (arg) */
+#define MAGIOCGSCALE		_MAGIOC(6)
+
+/** perform self-calibration, update scale factors to canonical units */
+#define MAGIOCCALIBRATE		_MAGIOC(7)
+
 #endif /* _DRV_MAG_H */

apps/drivers/drv_orb_dev.h

@@ -81,4 +81,7 @@
 /** Set the minimum interval at which the topic can be seen to be updated for this subscription */
 #define ORBIOCSETINTERVAL	_ORBIOC(12)
 
+/** Get the global advertiser handle for the topic */
+#define ORBIOCGADVERTISER	_ORBIOC(13)
+
 #endif /* _DRV_UORB_H */

apps/drivers/hmc5883/Makefile

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# HMC5883 driver
+#
+
+APPNAME		 = hmc5883
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk

apps/drivers/hmc5883/hmc5883.cpp

@@ -0,0 +1,931 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hmc5883.cpp
+ *
+ * Driver for the HMC5883 magnetometer 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/up_hrt.h>
+
+#include <systemlib/perf_counter.h>
+
+#include <drivers/drv_mag.h>
+
+/*
+ * HMC5883 internal constants and data structures.
+ */
+
+/* Max measurement rate is 160Hz */
+#define HMC5883_CONVERSION_INTERVAL	(1000000 / 160)	/* microseconds */
+
+#define ADDR_CONF_A			0x00
+#define ADDR_CONF_B			0x01
+#define ADDR_MODE			0x02
+#define ADDR_DATA_OUT_X_MSB		0x03
+#define ADDR_DATA_OUT_X_LSB		0x04
+#define ADDR_DATA_OUT_Z_MSB		0x05
+#define ADDR_DATA_OUT_Z_LSB		0x06
+#define ADDR_DATA_OUT_Y_MSB		0x07
+#define ADDR_DATA_OUT_Y_LSB		0x08
+#define ADDR_STATUS			0x09
+#define ADDR_ID_A			0x0a
+#define ADDR_ID_B			0x0b
+#define ADDR_ID_C			0x0c
+
+#define HMC5883L_ADDRESS		0x1E
+
+/* modes not changeable outside of driver */
+#define HMC5883L_MODE_NORMAL		(0 << 0)  /* default */
+#define HMC5883L_MODE_POSITIVE_BIAS	(1 << 0)  /* positive bias */
+#define HMC5883L_MODE_NEGATIVE_BIAS	(1 << 1)  /* negative bias */
+
+#define HMC5883L_AVERAGING_1		(0 << 5) /* conf a register */
+#define HMC5883L_AVERAGING_2		(1 << 5)
+#define HMC5883L_AVERAGING_4		(2 << 5)
+#define HMC5883L_AVERAGING_8		(3 << 5)
+
+#define MODE_REG_CONTINOUS_MODE		(0 << 0)
+#define MODE_REG_SINGLE_MODE		(1 << 0) /* default */
+
+#define STATUS_REG_DATA_OUT_LOCK	(1 << 1) /* page 16: set if data is only partially read, read device to reset */
+#define STATUS_REG_DATA_READY		(1 << 0) /* page 16: set if all axes have valid measurements */
+
+#define ID_A_WHO_AM_I			'H'
+#define ID_B_WHO_AM_I			'4'
+#define ID_C_WHO_AM_I			'3'
+
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+class HMC5883 : public device::I2C
+{
+public:
+	HMC5883(int bus);
+	~HMC5883();
+
+	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);
+
+	virtual int		open_first(struct file *filp);
+	virtual int		close_last(struct file *filp);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void			print_info();
+
+protected:
+	virtual int		probe();
+
+private:
+	work_s			_work;
+	unsigned		_measure_ticks;
+
+	unsigned		_num_reports;
+	volatile unsigned	_next_report;
+	volatile unsigned	_oldest_report;
+	mag_report		*_reports;
+	mag_scale		_scale;
+	bool			_collect_phase;
+
+	orb_advert_t		_mag_topic;
+
+	unsigned		_reads;
+	unsigned		_measure_errors;
+	unsigned		_read_errors;
+	unsigned		_buf_overflows;
+
+	perf_counter_t		_sample_perf;
+
+	/**
+	 * 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.
+	 *
+	 * This is the heart of the measurement state machine.  This function
+	 * alternately starts a measurement, or collects the data from the
+	 * previous measurement.
+	 *
+	 * When the interval between measurements is greater than the minimum
+	 * measurement interval, a gap is inserted between collection
+	 * and measurement to provide the most recent measurement possible
+	 * at the next interval.
+	 */
+	void			cycle();
+
+	/**
+	 * 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);
+
+	/**
+	 * Write a register.
+	 *
+	 * @param reg		The register to write.
+	 * @param val		The value to write.
+	 * @return		OK on write success.
+	 */
+	int			write_reg(uint8_t reg, uint8_t val);
+
+	/**
+	 * Read a register.
+	 *
+	 * @param reg		The register to read.
+	 * @param val		The value read.
+	 * @return		OK on read success.
+	 */
+	int			read_reg(uint8_t reg, uint8_t &val);
+
+	/**
+	 * Issue a measurement command.
+	 *
+	 * @return		OK if the measurement command was successful.
+	 */
+	int			measure();
+
+	/**
+	 * Collect the result of the most recent measurement.
+	 */
+	int			collect();
+
+	/**
+	 * Convert a big-endian signed 16-bit value to a float.
+	 *
+	 * @param in		A signed 16-bit big-endian value.
+	 * @return		The floating-point representation of the value.
+	 */
+	float			meas_to_float(uint8_t in[2]);
+
+};
+
+/* 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 hmc5883_main(int argc, char *argv[]);
+
+
+HMC5883::HMC5883(int bus) :
+	I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
+	_measure_ticks(0),
+	_num_reports(0),
+	_next_report(0),
+	_oldest_report(0),
+	_reports(nullptr),
+	_mag_topic(-1),
+	_reads(0),
+	_measure_errors(0),
+	_read_errors(0),
+	_buf_overflows(0),
+	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+
+	// default scaling
+	_scale.x_offset = 0;
+	_scale.x_scale = 1.0f / 1090.0f;	/* default range scale from counts to gauss */
+	_scale.y_offset = 0;
+	_scale.y_scale = 1.0f / 1090.0f;	/* default range scale from counts to gauss */
+	_scale.z_offset = 0;
+	_scale.z_scale = 1.0f / 1090.0f;	/* default range scale from counts to gauss */
+
+	// work_cancel in the dtor will explode if we don't do this...
+	_work.worker = nullptr;
+}
+
+HMC5883::~HMC5883()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete[] _reports;
+}
+
+int
+HMC5883::init()
+{
+	int ret = ERROR;
+
+	/* do I2C init (and probe) first */
+	ret = I2C::init();
+
+	if (ret != OK)
+		goto out;
+
+out:
+	return ret;
+}
+
+int
+HMC5883::open_first(struct file *filp)
+{
+	/* reset to manual-poll mode */
+	_measure_ticks = 0;
+
+	/* allocate basic report buffers */
+	_num_reports = 2;
+	_reports = new struct mag_report[_num_reports];
+	_oldest_report = _next_report = 0;
+
+	return OK;
+}
+
+int
+HMC5883::close_last(struct file *filp)
+{
+	/* stop measurement */
+	stop();
+
+	/* free report buffers */
+	if (_reports != nullptr) {
+		delete[] _reports;
+		_num_reports = 0;
+	}
+
+	_measure_ticks = 0;
+
+	return OK;
+}
+
+int
+HMC5883::probe()
+{
+	uint8_t data[3];
+
+	if (read_reg(ADDR_ID_A, data[0]) ||
+	    read_reg(ADDR_ID_B, data[1]) ||
+	    read_reg(ADDR_ID_C, data[2]))
+		debug("read_reg fail");
+
+	if ((data[0] != ID_A_WHO_AM_I) ||
+	    (data[1] != ID_B_WHO_AM_I) ||
+	    (data[2] != ID_C_WHO_AM_I)) {
+		debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]);
+		return -EIO;
+	}
+
+	return OK;
+}
+
+ssize_t
+HMC5883::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct mag_report);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_oldest_report != _next_report) {
+				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
+				ret += sizeof(_reports[0]);
+				INCREMENT(_oldest_report, _num_reports);
+			}
+		}
+
+		_reads++;
+
+		/* 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(HMC5883_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);
+		_reads++;
+
+	} while (0);
+
+	return ret;
+}
+
+int
+HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case MAGIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case MAG_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+				/* external signalling (DRDY) not supported */
+			case MAG_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+				/* 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(HMC5883_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 MAGIOCSQUEUEDEPTH: {
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100))
+				return -EINVAL;
+
+			/* allocate new buffer */
+			struct mag_report *buf = new struct mag_report[arg];
+
+			if (nullptr == buf)
+				return -ENOMEM;
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop();
+			delete[] _reports;
+			_num_reports = arg;
+			_reports = buf;
+			start();
+
+			return OK;
+		}
+
+	case MAGIOCSSCALE:
+		/* set new scale factors */
+		memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
+		return 0;
+
+	case MAGIOCGSCALE:
+		/* copy out scale factors */
+		memcpy((mag_scale *)arg, &_scale, sizeof(_scale));
+		return 0;
+
+	case MAGIOCCALIBRATE:
+		/* XXX perform auto-calibration */
+		return -EINVAL;
+
+	case MAGIOCSSAMPLERATE:
+		/* not supported, always 1 sample per poll */
+		return -EINVAL;
+
+	case MAGIOCSLOWPASS:
+		/* not supported, no internal filtering */
+		return -EINVAL;
+
+	case MAGIOCSREPORTFORMAT:
+		/* not supported, no custom report format */
+		return -EINVAL;
+
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+void
+HMC5883::start()
+{
+	/* make sure we are stopped first */
+	stop();
+
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_oldest_report = _next_report = 0;
+
+	/* schedule a cycle to start things */
+	work_queue(&_work, (worker_t)&HMC5883::cycle_trampoline, this, 1);
+}
+
+void
+HMC5883::stop()
+{
+	work_cancel(&_work);
+}
+
+void
+HMC5883::cycle_trampoline(void *arg)
+{
+	HMC5883 *dev = (HMC5883 *)arg;
+
+	dev->cycle();
+}
+
+void
+HMC5883::cycle()
+{
+	/*
+	 * We have to publish the mag topic in the context of the workq
+	 * in order to ensure that the descriptor is valid when we go to publish.
+	 *
+	 * @bug	We can't really ever be torn down and restarted, since this
+	 *      descriptor will never be closed and on the restart we will be
+	 *      unable to re-advertise.
+	 */
+	if (_mag_topic == -1) {
+		struct mag_report m;
+
+		/* if this fails (e.g. no object in the system) we will cope */
+		memset(&m, 0, sizeof(m));
+		_mag_topic = orb_advertise(ORB_ID(sensor_mag), &m);
+
+		if (_mag_topic < 0)
+			debug("failed to create sensor_mag object");
+	}
+
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		if (OK != collect()) {
+			log("FATAL collection error - restarting\n");
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(HMC5883_CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(&_work,
+				   (worker_t)&HMC5883::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(HMC5883_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure()) {
+		log("FATAL measure error - restarting\n");
+		start();
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(&_work,
+		   (worker_t)&HMC5883::cycle_trampoline,
+		   this,
+		   USEC2TICK(HMC5883_CONVERSION_INTERVAL));
+}
+
+int
+HMC5883::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
+
+	if (OK != ret)
+		_measure_errors++;
+
+	return ret;
+}
+
+int
+HMC5883::collect()
+{
+#pragma pack(push, 1)
+	struct { /* status register and data as read back from the device */
+		uint8_t		x[2];
+		uint8_t		y[2];
+		uint8_t		z[2];
+	}	hmc_report;
+#pragma pack(pop)
+	struct {
+		int16_t		x, y, z;
+	} report;
+	int	ret = -EIO;
+	uint8_t	cmd;
+
+
+	perf_begin(_sample_perf);
+
+	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
+	_reports[_next_report].timestamp = hrt_absolute_time();
+
+	/*
+	 * @note  We could read the status register here, which could tell us that
+	 *        we were too early and that the output registers are still being
+	 *        written.  In the common case that would just slow us down, and
+	 *        we're better off just never being early.
+	 */
+
+	/* get measurements from the device */
+	cmd = ADDR_DATA_OUT_X_MSB;
+	ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report));
+
+	if (ret != OK) {
+		debug("data/status read error");
+		goto out;
+	}
+
+	/* swap the data we just received */
+	report.x = ((int16_t)hmc_report.x[0] << 8) + hmc_report.x[1];
+	report.y = ((int16_t)hmc_report.y[0] << 8) + hmc_report.y[1];
+	report.z = ((int16_t)hmc_report.z[0] << 8) + hmc_report.z[1];
+
+	/*
+	 * If any of the values are -4096, there was an internal math error in the sensor.
+	 * Generalise this to a simple range check that will also catch some bit errors.
+	 */
+	if ((abs(report.x) > 2048) ||
+	    (abs(report.y) > 2048) ||
+	    (abs(report.z) > 2048))
+		goto out;
+
+	/* scale values for output */
+	_reports[_next_report].x = report.x * _scale.x_scale + _scale.x_offset;
+	_reports[_next_report].y = report.y * _scale.y_scale + _scale.y_offset;
+	_reports[_next_report].z = report.z * _scale.z_scale + _scale.z_offset;
+
+	/* publish it */
+	orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]);
+
+	/* post a report to the ring - note, not locked */
+	INCREMENT(_next_report, _num_reports);
+
+	/* if we are running up against the oldest report, toss it */
+	if (_next_report == _oldest_report) {
+		_buf_overflows++;
+		INCREMENT(_oldest_report, _num_reports);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+out:
+	perf_end(_sample_perf);
+	return ret;
+}
+
+int
+HMC5883::write_reg(uint8_t reg, uint8_t val)
+{
+	uint8_t cmd[] = { reg, val };
+
+	return transfer(&cmd[0], 2, nullptr, 0);
+}
+
+int
+HMC5883::read_reg(uint8_t reg, uint8_t &val)
+{
+	return transfer(&reg, 1, &val, 1);
+}
+
+float
+HMC5883::meas_to_float(uint8_t in[2])
+{
+	union {
+		uint8_t	b[2];
+		int16_t	w;
+	} u;
+
+	u.b[0] = in[1];
+	u.b[1] = in[0];
+
+	return (float) u.w;
+}
+
+void
+HMC5883::print_info()
+{
+	printf("reads:          %u\n", _reads);
+	printf("measure errors: %u\n", _measure_errors);
+	printf("read errors:    %u\n", _read_errors);
+	printf("read overflows: %u\n", _buf_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
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+HMC5883	*g_dev;
+
+/*
+ * XXX this should just be part of the generic sensors test...
+ */
+
+
+int
+test_fail(const char *fmt, ...)
+{
+	va_list ap;
+
+	fprintf(stderr, "FAIL: ");
+	va_start(ap, fmt);
+	vfprintf(stderr, fmt, ap);
+	va_end(ap);
+	fprintf(stderr, "\n");
+	fflush(stderr);
+	return ERROR;
+}
+
+int
+test_note(const char *fmt, ...)
+{
+	va_list ap;
+
+	fprintf(stderr, "note: ");
+	va_start(ap, fmt);
+	vfprintf(stderr, fmt, ap);
+	va_end(ap);
+	fprintf(stderr, "\n");
+	fflush(stderr);
+	return OK;
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ *
+ * @param fd		An open file descriptor on the driver.
+ */
+int
+test(int fd)
+{
+	struct mag_report report;
+	ssize_t sz;
+	int ret;
+
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report))
+		return test_fail("immediate read failed: %d", errno);
+
+	test_note("single read");
+	test_note("measurement: %.6f  %.6f  %.6f", report.x, report.y, report.z);
+	test_note("time:        %lld", report.timestamp);
+	usleep(1000000);
+
+	/* set the queue depth to 10 */
+	if (OK != ioctl(fd, MAGIOCSQUEUEDEPTH, 10))
+		return test_fail("failed to set queue depth");
+
+	/* start the sensor polling at 2Hz */
+	if (OK != ioctl(fd, MAGIOCSPOLLRATE, 2))
+		return test_fail("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)
+			return test_fail("timed out waiting for sensor data");
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report))
+			return test_fail("periodic read failed: %d", errno);
+
+		test_note("periodic read %u", i);
+		test_note("measurement: %.6f  %.6f  %.6f", report.x, report.y, report.z);
+		test_note("time:        %lld", report.timestamp);
+	}
+
+	return test_note("PASS");
+	return OK;
+}
+
+int
+info()
+{
+	if (g_dev == nullptr) {
+		fprintf(stderr, "HMC5883: driver not running\n");
+		return -ENOENT;
+	}
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	return OK;
+}
+
+
+} // namespace
+
+int
+hmc5883_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 *
+	 * XXX it would be nice to have a wrapper for this...
+	 */
+	if (!strcmp(argv[1], "start")) {
+
+		if (g_dev != nullptr) {
+			fprintf(stderr, "HMC5883: already loaded\n");
+			return -EBUSY;
+		}
+
+		/* create the driver */
+		/* XXX HORRIBLE hack - the bus number should not come from here */
+		g_dev = new HMC5883(2);
+
+		if (g_dev == nullptr) {
+			fprintf(stderr, "HMC5883: driver alloc failed\n");
+			return -ENOMEM;
+		}
+
+		if (OK != g_dev->init()) {
+			fprintf(stderr, "HMC5883: driver init failed\n");
+			usleep(100000);
+			delete g_dev;
+			g_dev = nullptr;
+			return -EIO;
+		}
+
+		return OK;
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test")) {
+		int fd, ret;
+
+		fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+		if (fd < 0)
+			return test_fail("driver open failed: %d", errno);
+
+		ret = test(fd);
+		close(fd);
+		return ret;
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info"))
+		return info();
+
+	fprintf(stderr, "unrecognised command, try 'start', 'test' or 'info'\n");
+	return -EINVAL;
+}

apps/drivers/mpu6000/mpu6000.cpp

@@ -181,12 +181,12 @@ private:
 	struct accel_report	_accel_report;
 	struct accel_scale	_accel_scale;
 	float			_accel_range_scale;
-	int			_accel_topic;
+	orb_advert_t		_accel_topic;
 
 	struct gyro_report	_gyro_report;
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
-	int			_gyro_topic;
+	orb_advert_t		_gyro_topic;
 
 	unsigned		_reads;
 	perf_counter_t		_sample_perf;

apps/drivers/ms5611/ms5611.cpp

@@ -125,7 +125,7 @@ private:
 	int32_t			_dT;
 	int64_t			_temp64;
 
-	int			_baro_topic;
+	orb_advert_t		_baro_topic;
 
 	unsigned		_reads;
 	unsigned		_measure_errors;
@@ -246,6 +246,7 @@ MS5611::MS5611(int bus) :
 	_measure_phase(0),
 	_dT(0),
 	_temp64(0),
+	_baro_topic(-1),
 	_reads(0),
 	_measure_errors(0),
 	_read_errors(0),
@@ -277,18 +278,6 @@ MS5611::init()
 	/* do I2C init (and probe) first */
 	ret = I2C::init();
 
-	/* assuming we're good, advertise the object */
-	if (ret == OK) {
-		struct baro_report b;
-
-		/* if this fails (e.g. no object in the system) that's OK */
-		memset(&b, 0, sizeof(b));
-		_baro_topic = orb_advertise(ORB_ID(sensor_baro), &b);
-
-		if (_baro_topic < 0)
-			debug("failed to create sensor_baro object");
-	}
-
 	return ret;
 }
 
@@ -538,6 +527,25 @@ MS5611::cycle_trampoline(void *arg)
 void
 MS5611::cycle()
 {
+	/*
+	 * We have to publish the baro topic in the context of the workq
+	 * in order to ensure that the descriptor is valid when we go to publish.
+	 *
+	 * @bug	We can't really ever be torn down and restarted, since this
+	 *      descriptor will never be closed and on the restart we will be
+	 *      unable to re-advertise.
+	 */
+	if (_baro_topic == -1) {
+		struct baro_report b;
+
+		/* if this fails (e.g. no object in the system) we will cope */
+		memset(&b, 0, sizeof(b));
+		_baro_topic = orb_advertise(ORB_ID(sensor_baro), &b);
+
+		if (_baro_topic < 0)
+			debug("failed to create sensor_baro object");
+	}
+
 	/* collection phase? */
 	if (_collect_phase) {
 

apps/examples/px4_simple_app/px4_simple_app.c

@@ -59,7 +59,7 @@ int px4_simple_app_main(int argc, char *argv[])
 	/* advertise attitude topic */
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
-	int att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att);
+	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
 	/* one could wait for multiple topics with this technique, just using one here */
 	struct pollfd fds[] = {
@@ -103,7 +103,7 @@ int px4_simple_app_main(int argc, char *argv[])
 				att.roll = raw.accelerometer_m_s2[0];
 				att.pitch = raw.accelerometer_m_s2[1];
 				att.yaw = raw.accelerometer_m_s2[2];
-				orb_publish(ORB_ID(vehicle_attitude), att_pub_fd, &att);
+				orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
 			}
 			/* there could be more file descriptors here, in the form like:
 			 * if (fds[1..n].revents & POLLIN) {}

apps/fixedwing_control/fixedwing_control.c

@@ -675,7 +675,7 @@ int fixedwing_control_main(int argc, char *argv[])
 
 	/* Set up to publish fixed wing control messages */
 	struct fixedwing_control_s control;
-	int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control);
+	orb_advert_t fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control);
 
 	/* Subscribe to global position, attitude and rc */
 	struct vehicle_global_position_s global_pos;

apps/gps/mtk.c

@@ -311,7 +311,7 @@ void *mtk_loop(void *arg)
 	/* advertise GPS topic */
 	struct vehicle_gps_position_s mtk_gps_d;
 	mtk_gps = &mtk_gps_d;
-	int gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), &mtk_gps);
+	orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), &mtk_gps);
 
 	while (1) {
 		/* Parse a message from the gps receiver */

apps/gps/nmea_helper.c

@@ -176,7 +176,7 @@ void *nmea_loop(void *arg)
 	/* advertise GPS topic */
 	struct vehicle_gps_position_s nmea_gps_d = {0};
 	nmea_gps = &nmea_gps_d;
-	int gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps);
+	orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps);
 
 	while (1) {
 		/* Parse a message from the gps receiver */

apps/gps/ubx.c

@@ -842,7 +842,7 @@ void *ubx_loop(void *arg)
 
 	ubx_gps = &ubx_gps_d;
 
-	int gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps);
+	orb_advert_t gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps);
 
 	while (1) {
 		/* Parse a message from the gps receiver */

apps/mavlink/mavlink.c

@@ -113,7 +113,7 @@ static struct vehicle_status_s v_status;
 static struct rc_channels_s rc;
 
 /* HIL publishers */
-static int pub_hil_attitude = -1;
+static orb_advert_t pub_hil_attitude = -1;
 
 /** HIL attitude */
 static struct vehicle_attitude_s hil_attitude;
@@ -126,16 +126,16 @@ static struct ardrone_motors_setpoint_s ardrone_motors;
 
 static struct vehicle_command_s vcmd;
 
-static int pub_hil_global_pos = -1;
-static int ardrone_motors_pub = -1;
-static int cmd_pub = -1;
+static orb_advert_t pub_hil_global_pos = -1;
+static orb_advert_t ardrone_motors_pub = -1;
+static orb_advert_t cmd_pub = -1;
 static int sensor_sub = -1;
 static int att_sub = -1;
 static int global_pos_sub = -1;
 static int local_pos_sub = -1;
-static int flow_pub = -1;
-static int global_position_setpoint_pub = -1;
-static int local_position_setpoint_pub = -1;
+static orb_advert_t flow_pub = -1;
+static orb_advert_t global_position_setpoint_pub = -1;
+static orb_advert_t local_position_setpoint_pub = -1;
 static bool mavlink_hil_enabled = false;
 
 static char mavlink_message_string[51] = {0};

apps/multirotor_att_control/multirotor_att_control_main.c

@@ -119,9 +119,9 @@ mc_thread_main(int argc, char *argv[])
 	/* publish actuator controls */
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
 		actuators.control[i] = 0.0f;
-	int actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
+	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
 	armed.armed = true;
-	int armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
 
 	/* register the perf counter */
 	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control");

apps/multirotor_pos_control/multirotor_pos_control.c

@@ -89,7 +89,7 @@ mpc_thread_main(int argc, char *argv[])
 	int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
 
 	/* publish attitude setpoint */
-	int att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 
 	while (1) {
 		/* get a local copy of the vehicle state */

apps/position_estimator/position_estimator_main.c

@@ -297,7 +297,7 @@ int position_estimator_main(int argc, char *argv[])
 		.lon = lon_current * 1e7,
 		.alt = gps.alt
 	};
-	int global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+	orb_advert_t global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
 
 	printf("[multirotor position estimator] initialized projection with: lat: %.10f,  lon:%.10f\n", lat_current, lon_current);
 

apps/px4/attitude_estimator_bm/attitude_estimator_bm.c

@@ -87,14 +87,25 @@ int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *eul
 	gyro_values.z =  raw->gyro_rad_s[2];
 
 	float_vect3 accel_values;
-	accel_values.x = raw->accelerometer_m_s2[0] * 9.81f * 9.0f;
-	accel_values.y = raw->accelerometer_m_s2[1] * 9.81f * 9.0f;
-	accel_values.z = raw->accelerometer_m_s2[2] * 9.81f * 9.0f;
+	accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 100;
+	accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 100;
+	accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 100;
 
 	float_vect3 mag_values;
-	mag_values.x = raw->magnetometer_ga[0]*510.0f;
-	mag_values.y = raw->magnetometer_ga[1]*510.0f;
-	mag_values.z = raw->magnetometer_ga[2]*510.0f;
+	mag_values.x = raw->magnetometer_ga[0]*456.0f;
+	mag_values.y = raw->magnetometer_ga[1]*456.0f;
+	mag_values.z = raw->magnetometer_ga[2]*456.0f;
+
+	static int i = 0;
+
+	if (i == 500) {
+		printf("gyro: %8.4f\t%8.4f\t%8.4f\t accel: %8.4f\t%8.4f\t%8.4f\t mag: %8.4f\t%8.4f\t%8.4f\t\n",
+			gyro_values.x, gyro_values.y, gyro_values.z,
+			accel_values.x, accel_values.y, accel_values.z,
+			mag_values.x, mag_values.y, mag_values.z);
+		i = 0;
+	}
+	i++;
 
 	attitude_blackmagic(&accel_values, &mag_values, &gyro_values);
 
@@ -143,7 +154,7 @@ int attitude_estimator_bm_main(int argc, char *argv[])
 	bool publishing = false;
 
 	/* advertise attitude */
-	int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
+	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 	publishing = true;
 
 	struct pollfd fds[] = {

apps/px4/fmu/fmu.cpp

@@ -64,6 +64,7 @@
 #include <arch/board/up_pwm_servo.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_outputs.h>
 
 
 class FMUServo : public device::CDev
@@ -88,6 +89,7 @@ private:
 	int		_task;
 	int		_t_actuators;
 	int		_t_armed;
+	orb_advert_t	_t_outputs;
 	unsigned	_num_outputs;
 
 	volatile bool	_task_should_exit;
@@ -98,15 +100,12 @@ private:
 	actuator_controls_s _controls;
 
 	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main();
+	void		task_main() __attribute__((noreturn));
 
-	static int	control_callback_trampoline(uintptr_t handle,
+	static int	control_callback(uintptr_t handle,
 			uint8_t control_group,
 			uint8_t control_index,
 			float &input);
-	int		control_callback(uint8_t control_group,
-					 uint8_t control_index,
-					 float &input);
 };
 
 namespace
@@ -212,6 +211,11 @@ FMUServo::task_main()
 	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
 	orb_set_interval(_t_armed, 100);		/* 10Hz update rate */
 
+	/* advertise the mixed control outputs */
+	struct actuator_output_s outputs;
+	memset(&outputs, 0, sizeof(outputs));
+	_t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
+
 	struct pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
@@ -237,7 +241,6 @@ FMUServo::task_main()
 
 		/* do we have a control update? */
 		if (fds[0].revents & POLLIN) {
-			float outputs[num_outputs];
 
 			/* get controls - must always do this to avoid spinning */
 			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
@@ -246,14 +249,20 @@ FMUServo::task_main()
 			if (_mixers != nullptr) {
 
 				/* do mixing */
-				_mixers->mix(&outputs[0], num_outputs);
+				_mixers->mix(&outputs.output[0], num_outputs);
 
 				/* iterate actuators */
 				for (unsigned i = 0; i < num_outputs; i++) {
 
 					/* scale for PWM output 900 - 2100us */
-					up_pwm_servo_set(i, 1500 + (600 * outputs[i]));
+					outputs.output[i] = 1500 + (600 * outputs.output[i]);
+
+					/* output to the servo */
+					up_pwm_servo_set(i, outputs.output[i]);
 				}
+
+				/* and publish for anyone that cares to see */
+				orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
 			}
 		}
 
@@ -264,13 +273,14 @@ FMUServo::task_main()
 			/* get new value */
 			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
 
-			/* update PMW servo armed status */
+			/* update PWM servo armed status */
 			up_pwm_servo_arm(aa.armed);
 		}
 	}
 
 	::close(_t_actuators);
 	::close(_t_armed);
+	::close(_t_outputs);
 
 	/* make sure servos are off */
 	up_pwm_servo_deinit();
@@ -285,24 +295,14 @@ FMUServo::task_main()
 }
 
 int
-FMUServo::control_callback_trampoline(uintptr_t handle,
+FMUServo::control_callback(uintptr_t handle,
 				      uint8_t control_group,
 				      uint8_t control_index,
 				      float &input)
 {
-	return ((FMUServo *)handle)->control_callback(control_group, control_index, input);
-}
+	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
-int
-FMUServo::control_callback(uint8_t control_group,
-			   uint8_t control_index,
-			   float &input)
-{
-	/* XXX currently only supporting group zero */
-	if ((control_group != 0) || (control_index > NUM_ACTUATOR_CONTROLS))
-		return -1;
-
-	input = _controls.control[control_index];
+	input = controls->control[control_index];
 	return 0;
 }
 
@@ -377,7 +377,8 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MIXERIOCADDSIMPLE: {
 			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
 
-			SimpleMixer *mixer = new SimpleMixer(control_callback_trampoline, (uintptr_t)this, mixinfo);
+			SimpleMixer *mixer = new SimpleMixer(control_callback,
+							     (uintptr_t)&_controls, mixinfo);
 
 			if (mixer->check()) {
 				delete mixer;
@@ -385,7 +386,8 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 			} else {
 				if (_mixers == nullptr)
-					_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
+					_mixers = new MixerGroup(control_callback,
+								 (uintptr_t)&_controls);
 
 				_mixers->add_mixer(mixer);
 			}
@@ -406,7 +408,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 				_mixers = nullptr;
 			}
 
-			_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
+			_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
 
 			if (_mixers->load_from_file(path) != 0) {
 				delete _mixers;
@@ -564,15 +566,13 @@ fake(int argc, char *argv[])
 
 	ac.control[3] = strtol(argv[4], 0, 0) / 100.0f;
 
-	int handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
+	orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
 
 	if (handle < 0) {
 		puts("advertise failed");
 		exit(1);
 	}
 
-	close(handle);
-
 	exit(0);
 }
 

apps/px4/px4io/driver/px4io.cpp

@@ -99,7 +99,7 @@ protected:
 	void			set_channels(unsigned count, const servo_position_t *data);
 
 private:
-	int			_publication;
+	orb_advert_t		_publication;
 	struct rc_input_values	_input;
 };
 

apps/sensors/sensors.c

@@ -124,8 +124,8 @@ extern unsigned ppm_decoded_channels;
 extern uint64_t ppm_last_valid_decode;
 #endif
 
-/* file handle that will be used for subscribing */
-static int sensor_pub;
+/* ORB topic publishing our results */
+static orb_advert_t sensor_pub;
 
 /**
  * Sensor readout and publishing.
@@ -404,7 +404,7 @@ int sensors_thread_main(int argc, char *argv[])
 						   .yaw = 0.0f,
 						   .throttle = 0.0f };
 
-	int manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
+	orb_advert_t manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
 
 	if (manual_control_pub < 0) {
 		fprintf(stderr, "[sensors] ERROR: orb_advertise for topic manual_control_setpoint failed.\n");
@@ -413,7 +413,7 @@ int sensors_thread_main(int argc, char *argv[])
 	/* advertise the rc topic */
 	struct rc_channels_s rc;
 	memset(&rc, 0, sizeof(rc));
-	int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
+	orb_advert_t rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
 
 	if (rc_pub < 0) {
 		fprintf(stderr, "[sensors] ERROR: orb_advertise for topic rc_channels failed.\n");
@@ -466,7 +466,6 @@ int sensors_thread_main(int argc, char *argv[])
 				if (vstatus.flag_hil_enabled && !hil_enabled) {
 					hil_enabled = true;
 					publishing = false;
-					int ret = close(sensor_pub);
 					printf("[sensors] Closing sensor pub: %i \n", ret);
 
 					/* switching from HIL to non-HIL mode */
@@ -514,6 +513,7 @@ int sensors_thread_main(int argc, char *argv[])
 				mag_offset[1] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_YOFFSET];
 				mag_offset[2] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_ZOFFSET];
 
+
 				paramcounter = 0;
 			}
 
@@ -565,7 +565,7 @@ int sensors_thread_main(int argc, char *argv[])
 			if (ret_accelerometer != sizeof(buf_accelerometer)) {
 				acc_fail_count++;
 
-				if (acc_fail_count & 0b1000 || (acc_fail_count > 20 && acc_fail_count < 100)) {
+				if (acc_fail_count & 0b111 || (acc_fail_count > 20 && acc_fail_count < 100)) {
 					fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
 				}
 
@@ -618,7 +618,7 @@ int sensors_thread_main(int argc, char *argv[])
 				if (ret_magnetometer != sizeof(buf_magnetometer)) {
 					mag_fail_count++;
 
-					if (mag_fail_count & 0b1000 || (mag_fail_count > 20 && mag_fail_count < 100)) {
+					if (mag_fail_count & 0b111 || (mag_fail_count > 20 && mag_fail_count < 100)) {
 						fprintf(stderr, "[sensors] HMC5883L ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
 					}
 

apps/systemlib/mixer/mixer_group.cpp

@@ -217,7 +217,7 @@ mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, co
 	char geomname[8];
 	int s[4];
 
-	if (sscanf(buf, "R: %7s %d %d %d %d", geomname, &s[0], &s[1], &s[2], &s[3]) != 5) {
+	if (sscanf(buf, "R: %s %d %d %d %d", geomname, &s[0], &s[1], &s[2], &s[3]) != 5) {
 		debug("multirotor parse failed on '%s'", buf);
 		return nullptr;
 	}

apps/uORB/Makefile

@@ -37,6 +37,6 @@
 
 APPNAME		 = uorb
 PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
+STACKSIZE	 = 4096
 
 include $(APPDIR)/mk/app.mk

apps/uORB/objects_common.cpp

@@ -114,3 +114,9 @@ ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
 ORB_DEFINE(actuator_armed, struct actuator_armed_s);
+
+#include "topics/actuator_outputs.h"
+ORB_DEFINE(actuator_outputs_0, struct actuator_output_s);
+ORB_DEFINE(actuator_outputs_1, struct actuator_output_s);
+ORB_DEFINE(actuator_outputs_2, struct actuator_output_s);
+ORB_DEFINE(actuator_outputs_3, struct actuator_output_s);

apps/uORB/topics/actuator_outputs.h

@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file actuator_outputs.h
+ *
+ * Actuator output values.
+ *
+ * Values published to these topics are the outputs of the control mixing
+ * system as sent to the actuators (servos, motors, etc.) that operate
+ * the vehicle.
+ *
+ * Each topic can be published by a single output driver.
+ */
+
+#ifndef TOPIC_ACTUATOR_OUTPUTS_H
+#define TOPIC_ACTUATOR_OUTPUTS_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+#define NUM_ACTUATOR_OUTPUTS		16
+#define NUM_ACTUATOR_OUTPUT_GROUPS	4	/**< for sanity checking */
+
+struct actuator_output_s {
+	float	output[NUM_ACTUATOR_OUTPUTS];
+};
+
+/* actuator output sets; this list can be expanded as more drivers emerge */
+ORB_DECLARE(actuator_outputs_0);
+ORB_DECLARE(actuator_outputs_1);
+ORB_DECLARE(actuator_outputs_2);
+ORB_DECLARE(actuator_outputs_3);
+
+/* output sets with pre-defined applications */
+#define ORB_ID_VEHICLE_CONTROLS		ORB_ID(actuator_outputs_0)
+
+#endif

apps/uORB/uORB.cpp

@@ -112,6 +112,8 @@ public:
 	virtual ssize_t		write(struct file *filp, const char *buffer, size_t buflen);
 	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
 
+	static ssize_t		publish(const orb_metadata *meta, orb_advert_t handle, const void *data);
+
 protected:
 	virtual pollevent_t	poll_state(struct file *filp);
 	virtual void		poll_notify_one(struct pollfd *fds, pollevent_t events);
@@ -298,6 +300,8 @@ ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen)
 	 *
 	 * Writes outside interrupt context will allocate the object
 	 * if it has not yet been allocated.
+	 *
+	 * Note that filp will usually be NULL.
 	 */
 	if (nullptr == _data) {
 		if (!up_interrupt_context()) {
@@ -353,12 +357,42 @@ ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg)
 		sd->update_interval = arg;
 		return OK;
 
+	case ORBIOCGADVERTISER:
+		*(uintptr_t *)arg = (uintptr_t)this;
+		return OK;
+
 	default:
 		/* give it to the superclass */
 		return CDev::ioctl(filp, cmd, arg);
 	}
 }
 
+ssize_t
+ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data)
+{
+	ORBDevNode *devnode = (ORBDevNode *)handle;
+	int ret;
+
+	/* this is a bit risky, since we are trusting the handle in order to deref it */
+	if (devnode->_meta != meta) {
+		errno = EINVAL;
+		return ERROR;
+	}
+
+	/* call the devnode write method with no file pointer */
+	ret = devnode->write(nullptr, (const char *)data, meta->o_size);
+
+	if (ret < 0)
+		return ERROR;
+
+	if (ret != (int)meta->o_size) {
+		errno = EIO;
+		return ERROR;
+	}
+
+	return OK;
+}
+
 pollevent_t
 ORBDevNode::poll_state(struct file *filp)
 {
@@ -614,7 +648,7 @@ test()
 	if (pfd < 0)
 		return test_fail("advertise failed: %d", errno);
 
-	test_note("publish fd %d", pfd);
+	test_note("publish handle 0x%08x", pfd);
 	sfd = orb_subscribe(ORB_ID(orb_test));
 
 	if (sfd < 0)
@@ -877,29 +911,35 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
 		return ERROR;
 	}
 
-	/* the advertiser must perform an initial publish to initialise the object */
-	if (advertiser) {
-		ret = orb_publish(meta, fd, data);
-
-		if (ret != OK) {
-			/* save errno across the close */
-			ret = errno;
-			close(fd);
-			errno = ret;
-			return ERROR;
-		}
-	}
-
 	/* everything has been OK, we can return the handle now */
 	return fd;
 }
 
 } // namespace
 
-int
+orb_advert_t
 orb_advertise(const struct orb_metadata *meta, const void *data)
 {
-	return node_open(PUBSUB, meta, data, true);
+	int result, fd;
+	orb_advert_t advertiser;
+
+	/* open the node as an advertiser */
+	fd = node_open(PUBSUB, meta, data, true);
+	if (fd == ERROR)
+		return ERROR;
+
+	/* get the advertiser handle and close the node */
+	result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
+	close(fd);
+	if (result == ERROR)
+		return ERROR;
+
+	/* the advertiser must perform an initial publish to initialise the object */
+	result= orb_publish(meta, advertiser, data);
+	if (result == ERROR)
+		return ERROR;
+
+	return advertiser;
 }
 
 int
@@ -915,21 +955,9 @@ orb_unsubscribe(int handle)
 }
 
 int
-orb_publish(const struct orb_metadata *meta, int handle, const void *data)
+orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
 {
-	int ret;
-
-	ret = write(handle, data, meta->o_size);
-
-	if (ret < 0)
-		return ERROR;
-
-	if (ret != (int)meta->o_size) {
-		errno = EIO;
-		return ERROR;
-	}
-
-	return OK;
+	return ORBDevNode::publish(meta, handle, data);
 }
 
 int

apps/uORB/uORB.h

@@ -105,12 +105,27 @@ struct orb_metadata {
 
 __BEGIN_DECLS
 
+/**
+ * ORB topic advertiser handle.
+ *
+ * Advertiser handles are global; once obtained they can be shared freely
+ * and do not need to be closed or released.
+ *
+ * This permits publication from interrupt context and other contexts where
+ * a file-descriptor-based handle would not otherwise be in scope for the
+ * publisher.
+ */
+typedef intptr_t	orb_advert_t;
+
 /**
  * Advertise as the publisher of a topic.
  *
  * This performs the initial advertisement of a topic; it creates the topic
  * node in /obj if required and publishes the initial data.
  *
+ * Any number of advertisers may publish to a topic; publications are atomic
+ * but co-ordination between publishers is not provided by the ORB. 
+ *
  * @param meta		The uORB metadata (usually from the ORB_ID() macro)
  *			for the topic.
  * @param data		A pointer to the initial data to be published.
@@ -122,7 +137,7 @@ __BEGIN_DECLS
  *			ORB_DEFINE with no corresponding ORB_DECLARE)
  *			this function will return -1 and set errno to ENOENT.
  */
-extern int	orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT;
+extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT;
 
 /**
  * Publish new data to a topic.
@@ -131,13 +146,13 @@ extern int	orb_advertise(const struct orb_metadata *meta, const void *data) __EX
  * will be notified.  Subscribers that are not waiting can check the topic
  * for updates using orb_check and/or orb_stat.
  *
- * @handle		The handle returned from orb_advertise.
  * @param meta		The uORB metadata (usually from the ORB_ID() macro)
  *			for the topic.
+ * @handle		The handle returned from orb_advertise.
  * @param data		A pointer to the data to be published.
  * @return		OK on success, ERROR otherwise with errno set accordingly.
  */
-extern int	orb_publish(const struct orb_metadata *meta, int handle, const void *data) __EXPORT;
+extern int	orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT;
 
 /**
  * Subscribe to a topic.