sensors: create vehicle_angular_velocity module (#12596)

 * split out filtered sensor_gyro aggregation from mc_att_control and move to wq:rate_ctrl

msg/CMakeLists.txt

@@ -123,6 +123,7 @@ set(msg_files
 	ulog_stream.msg
 	ulog_stream_ack.msg
 	vehicle_air_data.msg
+	vehicle_angular_velocity.msg
 	vehicle_attitude.msg
 	vehicle_attitude_setpoint.msg
 	vehicle_command.msg

msg/rate_ctrl_status.msg

@@ -1,10 +1,5 @@
 uint64 timestamp		# time since system start (microseconds)
 
-# rates used by the controller
-float32 rollspeed		# Bias corrected angular velocity about X body axis in rad/s
-float32 pitchspeed		# Bias corrected angular velocity about Y body axis in rad/s
-float32 yawspeed		# Bias corrected angular velocity about Z body axis in rad/s
-
 # rate controller integrator status
 float32 rollspeed_integ
 float32 pitchspeed_integ

msg/tools/uorb_rtps_message_ids.yaml

@@ -198,6 +198,8 @@ rtps:
     id: 85
   - msg: vehicle_air_data
     id: 86
+  - msg: vehicle_angular_velocity
+    id: 134
   - msg: vehicle_attitude
     id: 87
     send: true
@@ -279,6 +281,9 @@ rtps:
   - msg: fw_virtual_attitude_setpoint
     id: 127
     alias: vehicle_attitude_setpoint
+  - msg: vehicle_angular_velocity_groundtruth
+    id: 135
+    alias: vehicle_angular_velocity
   - msg: vehicle_attitude_groundtruth
     id: 128
     alias: vehicle_attitude

msg/vehicle_angular_velocity.msg

@@ -0,0 +1,8 @@
+
+uint64 timestamp	# time since system start (microseconds)
+
+uint64 timestamp_sample	# the timestamp of the raw data (microseconds)
+
+float32[3] xyz		# Bias corrected angular velocity about X, Y, Z body axis in rad/s
+
+# TOPICS vehicle_angular_velocity vehicle_angular_velocity_groundtruth

msg/vehicle_attitude.msg

@@ -2,10 +2,6 @@
 
 uint64 timestamp		# time since system start (microseconds)
 
-float32 rollspeed		# Bias corrected angular velocity about X body axis in rad/s
-float32 pitchspeed		# Bias corrected angular velocity about Y body axis in rad/s
-float32 yawspeed		# Bias corrected angular velocity about Z body axis in rad/s
-
 float32[4] q			# Quaternion rotation from NED earth frame to XYZ body frame
 float32[4] delta_q_reset 	# Amount by which quaternion has changed during last reset
 uint8 quat_reset_counter	# Quaternion reset counter

src/examples/segway/BlockSegwayController.cpp

@@ -42,7 +42,7 @@ void BlockSegwayController::update()
 	Eulerf euler = Eulerf(Quatf(_att.get().q));
 
 	// compute speed command
-	float spdCmd = -th2v.update(euler.theta()) - q2v.update(_att.get().pitchspeed);
+	float spdCmd = -th2v.update(euler.theta()) - q2v.update(_angular_velocity.get().xyz[1]);
 
 	// handle autopilot modes
 	if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||

src/examples/segway/blocks.cpp

@@ -89,6 +89,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c
 	_param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz
 	_missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()),
 	_att(ORB_ID(vehicle_attitude), 20, 0, &getSubscriptions()),
+	_angular_velocity(ORB_ID(vehicle_angular_velocity), 20, 0, &getSubscriptions()),
 	_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, 0, &getSubscriptions()),
 	_pos(ORB_ID(vehicle_global_position), 20, 0, &getSubscriptions()),
 	_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()),

src/examples/segway/blocks.hpp

@@ -52,6 +52,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -96,6 +97,7 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
 	uORB::SubscriptionPollable<parameter_update_s> _param_update;
 	uORB::SubscriptionPollable<position_setpoint_triplet_s> _missionCmd;
 	uORB::SubscriptionPollable<vehicle_attitude_s> _att;
+	uORB::SubscriptionPollable<vehicle_angular_velocity_s> _angular_velocity;
 	uORB::SubscriptionPollable<vehicle_attitude_setpoint_s> _attCmd;
 	uORB::SubscriptionPollable<vehicle_global_position_s> _pos;
 	uORB::SubscriptionPollable<vehicle_rates_setpoint_s> _ratesCmd;

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -425,9 +425,6 @@ void AttitudeEstimatorQ::task_main()
 		if (update(dt)) {
 			vehicle_attitude_s att = {};
 			att.timestamp = sensors.timestamp;
-			att.rollspeed = _rates(0);
-			att.pitchspeed = _rates(1);
-			att.yawspeed = _rates(2);
 			_q.copyTo(att.q);
 
 			/* the instance count is not used here */

src/modules/ekf2/ekf2_main.cpp

@@ -1754,13 +1754,6 @@ bool Ekf2::publish_attitude(const sensor_combined_s &sensors, const hrt_abstime
 
 		_ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);
 
-		// In-run bias estimates
-		float gyro_bias[3];
-		_ekf.get_gyro_bias(gyro_bias);
-		att.rollspeed = sensors.gyro_rad[0] - gyro_bias[0];
-		att.pitchspeed = sensors.gyro_rad[1] - gyro_bias[1];
-		att.yawspeed = sensors.gyro_rad[2] - gyro_bias[2];
-
 		_att_pub.publish(att);
 
 		return true;

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -504,6 +504,12 @@ void FixedwingAttitudeControl::run()
 			/* get current rotation matrix and euler angles from control state quaternions */
 			matrix::Dcmf R = matrix::Quatf(_att.q);
 
+			vehicle_angular_velocity_s angular_velocity{};
+			_vehicle_rates_sub.copy(&angular_velocity);
+			float rollspeed = angular_velocity.xyz[0];
+			float pitchspeed = angular_velocity.xyz[1];
+			float yawspeed = angular_velocity.xyz[2];
+
 			if (_is_tailsitter) {
 				/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
 				 *
@@ -542,9 +548,9 @@ void FixedwingAttitudeControl::run()
 				R = R_adapted;
 
 				/* lastly, roll- and yawspeed have to be swaped */
-				float helper = _att.rollspeed;
-				_att.rollspeed = -_att.yawspeed;
-				_att.yawspeed = helper;
+				float helper = rollspeed;
+				rollspeed = -yawspeed;
+				yawspeed = helper;
 			}
 
 			const matrix::Eulerf euler_angles(R);
@@ -624,9 +630,9 @@ void FixedwingAttitudeControl::run()
 				control_input.roll = euler_angles.phi();
 				control_input.pitch = euler_angles.theta();
 				control_input.yaw = euler_angles.psi();
-				control_input.body_x_rate = _att.rollspeed;
-				control_input.body_y_rate = _att.pitchspeed;
-				control_input.body_z_rate = _att.yawspeed;
+				control_input.body_x_rate = rollspeed;
+				control_input.body_y_rate = pitchspeed;
+				control_input.body_z_rate = yawspeed;
 				control_input.roll_setpoint = _att_sp.roll_body;
 				control_input.pitch_setpoint = _att_sp.pitch_body;
 				control_input.yaw_setpoint = _att_sp.yaw_body;
@@ -801,9 +807,6 @@ void FixedwingAttitudeControl::run()
 
 				rate_ctrl_status_s rate_ctrl_status;
 				rate_ctrl_status.timestamp = hrt_absolute_time();
-				rate_ctrl_status.rollspeed = _att.rollspeed;
-				rate_ctrl_status.pitchspeed = _att.pitchspeed;
-				rate_ctrl_status.yawspeed = _att.yawspeed;
 				rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator();
 				rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator();
 				rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator();

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -53,6 +53,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/rate_ctrl_status.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -103,6 +104,7 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
 	uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle status subscription */
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};	/**< vehicle land detected subscription */
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};			/**< vehicle status subscription */
+	uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
 
 	uORB::SubscriptionData<airspeed_s> _airspeed_sub{ORB_ID(airspeed)};
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1016,7 +1016,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 		    fabsf(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) {
 
 			/* heading / roll is zero, lock onto current heading */
-			if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
+			if (fabsf(_vehicle_rates_sub.get().xyz[2]) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
 				// little yaw movement, lock to current heading
 				_yaw_lock_engaged = true;
 
@@ -1759,6 +1759,7 @@ FixedwingPositionControl::run()
 			vehicle_control_mode_poll();
 			_vehicle_land_detected_sub.update(&_vehicle_land_detected);
 			vehicle_status_poll();
+			_vehicle_rates_sub.update();
 
 			Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon);
 			Vector2f ground_speed(_global_pos.vel_n, _global_pos.vel_e);

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -76,6 +76,7 @@
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_bias.h>
 #include <uORB/topics/tecs_status.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
@@ -163,6 +164,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};		///< vehicle command subscription */
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};	///< vehicle land detected subscription */
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};			///< vehicle status subscription */
+	uORB::SubscriptionData<vehicle_angular_velocity_s>	_vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
 
 	orb_advert_t	_attitude_sp_pub{nullptr};		///< attitude setpoint */
 	orb_advert_t	_pos_ctrl_status_pub{nullptr};		///< navigation capabilities publication */

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -95,6 +95,7 @@ void MulticopterLandDetector::_update_topics()
 	_actuator_controls_sub.update(&_actuator_controls);
 	_battery_sub.update(&_battery_status);
 	_sensor_bias_sub.update(&_sensor_bias);
+	_vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity);
 	_vehicle_attitude_sub.update(&_vehicle_attitude);
 	_vehicle_control_mode_sub.update(&_control_mode);
 	_vehicle_local_position_sub.update(&_vehicle_local_position);
@@ -215,9 +216,9 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 	// Next look if all rotation angles are not moving.
 	float maxRotationScaled = _params.maxRotation_rad_s * landThresholdFactor;
 
-	bool rotating = (fabsf(_vehicle_attitude.rollspeed)  > maxRotationScaled) ||
-			(fabsf(_vehicle_attitude.pitchspeed) > maxRotationScaled) ||
-			(fabsf(_vehicle_attitude.yawspeed) > maxRotationScaled);
+	bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0])  > maxRotationScaled) ||
+			(fabsf(_vehicle_angular_velocity.xyz[1]) > maxRotationScaled) ||
+			(fabsf(_vehicle_angular_velocity.xyz[2]) > maxRotationScaled);
 
 	// Return status based on armed state and throttle if no position lock is available.
 	if (!_has_altitude_lock() && !rotating) {

src/modules/land_detector/MulticopterLandDetector.h

@@ -53,6 +53,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_bias.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -126,6 +127,7 @@ class MulticopterLandDetector : public LandDetector
 	uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
 	uORB::Subscription _battery_sub{ORB_ID(battery_status)};
 	uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
+	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
@@ -136,6 +138,7 @@ class MulticopterLandDetector : public LandDetector
 	vehicle_control_mode_s            _control_mode {};
 	sensor_bias_s                     _sensor_bias {};
 	vehicle_attitude_s                _vehicle_attitude {};
+	vehicle_angular_velocity_s        _vehicle_angular_velocity{};
 	vehicle_local_position_s          _vehicle_local_position {};
 	vehicle_local_position_setpoint_s _vehicle_local_position_setpoint {};
 

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -25,6 +25,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
 	_sub_land(ORB_ID(vehicle_land_detected), 1000 / 2, 0, &getSubscriptions()),
 	_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
+	_sub_angular_velocity(ORB_ID(vehicle_angular_velocity), 1000 / 100, 0, &getSubscriptions()),
 	// set flow max update rate higher than expected to we don't lose packets
 	_sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()),
 	// main prediction loop, 100 hz
@@ -663,9 +664,9 @@ void BlockLocalPositionEstimator::publishOdom()
 		_pub_odom.get().vz = xLP(X_vz);		// vel down
 
 		// angular velocity
-		_pub_odom.get().rollspeed = _sub_att.get().rollspeed;	// roll rate
-		_pub_odom.get().pitchspeed = _sub_att.get().pitchspeed;	// pitch rate
-		_pub_odom.get().yawspeed = _sub_att.get().yawspeed;	// yaw rate
+		_pub_odom.get().rollspeed = _sub_angular_velocity.get().xyz[0]; // roll rate
+		_pub_odom.get().pitchspeed = _sub_angular_velocity.get().xyz[1]; // pitch rate
+		_pub_odom.get().yawspeed = _sub_angular_velocity.get().xyz[2]; // yaw rate
 
 		// get the covariance matrix size
 		const size_t POS_URT_SIZE = sizeof(_pub_odom.get().pose_covariance) / sizeof(_pub_odom.get().pose_covariance[0]);

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -12,6 +12,7 @@
 #include <uORB/SubscriptionPollable.hpp>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -249,6 +250,7 @@ class BlockLocalPositionEstimator : public control::SuperBlock, public ModulePar
 	uORB::SubscriptionPollable<actuator_armed_s> _sub_armed;
 	uORB::SubscriptionPollable<vehicle_land_detected_s> _sub_land;
 	uORB::SubscriptionPollable<vehicle_attitude_s> _sub_att;
+	uORB::SubscriptionPollable<vehicle_angular_velocity_s> _sub_angular_velocity;
 	uORB::SubscriptionPollable<optical_flow_s> _sub_flow;
 	uORB::SubscriptionPollable<sensor_combined_s> _sub_sensor;
 	uORB::SubscriptionPollable<parameter_update_s> _sub_param_update;

src/modules/local_position_estimator/sensors/flow.cpp

@@ -152,9 +152,10 @@ void BlockLocalPositionEstimator::flowCorrect()
 	// compute polynomial value
 	float flow_vxy_stddev = p[0] * h + p[1] * h * h + p[2] * v + p[3] * v * h + p[4] * v * h * h;
 
-	float rotrate_sq = _sub_att.get().rollspeed * _sub_att.get().rollspeed
-			   + _sub_att.get().pitchspeed * _sub_att.get().pitchspeed
-			   + _sub_att.get().yawspeed * _sub_att.get().yawspeed;
+	const Vector3f rates{_sub_angular_velocity.get().xyz};
+	float rotrate_sq = rates(0) * rates(0)
+			   + rates(1) * rates(1)
+			   + rates(2) * rates(2);
 
 	matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
 	float rot_sq = euler.phi() * euler.phi() + euler.theta() * euler.theta();

src/modules/logger/logger.cpp

@@ -594,15 +594,16 @@ void Logger::add_default_topics()
 	add_topic("optical_flow", 50);
 	add_topic("position_setpoint_triplet", 200);
 	//add_topic("radio_status");
-	add_topic("rate_ctrl_status", 30);
+	add_topic("rate_ctrl_status", 200);
 	add_topic("sensor_combined", 100);
 	add_topic("sensor_preflight", 200);
 	add_topic("system_power", 500);
 	add_topic("tecs_status", 200);
-	add_topic("trajectory_setpoint", 200);
 	add_topic("telemetry_status");
+	add_topic("trajectory_setpoint", 200);
 	add_topic("vehicle_air_data", 200);
-	add_topic("vehicle_attitude", 30);
+	add_topic("vehicle_angular_velocity", 20);
+	add_topic("vehicle_attitude", 50);
 	add_topic("vehicle_attitude_setpoint", 100);
 	add_topic("vehicle_command");
 	add_topic("vehicle_global_position", 200);
@@ -611,7 +612,7 @@ void Logger::add_default_topics()
 	add_topic("vehicle_local_position", 100);
 	add_topic("vehicle_local_position_setpoint", 100);
 	add_topic("vehicle_magnetometer", 200);
-	add_topic("vehicle_rates_setpoint", 30);
+	add_topic("vehicle_rates_setpoint", 20);
 	add_topic("vehicle_status", 200);
 	add_topic("vehicle_status_flags");
 	add_topic("vtol_vehicle_status", 200);
@@ -623,9 +624,10 @@ void Logger::add_default_topics()
 	add_topic("fw_virtual_attitude_setpoint");
 	add_topic("mc_virtual_attitude_setpoint");
 	add_topic("multirotor_motor_limits");
-	add_topic("position_controller_status");
 	add_topic("offboard_control_mode");
+	add_topic("position_controller_status");
 	add_topic("time_offset");
+	add_topic("vehicle_angular_velocity", 10);
 	add_topic("vehicle_attitude_groundtruth", 10);
 	add_topic("vehicle_global_position_groundtruth", 100);
 	add_topic("vehicle_local_position_groundtruth", 100);
@@ -641,17 +643,18 @@ void Logger::add_high_rate_topics()
 	add_topic("manual_control_setpoint");
 	add_topic("rate_ctrl_status");
 	add_topic("sensor_combined");
+	add_topic("vehicle_angular_velocity");
 	add_topic("vehicle_attitude");
 	add_topic("vehicle_attitude_setpoint");
 	add_topic("vehicle_rates_setpoint");
 }
 
 void Logger::add_debug_topics()
 {
+	add_topic("debug_array");
 	add_topic("debug_key_value");
 	add_topic("debug_value");
 	add_topic("debug_vect");
-	add_topic("debug_array");
 }
 
 void Logger::add_estimator_replay_topics()