sensors/vehicle_angular_velocity: use single uORB::Subscription, but change instance

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -77,10 +77,7 @@ bool VehicleAngularVelocity::Start()
 void VehicleAngularVelocity::Stop()
 {
 	// clear all registered callbacks
-	for (auto &sub : _sensor_sub) {
+	_sensor_sub.unregisterCallback();
-		sub.unregisterCallback();
-	}
-
 	_sensor_selection_sub.unregisterCallback();
 
 	Deinit();
@@ -110,11 +107,11 @@ void VehicleAngularVelocity::CheckFilters()
 					const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f,
 										(float)sensor_gyro_s::ORB_QUEUE_LENGTH);
 
-					_sensor_sub[_selected_sensor_sub_index].set_required_updates(samples);
+					_sensor_sub.set_required_updates(samples);
 					_required_sample_updates = samples;
 
 				} else {
-					_sensor_sub[_selected_sensor_sub_index].set_required_updates(1);
+					_sensor_sub.set_required_updates(1);
 					_required_sample_updates = 1;
 				}
 			}
@@ -181,17 +178,12 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
 		_sensor_selection_sub.copy(&sensor_selection);
 
 		if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
-			// clear all registered callbacks
+			for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
-			for (auto &sub : _sensor_sub) {
+				uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
-				sub.unregisterCallback();
-			}
 
-			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				if ((sensor_gyro_sub.get().device_id != 0) && (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id)) {
-				sensor_gyro_s report{};
-				_sensor_sub[i].copy(&report);
 
-				if ((report.device_id != 0) && (report.device_id == sensor_selection.gyro_device_id)) {
+					if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) {
-					if (_sensor_sub[i].registerCallback()) {
 						PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor_sub_index, i);
 
 						// record selected sensor (array index)
@@ -201,7 +193,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
 						// clear bias and corrections
 						_bias.zero();
 
-						_calibration.set_device_id(report.device_id);
+						_calibration.set_device_id(sensor_gyro_sub.get().device_id);
 
 						// reset sample interval accumulator on sensor change
 						_timestamp_sample_last = 0;
@@ -247,17 +239,11 @@ void VehicleAngularVelocity::Run()
 	SensorBiasUpdate(selection_updated);
 	ParametersUpdate();
 
-	bool sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();
-
 	// process all outstanding messages
-	while (sensor_updated || selection_updated) {
-		selection_updated = false;
-
 	sensor_gyro_s sensor_data;
 
-		if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
+	while (_sensor_sub.update(&sensor_data)) {
 
-			if (sensor_updated) {
 		// collect sample interval average for filters
 		if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) {
 			_interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last);
@@ -269,7 +255,6 @@ void VehicleAngularVelocity::Run()
 		}
 
 		_timestamp_sample_last = sensor_data.timestamp_sample;
-			}
 
 		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
 		const float dt = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_prev) / 1e6f), 0.0002f, 0.02f);
@@ -298,9 +283,7 @@ void VehicleAngularVelocity::Run()
 		CheckFilters();
 
 		// publish once all new samples are processed
-			sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();
+		if (!_sensor_sub.updated()) {
-
-			if (!sensor_updated) {
 			bool publish = true;
 
 			if (_param_imu_gyro_rate_max.get() > 0) {
@@ -332,7 +315,6 @@ void VehicleAngularVelocity::Run()
 		}
 	}
 }
-}
 
 void VehicleAngularVelocity::PrintStatus()
 {

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -84,11 +84,7 @@ private:
 	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
-	uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
+	uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_gyro)};
-		{this, ORB_ID(sensor_gyro), 0},
-		{this, ORB_ID(sensor_gyro), 1},
-		{this, ORB_ID(sensor_gyro), 2}
-	};
 
 	calibration::Gyroscope _calibration{};