uORB: introduce SubscriptionMultiArray for working with multi-instances

src/drivers/uavcan/sensors/sensor_bridge.hpp

@@ -105,7 +105,7 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD
 	bool _out_of_channels = false;
 
 protected:
-	static constexpr unsigned DEFAULT_MAX_CHANNELS = ORB_MULTI_MAX_INSTANCES;
+	static constexpr unsigned DEFAULT_MAX_CHANNELS = 4;
 	const unsigned _max_channels;
 
 	UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,

src/drivers/uavcannode/UavcanNode.cpp

@@ -380,7 +380,7 @@ void UavcanNode::Run()
 	}
 
 	// distance_sensor[] -> uavcan::equipment::range_sensor::Measurement
-	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+	for (int i = 0; i < MAX_INSTANCES; i++) {
 		distance_sensor_s dist;
 
 		if (_distance_sensor_sub[i].update(&dist)) {

src/drivers/uavcannode/UavcanNode.hpp

@@ -183,7 +183,8 @@ private:
 
 	uORB::SubscriptionCallbackWorkItem _battery_status_sub{this, ORB_ID(battery_status)};
 	uORB::SubscriptionCallbackWorkItem _diff_pressure_sub{this, ORB_ID(differential_pressure)};
-	uORB::SubscriptionCallbackWorkItem _distance_sensor_sub[ORB_MULTI_MAX_INSTANCES] {
+	static constexpr int MAX_INSTANCES = 4;
+	uORB::SubscriptionCallbackWorkItem _distance_sensor_sub[MAX_INSTANCES] {
 		{this, ORB_ID(distance_sensor), 0},
 		{this, ORB_ID(distance_sensor), 1},
 		{this, ORB_ID(distance_sensor), 2},

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -200,13 +200,10 @@ CollisionPrevention::_updateObstacleMap()
 	_sub_vehicle_attitude.update();
 
 	// add distance sensor data
-	for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+	for (auto &dist_sens_sub : _distance_sensor_subs) {
-
+		distance_sensor_s distance_sensor;
-		// if a new distance sensor message has arrived
-		if (_sub_distance_sensor[i].updated()) {
-			distance_sensor_s distance_sensor {};
-			_sub_distance_sensor[i].copy(&distance_sensor);
 
+		if (dist_sens_sub.update(&distance_sensor)) {
 			// consider only instances with valid data and orientations useful for collision prevention
 			if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
 			    (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -51,6 +51,7 @@
 #include <systemlib/mavlink_log.h>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/collision_constraints.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/mavlink_log.h>
@@ -131,8 +132,8 @@ private:
 	uORB::PublicationQueued<vehicle_command_s>	_vehicle_command_pub{ORB_ID(vehicle_command)};			/**< vehicle command do publication */
 
 	uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */
-	uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */
 	uORB::SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude{ORB_ID(vehicle_attitude)};
+	uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
 
 	static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms};
 	static constexpr uint64_t TIMEOUT_HOLD_US{5_s};

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -44,6 +44,7 @@
 #include <systemlib/mavlink_log.h>
 
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/airspeed_validated.h>
 #include <uORB/topics/estimator_status.h>
@@ -113,7 +114,7 @@ private:
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
-	uORB::Subscription _airspeed_sub[MAX_NUM_AIRSPEED_SENSORS] {{ORB_ID(airspeed), 0}, {ORB_ID(airspeed), 1}, {ORB_ID(airspeed), 2}}; /**< raw airspeed topics subscriptions. */
+	uORB::SubscriptionMultiArray<airspeed_s, MAX_NUM_AIRSPEED_SENSORS> _airspeed_subs{ORB_ID::airspeed};
 
 	estimator_status_s _estimator_status {};
 	vehicle_acceleration_s _accel {};
@@ -244,8 +245,8 @@ AirspeedModule::check_for_connected_airspeed_sensors()
 
 	if (_param_airspeed_primary_index.get() > 0) {
 
-		for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
+		for (int i = 0; i < _airspeed_subs.size(); i++) {
-			if (!_airspeed_sub[i].advertised()) {
+			if (!_airspeed_subs[i].advertised()) {
 				break;
 			}
 
@@ -323,30 +324,32 @@ AirspeedModule::Run()
 		/* iterate through all airspeed sensors, poll new data from them and update their validators */
 		for (int i = 0; i < _number_of_airspeed_sensors; i++) {
 
-			/* poll airspeed data */
+			// poll raw airspeed topic of the i-th sensor
-			airspeed_s airspeed_raw = {};
+			airspeed_s airspeed_raw;
-			_airspeed_sub[i].copy(&airspeed_raw); // poll raw airspeed topic of the i-th sensor
-			input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s;
-			input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s;
-			input_data.airspeed_timestamp = airspeed_raw.timestamp;
-			input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius;
 
-			/* update in_fixed_wing_flight for the current airspeed sensor validator */
+			if (_airspeed_subs[i].update(&airspeed_raw)) {
-			/* takeoff situation is active from start till one of the sensors' IAS or groundspeed_EAS is above stall speed */
+
-			if (airspeed_raw.indicated_airspeed_m_s > _airspeed_stall.get() || _ground_minus_wind_EAS > _airspeed_stall.get()) {
+				input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s;
-				_in_takeoff_situation = false;
+				input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s;
+				input_data.airspeed_timestamp = airspeed_raw.timestamp;
+				input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius;
+
+				/* update in_fixed_wing_flight for the current airspeed sensor validator */
+				/* takeoff situation is active from start till one of the sensors' IAS or groundspeed_EAS is above stall speed */
+				if (airspeed_raw.indicated_airspeed_m_s > _airspeed_stall.get() || _ground_minus_wind_EAS > _airspeed_stall.get()) {
+					_in_takeoff_situation = false;
+				}
+
+				/* reset takeoff_situation to true when not in air or not in fixed-wing mode */
+				if (!in_air || !fixed_wing) {
+					_in_takeoff_situation = true;
+				}
+
+				input_data.in_fixed_wing_flight = (armed && fixed_wing && in_air && !_in_takeoff_situation);
+
+				/* push input data into airspeed validator */
+				_airspeed_validator[i].update_airspeed_validator(input_data);
 			}
-
-			/* reset takeoff_situation to true when not in air or not in fixed-wing mode */
-			if (!in_air || !fixed_wing) {
-				_in_takeoff_situation = true;
-			}
-
-			input_data.in_fixed_wing_flight = (armed && fixed_wing && in_air && !_in_takeoff_situation);
-
-			/* push input data into airspeed validator */
-			_airspeed_validator[i].update_airspeed_validator(input_data);
-
 		}
 	}
 

src/modules/commander/Commander.cpp

@@ -3755,7 +3755,7 @@ void Commander::mission_init()
 
 void Commander::data_link_check()
 {
-	for (auto &telemetry_status :  _telemetry_status_sub) {
+	for (auto &telemetry_status :  _telemetry_status_subs) {
 		telemetry_status_s telemetry;
 
 		if (telemetry_status.update(&telemetry)) {
@@ -3924,14 +3924,13 @@ void Commander::data_link_check()
 
 void Commander::avoidance_check()
 {
+	for (auto &dist_sens_sub : _distance_sensor_subs) {
+		distance_sensor_s distance_sensor;
 
-	for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		if (dist_sens_sub.update(&distance_sensor)) {
-		if (_sub_distance_sensor[i].updated()) {
-			distance_sensor_s distance_sensor {};
-			_sub_distance_sensor[i].copy(&distance_sensor);
-
 			if ((distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
 			    (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {
+
 				_valid_distance_sensor_time_us = distance_sensor.timestamp;
 			}
 		}
@@ -3961,28 +3960,24 @@ void Commander::avoidance_check()
 
 void Commander::battery_status_check()
 {
-	bool battery_sub_updated = false;
+	// We need to update the status flag if ANY battery is updated, because the system source might have
+	// changed, or might be nothing (if there is no battery connected)
+	if (!_battery_status_subs.updated()) {
+		// Nothing has changed since the last time this function was called, so nothing needs to be done now.
+		return;
+	}
 
-	battery_status_s batteries[ORB_MULTI_MAX_INSTANCES];
+	battery_status_s batteries[_battery_status_subs.size()];
 	size_t num_connected_batteries = 0;
 
-	for (size_t i = 0; i < sizeof(_battery_subs) / sizeof(_battery_subs[0]); i++) {
+	for (auto &battery_sub : _battery_status_subs) {
-		// We need to update the status flag if ANY battery is updated, because the system source might have
+		if (battery_sub.copy(&batteries[num_connected_batteries])) {
-		// changed, or might be nothing (if there is no battery connected)
-		battery_sub_updated |= _battery_subs[i].updated();
-
-		if (_battery_subs[i].copy(&batteries[num_connected_batteries])) {
 			if (batteries[num_connected_batteries].connected) {
 				num_connected_batteries++;
 			}
 		}
 	}
 
-	if (!battery_sub_updated) {
-		// Nothing has changed since the last time this function was called, so nothing needs to be done now.
-		return;
-	}
-
 	// There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest
 	// option is to check if ANY of them have a warning, and specifically find which one has the most
 	// urgent warning.

src/modules/commander/Commander.hpp

@@ -56,6 +56,7 @@
 
 // subscriptions
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
@@ -384,21 +385,8 @@ private:
 
 	// Subscriptions
 	uORB::Subscription					_actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};
-#if BOARD_NUMBER_BRICKS > 1
-	uORB::Subscription					_battery_subs[ORB_MULTI_MAX_INSTANCES] {
-		uORB::Subscription(ORB_ID(battery_status), 0),
-		uORB::Subscription(ORB_ID(battery_status), 1),
-		uORB::Subscription(ORB_ID(battery_status), 2),
-		uORB::Subscription(ORB_ID(battery_status), 3),
-	};
-#else
-	uORB::Subscription					_battery_subs[1] {
-		uORB::Subscription(ORB_ID(battery_status), 0)
-	};
-#endif
 	uORB::Subscription					_cmd_sub {ORB_ID(vehicle_command)};
 	uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
-	uORB::Subscription					_sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */
 	uORB::Subscription					_esc_status_sub{ORB_ID(esc_status)};
 	uORB::Subscription					_geofence_result_sub{ORB_ID(geofence_result)};
 	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
@@ -408,9 +396,11 @@ private:
 	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription					_subsys_sub{ORB_ID(subsystem_info)};
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
-	uORB::Subscription					_telemetry_status_sub[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(telemetry_status), 0}, {ORB_ID(telemetry_status), 1}, {ORB_ID(telemetry_status), 2}, {ORB_ID(telemetry_status), 3}};
 	uORB::Subscription					_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
+	uORB::SubscriptionMultiArray<battery_status_s>          _battery_status_subs{ORB_ID::battery_status};
+	uORB::SubscriptionMultiArray<distance_sensor_s>         _distance_sensor_subs{ORB_ID::distance_sensor};
+	uORB::SubscriptionMultiArray<telemetry_status_s>        _telemetry_status_subs{ORB_ID::telemetry_status};
 
 #if defined(BOARD_HAS_POWER_CONTROL)
 	uORB::Subscription					_power_button_state_sub {ORB_ID(power_button_state)};

src/modules/commander/accelerometer_calibration.cpp

@@ -141,6 +141,7 @@
 #include <lib/systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionBlocking.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/vehicle_attitude.h>
 
@@ -437,11 +438,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 	sensor_correction_s sensor_correction{};
 	sensor_correction_sub.copy(&sensor_correction);
 
-	uORB::Subscription accel_sub[MAX_ACCEL_SENS] {
+	uORB::SubscriptionMultiArray<sensor_accel_s, MAX_ACCEL_SENS> accel_subs{ORB_ID::sensor_accel};
-		{ORB_ID(sensor_accel), 0},
-		{ORB_ID(sensor_accel), 1},
-		{ORB_ID(sensor_accel), 2},
-	};
 
 	/* use the first sensor to pace the readout, but do per-sensor counts */
 	for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
@@ -449,7 +446,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 		Vector3f accel_sum{};
 		unsigned count = 0;
 
-		while (accel_sub[accel_index].update(&arp)) {
+		while (accel_subs[accel_index].update(&arp)) {
 			// fetch optional thermal offset corrections in sensor/board frame
 			if ((arp.timestamp > 0) && (arp.device_id != 0)) {
 				Vector3f offset{0, 0, 0};

src/modules/ekf2/ekf2_main.cpp

@@ -54,6 +54,7 @@
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/ekf2_timestamps.h>
@@ -256,8 +257,7 @@ private:
 	int _lockstep_component{-1};
 
 	// because we can have several distance sensor instances with different orientations
-	static constexpr int MAX_RNG_SENSOR_COUNT = 4;
+	uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
-	uORB::Subscription _range_finder_subs[MAX_RNG_SENSOR_COUNT] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}};
 	int _range_finder_sub_index = -1; // index for downward-facing range finder subscription
 
 	// because we can have multiple GPS instances
@@ -1078,10 +1078,10 @@ void Ekf2::Run()
 
 		if (_range_finder_sub_index >= 0) {
 
-			if (_range_finder_subs[_range_finder_sub_index].updated()) {
+			if (_distance_sensor_subs[_range_finder_sub_index].updated()) {
 				distance_sensor_s range_finder;
 
-				if (_range_finder_subs[_range_finder_sub_index].copy(&range_finder)) {
+				if (_distance_sensor_subs[_range_finder_sub_index].copy(&range_finder)) {
 					rangeSample range_sample {};
 					range_sample.rng = range_finder.current_distance;
 					range_sample.quality = range_finder.signal_quality;
@@ -1789,10 +1789,10 @@ void Ekf2::resetPreFlightChecks()
 
 int Ekf2::getRangeSubIndex()
 {
-	for (unsigned i = 0; i < MAX_RNG_SENSOR_COUNT; i++) {
+	for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) {
-		distance_sensor_s report{};
+		distance_sensor_s report;
 
-		if (_range_finder_subs[i].update(&report)) {
+		if (_distance_sensor_subs[i].update(&report)) {
 			// only use the first instace which has the correct orientation
 			if (report.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
 				PX4_INFO("Found range finder with instance %d", i);

src/modules/mavlink/mavlink_high_latency2.cpp

@@ -72,7 +72,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t)
 		bool updated = _airspeed.valid();
 		updated |= _airspeed_sp.valid();
 
-		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		for (int i = 0; i < MAX_BATTERIES; i++) {
 			updated |= _batteries[i].analyzer.valid();
 		}
 
@@ -112,7 +112,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t)
 
 			int lowest = 0;
 
-			for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) {
+			for (int i = 1; i < MAX_BATTERIES; i++) {
 				const bool battery_connected = _batteries[i].connected && _batteries[i].analyzer.valid();
 				const bool battery_is_lowest = _batteries[i].analyzer.get_scaled(100.0f) <= _batteries[lowest].analyzer.get_scaled(
 								       100.0f);
@@ -183,7 +183,7 @@ void MavlinkStreamHighLatency2::reset_analysers(const hrt_abstime t)
 	_airspeed.reset();
 	_airspeed_sp.reset();
 
-	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+	for (int i = 0; i < MAX_BATTERIES; i++) {
 		_batteries[i].analyzer.reset();
 	}
 
@@ -230,7 +230,7 @@ bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *ms
 	struct battery_status_s battery;
 	bool updated = false;
 
-	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+	for (int i = 0; i < MAX_BATTERIES; i++) {
 		if (_batteries[i].subscription.update(&battery)) {
 			updated = true;
 			_batteries[i].connected = battery.connected;
@@ -486,7 +486,7 @@ void MavlinkStreamHighLatency2::update_battery_status()
 {
 	battery_status_s battery;
 
-	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+	for (int i = 0; i < MAX_BATTERIES; i++) {
 		if (_batteries[i].subscription.update(&battery)) {
 			_batteries[i].connected = battery.connected;
 			_batteries[i].analyzer.add_value(battery.remaining, _update_rate_filtered);

src/modules/mavlink/mavlink_high_latency2.h

@@ -138,7 +138,8 @@ private:
 	hrt_abstime _last_update_time = 0;
 	float _update_rate_filtered = 0.0f;
 
-	PerBatteryData _batteries[ORB_MULTI_MAX_INSTANCES] {0, 1, 2, 3};
+	static constexpr int MAX_BATTERIES = 4;
+	PerBatteryData _batteries[MAX_BATTERIES] {0, 1, 2, 3};
 
 	/* do not allow top copying this class */
 	MavlinkStreamHighLatency2(MavlinkStreamHighLatency2 &);

src/modules/mavlink/mavlink_messages.cpp

@@ -61,6 +61,7 @@
 #include <math.h>
 
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
@@ -559,9 +560,7 @@ public:
 private:
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
-	uORB::Subscription _battery_status_sub[ORB_MULTI_MAX_INSTANCES] {
+	uORB::SubscriptionMultiArray<battery_status_s> _battery_status_subs{ORB_ID::battery_status};
-		{ORB_ID(battery_status), 0}, {ORB_ID(battery_status), 1}, {ORB_ID(battery_status), 2}, {ORB_ID(battery_status), 3}
-	};
 
 	/* do not allow top copying this class */
 	MavlinkStreamSysStatus(MavlinkStreamSysStatus &) = delete;
@@ -574,15 +573,7 @@ protected:
 
 	bool send(const hrt_abstime t) override
 	{
-		bool updated_battery = false;
+		if (_status_sub.updated() || _cpuload_sub.updated() || _battery_status_subs.updated()) {
-
-		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-			if (_battery_status_sub[i].updated()) {
-				updated_battery = true;
-			}
-		}
-
-		if (_status_sub.updated() || _cpuload_sub.updated() || updated_battery) {
 			vehicle_status_s status{};
 			_status_sub.copy(&status);
 
@@ -591,13 +582,13 @@ protected:
 
 			battery_status_s battery_status[ORB_MULTI_MAX_INSTANCES] {};
 
-			for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+			for (int i = 0; i < _battery_status_subs.size(); i++) {
-				_battery_status_sub[i].copy(&battery_status[i]);
+				_battery_status_subs[i].copy(&battery_status[i]);
 			}
 
 			int lowest_battery_index = 0;
 
-			for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+			for (int i = 0; i < _battery_status_subs.size(); i++) {
 				if (battery_status[i].connected && (battery_status[i].remaining < battery_status[lowest_battery_index].remaining)) {
 					lowest_battery_index = i;
 				}
@@ -668,21 +659,12 @@ public:
 
 	unsigned get_size() override
 	{
-		unsigned total_size = 0;
+		static constexpr unsigned size_per_battery = MAVLINK_MSG_ID_BATTERY_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-
+		return size_per_battery * _battery_status_subs.advertised_count();
-		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-			if (_battery_status_sub[i].advertised()) {
-				total_size += MAVLINK_MSG_ID_BATTERY_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-			}
-		}
-
-		return total_size;
 	}
 
 private:
-	uORB::Subscription _battery_status_sub[ORB_MULTI_MAX_INSTANCES] {
+	uORB::SubscriptionMultiArray<battery_status_s> _battery_status_subs{ORB_ID::battery_status};
-		{ORB_ID(battery_status), 0}, {ORB_ID(battery_status), 1}, {ORB_ID(battery_status), 2}, {ORB_ID(battery_status), 3}
-	};
 
 	/* do not allow top copying this class */
 	MavlinkStreamBatteryStatus(MavlinkStreamSysStatus &) = delete;
@@ -697,10 +679,10 @@ protected:
 	{
 		bool updated = false;
 
-		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		for (auto &battery_sub : _battery_status_subs) {
 			battery_status_s battery_status;
 
-			if (_battery_status_sub[i].update(&battery_status)) {
+			if (battery_sub.update(&battery_status)) {
 				/* battery status message with higher resolution */
 				mavlink_battery_status_t bat_msg{};
 				// TODO: Determine how to better map between battery ID within the firmware and in MAVLink
@@ -764,7 +746,6 @@ protected:
 
 				updated = true;
 			}
-
 		}
 
 		return updated;
@@ -802,21 +783,12 @@ public:
 
 	unsigned get_size() override
 	{
-		unsigned total_size = 0;
+		static constexpr unsigned size_per_battery = MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-
+		return size_per_battery * _battery_status_subs.advertised_count();
-		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-			if (_battery_status_sub[i].advertised()) {
-				total_size += MAVLINK_MSG_ID_BATTERY_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-			}
-		}
-
-		return total_size;
 	}
 
 private:
-	uORB::Subscription _battery_status_sub[ORB_MULTI_MAX_INSTANCES] {
+	uORB::SubscriptionMultiArray<battery_status_s> _battery_status_subs{ORB_ID::battery_status};
-		{ORB_ID(battery_status), 0}, {ORB_ID(battery_status), 1}, {ORB_ID(battery_status), 2}, {ORB_ID(battery_status), 3}
-	};
 
 	/* do not allow top copying this class */
 	MavlinkStreamSmartBatteryInfo(MavlinkStreamSysStatus &) = delete;
@@ -831,10 +803,10 @@ protected:
 	{
 		bool updated = false;
 
-		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		for (auto &battery_sub : _battery_status_subs) {
 			battery_status_s battery_status;
 
-			if (_battery_status_sub[i].update(&battery_status)) {
+			if (battery_sub.update(&battery_status)) {
 				if (battery_status.serial_number == 0) {
 					//This is not smart battery
 					continue;
@@ -899,7 +871,7 @@ public:
 	}
 
 private:
-	uORB::Subscription _vehicle_imu_sub[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(vehicle_imu), 0}, {ORB_ID(vehicle_imu), 1}, {ORB_ID(vehicle_imu), 2}, {ORB_ID(vehicle_imu), 3}};
+	uORB::SubscriptionMultiArray<vehicle_imu_s> _vehicle_imu_subs{ORB_ID::vehicle_imu};
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _bias_sub{ORB_ID(estimator_sensor_bias)};
 	uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)};
@@ -923,7 +895,7 @@ protected:
 
 		vehicle_imu_s imu;
 
-		for (auto &imu_sub : _vehicle_imu_sub) {
+		for (auto &imu_sub : _vehicle_imu_subs) {
 			if (imu_sub.update(&imu)) {
 				if (imu.accel_device_id == sensor_selection.accel_device_id) {
 					updated = true;
@@ -2923,10 +2895,8 @@ public:
 			return size;
 		}
 
-		for (auto &x : _vehicle_imu_status_sub) {
+		if (_vehicle_imu_status_subs.advertised()) {
-			if (x.advertised()) {
+			return size;
-				return size;
-			}
 		}
 
 		return 0;
@@ -2934,13 +2904,7 @@ public:
 
 private:
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
-
+	uORB::SubscriptionMultiArray<vehicle_imu_status_s> _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status};
-	uORB::Subscription _vehicle_imu_status_sub[ORB_MULTI_MAX_INSTANCES] {
-		{ORB_ID(vehicle_imu_status), 0},
-		{ORB_ID(vehicle_imu_status), 1},
-		{ORB_ID(vehicle_imu_status), 2},
-		{ORB_ID(vehicle_imu_status), 3},
-	};
 
 	/* do not allow top copying this class */
 	MavlinkStreamVibration(MavlinkStreamVibration &) = delete;
@@ -2952,19 +2916,7 @@ protected:
 
 	bool send(const hrt_abstime t) override
 	{
-		bool updated = _sensor_selection_sub.updated();
+		if (_sensor_selection_sub.updated() || _vehicle_imu_status_subs.updated()) {
-
-		// check for vehicle_imu_status update
-		if (!updated) {
-			for (auto &sub : _vehicle_imu_status_sub) {
-				if (sub.updated()) {
-					updated = true;
-					break;
-				}
-			}
-		}
-
-		if (updated) {
 
 			mavlink_vibration_t msg{};
 			msg.time_usec = hrt_absolute_time();
@@ -2979,7 +2931,7 @@ protected:
 
 			// primary accel high frequency vibration metric
 			if (sensor_selection.accel_device_id != 0) {
-				for (auto &x : _vehicle_imu_status_sub) {
+				for (auto &x : _vehicle_imu_status_subs) {
 					vehicle_imu_status_s status;
 
 					if (x.copy(&status)) {
@@ -2994,10 +2946,10 @@ protected:
 			}
 
 			// accel 0, 1, 2 cumulative clipping
-			for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+			for (int i = 0; i < math::min(static_cast<uint8_t>(3), _vehicle_imu_status_subs.size()); i++) {
 				vehicle_imu_status_s status;
 
-				if (_vehicle_imu_status_sub[i].copy(&status)) {
+				if (_vehicle_imu_status_subs[i].copy(&status)) {
 
 					const uint32_t clipping = status.accel_clipping[0] + status.accel_clipping[1] + status.accel_clipping[2];
 

src/modules/sensors/voted_sensors_update.cpp

@@ -145,7 +145,7 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 
 			// copy corresponding vehicle_imu_status for accel & gyro error counts
 			vehicle_imu_status_s imu_status{};
-			_vehicle_imu_status_sub[uorb_index].copy(&imu_status);
+			_vehicle_imu_status_subs[uorb_index].copy(&imu_status);
 
 			_accel_device_id[uorb_index] = imu_report.accel_device_id;
 			_gyro_device_id[uorb_index] = imu_report.gyro_device_id;

src/modules/sensors/voted_sensors_update.h

@@ -48,6 +48,7 @@
 #include <matrix/math.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_gyro.h>
@@ -168,11 +169,7 @@ private:
 	uORB::PublicationQueued<subsystem_info_s> _info_pub{ORB_ID(subsystem_info)};	/* subsystem info publication */
 
 	uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[3];
-	uORB::Subscription _vehicle_imu_status_sub[ACCEL_COUNT_MAX] {
+	uORB::SubscriptionMultiArray<vehicle_imu_status_s, ACCEL_COUNT_MAX> _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status};
-		{ORB_ID(vehicle_imu_status), 0},
-		{ORB_ID(vehicle_imu_status), 1},
-		{ORB_ID(vehicle_imu_status), 2},
-	};
 
 	sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {};	/**< latest sensor data from all sensors instances */
 

src/modules/simulator/simulator.h

@@ -225,8 +225,9 @@ private:
 	uORB::Publication<input_rc_s>			_input_rc_pub{ORB_ID(input_rc)};
 
 	// HIL GPS
-	uORB::PublicationMulti<vehicle_gps_position_s>	*_vehicle_gps_position_pubs[ORB_MULTI_MAX_INSTANCES] {};
+	static constexpr int MAX_GPS = 3;
-	uint8_t _gps_ids[ORB_MULTI_MAX_INSTANCES] {};
+	uORB::PublicationMulti<vehicle_gps_position_s>	*_vehicle_gps_position_pubs[MAX_GPS] {};
+	uint8_t _gps_ids[MAX_GPS] {};
 	std::default_random_engine _gen{};
 
 	// uORB subscription handlers

src/modules/uORB/CMakeLists.txt

@@ -49,6 +49,7 @@ if(NOT PX4_BOARD MATCHES "px4_io") # TODO: fix this hack (move uORB to platform
 			Subscription.hpp
 			SubscriptionCallback.hpp
 			SubscriptionInterval.hpp
+			SubscriptionMultiArray.hpp
 			uORB.cpp
 			uORB.h
 			uORBCommon.hpp

src/modules/uORB/Subscription.hpp

@@ -112,7 +112,7 @@ public:
 
 	/**
 	 * Check if there is a new update.
-	 * */
+	 */
 	bool updated() { return advertised() && (_node->published_message_count() != _last_generation); }
 
 	/**

src/modules/uORB/SubscriptionInterval.hpp

@@ -86,6 +86,7 @@ public:
 	~SubscriptionInterval() = default;
 
 	bool subscribe() { return _subscription.subscribe(); }
+	void unsubscribe() { _subscription.unsubscribe(); }
 
 	bool advertised() { return _subscription.advertised(); }
 

src/modules/uORB/SubscriptionMultiArray.hpp

@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file SubscriptionMultiArray.hpp
+ *
+ */
+
+#pragma once
+
+#include <uORB/uORB.h>
+
+#include <px4_platform_common/defines.h>
+#include <lib/mathlib/mathlib.h>
+
+#include "SubscriptionInterval.hpp"
+
+namespace uORB
+{
+
+/**
+ * An array of uORB::Subscriptions of the same topic
+ */
+template<typename T, uint8_t SIZE = ORB_MULTI_MAX_INSTANCES>
+class SubscriptionMultiArray
+{
+public:
+	static_assert(SIZE <= ORB_MULTI_MAX_INSTANCES, "size must be <= uORB max instances");
+
+	static constexpr uint8_t size() { return SIZE; }
+
+	/**
+	 * Constructor
+	 *
+	 * @param id The uORB ORB_ID enum for the topic.
+	 */
+	explicit SubscriptionMultiArray(ORB_ID id)
+	{
+		for (uint8_t i = 0; i < SIZE; i++) {
+			_subscriptions[i] = SubscriptionInterval{id, i};
+			_subscriptions[i].subscribe();
+		}
+	}
+
+	~SubscriptionMultiArray() = default;
+
+	SubscriptionInterval &operator [](int i) { return _subscriptions[i]; }
+	const SubscriptionInterval &operator [](int i) const { return _subscriptions[i]; }
+
+	SubscriptionInterval *begin() { return &_subscriptions[0]; }
+	SubscriptionInterval *end() { return &_subscriptions[SIZE - 1]; }
+
+	const SubscriptionInterval *begin() const { return &_subscriptions[0]; }
+	const SubscriptionInterval *end() const { return &_subscriptions[SIZE - 1]; }
+
+	// true if any instance is advertised
+	bool advertised()
+	{
+		for (auto &s : _subscriptions) {
+			if (s.advertised()) {
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+	// return the number of instances currently advertised
+	uint8_t advertised_count()
+	{
+		uint8_t count = 0;
+
+		for (auto &s : _subscriptions) {
+			if (s.advertised()) {
+				count++;
+			}
+		}
+
+		return count;
+	}
+
+	// true if any instance is updated
+	bool updated()
+	{
+		for (auto &s : _subscriptions) {
+			if (s.updated()) {
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+private:
+	SubscriptionInterval _subscriptions[SIZE];
+};
+
+} // namespace uORB