[DO NOT MERGE] introduce sensor_gyro_control message and update mc_at…

…t_control

msg/CMakeLists.txt

@@ -104,6 +104,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,9 @@
+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 x		# filtered angular velocity in the NED X board axis in rad/s
+float32 y		# filtered angular velocity in the NED Y board axis in rad/s
+float32 z		# filtered angular velocity in the NED Z board axis in rad/s

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -40,12 +40,13 @@ 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();
@@ -96,6 +97,7 @@ void 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 PX4Gyroscope::set_sample_rate(unsigned rate)
@@ -121,6 +123,15 @@ void PX4Gyroscope::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t 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;
+	control.x = val_filtered(0);
+	control.y = val_filtered(1);
+	control.z = val_filtered(2);
+	control.timestamp = hrt_absolute_time();
+	_sensor_gyro_control_pub.update();	// publish
+
 	// Integrated values
 	matrix::Vector3f integrated_value;
 	uint32_t integral_dt = 0;
@@ -142,7 +153,7 @@ void PX4Gyroscope::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z
 		report.z_integral = integrated_value(2);
 
 		poll_notify(POLLIN);
-		_sensor_gyro_pub.update();
+		_sensor_gyro_pub.update();	// publish
 	}
 }
 

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

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

src/modules/mc_att_control/mc_att_control.hpp

@@ -51,7 +51,7 @@
 #include <uORB/topics/rate_ctrl_status.h>
 #include <uORB/topics/sensor_bias.h>
 #include <uORB/topics/sensor_correction.h>
-#include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_gyro_control.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -155,7 +155,7 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};	/**< vehicle land detected subscription */
 	uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)};
 
-	uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub{this, ORB_ID(sensor_gyro)};	/**< gyro data subscription */
+	uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub{this, ORB_ID(sensor_gyro_control)};	/**< gyro data subscription */
 
 	unsigned _gyro_count{1};
 	int _selected_gyro{0};
@@ -179,7 +179,8 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 	struct actuator_controls_s		_actuators {};		/**< actuator controls */
 	struct vehicle_status_s			_vehicle_status {};	/**< vehicle status */
 	struct battery_status_s			_battery_status {};	/**< battery status */
-	struct sensor_gyro_s			_sensor_gyro {};	/**< gyro data before thermal correctons and ekf bias estimates are applied */
+	struct sensor_gyro_control_s
+		_sensor_gyro {};	/**< gyro data before thermal correctons and ekf bias estimates are applied */
 	struct sensor_correction_s		_sensor_correction {};	/**< sensor thermal corrections */
 	struct sensor_bias_s			_sensor_bias {};	/**< sensor in-run bias corrections */
 	struct vehicle_land_detected_s		_vehicle_land_detected {};

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -124,7 +124,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 
 	parameters_updated();
 
-	_gyro_count = math::constrain(orb_group_count(ORB_ID(sensor_gyro)), 1, MAX_GYRO_COUNT);
+	_gyro_count = math::constrain(orb_group_count(ORB_ID(sensor_gyro_control)), 1, MAX_GYRO_COUNT);
 }
 
 void
@@ -558,7 +558,7 @@ MulticopterAttitudeControl::publish_actuator_controls()
 	_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
 	_actuators.control[7] = (float)_landing_gear.landing_gear;
 	_actuators.timestamp = hrt_absolute_time();
-	_actuators.timestamp_sample = _sensor_gyro.timestamp;
+	_actuators.timestamp_sample = _sensor_gyro.timestamp_sample;
 
 	/* scale effort by battery status */
 	if (_param_mc_bat_scale_en.get() && _battery_status.scale > 0.0f) {
@@ -581,7 +581,7 @@ MulticopterAttitudeControl::Run()
 	/* update the latest gyro selection */
 	if (_sensor_correction.selected_gyro_instance < _gyro_count) {
 		if (_selected_gyro != _sensor_correction.selected_gyro_instance) {
-			if (_sensor_gyro_sub.change_topic(ORB_ID(sensor_gyro), _selected_gyro)) {
+			if (_sensor_gyro_sub.change_topic(ORB_ID(sensor_gyro_control), _selected_gyro)) {
 				_selected_gyro = _sensor_correction.selected_gyro_instance;
 				PX4_WARN("selected gyro changed %d -> %d", _selected_gyro, _sensor_correction.selected_gyro_instance);
 			}