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

Conflicts:
	src/drivers/px4fmu/fmu.cpp

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[] = {
-			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]);
+		_act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs), 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));
-	_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
-	_mag_sub = orb_subscribe(ORB_ID(sensor_mag0));
-	_gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1));
-	_accel1_sub = orb_subscribe(ORB_ID(sensor_accel1));
-	_mag1_sub = orb_subscribe(ORB_ID(sensor_mag1));
-	_gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2));
-	_accel2_sub = orb_subscribe(ORB_ID(sensor_accel2));
-	_mag2_sub = orb_subscribe(ORB_ID(sensor_mag2));
+	_gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);
+	_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
+	_mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0);
+	_gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1);
+	_accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1);
+	_mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1);
+	_gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2);
+	_accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2);
+	_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));
-	_baro1_sub = orb_subscribe(ORB_ID(sensor_baro1));
+	_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
+	_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	= 4096
+MODULE_STACKSIZE	= 2048
 
 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_mag1, struct mag_report);
-ORB_DEFINE(sensor_mag2, struct mag_report);
+ORB_DEFINE(sensor_mag, struct mag_report);
 
 #include <drivers/drv_accel.h>
-ORB_DEFINE(sensor_accel0, struct accel_report);
-ORB_DEFINE(sensor_accel1, struct accel_report);
-ORB_DEFINE(sensor_accel2, struct accel_report);
+ORB_DEFINE(sensor_accel, struct accel_report);
 
 #include <drivers/drv_gyro.h>
-ORB_DEFINE(sensor_gyro0, struct gyro_report);
-ORB_DEFINE(sensor_gyro1, struct gyro_report);
-ORB_DEFINE(sensor_gyro2, struct gyro_report);
+ORB_DEFINE(sensor_gyro, struct gyro_report);
 
 #include <drivers/drv_baro.h>
-ORB_DEFINE(sensor_baro0, struct baro_report);
-ORB_DEFINE(sensor_baro1, struct baro_report);
-ORB_DEFINE(sensor_baro2, struct baro_report);
+ORB_DEFINE(sensor_baro, 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_1, struct actuator_outputs_s);
-ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
-ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
+ORB_DEFINE(actuator_outputs, 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_1);
-ORB_DECLARE(actuator_outputs_2);
-ORB_DECLARE(actuator_outputs_3);
+ORB_DECLARE(actuator_outputs);
 
-/* output sets with pre-defined applications */
-#define ORB_ID_VEHICLE_CONTROLS		ORB_ID(actuator_outputs_0)
-
-#endif
+#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",
-		       (f == PUBSUB) ? "obj" : "param",
-		       meta->o_name);
+	unsigned index = 0;
 
-	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 */
-			ret = node_mkpath(nodepath, _flavor, meta);
-
-			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);
+			/* set instance to zero - we could allow selective multi-pubs later based on value */
+			if (adv->instance != nullptr) {
+				*(adv->instance) = 0;
 			}
 
+			/* 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 is a hacky test that exploits the sensors app to test rate-limiting */
+	/* this routine tests the multi-topic support */
+	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;
-	unsigned count;
+		test_note("advertised");
+	usleep(300000);
 
-	start = hrt_absolute_time();
-	count = 0;
+	int instance1;
+	int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN);
 
-	do {
-		orb_check(sfd, &updated);
+	if (instance0 != 0)
+		return test_fail("mult. id0: %d", instance0);
 
-		if (updated) {
-			orb_copy(ORB_ID(sensor_combined), sfd, nullptr);
-			count++;
-		}
-	} while (count < 100);
+	if (instance1 != 1)
+		return test_fail("mult. id1: %d", instance1);
 
-	end = hrt_absolute_time();
-	test_note("full-speed, 100 updates in %llu", end - start);
+	t.val = 103;
+	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();
-	count = 0;
+	t.val = 203;
+	if (OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t))
+		return test_fail("mult. pub1 fail");
 
-	do {
-		orb_check(sfd, &updated);
+	/* subscribe to both topics and ensure valid data is received */
+	int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
 
-		if (updated) {
-			orb_copy(ORB_ID(sensor_combined), sfd, nullptr);
-			count++;
-		}
-	} while (count < 100);
+	if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &t))
+		return test_fail("sub #0 copy failed: %d", errno);
 
-	end = hrt_absolute_time();
-	test_note("100Hz, 100 updates in %llu", end - start);
+	if (t.val != 103)
+		return test_fail("sub #0 val. mismatch: %d", t.val);
 
-	orb_unsubscribe(sfd);
-#endif
+	int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1);
+
+	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");
-	return -EINVAL;
+	errx(-EINVAL, "unrecognized command, try 'start', 'test' or 'status'");
 }
 
 /*
@@ -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".
-	 * @return nullptr if such bridge can't be created.
+	 * Creates all known sensor bridges and puts them in the linked list.
 	 */
 	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]),
-	_channels(new Channel[MaxChannels])
+	_orb_topic(orb_topic_sensor),
+	_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;
 	}