introduce sensor_gyro_control message for vehicle_angular_velocity (#12145)

msg/CMakeLists.txt

@@ -105,6 +105,7 @@ set(msg_files
 	sensor_combined.msg
 	sensor_correction.msg
 	sensor_gyro.msg
+	sensor_gyro_control.msg
 	sensor_mag.msg
 	sensor_preflight.msg
 	sensor_selection.msg

msg/sensor_gyro_control.msg

@@ -0,0 +1,11 @@
+
+# WARNING: Not recommend for custom development.
+#  This topic is part of an incremental refactoring of the sensor pipeline and will likely disappear post PX4 release v1.10
+
+uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+
+uint64 timestamp	# time since system start (microseconds)
+
+uint64 timestamp_sample	# time the raw data was sampled (microseconds)
+
+float32[3] xyz		# filtered angular velocity in the board axis in rad/s

msg/tools/uorb_rtps_message_ids.yaml

@@ -258,6 +258,8 @@ rtps:
     id: 112
   - msg: vehicle_acceleration
     id: 113
+  - msg: sensor_gyro_control
+    id: 114
   # multi topics
   - msg: actuator_controls_0
     id: 120

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -40,12 +40,14 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r
 	CDev(nullptr),
 	ModuleParams(nullptr),
 	_sensor_gyro_pub{ORB_ID(sensor_gyro), priority},
+	_sensor_gyro_control_pub{ORB_ID(sensor_gyro_control), priority},
 	_rotation{rotation}
 {
 	_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
 
 	_sensor_gyro_pub.get().device_id = device_id;
 	_sensor_gyro_pub.get().scaling = 1.0f;
+	_sensor_gyro_control_pub.get().device_id = device_id;
 
 	// set software low pass filter for controllers
 	updateParams();
@@ -94,6 +96,7 @@ PX4Gyroscope::set_device_type(uint8_t devtype)
 
 	// copy back to report
 	_sensor_gyro_pub.get().device_id = device_id.devid;
+	_sensor_gyro_control_pub.get().device_id = device_id.devid;
 }
 
 void
@@ -120,6 +123,13 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
 	// Filtered values
 	const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)};
 
+	// publish control data (filtered gyro) immediately
+	sensor_gyro_control_s &control = _sensor_gyro_control_pub.get();
+	control.timestamp_sample = timestamp;
+	val_filtered.copyTo(control.xyz);
+	control.timestamp = hrt_absolute_time();
+	_sensor_gyro_control_pub.update();	// publish
+
 	// Integrated values
 	matrix::Vector3f integrated_value;
 	uint32_t integral_dt = 0;
@@ -141,7 +151,7 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
 		report.z_integral = integrated_value(2);
 
 		poll_notify(POLLIN);
-		_sensor_gyro_pub.update();
+		_sensor_gyro_pub.update();	// publish
 	}
 }
 
@@ -158,4 +168,5 @@ PX4Gyroscope::print_status()
 		 (double)_calibration_offset(2));
 
 	print_message(_sensor_gyro_pub.get());
+	print_message(_sensor_gyro_control_pub.get());
 }

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -43,6 +43,7 @@
 #include <uORB/uORB.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_gyro_control.h>
 
 class PX4Gyroscope : public cdev::CDev, public ModuleParams
 {
@@ -68,7 +69,8 @@ private:
 
 	void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
 
-	uORB::PublicationMultiData<sensor_gyro_s>	_sensor_gyro_pub;
+	uORB::PublicationMultiData<sensor_gyro_s>		_sensor_gyro_pub;
+	uORB::PublicationMultiData<sensor_gyro_control_s>	_sensor_gyro_control_pub;
 
 	math::LowPassFilter2pVector3f _filter{1000, 100};
 	Integrator _integrator{4000, true};

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -103,6 +103,17 @@ VehicleAngularVelocity::SensorBiasUpdate(bool force)
 bool
 VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
 {
+	if (_sensor_selection_sub.updated() || force) {
+		sensor_selection_s sensor_selection;
+
+		if (_sensor_selection_sub.copy(&sensor_selection)) {
+			if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
+				_selected_sensor_device_id = sensor_selection.gyro_device_id;
+				force = true;
+			}
+		}
+	}
+
 	// check if the selected sensor has updated
 	if (_sensor_correction_sub.updated() || force) {
 
@@ -131,12 +142,40 @@ VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
 		if ((_selected_sensor != corrections.selected_gyro_instance) || force) {
 			if (corrections.selected_gyro_instance < MAX_SENSOR_COUNT) {
 				// clear all registered callbacks
+				for (auto &sub : _sensor_control_sub) {
+					sub.unregister_callback();
+				}
+
 				for (auto &sub : _sensor_sub) {
 					sub.unregister_callback();
 				}
 
 				const int sensor_new = corrections.selected_gyro_instance;
 
+				// subscribe to sensor_gyro_control if available
+				//  currently not all drivers (eg df_*) provide sensor_gyro_control
+				for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+					sensor_gyro_control_s report{};
+					_sensor_control_sub[i].copy(&report);
+
+					if ((report.device_id != 0) && (report.device_id == _selected_sensor_device_id)) {
+						if (_sensor_control_sub[i].register_callback()) {
+							PX4_DEBUG("selected sensor (control) changed %d -> %d", _selected_sensor, i);
+							_selected_sensor_control = i;
+
+							_sensor_control_available = true;
+
+							// record selected sensor (sensor_gyro orb index)
+							_selected_sensor = sensor_new;
+
+							return true;
+						}
+					}
+				}
+
+				// otherwise fallback to using sensor_gyro (legacy that will be removed)
+				_sensor_control_available = false;
+
 				if (_sensor_sub[sensor_new].register_callback()) {
 					PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new);
 					_selected_sensor = sensor_new;
@@ -183,32 +222,62 @@ VehicleAngularVelocity::Run()
 	// update corrections first to set _selected_sensor
 	SensorCorrectionsUpdate();
 
-	sensor_gyro_s sensor_data;
+	if (_sensor_control_available) {
+		//  using sensor_gyro_control is preferred, but currently not all drivers (eg df_*) provide sensor_gyro_control
+		sensor_gyro_control_s sensor_data;
 
-	if (_sensor_sub[_selected_sensor].update(&sensor_data)) {
-		perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp));
+		if (_sensor_control_sub[_selected_sensor].update(&sensor_data)) {
+			perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp));
 
-		ParametersUpdate();
-		SensorBiasUpdate();
+			ParametersUpdate();
+			SensorBiasUpdate();
 
-		// get the sensor data and correct for thermal errors
-		const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
+			// get the sensor data and correct for thermal errors (apply offsets and scale)
+			Vector3f rates{(Vector3f{sensor_data.xyz} - _offset).emult(_scale)};
 
-		// apply offsets and scale
-		Vector3f rates{(val - _offset).emult(_scale)};
+			// rotate corrected measurements from sensor to body frame
+			rates = _board_rotation * rates;
 
-		// rotate corrected measurements from sensor to body frame
-		rates = _board_rotation * rates;
+			// correct for in-run bias errors
+			rates -= _bias;
 
-		// correct for in-run bias errors
-		rates -= _bias;
+			vehicle_angular_velocity_s angular_velocity;
+			angular_velocity.timestamp_sample = sensor_data.timestamp;
+			rates.copyTo(angular_velocity.xyz);
+			angular_velocity.timestamp = hrt_absolute_time();
 
-		vehicle_angular_velocity_s angular_velocity;
-		angular_velocity.timestamp_sample = sensor_data.timestamp;
-		rates.copyTo(angular_velocity.xyz);
-		angular_velocity.timestamp = hrt_absolute_time();
+			_vehicle_angular_velocity_pub.publish(angular_velocity);
+		}
 
-		_vehicle_angular_velocity_pub.publish(angular_velocity);
+	} else {
+		// otherwise fallback to using sensor_gyro (legacy that will be removed)
+		sensor_gyro_s sensor_data;
+
+		if (_sensor_sub[_selected_sensor].update(&sensor_data)) {
+			perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp));
+
+			ParametersUpdate();
+			SensorBiasUpdate();
+
+			// get the sensor data and correct for thermal errors
+			const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
+
+			// apply offsets and scale
+			Vector3f rates{(val - _offset).emult(_scale)};
+
+			// rotate corrected measurements from sensor to body frame
+			rates = _board_rotation * rates;
+
+			// correct for in-run bias errors
+			rates -= _bias;
+
+			vehicle_angular_velocity_s angular_velocity;
+			angular_velocity.timestamp_sample = sensor_data.timestamp;
+			rates.copyTo(angular_velocity.xyz);
+			angular_velocity.timestamp = hrt_absolute_time();
+
+			_vehicle_angular_velocity_pub.publish(angular_velocity);
+		}
 	}
 
 	perf_end(_cycle_perf);
@@ -219,6 +288,13 @@ VehicleAngularVelocity::PrintStatus()
 {
 	PX4_INFO("selected sensor: %d", _selected_sensor);
 
+	if (_selected_sensor_device_id != 0) {
+		PX4_INFO("using sensor_gyro_control: %d (%d)", _selected_sensor_device_id, _selected_sensor_control);
+
+	} else {
+		PX4_WARN("sensor_gyro_control unavailable for selected sensor: %d (%d)", _selected_sensor_device_id,  _selected_sensor);
+	}
+
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);
 	perf_print_counter(_sensor_latency_perf);

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -50,6 +50,7 @@
 #include <uORB/topics/sensor_selection.h>
 
 #include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_gyro_control.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 
 class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
@@ -89,12 +90,19 @@ private:
 	uORB::Subscription			_sensor_correction_sub{ORB_ID(sensor_correction)};	/**< sensor thermal correction subscription */
 
 	uORB::SubscriptionCallbackWorkItem	_sensor_selection_sub{this, ORB_ID(sensor_selection)};	/**< selected primary sensor subscription */
+
 	uORB::SubscriptionCallbackWorkItem	_sensor_sub[MAX_SENSOR_COUNT] {				/**< sensor data subscription */
 		{this, ORB_ID(sensor_gyro), 0},
 		{this, ORB_ID(sensor_gyro), 1},
 		{this, ORB_ID(sensor_gyro), 2}
 	};
 
+	uORB::SubscriptionCallbackWorkItem	_sensor_control_sub[MAX_SENSOR_COUNT] {			/**< sensor control data subscription */
+		{this, ORB_ID(sensor_gyro_control), 0},
+		{this, ORB_ID(sensor_gyro_control), 1},
+		{this, ORB_ID(sensor_gyro_control), 2}
+	};
+
 	matrix::Dcmf				_board_rotation;				/**< rotation matrix for the orientation that the board is mounted */
 
 	matrix::Vector3f			_offset;
@@ -105,6 +113,9 @@ private:
 	perf_counter_t				_interval_perf;
 	perf_counter_t				_sensor_latency_perf;
 
+	uint32_t				_selected_sensor_device_id{0};
 	uint8_t					_selected_sensor{0};
+	uint8_t					_selected_sensor_control{0};
+	bool					_sensor_control_available{false};
 
 };