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) {
-		sub.unregisterCallback();
-	}
-
+	_sensor_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 (auto &sub : _sensor_sub) {
-				sub.unregisterCallback();
-			}
+			for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
+				uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
 
-			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-				sensor_gyro_s report{};
-				_sensor_sub[i].copy(&report);
+				if ((sensor_gyro_sub.get().device_id != 0) && (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id)) {
 
-				if ((report.device_id != 0) && (report.device_id == sensor_selection.gyro_device_id)) {
-					if (_sensor_sub[i].registerCallback()) {
+					if (_sensor_sub.ChangeInstance(i) && _sensor_sub.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,88 +239,78 @@ 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;
 
-		sensor_gyro_s sensor_data;
+	while (_sensor_sub.update(&sensor_data)) {
 
-		if (_sensor_sub[_selected_sensor_sub_index].copy(&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);
-					_interval_count++;
-
-				} else {
-					_interval_sum = 0.f;
-					_interval_count = 0.f;
-				}
+		// 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);
+			_interval_count++;
 
-				_timestamp_sample_last = sensor_data.timestamp_sample;
-			}
+		} else {
+			_interval_sum = 0.f;
+			_interval_count = 0.f;
+		}
 
-			// 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);
-			_timestamp_sample_prev = sensor_data.timestamp_sample;
+		_timestamp_sample_last = sensor_data.timestamp_sample;
 
-			// get the sensor data and correct for thermal errors (apply offsets and scale)
-			const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
+		// 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);
+		_timestamp_sample_prev = sensor_data.timestamp_sample;
 
-			// correct for in-run bias errors
-			const Vector3f angular_velocity_raw = _calibration.Correct(val) - _bias;
+		// get the sensor data and correct for thermal errors (apply offsets and scale)
+		const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
 
-			// Gyro filtering:
-			// - Apply general notch filter (IMU_GYRO_NF_FREQ)
-			// - Apply general low-pass filter (IMU_GYRO_CUTOFF)
-			// - Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF)
+		// correct for in-run bias errors
+		const Vector3f angular_velocity_raw = _calibration.Correct(val) - _bias;
 
-			const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)};
+		// Gyro filtering:
+		// - Apply general notch filter (IMU_GYRO_NF_FREQ)
+		// - Apply general low-pass filter (IMU_GYRO_CUTOFF)
+		// - Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF)
 
-			const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)};
+		const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)};
 
-			const Vector3f angular_acceleration_raw = (angular_velocity - _angular_velocity_prev) / dt;
-			_angular_velocity_prev = angular_velocity;
-			_angular_acceleration_prev = angular_acceleration_raw;
-			const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)};
+		const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)};
 
-			CheckFilters();
+		const Vector3f angular_acceleration_raw = (angular_velocity - _angular_velocity_prev) / dt;
+		_angular_velocity_prev = angular_velocity;
+		_angular_acceleration_prev = angular_acceleration_raw;
+		const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)};
 
-			// publish once all new samples are processed
-			sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();
+		CheckFilters();
 
-			if (!sensor_updated) {
-				bool publish = true;
+		// publish once all new samples are processed
+		if (!_sensor_sub.updated()) {
+			bool publish = true;
 
-				if (_param_imu_gyro_rate_max.get() > 0) {
-					const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
+			if (_param_imu_gyro_rate_max.get() > 0) {
+				const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
 
-					if (hrt_elapsed_time(&_last_publish) < interval) {
-						publish = false;
-					}
+				if (hrt_elapsed_time(&_last_publish) < interval) {
+					publish = false;
 				}
+			}
 
-				if (publish) {
-					// Publish vehicle_angular_acceleration
-					vehicle_angular_acceleration_s v_angular_acceleration;
-					v_angular_acceleration.timestamp_sample = sensor_data.timestamp_sample;
-					angular_acceleration.copyTo(v_angular_acceleration.xyz);
-					v_angular_acceleration.timestamp = hrt_absolute_time();
-					_vehicle_angular_acceleration_pub.publish(v_angular_acceleration);
-
-					// Publish vehicle_angular_velocity
-					vehicle_angular_velocity_s v_angular_velocity;
-					v_angular_velocity.timestamp_sample = sensor_data.timestamp_sample;
-					angular_velocity.copyTo(v_angular_velocity.xyz);
-					v_angular_velocity.timestamp = hrt_absolute_time();
-					_vehicle_angular_velocity_pub.publish(v_angular_velocity);
-
-					_last_publish = v_angular_velocity.timestamp_sample;
-					return;
-				}
+			if (publish) {
+				// Publish vehicle_angular_acceleration
+				vehicle_angular_acceleration_s v_angular_acceleration;
+				v_angular_acceleration.timestamp_sample = sensor_data.timestamp_sample;
+				angular_acceleration.copyTo(v_angular_acceleration.xyz);
+				v_angular_acceleration.timestamp = hrt_absolute_time();
+				_vehicle_angular_acceleration_pub.publish(v_angular_acceleration);
+
+				// Publish vehicle_angular_velocity
+				vehicle_angular_velocity_s v_angular_velocity;
+				v_angular_velocity.timestamp_sample = sensor_data.timestamp_sample;
+				angular_velocity.copyTo(v_angular_velocity.xyz);
+				v_angular_velocity.timestamp = hrt_absolute_time();
+				_vehicle_angular_velocity_pub.publish(v_angular_velocity);
+
+				_last_publish = v_angular_velocity.timestamp_sample;
+				return;
 			}
 		}
 	}

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] {
-		{this, ORB_ID(sensor_gyro), 0},
-		{this, ORB_ID(sensor_gyro), 1},
-		{this, ORB_ID(sensor_gyro), 2}
-	};
+	uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_gyro)};
 
 	calibration::Gyroscope _calibration{};