Merge remote-tracking branch 'upstream/master' into ros_messagelayer_merge2_attctrl_posctrl

Conflicts:
	src/drivers/px4fmu/fmu.cpp

ROMFS/px4fmu_test/init.d/rcS

@@ -97,6 +97,13 @@ else
 	set unit_test_failure_list "${unit_test_failure_list} commander_tests"
 fi
 
+if uorb test
+then
+else
+	set unit_test_failure 1
+	set unit_test_failure_list "${unit_test_failure_list} uorb_tests"
+fi
+
 if hmc5883 -I start
 then
 	# This is an FMUv3

makefiles/config_px4fmu-v2_test.mk

@@ -32,6 +32,7 @@ MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/ver
+MODULES		+= systemcmds/top
 
 #
 # System commands

nuttx-configs/px4fmu-v1/nsh/defconfig

@@ -92,7 +92,7 @@ CONFIG_ARCH_HAVE_MPU=y
 #
 # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
 CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
-CONFIG_ARMV7M_STACKCHECK=y
+CONFIG_ARMV7M_STACKCHECK=n
 CONFIG_SERIAL_TERMIOS=y
 
 #

nuttx-configs/px4io-v1/nsh/defconfig

@@ -93,7 +93,7 @@ CONFIG_ARCH_DMA=y
 CONFIG_ARCH_MATH_H=y
 
 CONFIG_ARMV7M_CMNVECTOR=y
-# CONFIG_ARMV7M_STACKCHECK is not set
+CONFIG_ARMV7M_STACKCHECK=n
 
 #
 # JTAG Enable settings (by default JTAG-DP and SW-DP are disabled):

nuttx-configs/px4io-v2/nsh/defconfig

@@ -89,7 +89,7 @@ CONFIG_ARCH_DMA=y
 CONFIG_ARCH_MATH_H=y
 
 CONFIG_ARMV7M_CMNVECTOR=y
-
+CONFIG_ARMV7M_STACKCHECK=n
 #
 # JTAG Enable settings (by default JTAG-DP and SW-DP are disabled):
 #

src/drivers/ardrone_interface/ardrone_motor_control.c

@@ -339,7 +339,8 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
 	outputs.output[3] = motor4;
 	static orb_advert_t pub = 0;
 	if (pub == 0) {
-		pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
+		/* advertise to channel 0 / primary */
+		pub = orb_advertise(ORB_ID(actuator_outputs), &outputs);
 	}
 
 	if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
@@ -350,7 +351,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
 		fsync(ardrone_fd);
 
 		/* publish just written values */
-		orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs);
+		orb_publish(ORB_ID(actuator_outputs), pub, &outputs);
 
 		if (ret == sizeof(buf)) {
 			return OK;

src/drivers/drv_accel.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -81,9 +81,7 @@ struct accel_scale {
 /*
  * ObjDev tag for raw accelerometer data.
  */
-ORB_DECLARE(sensor_accel0);
+ORB_DECLARE(sensor_accel);
-ORB_DECLARE(sensor_accel1);
-ORB_DECLARE(sensor_accel2);
 
 /*
  * ioctl() definitions

src/drivers/drv_baro.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -63,9 +63,7 @@ struct baro_report {
 /*
  * ObjDev tag for raw barometer data.
  */
-ORB_DECLARE(sensor_baro0);
+ORB_DECLARE(sensor_baro);
-ORB_DECLARE(sensor_baro1);
-ORB_DECLARE(sensor_baro2);
 
 /*
  * ioctl() definitions

src/drivers/drv_blinkm.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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

src/drivers/drv_gyro.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -81,9 +81,7 @@ struct gyro_scale {
 /*
  * ObjDev tag for raw gyro data.
  */
-ORB_DECLARE(sensor_gyro0);
+ORB_DECLARE(sensor_gyro);
-ORB_DECLARE(sensor_gyro1);
-ORB_DECLARE(sensor_gyro2);
 
 /*
  * ioctl() definitions

src/drivers/drv_mag.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -41,7 +41,6 @@
 #include <stdint.h>
 #include <sys/ioctl.h>
 
-#include "drv_device.h"
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
 
@@ -81,15 +80,8 @@ struct mag_scale {
 /*
  * ObjDev tag for raw magnetometer data.
  */
-ORB_DECLARE(sensor_mag0);
+ORB_DECLARE(sensor_mag);
-ORB_DECLARE(sensor_mag1);
-ORB_DECLARE(sensor_mag2);
 
-/*
- * mag device types, for _device_id
- */
-#define DRV_MAG_DEVTYPE_HMC5883 1
-#define DRV_MAG_DEVTYPE_LSM303D 2
 
 /*
  * ioctl() definitions

src/drivers/drv_orb_dev.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,7 +35,9 @@
 #define _DRV_UORB_H
 
 /**
- * @file uORB published object driver.
+ * @file drv_orb_dev.h
+ * 
+ * uORB published object driver.
  */
 
 #include <sys/types.h>
@@ -84,4 +86,7 @@
 /** Get the global advertiser handle for the topic */
 #define ORBIOCGADVERTISER	_ORBIOC(13)
 
+/** Get the priority for the topic */
+#define ORBIOCGPRIORITY		_ORBIOC(14)
+
 #endif /* _DRV_UORB_H */

src/drivers/drv_sensor.h

@@ -43,6 +43,24 @@
 #include <stdint.h>
 #include <sys/ioctl.h>
 
+#include "drv_device.h"
+
+/**
+ * Sensor type definitions.
+ *
+ * Used to create a unique device id for redundant and combo sensors
+ */
+
+#define DRV_MAG_DEVTYPE_HMC5883 0x01
+#define DRV_MAG_DEVTYPE_LSM303D 0x02
+#define DRV_ACC_DEVTYPE_LSM303D 0x11
+#define DRV_ACC_DEVTYPE_BMA180  0x12
+#define DRV_ACC_DEVTYPE_MPU6000 0x13
+#define DRV_GYR_DEVTYPE_MPU6000 0x21
+#define DRV_GYR_DEVTYPE_L3GD20  0x22
+#define DRV_RNG_DEVTYPE_MB12XX  0x31
+#define DRV_RNG_DEVTYPE_LL40LS  0x32
+
 /*
  * ioctl() definitions
  *

src/drivers/hil/hil.cpp

@@ -332,8 +332,9 @@ HIL::task_main()
 	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
 	/* advertise the mixed control outputs */
-	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
+	int dummy;
-				   &outputs);
+	_t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
+				   &outputs, &dummy, ORB_PRIO_LOW);
 
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
@@ -425,7 +426,7 @@ HIL::task_main()
 				}
 
 				/* and publish for anyone that cares to see */
-				orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
+				orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs);
 			}
 		}
 

src/drivers/hmc5883/hmc5883.cpp

@@ -69,7 +69,6 @@
 #include <drivers/drv_device.h>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 
 #include <float.h>
 #include <getopt.h>
@@ -157,10 +156,9 @@ private:
 	float 			_range_ga;
 	bool			_collect_phase;
 	int			_class_instance;
+	int			_orb_class_instance;
 
 	orb_advert_t		_mag_topic;
-	orb_advert_t		_subsystem_pub;
-	orb_id_t		_mag_orb_id;
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
@@ -348,9 +346,8 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota
 	_range_ga(1.3f),
 	_collect_phase(false),
 	_class_instance(-1),
+	_orb_class_instance(-1),
 	_mag_topic(-1),
-	_subsystem_pub(-1),
-	_mag_orb_id(nullptr),
 	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@@ -419,7 +416,6 @@ HMC5883::init()
 	reset();
 
 	_class_instance = register_class_devname(MAG_DEVICE_PATH);
-	_mag_orb_id = ORB_ID_TRIPLE(sensor_mag, _class_instance);
 
 	ret = OK;
 	/* sensor is ok, but not calibrated */
@@ -850,6 +846,7 @@ HMC5883::collect()
 
 	perf_begin(_sample_perf);
 	struct mag_report new_report;
+	bool sensor_is_onboard = false;
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	new_report.timestamp = hrt_absolute_time();
@@ -902,7 +899,8 @@ HMC5883::collect()
 
 	// XXX revisit for SPI part, might require a bus type IOCTL
 	unsigned dummy;
-	if (!_interface->ioctl(MAGIOCGEXTERNAL, dummy)) {
+	sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
+	if (sensor_is_onboard) {
 		// convert onboard so it matches offboard for the
 		// scaling below
 		report.y = -report.y;
@@ -925,9 +923,10 @@ HMC5883::collect()
 
 		if (_mag_topic != -1) {
 			/* publish it */
-			orb_publish(_mag_orb_id, _mag_topic, &new_report);
+			orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
 		} else {
-			_mag_topic = orb_advertise(_mag_orb_id, &new_report);
+			_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
+				&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
 
 			if (_mag_topic < 0)
 				debug("ADVERT FAIL");
@@ -1185,24 +1184,6 @@ int HMC5883::check_calibration()
 		warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
 					  (offset_valid) ? "" : "offset invalid");
 		_calibrated = (offset_valid && scale_valid);
-
-
-		// XXX Change advertisement
-
-		/* notify about state change */
-		struct subsystem_info_s info = {
-			true,
-			true,
-			_calibrated,
-			SUBSYSTEM_TYPE_MAG};
-
-		if (!(_pub_blocked)) {
-			if (_subsystem_pub > 0) {
-				orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info);
-			} else {
-				_subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info);
-			}
-		}
 	}
 
 	/* return 0 if calibrated, 1 else */

src/drivers/l3gd20/l3gd20.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -33,7 +33,7 @@
 
 /**
  * @file l3gd20.cpp
- * Driver for the ST L3GD20 MEMS gyro connected via SPI.
+ * Driver for the ST L3GD20 MEMS and L3GD20H mems gyros connected via SPI.
  *
  * Note: With the exception of the self-test feature, the ST L3G4200D is
  *       also supported by this driver.
@@ -179,6 +179,12 @@ static const int ERROR = -1;
 #define L3GD20_DEFAULT_FILTER_FREQ		30
 #define L3GD20_TEMP_OFFSET_CELSIUS		40
 
+#ifdef PX4_SPI_BUS_EXT
+#define EXTERNAL_BUS PX4_SPI_BUS_EXT
+#else
+#define EXTERNAL_BUS 0
+#endif
+
 #ifndef SENSOR_BOARD_ROTATION_DEFAULT
 #define SENSOR_BOARD_ROTATION_DEFAULT		SENSOR_BOARD_ROTATION_270_DEG
 #endif
@@ -214,14 +220,14 @@ private:
 
 	struct hrt_call		_call;
 	unsigned		_call_interval;
-	
+
 	RingBuffer		*_reports;
 
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
 	float			_gyro_range_rad_s;
 	orb_advert_t		_gyro_topic;
-	orb_id_t		_orb_id;
+	int			_orb_class_instance;
 	int			_class_instance;
 
 	unsigned		_current_rate;
@@ -273,6 +279,13 @@ private:
 	 */
 	void			disable_i2c();
 
+	/**
+	 * Get the internal / external state
+	 *
+	 * @return true if the sensor is not on the main MCU board
+	 */
+	bool			is_external() { return (_bus == EXTERNAL_BUS); }
+
 	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
@@ -391,7 +404,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
 	_gyro_topic(-1),
-	_orb_id(nullptr),
+	_orb_class_instance(-1),
 	_class_instance(-1),
 	_current_rate(0),
 	_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
@@ -411,6 +424,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
 	// enable debug() calls
 	_debug_enabled = true;
 
+	_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_L3GD20;
+
 	// default scale factors
 	_gyro_scale.x_offset = 0;
 	_gyro_scale.x_scale  = 1.0f;
@@ -456,20 +471,6 @@ L3GD20::init()
 
 	_class_instance = register_class_devname(GYRO_DEVICE_PATH);
 
-	switch (_class_instance) {
-		case CLASS_DEVICE_PRIMARY:
-			_orb_id = ORB_ID(sensor_gyro0);
-			break;
-
-		case CLASS_DEVICE_SECONDARY:
-			_orb_id = ORB_ID(sensor_gyro1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_orb_id = ORB_ID(sensor_gyro2);
-			break;
-	}
-
 	reset();
 
 	measure();
@@ -478,7 +479,8 @@ L3GD20::init()
 	struct gyro_report grp;
 	_reports->get(&grp);
 
-	_gyro_topic = orb_advertise(_orb_id, &grp);
+	_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
+		&_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
 
 	if (_gyro_topic < 0) {
 		debug("failed to create sensor_gyro publication");
@@ -639,7 +641,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 			return -ENOMEM;
 		}
 		irqrestore(flags);
-		
+
 		return OK;
 	}
 
@@ -867,7 +869,7 @@ L3GD20::reset()
 	disable_i2c();
 
 	/* set default configuration */
-	write_checked_reg(ADDR_CTRL_REG1, 
+	write_checked_reg(ADDR_CTRL_REG1,
                           REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
 	write_checked_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
 	write_checked_reg(ADDR_CTRL_REG3, 0x08);        /* DRDY enable */
@@ -911,7 +913,7 @@ L3GD20::check_registers(void)
 		  if we get the wrong value then we know the SPI bus
 		  or sensor is very sick. We set _register_wait to 20
 		  and wait until we have seen 20 good values in a row
-		  before we consider the sensor to be OK again. 
+		  before we consider the sensor to be OK again.
 		 */
 		perf_count(_bad_registers);
 
@@ -974,7 +976,7 @@ L3GD20::measure()
               we waited for DRDY, but did not see DRDY on all axes
               when we captured. That means a transfer error of some sort
              */
-            perf_count(_errors);            
+            perf_count(_errors);
             return;
         }
 #endif
@@ -994,7 +996,7 @@ L3GD20::measure()
 	 */
 	report.timestamp = hrt_absolute_time();
         report.error_count = perf_event_count(_bad_registers);
-	
+
 	switch (_orientation) {
 
 		case SENSOR_BOARD_ROTATION_000_DEG:
@@ -1050,7 +1052,7 @@ L3GD20::measure()
 	/* publish for subscribers */
 	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(_orb_id, _gyro_topic, &report);
+		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
 	}
 
 	_read++;
@@ -1072,7 +1074,7 @@ L3GD20::print_info()
         for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {
             uint8_t v = read_reg(_checked_registers[i]);
             if (v != _checked_values[i]) {
-                ::printf("reg %02x:%02x should be %02x\n", 
+                ::printf("reg %02x:%02x should be %02x\n",
                          (unsigned)_checked_registers[i],
                          (unsigned)v,
                          (unsigned)_checked_values[i]);

src/drivers/lsm303d/lsm303d.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2015 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
@@ -211,6 +211,12 @@ static const int ERROR = -1;
 
 #define LSM303D_ONE_G					9.80665f
 
+#ifdef PX4_SPI_BUS_EXT
+#define EXTERNAL_BUS PX4_SPI_BUS_EXT
+#else
+#define EXTERNAL_BUS 0
+#endif
+
 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
 
 
@@ -275,7 +281,7 @@ private:
 	unsigned		_mag_samplerate;
 
 	orb_advert_t		_accel_topic;
-	orb_id_t		_accel_orb_id;
+	int			_accel_orb_class_instance;
 	int			_accel_class_instance;
 
 	unsigned		_accel_read;
@@ -295,7 +301,7 @@ private:
 
 	enum Rotation		_rotation;
 
-	// values used to 
+	// values used to
 	float			_last_accel[3];
 	uint8_t			_constant_accel_count;
 
@@ -329,6 +335,13 @@ private:
 	 */
 	void			disable_i2c();
 
+	/**
+	 * Get the internal / external state
+	 *
+	 * @return true if the sensor is not on the main MCU board
+	 */
+	bool			is_external() { return (_bus == EXTERNAL_BUS); }
+
 	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
@@ -507,7 +520,7 @@ private:
 	LSM303D				*_parent;
 
 	orb_advert_t			_mag_topic;
-	orb_id_t			_mag_orb_id;
+	int				_mag_orb_class_instance;
 	int				_mag_class_instance;
 
 	void				measure();
@@ -539,7 +552,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
 	_mag_range_scale(0.0f),
 	_mag_samplerate(0),
 	_accel_topic(-1),
-	_accel_orb_id(nullptr),
+	_accel_orb_class_instance(-1),
 	_accel_class_instance(-1),
 	_accel_read(0),
 	_mag_read(0),
@@ -556,11 +569,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
 	_constant_accel_count(0),
 	_checked_next(0)
 {
-	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
+
 
 	// enable debug() calls
 	_debug_enabled = true;
 
+	_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D;
+	
+	/* Prime _mag with parents devid. */
+	_mag->_device_id.devid = _device_id.devid;
+	_mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
+
+
 	// default scale factors
 	_accel_scale.x_offset = 0.0f;
 	_accel_scale.x_scale  = 1.0f;
@@ -618,7 +638,6 @@ LSM303D::init()
 	if (_accel_reports == nullptr)
 		goto out;
 
-	/* advertise accel topic */
 	_mag_reports = new RingBuffer(2, sizeof(mag_report));
 
 	if (_mag_reports == nullptr)
@@ -641,26 +660,14 @@ LSM303D::init()
 	_mag_reports->get(&mrp);
 
 	/* measurement will have generated a report, publish */
-	switch (_mag->_mag_class_instance) {
+	_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
-		case CLASS_DEVICE_PRIMARY:
+		&_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
-			_mag->_mag_orb_id = ORB_ID(sensor_mag0);
-			break;
-
-		case CLASS_DEVICE_SECONDARY:
-			_mag->_mag_orb_id = ORB_ID(sensor_mag1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_mag->_mag_orb_id = ORB_ID(sensor_mag2);
-			break;
-	}
-
-	_mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp);
 
 	if (_mag->_mag_topic < 0) {
 		warnx("ADVERT ERR");
 	}
 
+
 	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
 
 	/* advertise sensor topic, measure manually to initialize valid report */
@@ -668,21 +675,8 @@ LSM303D::init()
 	_accel_reports->get(&arp);
 
 	/* measurement will have generated a report, publish */
-	switch (_accel_class_instance) {
+	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
-		case CLASS_DEVICE_PRIMARY:
+		&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
-			_accel_orb_id = ORB_ID(sensor_accel0);
-			break;
-
-		case CLASS_DEVICE_SECONDARY:
-			_accel_orb_id = ORB_ID(sensor_accel1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_accel_orb_id = ORB_ID(sensor_accel2);
-			break;
-	}
-
-	_accel_topic = orb_advertise(_accel_orb_id, &arp);
 
 	if (_accel_topic < 0) {
 		warnx("ADVERT ERR");
@@ -712,7 +706,7 @@ LSM303D::reset()
 	disable_i2c();
 
 	/* enable accel*/
-	write_checked_reg(ADDR_CTRL_REG1, 
+	write_checked_reg(ADDR_CTRL_REG1,
 			  REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A);
 
 	/* enable mag */
@@ -746,7 +740,7 @@ LSM303D::probe()
 
 	/* verify that the device is attached and functioning */
 	bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
-	
+
 	if (success) {
 		_checked_values[0] = WHO_I_AM;
 		return OK;
@@ -1019,7 +1013,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 			return SENSOR_POLLRATE_MANUAL;
 
 		return 1000000 / _call_mag_interval;
-	
+
 	case SENSORIOCSQUEUEDEPTH: {
 		/* lower bound is mandatory, upper bound is a sanity check */
 		if ((arg < 1) || (arg > 100))
@@ -1424,7 +1418,7 @@ LSM303D::check_registers(void)
 		  if we get the wrong value then we know the SPI bus
 		  or sensor is very sick. We set _register_wait to 20
 		  and wait until we have seen 20 good values in a row
-		  before we consider the sensor to be OK again. 
+		  before we consider the sensor to be OK again.
 		 */
 		perf_count(_bad_registers);
 
@@ -1548,7 +1542,7 @@ LSM303D::measure()
 		perf_count(_bad_values);
 		_constant_accel_count = 0;
 	}
-	    
+
 	_last_accel[0] = x_in_new;
 	_last_accel[1] = y_in_new;
 	_last_accel[2] = z_in_new;
@@ -1570,7 +1564,7 @@ LSM303D::measure()
 
 	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(_accel_orb_id, _accel_topic, &accel_report);
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
 	}
 
 	_accel_read++;
@@ -1641,7 +1635,7 @@ LSM303D::mag_measure()
 
 	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report);
+		orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
 	}
 
 	_mag_read++;
@@ -1666,7 +1660,7 @@ LSM303D::print_info()
         for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) {
             uint8_t v = read_reg(_checked_registers[i]);
             if (v != _checked_values[i]) {
-                ::printf("reg %02x:%02x should be %02x\n", 
+                ::printf("reg %02x:%02x should be %02x\n",
                          (unsigned)_checked_registers[i],
                          (unsigned)v,
                          (unsigned)_checked_values[i]);
@@ -1742,7 +1736,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) :
 	CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
 	_parent(parent),
 	_mag_topic(-1),
-	_mag_orb_id(nullptr),
+	_mag_orb_class_instance(-1),
 	_mag_class_instance(-1)
 {
 }
@@ -1783,7 +1777,13 @@ LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen)
 int
 LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
-	return _parent->mag_ioctl(filp, cmd, arg);
+	switch (cmd) {
+		case DEVIOCGDEVICEID:
+			return (int)CDev::ioctl(filp, cmd, arg);
+			break;
+		default:
+			return _parent->mag_ioctl(filp, cmd, arg);
+	}
 }
 
 void

src/drivers/mkblctrl/mkblctrl.cpp

@@ -457,8 +457,9 @@ MK::task_main()
 	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
 	/* advertise the mixed control outputs */
-	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
+	int dummy;
-				   &outputs);
+	_t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
+				   &outputs, &dummy, ORB_PRIO_HIGH);
 
 	/* advertise the blctrl status */
 	esc_status_s esc;

src/drivers/mpu6000/mpu6000.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -175,6 +175,12 @@
 
 #define MPU6000_ONE_G					9.80665f
 
+#ifdef PX4_SPI_BUS_EXT
+#define EXTERNAL_BUS PX4_SPI_BUS_EXT
+#else
+#define EXTERNAL_BUS 0
+#endif
+
 /*
   the MPU6000 can only handle high SPI bus speeds on the sensor and
   interrupt status registers. All other registers have a maximum 1MHz
@@ -234,7 +240,7 @@ private:
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
-	orb_id_t		_accel_orb_id;
+	int			_accel_orb_class_instance;
 	int			_accel_class_instance;
 
 	RingBuffer		*_gyro_reports;
@@ -360,6 +366,13 @@ private:
 	 */
 	uint16_t		swap16(uint16_t val) { return (val >> 8) | (val << 8);	}
 
+	/**
+	 * Get the internal / external state
+	 *
+	 * @return true if the sensor is not on the main MCU board
+	 */
+	bool			is_external() { return (_bus == EXTERNAL_BUS); }
+
 	/**
 	 * Measurement self test
 	 *
@@ -433,7 +446,7 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU
 									     MPUREG_INT_ENABLE,
 									     MPUREG_INT_PIN_CFG };
 
-					   
+
 
 /**
  * Helper class implementing the gyro driver node.
@@ -457,7 +470,7 @@ protected:
 private:
 	MPU6000			*_parent;
 	orb_advert_t		_gyro_topic;
-	orb_id_t		_gyro_orb_id;
+	int			_gyro_orb_class_instance;
 	int			_gyro_class_instance;
 
 	/* do not allow to copy this class due to pointer data members */
@@ -479,7 +492,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
 	_accel_topic(-1),
-	_accel_orb_id(nullptr),
+	_accel_orb_class_instance(-1),
 	_accel_class_instance(-1),
 	_gyro_reports(nullptr),
 	_gyro_scale{},
@@ -510,6 +523,12 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	// disable debug() calls
 	_debug_enabled = false;
 
+	_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000;
+
+	/* Prime _gyro with parents devid. */
+	_gyro->_device_id.devid = _device_id.devid;
+	_gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6000;
+
 	// default accel scale factors
 	_accel_scale.x_offset = 0;
 	_accel_scale.x_scale  = 1.0f;
@@ -596,6 +615,7 @@ MPU6000::init()
 	_gyro_scale.z_offset = 0;
 	_gyro_scale.z_scale  = 1.0f;
 
+
 	/* do CDev init for the gyro device node, keep it optional */
 	ret = _gyro->init();
 	/* if probe/setup failed, bail now */
@@ -613,22 +633,8 @@ MPU6000::init()
 	_accel_reports->get(&arp);
 
 	/* measurement will have generated a report, publish */
-	switch (_accel_class_instance) {
+	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
-		case CLASS_DEVICE_PRIMARY:
+		&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
-			_accel_orb_id = ORB_ID(sensor_accel0);
-			break;
-		
-		case CLASS_DEVICE_SECONDARY:
-			_accel_orb_id = ORB_ID(sensor_accel1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_accel_orb_id = ORB_ID(sensor_accel2);
-			break;
-
-	}
-
-	_accel_topic = orb_advertise(_accel_orb_id, &arp);
 
 	if (_accel_topic < 0) {
 		warnx("ADVERT FAIL");
@@ -639,22 +645,8 @@ MPU6000::init()
 	struct gyro_report grp;
 	_gyro_reports->get(&grp);
 
-	switch (_gyro->_gyro_class_instance) {
+	_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
-		case CLASS_DEVICE_PRIMARY:
+		&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
-			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro0);
-			break;
-
-		case CLASS_DEVICE_SECONDARY:
-			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro2);
-			break;
-
-	}
-
-	_gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp);
 
 	if (_gyro->_gyro_topic < 0) {
 		warnx("ADVERT FAIL");
@@ -683,7 +675,7 @@ int MPU6000::reset()
 		// for it to come out of sleep
 		write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
 		up_udelay(1000);
-		
+
 		// Disable I2C bus (recommended on datasheet)
 		write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
 		irqrestore(state);
@@ -741,7 +733,7 @@ int MPU6000::reset()
 	case MPU6000_REV_D9:
 	case MPU6000_REV_D10:
 	// default case to cope with new chip revisions, which
-	// presumably won't have the accel scaling bug		
+	// presumably won't have the accel scaling bug
 	default:
 		// Accel scale 8g (4096 LSB/g)
 		write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
@@ -819,7 +811,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
 {
 	uint8_t filter;
 
-	/* 
+	/*
 	   choose next highest filter frequency available
 	 */
         if (frequency_hz == 0) {
@@ -921,7 +913,7 @@ MPU6000::gyro_self_test()
 	if (self_test())
 		return 1;
 
-	/* 
+	/*
 	 * Maximum deviation of 20 degrees, according to
 	 * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf
 	 * Section 6.1, initial ZRO tolerance
@@ -982,7 +974,7 @@ MPU6000::factory_self_test()
 	// gyro self test has to be done at 250DPS
 	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS);
 
-	struct MPUReport mpu_report;	
+	struct MPUReport mpu_report;
 	float accel_baseline[3];
 	float gyro_baseline[3];
 	float accel[3];
@@ -1012,10 +1004,10 @@ MPU6000::factory_self_test()
 	}
 
 #if 1
-	write_reg(MPUREG_GYRO_CONFIG, 
+	write_reg(MPUREG_GYRO_CONFIG,
-		  BITS_FS_250DPS | 
+		  BITS_FS_250DPS |
-		  BITS_GYRO_ST_X | 
+		  BITS_GYRO_ST_X |
-		  BITS_GYRO_ST_Y | 
+		  BITS_GYRO_ST_Y |
 		  BITS_GYRO_ST_Z);
 
 	// accel 8g, self-test enabled all axes
@@ -1085,7 +1077,7 @@ MPU6000::factory_self_test()
 			::printf("FAIL\n");
 			ret = -EIO;
 		}
-	}	
+	}
 	for (uint8_t i=0; i<3; i++) {
 		float diff = gyro[i]-gyro_baseline[i];
 		float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i];
@@ -1100,7 +1092,7 @@ MPU6000::factory_self_test()
 			::printf("FAIL\n");
 			ret = -EIO;
 		}
-	}	
+	}
 
 	write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config);
 	write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config);
@@ -1247,14 +1239,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* lower bound is mandatory, upper bound is a sanity check */
 		if ((arg < 1) || (arg > 100))
 			return -EINVAL;
-		
+
 		irqstate_t flags = irqsave();
 		if (!_accel_reports->resize(arg)) {
 			irqrestore(flags);
 			return -ENOMEM;
 		}
 		irqrestore(flags);
-		
+
 		return OK;
 	}
 
@@ -1536,13 +1528,13 @@ MPU6000::check_registers(void)
 	  the data registers.
 	*/
 	uint8_t v;
-	if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != 
+	if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
 	    _checked_values[_checked_next]) {
 		/*
 		  if we get the wrong value then we know the SPI bus
 		  or sensor is very sick. We set _register_wait to 20
 		  and wait until we have seen 20 good values in a row
-		  before we consider the sensor to be OK again. 
+		  before we consider the sensor to be OK again.
 		 */
 		perf_count(_bad_registers);
 
@@ -1655,7 +1647,7 @@ MPU6000::measure()
 		_register_wait--;
 		return;
 	}
-	    
+
 
 	/*
 	 * Swap axes and negate y
@@ -1716,7 +1708,7 @@ MPU6000::measure()
 	float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
 	float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
 	float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-	
+
 	arb.x = _accel_filter_x.apply(x_in_new);
 	arb.y = _accel_filter_y.apply(y_in_new);
 	arb.z = _accel_filter_z.apply(z_in_new);
@@ -1737,7 +1729,7 @@ MPU6000::measure()
 	float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
 	float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
 	float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
-	
+
 	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
 	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
 	grb.z = _gyro_filter_z.apply(z_gyro_in_new);
@@ -1763,12 +1755,12 @@ MPU6000::measure()
 		perf_begin(_controller_latency_perf);
 		perf_begin(_system_latency_perf);
 		/* publish it */
-		orb_publish(_accel_orb_id, _accel_topic, &arb);
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
 	}
 
 	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb);
+		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
 	}
 
 	/* stop measuring */
@@ -1791,7 +1783,7 @@ MPU6000::print_info()
         for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) {
             uint8_t v = read_reg(_checked_registers[i], MPU6000_HIGH_BUS_SPEED);
             if (v != _checked_values[i]) {
-                ::printf("reg %02x:%02x should be %02x\n", 
+                ::printf("reg %02x:%02x should be %02x\n",
                          (unsigned)_checked_registers[i],
                          (unsigned)v,
                          (unsigned)_checked_values[i]);
@@ -1818,7 +1810,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
 	CDev("MPU6000_gyro", path),
 	_parent(parent),
 	_gyro_topic(-1),
-	_gyro_orb_id(nullptr),
+	_gyro_orb_class_instance(-1),
 	_gyro_class_instance(-1)
 {
 }
@@ -1863,7 +1855,14 @@ MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen)
 int
 MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
-	return _parent->gyro_ioctl(filp, cmd, arg);
+
+	switch (cmd) {
+		case DEVIOCGDEVICEID:
+			return (int)CDev::ioctl(filp, cmd, arg);
+			break;
+		default:
+			return _parent->gyro_ioctl(filp, cmd, arg);
+	}
 }
 
 /**

src/drivers/ms5611/ms5611.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -57,6 +57,7 @@
 #include <nuttx/clock.h>
 
 #include <arch/board/board.h>
+#include <board_config.h>
 
 #include <drivers/device/device.h>
 #include <drivers/drv_baro.h>
@@ -134,8 +135,7 @@ protected:
 	unsigned		_msl_pressure;	/* in Pa */
 
 	orb_advert_t		_baro_topic;
-	orb_id_t		_orb_id;
+	int			_orb_class_instance;
-
 	int			_class_instance;
 
 	perf_counter_t		_sample_perf;
@@ -171,6 +171,13 @@ protected:
 	 */
 	void			cycle();
 
+	/**
+	 * Get the internal / external state
+	 *
+	 * @return true if the sensor is not on the main MCU board
+	 */
+	bool			is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ }
+
 	/**
 	 * Static trampoline from the workq context; because we don't have a
 	 * generic workq wrapper yet.
@@ -210,6 +217,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char*
 	_SENS(0),
 	_msl_pressure(101325),
 	_baro_topic(-1),
+	_orb_class_instance(-1),
 	_class_instance(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
 	_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
@@ -263,7 +271,6 @@ MS5611::init()
 
 	/* register alternate interfaces if we have to */
 	_class_instance = register_class_devname(BARO_DEVICE_PATH);
-	_orb_id = ORB_ID_TRIPLE(sensor_baro, _class_instance);
 
 	struct baro_report brp;
 	/* do a first measurement cycle to populate reports with valid data */
@@ -303,7 +310,9 @@ MS5611::init()
 
 		ret = OK;
 
-		_baro_topic = orb_advertise(_orb_id, &brp);
+		_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
+				&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
+
 
 		if (_baro_topic < 0) {
 			warnx("failed to create sensor_baro publication");
@@ -725,7 +734,7 @@ MS5611::collect()
 		/* publish it */
 		if (!(_pub_blocked)) {
 			/* publish it */
-			orb_publish(_orb_id, _baro_topic, &report);
+			orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
 		}
 
 		if (_reports->force(&report)) {

src/drivers/px4fmu/fmu.cpp

@@ -145,7 +145,7 @@ private:
 	int		_control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	orb_id_t	_control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
-	orb_id_t	_actuator_output_topic;
+	int		_actuator_output_topic_instance;
 	pollfd	_poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	unsigned	_poll_fds_num;
 
@@ -260,7 +260,7 @@ PX4FMU::PX4FMU() :
 	_groups_required(0),
 	_groups_subscribed(0),
 	_control_subs{-1},
-	_actuator_output_topic(nullptr),
+	_actuator_output_topic_instance(-1),
 	_poll_fds_num(0),
 	_pwm_limit{},
 	_failsafe_pwm{0},
@@ -331,8 +331,6 @@ PX4FMU::init()
 		log("default PWM output device");
 	}
 
-	_actuator_output_topic = ORB_ID_DOUBLE(actuator_outputs_, _class_instance);
-
 	/* reset GPIOs */
 	gpio_reset();
 
@@ -683,10 +681,10 @@ PX4FMU::task_main()
 
 				/* publish mixed control outputs */
 				if (_outputs_pub < 0) {
-					_outputs_pub = orb_advertise(_actuator_output_topic, &outputs);
+					_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT);
 				} else {
 
-					orb_publish(_actuator_output_topic, _outputs_pub, &outputs);
+					orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
 				}
 			}
 		}

src/drivers/px4io/px4io.cpp

@@ -1685,17 +1685,12 @@ PX4IO::io_publish_pwm_outputs()
 
 	/* lazily advertise on first publication */
 	if (_to_outputs == 0) {
-		_to_outputs = orb_advertise((_primary_pwm_device ?
+		int instance;
-					     ORB_ID_VEHICLE_CONTROLS :
+		_to_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
-					     ORB_ID(actuator_outputs_1)),
+					    &outputs, &instance, ORB_PRIO_MAX);
-					    &outputs);
 
 	} else {
-		orb_publish((_primary_pwm_device ?
+		orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs);
-			     ORB_ID_VEHICLE_CONTROLS :
-			     ORB_ID(actuator_outputs_1)),
-			    _to_outputs,
-			    &outputs);
 	}
 
 	return OK;

src/examples/matlab_csv_serial/matlab_csv_serial.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.		 
+ *   Copyright (c) 2014-2015 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
@@ -190,10 +190,10 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
 	struct gyro_report gyro1;
 
 	/* subscribe to parameter changes */
-	int accel0_sub = orb_subscribe(ORB_ID(sensor_accel0));
+	int accel0_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
-	int accel1_sub = orb_subscribe(ORB_ID(sensor_accel1));
+	int accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1);
-	int gyro0_sub = orb_subscribe(ORB_ID(sensor_gyro0));
+	int gyro0_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);
-	int gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1));
+	int gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1);
 
 	thread_running = true;
 
@@ -224,10 +224,10 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
 			/* accel0 update available? */
 			if (fds[0].revents & POLLIN)
 			{
-				orb_copy(ORB_ID(sensor_accel0), accel0_sub, &accel0);
+				orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0);
-				orb_copy(ORB_ID(sensor_accel1), accel1_sub, &accel1);
+				orb_copy(ORB_ID(sensor_accel), accel1_sub, &accel1);
-				orb_copy(ORB_ID(sensor_gyro0), gyro0_sub, &gyro0);
+				orb_copy(ORB_ID(sensor_gyro), gyro0_sub, &gyro0);
-				orb_copy(ORB_ID(sensor_gyro1), gyro1_sub, &gyro1);
+				orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1);
 
 				// write out on accel 0, but collect for all other sensors as they have updates
 				dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw,

src/modules/commander/accelerometer_calibration.cpp

@@ -159,6 +159,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 int do_accel_calibration(int mavlink_fd)
 {
 	int fd;
+	int32_t device_id;
 
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 
@@ -180,6 +181,9 @@ int do_accel_calibration(int mavlink_fd)
 
 	/* reset all offsets to zero and all scales to one */
 	fd = open(ACCEL_DEVICE_PATH, 0);
+
+	device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
+
 	res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
 	close(fd);
 
@@ -226,6 +230,10 @@ int do_accel_calibration(int mavlink_fd)
 			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
 			res = ERROR;
 		}
+
+		if (param_set(param_find("SENS_ACC_ID"), &(device_id))) {
+				res = ERROR;
+		}
 	}
 
 	if (res == OK) {

src/modules/commander/gyro_calibration.cpp

@@ -62,6 +62,7 @@ static const char *sensor_name = "gyro";
 
 int do_gyro_calibration(int mavlink_fd)
 {
+	int32_t device_id;
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 	mavlink_log_info(mavlink_fd, "HOLD STILL");
 
@@ -81,6 +82,9 @@ int do_gyro_calibration(int mavlink_fd)
 
 	/* reset all offsets to zero and all scales to one */
 	int fd = open(GYRO_DEVICE_PATH, 0);
+
+	device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
+
 	res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
 	close(fd);
 
@@ -95,7 +99,7 @@ int do_gyro_calibration(int mavlink_fd)
 		unsigned poll_errcount = 0;
 
 		/* subscribe to gyro sensor topic */
-		int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0));
+		int sub_sensor_gyro = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);
 		struct gyro_report gyro_report;
 
 		while (calibration_counter < calibration_count) {
@@ -107,7 +111,7 @@ int do_gyro_calibration(int mavlink_fd)
 			int poll_ret = poll(fds, 1, 1000);
 
 			if (poll_ret > 0) {
-				orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report);
+				orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
 				gyro_scale.x_offset += gyro_report.x;
 				gyro_scale.y_offset += gyro_report.y;
 				gyro_scale.z_offset += gyro_report.z;
@@ -277,6 +281,9 @@ int do_gyro_calibration(int mavlink_fd)
 			mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
 			res = ERROR;
 		}
+		if (param_set(param_find("SENS_GYRO_ID"), &(device_id))) {
+				res = ERROR;
+			}
 	}
 
 	if (res == OK) {

src/modules/commander/mag_calibration.cpp

@@ -149,7 +149,7 @@ int do_mag_calibration(int mavlink_fd)
 	}
 
 	if (res == OK) {
-		int sub_mag = orb_subscribe(ORB_ID(sensor_mag0));
+		int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), 0);
 
 		if (sub_mag < 0) {
 			mavlink_log_critical(mavlink_fd, "No mag found, abort");
@@ -179,7 +179,7 @@ int do_mag_calibration(int mavlink_fd)
 				int poll_ret = poll(fds, 1, 1000);
 
 				if (poll_ret > 0) {
-					orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag);
+					orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
 
 					x[calibration_counter] = mag.x;
 					y[calibration_counter] = mag.y;

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -720,7 +720,7 @@ FixedwingEstimator::task_main()
 	 * do subscriptions
 	 */
 	_distance_sub = orb_subscribe(ORB_ID(sensor_range_finder));
-	_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
+	_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -1088,7 +1088,7 @@ FixedwingEstimator::task_main()
 
 			if (baro_updated) {
 
-				orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
+				orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
 
 				float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
 				baro_last = _baro.timestamp;
@@ -1217,7 +1217,7 @@ FixedwingEstimator::task_main()
 					initVelNED[2] = _gps.vel_d_m_s;
 
 					// Set up height correctly
-					orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
+					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
 					_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
 
 					// init filtered gps and baro altitudes

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -554,7 +554,7 @@ FixedwingAttitudeControl::vehicle_accel_poll()
 	orb_check(_accel_sub, &accel_updated);
 
 	if (accel_updated) {
-		orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel);
+		orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
 	}
 }
 
@@ -624,7 +624,7 @@ FixedwingAttitudeControl::task_main()
 	 */
 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
+	_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));

src/modules/mavlink/mavlink_main.cpp

@@ -904,20 +904,20 @@ Mavlink::send_statustext(unsigned char severity, const char *string)
 	mavlink_logbuffer_write(&_logbuffer, &logmsg);
 }
 
-MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
+MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance)
 {
 	/* check if already subscribed to this topic */
 	MavlinkOrbSubscription *sub;
 
 	LL_FOREACH(_subscriptions, sub) {
-		if (sub->get_topic() == topic) {
+		if (sub->get_topic() == topic && sub->get_instance() == instance) {
 			/* already subscribed */
 			return sub;
 		}
 	}
 
 	/* add new subscription */
-	MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic);
+	MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance);
 
 	LL_APPEND(_subscriptions, sub_new);
 

src/modules/mavlink/mavlink_main.h

@@ -171,7 +171,7 @@ public:
 
 	void			handle_message(const mavlink_message_t *msg);
 
-	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
+	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0);
 
 	int			get_instance_id();
 

src/modules/mavlink/mavlink_messages.cpp

@@ -1342,14 +1342,7 @@ protected:
 		_act_sub(nullptr),
 		_act_time(0)
 	{
-		orb_id_t act_topics[] = {
+		_act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N);
-			ORB_ID(actuator_outputs_0),
-			ORB_ID(actuator_outputs_1),
-			ORB_ID(actuator_outputs_2),
-			ORB_ID(actuator_outputs_3)
-		};
-
-		_act_sub = _mavlink->add_orb_subscription(act_topics[N]);
 	}
 
 	void send(const hrt_abstime t)
@@ -1424,7 +1417,7 @@ protected:
 		_status_time(0),
 		_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))),
 		_pos_sp_triplet_time(0),
-		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))),
+		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))),
 		_act_time(0)
 	{}
 

src/modules/mavlink/mavlink_orb_subscription.cpp

@@ -46,10 +46,11 @@
 
 #include "mavlink_orb_subscription.h"
 
-MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
+MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instance) :
 	next(nullptr),
 	_topic(topic),
-	_fd(orb_subscribe(_topic)),
+	_instance(instance),
+	_fd(orb_subscribe_multi(_topic, instance)),
 	_published(false)
 {
 }
@@ -65,6 +66,12 @@ MavlinkOrbSubscription::get_topic() const
 	return _topic;
 }
 
+int
+MavlinkOrbSubscription::get_instance() const
+{
+	return _instance;
+}
+
 bool
 MavlinkOrbSubscription::update(uint64_t *time, void* data)
 {

src/modules/mavlink/mavlink_orb_subscription.h

@@ -50,7 +50,7 @@ class MavlinkOrbSubscription
 public:
 	MavlinkOrbSubscription *next;	///< pointer to next subscription in list
 
-	MavlinkOrbSubscription(const orb_id_t topic);
+	MavlinkOrbSubscription(const orb_id_t topic, int instance);
 	~MavlinkOrbSubscription();
 
 	/**
@@ -77,9 +77,11 @@ public:
 	 */
 	bool is_published();
 	orb_id_t get_topic() const;
+	int get_instance() const;
 
 private:
 	const orb_id_t _topic;		///< topic metadata
+	const int _instance;		///< get topic instance
 	int _fd;			///< subscription handle
 	bool _published;		///< topic was ever published
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -1043,10 +1043,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		gyro.temperature = imu.temperature;
 
 		if (_gyro_pub < 0) {
-			_gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro);
+			_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
 
 		} else {
-			orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro);
+			orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro);
 		}
 	}
 
@@ -1065,10 +1065,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		accel.temperature = imu.temperature;
 
 		if (_accel_pub < 0) {
-			_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
 
 		} else {
-			orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
+			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
 		}
 	}
 
@@ -1086,10 +1086,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		mag.z = imu.zmag;
 
 		if (_mag_pub < 0) {
-			_mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag);
+			/* publish to the first mag topic */
+			_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
 
 		} else {
-			orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag);
+			orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
 		}
 	}
 
@@ -1104,10 +1105,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		baro.temperature = imu.temperature;
 
 		if (_baro_pub < 0) {
-			_baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro);
+			_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
 
 		} else {
-			orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro);
+			orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
 		}
 	}
 
@@ -1394,10 +1395,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		accel.temperature = 25.0f;
 
 		if (_accel_pub < 0) {
-			_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
 
 		} else {
-			orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
+			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
 		}
 	}
 

src/modules/navigator/mission.cpp

@@ -39,6 +39,7 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  * @author Anton Babushkin <anton.babushkin@me.com>
  * @author Ban Siesta <bansiesta@gmail.com>
+ * @author Simon Wilks <simon@uaventure.com>
  */
 
 #include <sys/types.h>
@@ -68,6 +69,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false),
 	_param_dist_1wp(this, "MIS_DIST_1WP", false),
 	_param_altmode(this, "MIS_ALTMODE", false),
+	_param_yawmode(this, "MIS_YAWMODE", false),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
 	_current_onboard_mission_index(-1),
@@ -80,6 +82,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_missionFeasiblityChecker(),
 	_min_current_sp_distance_xy(FLT_MAX),
 	_mission_item_previous_alt(NAN),
+  	_on_arrival_yaw(NAN),
 	_distance_current_previous(0.0f)
 {
 	/* load initial params */
@@ -166,6 +169,13 @@ Mission::on_active()
 			_navigator->set_can_loiter_at_sp(true);
 		}
 	}
+
+	/* see if we need to update the current yaw heading for rotary wing types */
+	if (_navigator->get_vstatus()->is_rotary_wing 
+			&& _param_yawmode.get() != MISSION_YAWMODE_NONE
+			&& _mission_type != MISSION_TYPE_NONE) {
+		heading_sp_update();
+	}
 }
 
 void
@@ -275,7 +285,7 @@ Mission::check_dist_1wp()
 						&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
 
 					/* check only items with valid lat/lon */
-					if (	mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
+					if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
 							mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
 							mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
 							mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
@@ -362,7 +372,6 @@ Mission::set_mission_items()
 			mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
-
 	} else {
 		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
@@ -396,6 +405,10 @@ Mission::set_mission_items()
 		return;
 	}
 
+	if (pos_sp_triplet->current.valid) {
+		_on_arrival_yaw = _mission_item.yaw;
+	}
+
 	/* do takeoff on first waypoint for rotary wing vehicles */
 	if (_navigator->get_vstatus()->is_rotary_wing) {
 		/* force takeoff if landed (additional protection) */
@@ -442,6 +455,7 @@ Mission::set_mission_items()
 			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
 			_mission_item.lat = _navigator->get_global_position()->lat;
 			_mission_item.lon = _navigator->get_global_position()->lon;
+			_mission_item.yaw = NAN;
 			_mission_item.altitude = takeoff_alt;
 			_mission_item.altitude_is_relative = false;
 			_mission_item.autocontinue = true;
@@ -481,7 +495,6 @@ Mission::set_mission_items()
 		if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) {
 			/* got next mission item, update setpoint triplet */
 			mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next);
-
 		} else {
 			/* next mission item is not available */
 			pos_sp_triplet->next.valid = false;
@@ -503,6 +516,59 @@ Mission::set_mission_items()
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
+void
+Mission::heading_sp_update()
+{
+	if (_takeoff) {
+		/* we don't want to be yawing during takeoff */
+		return;
+	}
+
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	/* Don't change setpoint if last and current waypoint are not valid */
+	if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid ||
+			!isfinite(_on_arrival_yaw)) {
+		return;
+	}
+
+	/* Don't do FOH for landing and takeoff waypoints, the ground may be near
+	 * and the FW controller has a custom landing logic */
+	if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+		return;
+	}
+
+	/* set yaw angle for the waypoint iff a loiter time has been specified */
+	if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
+		_mission_item.yaw = _on_arrival_yaw;
+	/* always keep the front of the rotary wing pointing to the next waypoint */
+	} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) {
+		_mission_item.yaw = get_bearing_to_next_waypoint(
+		        _navigator->get_global_position()->lat,
+		        _navigator->get_global_position()->lon,
+		        _mission_item.lat,
+		        _mission_item.lon);
+	/* always keep the back of the rotary wing pointing towards home */
+	} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) {
+		_mission_item.yaw = get_bearing_to_next_waypoint(
+		        _navigator->get_global_position()->lat,
+		        _navigator->get_global_position()->lon,
+		        _navigator->get_home_position()->lat,
+		        _navigator->get_home_position()->lon);
+	/* always keep the back of the rotary wing pointing towards home */
+	} else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
+		_mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint(
+		        _navigator->get_global_position()->lat,
+		        _navigator->get_global_position()->lon,
+		        _navigator->get_home_position()->lat,
+		        _navigator->get_home_position()->lon) + M_PI_F);
+	}
+
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+
 void
 Mission::altitude_sp_foh_update()
 {

src/modules/navigator/mission.h

@@ -83,6 +83,13 @@ public:
 		MISSION_ALTMODE_FOH = 1
 	};
 
+	enum mission_yaw_mode {
+		MISSION_YAWMODE_NONE = 0,
+		MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1,
+		MISSION_YAWMODE_FRONT_TO_HOME = 2,
+		MISSION_YAWMODE_BACK_TO_HOME = 3
+	};
+
 private:
 	/**
 	 * Update onboard mission topic
@@ -110,6 +117,11 @@ private:
 	 */
 	void set_mission_items();
 
+	/**
+	 * Updates the heading of the vehicle. Rotary wings only.
+	 */
+	void heading_sp_update();
+
 	/**
 	 * Updates the altitude sp to follow a foh
 	 */
@@ -155,6 +167,7 @@ private:
 	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;
 	control::BlockParamInt _param_altmode;
+	control::BlockParamInt _param_yawmode;
 
 	struct mission_s _onboard_mission;
 	struct mission_s _offboard_mission;
@@ -177,7 +190,8 @@ private:
 
 	float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint  */
 	float _mission_item_previous_alt; /**< holds the altitude of the previous mission item,
-					    can be replaced by a full copy of the previous mission item if needed*/
+					    can be replaced by a full copy of the previous mission item if needed */
+	float _on_arrival_yaw; /**< holds the yaw value that should be applied when the current waypoint is reached */
 	float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet,
 					    only use if current and previous are valid */
 };

src/modules/navigator/mission_block.cpp

@@ -134,6 +134,7 @@ MissionBlock::is_mission_item_reached()
 		}
 	}
 
+	/* Check if the waypoint and the requested yaw setpoint. */
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
 		/* TODO: removed takeoff, why? */
@@ -151,7 +152,7 @@ MissionBlock::is_mission_item_reached()
 		}
 	}
 
-	/* check if the current waypoint was reached */
+	/* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */
 	if (_waypoint_position_reached && _waypoint_yaw_reached) {
 
 		if (_time_first_inside_orbit == 0) {

src/modules/navigator/mission_params.c

@@ -95,3 +95,19 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
  * @group Mission
  */
 PARAM_DEFINE_INT32(MIS_ALTMODE, 0);
+
+/**
+ * Multirotor only. Yaw setpoint mode.
+ *
+ * 0: Set the yaw heading to the yaw value specified for the destination waypoint.
+ * 1: Maintain a yaw heading pointing towards the next waypoint.
+ * 2: Maintain a yaw heading that always points to the home location.
+ * 3: Maintain a yaw heading that always points away from the home location (ie: back always faces home).
+ *
+ * The values are defined in the enum mission_altitude_mode
+ *
+ * @min 0
+ * @max 3
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(MIS_YAWMODE, 0);

src/modules/sdlog2/sdlog2.c

@@ -1100,7 +1100,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
+	subs.act_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs));
 	subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1));
 	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
@@ -1477,7 +1477,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ACTUATOR OUTPUTS --- */
-		if (copy_if_updated(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs)) {
+		if (copy_if_updated(ORB_ID(actuator_outputs), subs.act_outputs_sub, &buf.act_outputs)) {
 			log_msg.msg_type = LOG_OUT0_MSG;
 			memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output));
 			LOGBUFFER_WRITE_AND_COUNT(OUT0);

src/modules/sensors/sensor_params.c

@@ -44,6 +44,13 @@
 #include <nuttx/config.h>
 #include <systemlib/param/param.h>
 
+/**
+ * ID of the Gyro that the calibration is for.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(SENS_GYRO_ID, 0);
+
 /**
  * Gyro X-axis offset
  *
@@ -153,6 +160,12 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f);
 
+/**
+ * ID of the Accelerometer that the calibration is for.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(SENS_ACC_ID, 0);
 
 /**
  * Accelerometer X-axis offset
@@ -270,7 +283,7 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
 /**
  * PX4Flow board rotation
  *
- * This parameter defines the rotation of the PX4FLOW board relative to the platform. 
+ * This parameter defines the rotation of the PX4FLOW board relative to the platform.
  * Zero rotation is defined as Y on flow board pointing towards front of vehicle
  * Possible values are:
  *    0 = No rotation

src/modules/sensors/sensors.cpp

@@ -1113,7 +1113,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 	if (accel_updated) {
 		struct accel_report	accel_report;
 
-		orb_copy(ORB_ID(sensor_accel0), _accel_sub, &accel_report);
+		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
 		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
 		vect = _board_rotation * vect;
@@ -1134,7 +1134,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 	if (accel_updated) {
 		struct accel_report	accel_report;
 
-		orb_copy(ORB_ID(sensor_accel1), _accel_sub, &accel_report);
+		orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report);
 
 		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
 		vect = _board_rotation * vect;
@@ -1155,7 +1155,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 	if (accel_updated) {
 		struct accel_report	accel_report;
 
-		orb_copy(ORB_ID(sensor_accel2), _accel_sub, &accel_report);
+		orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report);
 
 		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
 		vect = _board_rotation * vect;
@@ -1181,7 +1181,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 	if (gyro_updated) {
 		struct gyro_report	gyro_report;
 
-		orb_copy(ORB_ID(sensor_gyro0), _gyro_sub, &gyro_report);
+		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
 		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
 		vect = _board_rotation * vect;
@@ -1202,7 +1202,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 	if (gyro_updated) {
 		struct gyro_report	gyro_report;
 
-		orb_copy(ORB_ID(sensor_gyro1), _gyro1_sub, &gyro_report);
+		orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report);
 
 		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
 		vect = _board_rotation * vect;
@@ -1223,7 +1223,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 	if (gyro_updated) {
 		struct gyro_report	gyro_report;
 
-		orb_copy(ORB_ID(sensor_gyro2), _gyro_sub, &gyro_report);
+		orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report);
 
 		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
 		vect = _board_rotation * vect;
@@ -1249,7 +1249,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 	if (mag_updated) {
 		struct mag_report	mag_report;
 
-		orb_copy(ORB_ID(sensor_mag0), _mag_sub, &mag_report);
+		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
 		math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
 
@@ -1278,7 +1278,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 	if (mag_updated) {
 		struct mag_report	mag_report;
 
-		orb_copy(ORB_ID(sensor_mag1), _mag1_sub, &mag_report);
+		orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report);
 
 		raw.magnetometer1_raw[0] = mag_report.x_raw;
 		raw.magnetometer1_raw[1] = mag_report.y_raw;
@@ -1292,7 +1292,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 	if (mag_updated) {
 		struct mag_report	mag_report;
 
-		orb_copy(ORB_ID(sensor_mag2), _mag2_sub, &mag_report);
+		orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report);
 
 		raw.magnetometer2_raw[0] = mag_report.x_raw;
 		raw.magnetometer2_raw[1] = mag_report.y_raw;
@@ -1310,7 +1310,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 
 	if (baro_updated) {
 
-		orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_barometer);
+		orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer);
 
 		raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar
 		raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
@@ -1325,7 +1325,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 
 		struct baro_report	baro_report;
 
-		orb_copy(ORB_ID(sensor_baro1), _baro1_sub, &baro_report);
+		orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report);
 
 		raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar
 		raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters
@@ -1943,18 +1943,18 @@ Sensors::task_main()
 	/*
 	 * do subscriptions
 	 */
-	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0));
+	_gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);
-	_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
+	_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
-	_mag_sub = orb_subscribe(ORB_ID(sensor_mag0));
+	_mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0);
-	_gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1));
+	_gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1);
-	_accel1_sub = orb_subscribe(ORB_ID(sensor_accel1));
+	_accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1);
-	_mag1_sub = orb_subscribe(ORB_ID(sensor_mag1));
+	_mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1);
-	_gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2));
+	_gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2);
-	_accel2_sub = orb_subscribe(ORB_ID(sensor_accel2));
+	_accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2);
-	_mag2_sub = orb_subscribe(ORB_ID(sensor_mag2));
+	_mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2);
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
-	_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
+	_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
-	_baro1_sub = orb_subscribe(ORB_ID(sensor_baro1));
+	_baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1);
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));

src/modules/uORB/Publication.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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

src/modules/uORB/Publication.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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

src/modules/uORB/Subscription.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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

src/modules/uORB/Subscription.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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

src/modules/uORB/module.mk

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2012-2015 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
@@ -37,8 +37,7 @@
 
 MODULE_COMMAND		= uorb
 
-# XXX probably excessive, 2048 should be sufficient
+MODULE_STACKSIZE	= 2048
-MODULE_STACKSIZE	= 4096
 
 SRCS			= uORB.cpp \
 			  objects_common.cpp \

src/modules/uORB/objects_common.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -46,24 +46,16 @@
 #include <drivers/drv_orb_dev.h>
 
 #include <drivers/drv_mag.h>
-ORB_DEFINE(sensor_mag0, struct mag_report);
+ORB_DEFINE(sensor_mag, struct mag_report);
-ORB_DEFINE(sensor_mag1, struct mag_report);
-ORB_DEFINE(sensor_mag2, struct mag_report);
 
 #include <drivers/drv_accel.h>
-ORB_DEFINE(sensor_accel0, struct accel_report);
+ORB_DEFINE(sensor_accel, struct accel_report);
-ORB_DEFINE(sensor_accel1, struct accel_report);
-ORB_DEFINE(sensor_accel2, struct accel_report);
 
 #include <drivers/drv_gyro.h>
-ORB_DEFINE(sensor_gyro0, struct gyro_report);
+ORB_DEFINE(sensor_gyro, struct gyro_report);
-ORB_DEFINE(sensor_gyro1, struct gyro_report);
-ORB_DEFINE(sensor_gyro2, struct gyro_report);
 
 #include <drivers/drv_baro.h>
-ORB_DEFINE(sensor_baro0, struct baro_report);
+ORB_DEFINE(sensor_baro, struct baro_report);
-ORB_DEFINE(sensor_baro1, struct baro_report);
-ORB_DEFINE(sensor_baro2, struct baro_report);
 
 #include <drivers/drv_range_finder.h>
 ORB_DEFINE(sensor_range_finder, struct range_finder_report);
@@ -212,10 +204,7 @@ ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_virtual_fw_s);
 ORB_DEFINE(actuator_armed, struct actuator_armed_s);
 
 #include "topics/actuator_outputs.h"
-ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
+ORB_DEFINE(actuator_outputs, struct actuator_outputs_s);
-ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
-ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
-ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
 
 #include "topics/actuator_direct.h"
 ORB_DEFINE(actuator_direct, struct actuator_direct_s);

src/modules/uORB/topics/actuator_outputs.h

@@ -68,12 +68,6 @@ struct actuator_outputs_s {
  */
 
 /* actuator output sets; this list can be expanded as more drivers emerge */
-ORB_DECLARE(actuator_outputs_0);
+ORB_DECLARE(actuator_outputs);
-ORB_DECLARE(actuator_outputs_1);
-ORB_DECLARE(actuator_outputs_2);
-ORB_DECLARE(actuator_outputs_3);
 
-/* output sets with pre-defined applications */
+#endif
-#define ORB_ID_VEHICLE_CONTROLS		ORB_ID(actuator_outputs_0)
-
-#endif

src/modules/uORB/uORB.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (Cc) 2012-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -51,6 +51,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <systemlib/err.h>
 
 #include <nuttx/arch.h>
 #include <nuttx/wqueue.h>
@@ -68,6 +69,7 @@
 namespace
 {
 
+/* internal use only */
 static const unsigned orb_maxpath = 64;
 
 /* oddly, ERROR is not defined for c++ */
@@ -81,17 +83,30 @@ enum Flavor {
 	PARAM
 };
 
+struct orb_advertdata {
+	const struct orb_metadata *meta;
+	int *instance;
+	int priority;
+};
+
 int
-node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta)
+node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta, int *instance = nullptr)
 {
 	unsigned len;
 
-	len = snprintf(buf, orb_maxpath, "/%s/%s",
+	unsigned index = 0;
-		       (f == PUBSUB) ? "obj" : "param",
-		       meta->o_name);
 
-	if (len >= orb_maxpath)
+	if (instance != nullptr) {
+		index = *instance;
+	}
+
+	len = snprintf(buf, orb_maxpath, "/%s/%s%d",
+			(f == PUBSUB) ? "obj" : "param",
+			meta->o_name, index);
+
+	if (len >= orb_maxpath) {
 		return -ENAMETOOLONG;
+	}
 
 	return OK;
 }
@@ -104,7 +119,7 @@ node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta)
 class ORBDevNode : public device::CDev
 {
 public:
-	ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path);
+	ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority);
 	~ORBDevNode();
 
 	virtual int		open(struct file *filp);
@@ -126,6 +141,7 @@ private:
 		struct hrt_call	update_call;	/**< deferred wakeup call if update_period is nonzero */
 		void		*poll_priv;	/**< saved copy of fds->f_priv while poll is active */
 		bool		update_reported; /**< true if we have reported the update via poll/check */
+		int		priority;	/**< priority of publisher */
 	};
 
 	const struct orb_metadata *_meta;	/**< object metadata information */
@@ -133,6 +149,7 @@ private:
 	hrt_abstime		_last_update;	/**< time the object was last updated */
 	volatile unsigned 	_generation;	/**< object generation count */
 	pid_t			_publisher;	/**< if nonzero, current publisher */
+	const int		_priority;	/**< priority of topic */
 
 	SubscriberData		*filp_to_sd(struct file *filp) {
 		SubscriberData *sd = (SubscriberData *)(filp->f_priv);
@@ -160,13 +177,14 @@ private:
 	bool			appears_updated(SubscriberData *sd);
 };
 
-ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path) :
+ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) :
 	CDev(name, path),
 	_meta(meta),
 	_data(nullptr),
 	_last_update(0),
 	_generation(0),
-	_publisher(0)
+	_publisher(0),
+	_priority(priority)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -176,6 +194,7 @@ ORBDevNode::~ORBDevNode()
 {
 	if (_data != nullptr)
 		delete[] _data;
+
 }
 
 int
@@ -225,6 +244,9 @@ ORBDevNode::open(struct file *filp)
 		/* default to no pending update */
 		sd->generation = _generation;
 
+		/* set priority */
+		sd->priority = _priority;
+
 		filp->f_priv = (void *)sd;
 
 		ret = CDev::open(filp);
@@ -283,6 +305,9 @@ ORBDevNode::read(struct file *filp, char *buffer, size_t buflen)
 	/* track the last generation that the file has seen */
 	sd->generation = _generation;
 
+	/* set priority */
+	sd->priority = _priority;
+
 	/*
 	 * Clear the flag that indicates that an update has been reported, as
 	 * we have just collected it.
@@ -364,6 +389,10 @@ ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg)
 		*(uintptr_t *)arg = (uintptr_t)this;
 		return OK;
 
+	case ORBIOCGPRIORITY:
+		*(int *)arg = sd->priority;
+		return OK;
+
 	default:
 		/* give it to the superclass */
 		return CDev::ioctl(filp, cmd, arg);
@@ -556,40 +585,85 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	switch (cmd) {
 	case ORBIOCADVERTISE: {
-			const struct orb_metadata *meta = (const struct orb_metadata *)arg;
+			const struct orb_advertdata *adv = (const struct orb_advertdata *)arg;
+			const struct orb_metadata *meta = adv->meta;
 			const char *objname;
+			const char *devpath;
 			char nodepath[orb_maxpath];
 			ORBDevNode *node;
 
-			/* construct a path to the node - this also checks the node name */
+			/* set instance to zero - we could allow selective multi-pubs later based on value */
-			ret = node_mkpath(nodepath, _flavor, meta);
+			if (adv->instance != nullptr) {
-
+				*(adv->instance) = 0;
-			if (ret != OK)
-				return ret;
-
-			/* driver wants a permanent copy of the node name, so make one here */
-			objname = strdup(meta->o_name);
-
-			if (objname == nullptr)
-				return -ENOMEM;
-
-			/* construct the new node */
-			node = new ORBDevNode(meta, objname, nodepath);
-
-			/* initialise the node - this may fail if e.g. a node with this name already exists */
-			if (node != nullptr)
-				ret = node->init();
-
-			/* if we didn't get a device, that's bad */
-			if (node == nullptr)
-				return -ENOMEM;
-
-			/* if init failed, discard the node and its name */
-			if (ret != OK) {
-				delete node;
-				free((void *)objname);
 			}
 
+			/* construct a path to the node - this also checks the node name */
+			ret = node_mkpath(nodepath, _flavor, meta, adv->instance);
+
+			if (ret != OK) {
+				return ret;
+			}
+
+			/* ensure that only one advertiser runs through this critical section */
+			lock();
+
+			ret = ERROR;
+
+			/* try for topic groups */
+			const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1;
+			unsigned group_tries = 0;
+			do {
+				/* if path is modifyable change try index */
+				if (adv->instance != nullptr) {
+					/* replace the number at the end of the string */
+					nodepath[strlen(nodepath) - 1] = '0' + group_tries;
+					*(adv->instance) = group_tries;
+				}
+
+				/* driver wants a permanent copy of the node name, so make one here */
+				objname = strdup(meta->o_name);
+
+				if (objname == nullptr) {
+					return -ENOMEM;
+				}
+
+				/* driver wants a permanent copy of the path, so make one here */
+				devpath = strdup(nodepath);
+
+				if (devpath == nullptr) {
+					return -ENOMEM;
+				}
+
+				/* construct the new node */
+				node = new ORBDevNode(meta, objname, devpath, adv->priority);
+
+				/* if we didn't get a device, that's bad */
+				if (node == nullptr) {
+					unlock();
+					return -ENOMEM;
+				}
+
+				/* initialise the node - this may fail if e.g. a node with this name already exists */
+				ret = node->init();
+				
+				/* if init failed, discard the node and its name */
+				if (ret != OK) {
+					delete node;
+					free((void *)objname);
+					free((void *)devpath);
+				}
+
+				group_tries++;
+
+			} while (ret != OK && (group_tries < max_group_tries));
+
+			if (group_tries > max_group_tries) {
+				ret = -ENOMEM;
+			}
+
+			/* the file handle for the driver has been created, unlock */
+			unlock();
+
 			return ret;
 		}
 
@@ -614,6 +688,7 @@ struct orb_test {
 };
 
 ORB_DEFINE(orb_test, struct orb_test);
+ORB_DEFINE(orb_multitest, struct orb_test);
 
 int
 test_fail(const char *fmt, ...)
@@ -643,8 +718,6 @@ test_note(const char *fmt, ...)
 	return OK;
 }
 
-ORB_DECLARE(sensor_combined);
-
 int
 test()
 {
@@ -700,48 +773,65 @@ test()
 	orb_unsubscribe(sfd);
 	close(pfd);
 
-#if 0
+	/* this routine tests the multi-topic support */
-	/* this is a hacky test that exploits the sensors app to test rate-limiting */
+	test_note("try multi-topic support");
 
-	sfd = orb_subscribe(ORB_ID(sensor_combined));
+	int instance0;
+	int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX);
 
-	hrt_abstime start, end;
+		test_note("advertised");
-	unsigned count;
+	usleep(300000);
 
-	start = hrt_absolute_time();
+	int instance1;
-	count = 0;
+	int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN);
 
-	do {
+	if (instance0 != 0)
-		orb_check(sfd, &updated);
+		return test_fail("mult. id0: %d", instance0);
 
-		if (updated) {
+	if (instance1 != 1)
-			orb_copy(ORB_ID(sensor_combined), sfd, nullptr);
+		return test_fail("mult. id1: %d", instance1);
-			count++;
-		}
-	} while (count < 100);
 
-	end = hrt_absolute_time();
+	t.val = 103;
-	test_note("full-speed, 100 updates in %llu", end - start);
+	if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t))
+		return test_fail("mult. pub0 fail");
 
-	orb_set_interval(sfd, 10);
+		test_note("published");
+	usleep(300000);
 
-	start = hrt_absolute_time();
+	t.val = 203;
-	count = 0;
+	if (OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t))
+		return test_fail("mult. pub1 fail");
 
-	do {
+	/* subscribe to both topics and ensure valid data is received */
-		orb_check(sfd, &updated);
+	int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
 
-		if (updated) {
+	if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &t))
-			orb_copy(ORB_ID(sensor_combined), sfd, nullptr);
+		return test_fail("sub #0 copy failed: %d", errno);
-			count++;
-		}
-	} while (count < 100);
 
-	end = hrt_absolute_time();
+	if (t.val != 103)
-	test_note("100Hz, 100 updates in %llu", end - start);
+		return test_fail("sub #0 val. mismatch: %d", t.val);
 
-	orb_unsubscribe(sfd);
+	int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1);
-#endif
+
+	if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &t))
+		return test_fail("sub #1 copy failed: %d", errno);
+
+	if (t.val != 203)
+		return test_fail("sub #1 val. mismatch: %d", t.val);
+
+	/* test priorities */
+	int prio;
+	if (OK != orb_priority(sfd0, &prio))
+		return test_fail("prio #0");
+	
+	if (prio != ORB_PRIO_MAX)
+		return test_fail("prio: %d", prio);
+
+	if (OK != orb_priority(sfd1, &prio))
+		return test_fail("prio #1");
+
+	if (prio != ORB_PRIO_MIN)
+		return test_fail("prio: %d", prio);
 
 	return test_note("PASS");
 }
@@ -771,7 +861,7 @@ uorb_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (g_dev != nullptr) {
-			fprintf(stderr, "[uorb] already loaded\n");
+			warnx("already loaded");
 			/* user wanted to start uorb, its already running, no error */
 			return 0;
 		}
@@ -780,18 +870,17 @@ uorb_main(int argc, char *argv[])
 		g_dev = new ORBDevMaster(PUBSUB);
 
 		if (g_dev == nullptr) {
-			fprintf(stderr, "[uorb] driver alloc failed\n");
+			warnx("driver alloc failed");
 			return -ENOMEM;
 		}
 
 		if (OK != g_dev->init()) {
-			fprintf(stderr, "[uorb] driver init failed\n");
+			warnx("driver init failed");
 			delete g_dev;
 			g_dev = nullptr;
 			return -EIO;
 		}
 
-		printf("[uorb] ready\n");
 		return OK;
 	}
 
@@ -807,8 +896,7 @@ uorb_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status"))
 		return info();
 
-	fprintf(stderr, "unrecognised command, try 'start', 'test' or 'status'\n");
+	errx(-EINVAL, "unrecognized command, try 'start', 'test' or 'status'");
-	return -EINVAL;
 }
 
 /*
@@ -825,11 +913,14 @@ namespace
  *       we tried to advertise.
  */
 int
-node_advertise(const struct orb_metadata *meta)
+node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT)
 {
 	int fd = -1;
 	int ret = ERROR;
 
+	/* fill advertiser data */
+	const struct orb_advertdata adv = { meta, instance, priority };
+
 	/* open the control device */
 	fd = open(TOPIC_MASTER_DEVICE_PATH, 0);
 
@@ -837,11 +928,12 @@ node_advertise(const struct orb_metadata *meta)
 		goto out;
 
 	/* advertise the object */
-	ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)meta);
+	ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv);
 
 	/* it's OK if it already exists */
-	if ((OK != ret) && (EEXIST == errno))
+	if ((OK != ret) && (EEXIST == errno)) {
 		ret = OK;
+	}
 
 out:
 
@@ -858,7 +950,7 @@ out:
  * advertisers.
  */
 int
-node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser)
+node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT)
 {
 	char path[orb_maxpath];
 	int fd, ret;
@@ -883,7 +975,7 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
 	/*
 	 * Generate the path to the node and try to open it.
 	 */
-	ret = node_mkpath(path, f, meta);
+	ret = node_mkpath(path, f, meta, instance);
 
 	if (ret != OK) {
 		errno = -ret;
@@ -893,15 +985,34 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
 	/* open the path as either the advertiser or the subscriber */
 	fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
 
+	/* if we want to advertise and the node existed, we have to re-try again */
+	if ((fd >= 0) && (instance != nullptr) && (advertiser)) {
+		/* close the fd, we want a new one */
+		close(fd);
+		/* the node_advertise call will automatically go for the next free entry */
+		fd = -1;
+	}
+
 	/* we may need to advertise the node... */
 	if (fd < 0) {
 
 		/* try to create the node */
-		ret = node_advertise(meta);
+		ret = node_advertise(meta, instance, priority);
+
+		if (ret == OK) {
+			/* update the path, as it might have been updated during the node_advertise call */
+			ret = node_mkpath(path, f, meta, instance);
+
+			if (ret != OK) {
+				errno = -ret;
+				return ERROR;
+			}
+		}
 
 		/* on success, try the open again */
-		if (ret == OK)
+		if (ret == OK) {
 			fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
+		}
 	}
 
 	if (fd < 0) {
@@ -917,12 +1028,18 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
 
 orb_advert_t
 orb_advertise(const struct orb_metadata *meta, const void *data)
+{
+	return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT);
+}
+
+orb_advert_t
+orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
 {
 	int result, fd;
 	orb_advert_t advertiser;
 
 	/* open the node as an advertiser */
-	fd = node_open(PUBSUB, meta, data, true);
+	fd = node_open(PUBSUB, meta, data, true, instance, priority);
 	if (fd == ERROR)
 		return ERROR;
 
@@ -933,7 +1050,7 @@ orb_advertise(const struct orb_metadata *meta, const void *data)
 		return ERROR;
 
 	/* the advertiser must perform an initial publish to initialise the object */
-	result= orb_publish(meta, advertiser, data);
+	result = orb_publish(meta, advertiser, data);
 	if (result == ERROR)
 		return ERROR;
 
@@ -946,6 +1063,13 @@ orb_subscribe(const struct orb_metadata *meta)
 	return node_open(PUBSUB, meta, nullptr, false);
 }
 
+int
+orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
+{
+	int inst = instance;
+	return node_open(PUBSUB, meta, nullptr, false, &inst);
+}
+
 int
 orb_unsubscribe(int handle)
 {
@@ -988,6 +1112,12 @@ orb_stat(int handle, uint64_t *time)
 	return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time);
 }
 
+int
+orb_priority(int handle, int *priority)
+{
+	return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority);
+}
+
 int
 orb_set_interval(int handle, unsigned interval)
 {

src/modules/uORB/uORB.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -56,6 +56,25 @@ struct orb_metadata {
 
 typedef const struct orb_metadata *orb_id_t;
 
+/**
+ * Maximum number of multi topic instances
+ */
+#define ORB_MULTI_MAX_INSTANCES	3
+
+/**
+ * Topic priority.
+ * Relevant for multi-topics / topic groups
+ */
+enum ORB_PRIO {
+	ORB_PRIO_MIN = 0,
+	ORB_PRIO_VERY_LOW = 25,
+	ORB_PRIO_LOW = 50,
+	ORB_PRIO_DEFAULT = 75,
+	ORB_PRIO_HIGH = 100,
+	ORB_PRIO_VERY_HIGH = 125,
+	ORB_PRIO_MAX = 255
+};
+
 /**
  * Generates a pointer to the uORB metadata structure for
  * a given topic.
@@ -67,33 +86,6 @@ typedef const struct orb_metadata *orb_id_t;
  */
 #define ORB_ID(_name)		&__orb_##_name
 
-/**
- * Generates a pointer to the uORB metadata structure for
- * a given topic.
- *
- * The topic must have been declared previously in scope
- * with ORB_DECLARE().
- *
- * @param _name		The name of the topic.
- * @param _count	The class ID of the topic
- */
-#define ORB_ID_DOUBLE(_name, _count)	((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : &__orb_##_name##1)
-
-/**
- * Generates a pointer to the uORB metadata structure for
- * a given topic.
- *
- * The topic must have been declared previously in scope
- * with ORB_DECLARE().
- *
- * @param _name		The name of the topic.
- * @param _count	The class ID of the topic
- */
-#define ORB_ID_TRIPLE(_name, _count)					\
- 	((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : \
-			((_count == CLASS_DEVICE_SECONDARY) ? &__orb_##_name##1 : \
-				(((_count == CLASS_DEVICE_TERTIARY) ? &__orb_##_name##2 : 0))))
-
 /**
  * Declare (prototype) the uORB metadata for a topic.
  *
@@ -167,6 +159,34 @@ typedef intptr_t	orb_advert_t;
  */
 extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT;
 
+/**
+ * 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.
+ *			For topics updated by interrupt handlers, the advertisement
+ *			must be performed from non-interrupt context.
+ * @param instance	Pointer to an integer which will yield the instance ID (0-based)
+ *			of the publication.
+ * @param priority	The priority of the instance. If a subscriber subscribes multiple
+ *			instances, the priority allows the subscriber to prioritize the best
+ *			data source as long as its available.
+ * @return		ERROR on error, otherwise returns a handle
+ *			that can be used to publish to the topic.
+ *			If the topic in question is not known (due to an
+ *			ORB_DEFINE with no corresponding ORB_DECLARE)
+ *			this function will return -1 and set errno to ENOENT.
+ */
+extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) __EXPORT;
+
+
 /**
  * Publish new data to a topic.
  *
@@ -210,6 +230,37 @@ extern int	orb_publish(const struct orb_metadata *meta, orb_advert_t handle, con
  */
 extern int	orb_subscribe(const struct orb_metadata *meta) __EXPORT;
 
+/**
+ * Subscribe to a multi-instance of a topic.
+ *
+ * The returned value is a file descriptor that can be passed to poll()
+ * in order to wait for updates to a topic, as well as topic_read,
+ * orb_check and orb_stat.
+ *
+ * Subscription will succeed even if the topic has not been advertised;
+ * in this case the topic will have a timestamp of zero, it will never
+ * signal a poll() event, checking will always return false and it cannot
+ * be copied. When the topic is subsequently advertised, poll, check,
+ * stat and copy calls will react to the initial publication that is
+ * performed as part of the advertisement.
+ *
+ * Subscription will fail if the topic is not known to the system, i.e.
+ * there is nothing in the system that has declared the topic and thus it
+ * can never be published.
+ *
+ * @param meta		The uORB metadata (usually from the ORB_ID() macro)
+ *			for the topic.
+ * @param instance	The instance of the topic. Instance 0 matches the
+ *			topic of the orb_subscribe() call, higher indices
+ *			are for topics created with orb_publish_multi().
+ * @return		ERROR on error, otherwise returns a handle
+ *			that can be used to read and update the topic.
+ *			If the topic in question is not known (due to an
+ *			ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE)
+ *			this function will return -1 and set errno to ENOENT.
+ */
+extern int	orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) __EXPORT;
+
 /**
  * Unsubscribe from a topic.
  *
@@ -266,6 +317,18 @@ extern int	orb_check(int handle, bool *updated) __EXPORT;
  */
 extern int	orb_stat(int handle, uint64_t *time) __EXPORT;
 
+/**
+ * Return the priority of the topic
+ *
+ * @param handle	A handle returned from orb_subscribe.
+ * @param priority	Returns the priority of this topic. This is only relevant for
+ *			topics which are published by multiple publishers (e.g. mag0, mag1, etc.)
+ *			and allows a subscriber to automatically pick the topic with the highest
+ *			priority, independent of the startup order of the associated publishers.
+ * @return		OK on success, ERROR otherwise with errno set accordingly.
+ */
+extern int	orb_priority(int handle, int *priority) __EXPORT;
+
 /**
  * Set the minimum interval between which updates are seen for a subscription.
  *

src/modules/uavcan/module.mk

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
 #   Author: Pavel Kirienko <pavel.kirienko@gmail.com>
 #
 # Redistribution and use in source and binary forms, with or without

src/modules/uavcan/sensors/baro.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 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
@@ -38,15 +38,10 @@
 #include "baro.hpp"
 #include <cmath>
 
-static const orb_id_t BARO_TOPICS[2] = {
-	ORB_ID(sensor_baro0),
-	ORB_ID(sensor_baro1)
-};
-
 const char *const UavcanBarometerBridge::NAME = "baro";
 
 UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) :
-UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS),
+UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, ORB_ID(sensor_baro)),
 _sub_air_data(node)
 {
 }

src/modules/uavcan/sensors/baro.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 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

src/modules/uavcan/sensors/gnss.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 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

src/modules/uavcan/sensors/gnss.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 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

src/modules/uavcan/sensors/mag.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 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
@@ -39,16 +39,10 @@
 
 #include <systemlib/err.h>
 
-static const orb_id_t MAG_TOPICS[3] = {
-	ORB_ID(sensor_mag0),
-	ORB_ID(sensor_mag1),
-	ORB_ID(sensor_mag2)
-};
-
 const char *const UavcanMagnetometerBridge::NAME = "mag";
 
 UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
-UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
+UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, ORB_ID(sensor_mag)),
 _sub_mag(node)
 {
 	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;

src/modules/uavcan/sensors/mag.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 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

src/modules/uavcan/sensors/sensor_bridge.cpp

@@ -62,7 +62,6 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
 			(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
 		}
 	}
-	delete [] _orb_topics;
 	delete [] _channels;
 }
 
@@ -116,11 +115,10 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
 		}
 
 		// Publish to the appropriate topic, abort on failure
-		channel->orb_id         = _orb_topics[class_instance];
 		channel->node_id        = node_id;
 		channel->class_instance = class_instance;
 
-		channel->orb_advert = orb_advertise(channel->orb_id, report);
+		channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH);
 		if (channel->orb_advert < 0) {
 			log("ADVERTISE FAILED");
 			(void)unregister_class_devname(_class_devname, class_instance);
@@ -132,7 +130,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
 	}
 	assert(channel != nullptr);
 
-	(void)orb_publish(channel->orb_id, channel->orb_advert, report);
+	(void)orb_publish(_orb_topic, channel->orb_advert, report);
 }
 
 unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const

src/modules/uavcan/sensors/sensor_bridge.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 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
@@ -75,8 +75,7 @@ public:
 
 	/**
 	 * Sensor bridge factory.
-	 * Creates a bridge object by its ASCII name, e.g. "gnss", "mag".
+	 * Creates all known sensor bridges and puts them in the linked list.
-	 * @return nullptr if such bridge can't be created.
 	 */
 	static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
 };
@@ -90,28 +89,29 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD
 	struct Channel
 	{
 		int node_id              = -1;
-		orb_id_t orb_id          = nullptr;
 		orb_advert_t orb_advert  = -1;
 		int class_instance       = -1;
+		int orb_instance	 = -1;
 	};
 
 	const unsigned _max_channels;
 	const char *const _class_devname;
-	orb_id_t *const _orb_topics;
+	const orb_id_t _orb_topic;
 	Channel *const _channels;
 	bool _out_of_channels = false;
 
 protected:
-	template <unsigned MaxChannels>
+	static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody
+
 	UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
-	                           const orb_id_t (&orb_topics)[MaxChannels]) :
+	                           const orb_id_t orb_topic_sensor,
+	                           const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
 	device::CDev(name, devname),
-	_max_channels(MaxChannels),
+	_max_channels(max_channels),
 	_class_devname(class_devname),
-	_orb_topics(new orb_id_t[MaxChannels]),
+	_orb_topic(orb_topic_sensor),
-	_channels(new Channel[MaxChannels])
+	_channels(new Channel[max_channels])
 	{
-		memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
 		_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
 		_device_id.devid_s.bus = 0;
 	}

src/systemcmds/config/config.c

@@ -81,7 +81,7 @@ config_main(int argc, char *argv[])
 			do_device(argc - 1, argv + 1);
 		}
 	}
-	
+
 	errx(1, "expected a device, try '/dev/gyro', '/dev/accel', '/dev/mag'");
 }
 
@@ -192,8 +192,12 @@ do_gyro(int argc, char *argv[])
 		int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
 		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
 		int range = ioctl(fd, GYROIOCGRANGE, 0);
+		int id = ioctl(fd, DEVIOCGDEVICEID,0);
+		int32_t calibration_id = 0;
 
-		warnx("gyro: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", srate, prate, range);
+		param_get(param_find("SENS_GYRO_ID"), &(calibration_id));
+
+		warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", id, calibration_id, srate, prate, range);
 
 		close(fd);
 	}
@@ -267,9 +271,10 @@ do_mag(int argc, char *argv[])
 		int range = ioctl(fd, MAGIOCGRANGE, 0);
 		int id = ioctl(fd, DEVIOCGDEVICEID,0);
 		int32_t calibration_id = 0;
+
 		param_get(param_find("SENS_MAG_ID"), &(calibration_id));
 
-		warnx("mag: \n\tdevice id:\t%x\t(calibration is for device id %x)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range);
+		warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range);
 
 		close(fd);
 	}
@@ -341,8 +346,12 @@ do_accel(int argc, char *argv[])
 		int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
 		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
 		int range = ioctl(fd, ACCELIOCGRANGE, 0);
+		int id = ioctl(fd, DEVIOCGDEVICEID,0);
+		int32_t calibration_id = 0;
 
-		warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range);
+		param_get(param_find("SENS_ACC_ID"), &(calibration_id));
+
+		warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", id, calibration_id, srate, prate, range);
 
 		close(fd);
 	}

src/systemcmds/preflight_check/preflight_check.c

@@ -84,7 +84,7 @@ int preflight_check_main(int argc, char *argv[])
 	/* open text message output path */
 	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 	int ret;
-	int32_t mag_devid,mag_calibration_devid;
+	int32_t devid, calibration_devid;
 
 	/* give the system some time to sample the sensors in the background */
 	usleep(150000);
@@ -98,9 +98,9 @@ int preflight_check_main(int argc, char *argv[])
 		goto system_eval;
 	}
 
-	mag_devid = ioctl(fd, DEVIOCGDEVICEID,0);
+	devid = ioctl(fd, DEVIOCGDEVICEID,0);
-	param_get(param_find("SENS_MAG_ID"), &(mag_calibration_devid));
+	param_get(param_find("SENS_MAG_ID"), &(calibration_devid));
-	if (mag_devid != mag_calibration_devid){
+	if (devid != calibration_devid){
 		warnx("magnetometer calibration is for a different device - calibrate magnetometer first");
 		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID");
 		system_ok = false;
@@ -108,7 +108,7 @@ int preflight_check_main(int argc, char *argv[])
 	}
 
 	ret = ioctl(fd, MAGIOCSELFTEST, 0);
-	
+
 	if (ret != OK) {
 		warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
 		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL");
@@ -120,8 +120,18 @@ int preflight_check_main(int argc, char *argv[])
 
 	close(fd);
 	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	devid = ioctl(fd, DEVIOCGDEVICEID,0);
+	param_get(param_find("SENS_ACC_ID"), &(calibration_devid));
+	if (devid != calibration_devid){
+		warnx("accelerometer calibration is for a different device - calibrate accelerometer first");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID");
+		system_ok = false;
+		goto system_eval;
+	}
+
 	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
-	
+
 	if (ret != OK) {
 		warnx("accel self test failed");
 		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL");
@@ -156,8 +166,18 @@ int preflight_check_main(int argc, char *argv[])
 
 	close(fd);
 	fd = open(GYRO_DEVICE_PATH, 0);
+
+	devid = ioctl(fd, DEVIOCGDEVICEID,0);
+	param_get(param_find("SENS_GYRO_ID"), &(calibration_devid));
+	if (devid != calibration_devid){
+		warnx("gyro calibration is for a different device - calibrate gyro first");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID");
+		system_ok = false;
+		goto system_eval;
+	}
+
 	ret = ioctl(fd, GYROIOCSELFTEST, 0);
-	
+
 	if (ret != OK) {
 		warnx("gyro self test failed");
 		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL");
@@ -183,10 +203,10 @@ int preflight_check_main(int argc, char *argv[])
 	system_ok &= rc_ok;
 
 
-		
+
 
 system_eval:
-	
+
 	if (system_ok) {
 		/* all good, exit silently */
 		exit(0);