introduce sensor_gyro_control message for vehicle_angular_velocity (#12145)

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};