sensors: compute and publish vehicle_angular_acceleration

- introduces parameter IMU_DGYRO_CUTOFF to configure the angular acceleration low pass filter
 - the angular acceleration is computed by differentiating angular velocity after the notch filter (IMU_GYRO_NF_FREQ & IMU_GYRO_NF_BW) is applied

Co-authored-by: Julien Lecoeur <jlecoeur@users.noreply.github.com>

msg/CMakeLists.txt

@@ -135,6 +135,7 @@ set(msg_files
 	ulog_stream_ack.msg
 	vehicle_acceleration.msg
 	vehicle_air_data.msg
+	vehicle_angular_acceleration.msg
 	vehicle_angular_velocity.msg
 	vehicle_attitude.msg
 	vehicle_attitude_setpoint.msg

msg/tools/uorb_rtps_message_ids.yaml

@@ -277,6 +277,8 @@ rtps:
     id: 123
   - msg: vehicle_imu
     id: 124
+  - msg: vehicle_angular_acceleration
+    id: 125
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

msg/vehicle_angular_acceleration.msg

@@ -0,0 +1,5 @@
+
+uint64 timestamp        # time since system start (microseconds)
+uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
+
+float32[3] xyz          # angular acceleration about X, Y, Z body axis in rad/s^2,

msg/vehicle_angular_velocity.msg

@@ -1,7 +1,6 @@
 
 uint64 timestamp        # time since system start (microseconds)
-
+uint64 timestamp_sample # timestamp of the data sample on which this message is based (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
 

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -43,8 +43,10 @@ VehicleAngularVelocity::VehicleAngularVelocity() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
-	_lowpass_filter.set_cutoff_frequency(kInitialRateHz, _param_imu_gyro_cutoff.get());
+	_lp_filter_velocity.set_cutoff_frequency(kInitialRateHz, _param_imu_gyro_cutoff.get());
-	_notch_filter.setParameters(kInitialRateHz, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
+	_notch_filter_velocity.setParameters(kInitialRateHz, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
+
+	_lp_filter_acceleration.set_cutoff_frequency(kInitialRateHz, _param_imu_dgyro_cutoff.get());
 }
 
 VehicleAngularVelocity::~VehicleAngularVelocity()
@@ -81,7 +83,7 @@ void VehicleAngularVelocity::Stop()
 	_sensor_selection_sub.unregisterCallback();
 }
 
-void VehicleAngularVelocity::CheckFilters(const Vector3f &rates)
+void VehicleAngularVelocity::CheckFilters()
 {
 	if ((hrt_elapsed_time(&_filter_check_last) > 100_ms)) {
 		_filter_check_last = hrt_absolute_time();
@@ -104,20 +106,27 @@ void VehicleAngularVelocity::CheckFilters(const Vector3f &rates)
 		}
 
 		const bool sample_rate_updated = (_sample_rate_incorrect_count > 50);
-		const bool lowpass_updated = (fabsf(_lowpass_filter.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f);
-		const bool notch_updated = ((fabsf(_notch_filter.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f)
-					    || (fabsf(_notch_filter.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f));
 
-		if (sample_rate_updated || lowpass_updated || notch_updated) {
+		const bool lp_velocity_updated = (fabsf(_lp_filter_velocity.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f);
+		const bool notch_updated = ((fabsf(_notch_filter_velocity.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f)
+					    || (fabsf(_notch_filter_velocity.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f));
+
+		const bool lp_acceleration_updated = (fabsf(_lp_filter_acceleration.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) >
+						      0.01f);
+
+		if (sample_rate_updated || lp_velocity_updated || notch_updated || lp_acceleration_updated) {
 			PX4_INFO("updating filter, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
 			_filter_sample_rate = _update_rate_hz;
 
 			// update software low pass filters
-			_lowpass_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_gyro_cutoff.get());
+			_lp_filter_velocity.set_cutoff_frequency(_filter_sample_rate, _param_imu_gyro_cutoff.get());
-			_lowpass_filter.reset(rates);
+			_lp_filter_velocity.reset(_angular_velocity_prev);
 
-			_notch_filter.setParameters(_filter_sample_rate, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
+			_notch_filter_velocity.setParameters(_filter_sample_rate, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
-			_notch_filter.reset(rates);
+			_notch_filter_velocity.reset(_angular_velocity_prev);
+
+			_lp_filter_acceleration.set_cutoff_frequency(_filter_sample_rate, _param_imu_dgyro_cutoff.get());
+			_lp_filter_acceleration.reset(_angular_acceleration_prev);
 
 			// reset state
 			_sample_rate_incorrect_count = 0;
@@ -273,22 +282,35 @@ void VehicleAngularVelocity::Run()
 				perf_count_interval(_interval_perf, sensor_data.timestamp_sample);
 			}
 
+			// 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;
+
+
 			// 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};
 
 			// apply offsets and scale
-			Vector3f rates{(val - _offset).emult(_scale)};
+			Vector3f angular_velocity_raw{(val - _offset).emult(_scale)};
 
 			// rotate corrected measurements from sensor to body frame
-			rates = _board_rotation * rates;
+			angular_velocity_raw = _board_rotation * angular_velocity_raw;
 
 			// correct for in-run bias errors
-			rates -= _bias;
+			angular_velocity_raw -= _bias;
 
-			// Filter: apply notch and then low-pass
+			// Differentiate angular velocity (after notch filter)
-			CheckFilters(rates);
+			const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)};
-			const Vector3f rates_filtered = _lowpass_filter.apply(_notch_filter.apply(rates));
+			const Vector3f angular_acceleration_raw = (angular_velocity_notched - _angular_velocity_prev) / dt;
 
+			_angular_velocity_prev = angular_velocity_notched;
+			_angular_acceleration_prev = angular_acceleration_raw;
+
+			CheckFilters();
+
+			// Filter: apply low-pass
+			const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)};
+			const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)};
 
 			bool publish = true;
 
@@ -301,15 +323,21 @@ void VehicleAngularVelocity::Run()
 			}
 
 			if (publish) {
-				vehicle_angular_velocity_s out;
+				// 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);
 
-				out.timestamp_sample = sensor_data.timestamp_sample;
+				// Publish vehicle_angular_velocity
-				rates_filtered.copyTo(out.xyz);
+				vehicle_angular_velocity_s v_angular_velocity;
-				out.timestamp = hrt_absolute_time();
+				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);
 
-				_vehicle_angular_velocity_pub.publish(out);
+				_last_publish = v_angular_velocity.timestamp_sample;
-
-				_last_publish = out.timestamp_sample;
 			}
 		}
 	}
@@ -323,10 +351,10 @@ void VehicleAngularVelocity::PrintStatus()
 	PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2));
 
 	PX4_INFO("sample rate: %.3f Hz", (double)_update_rate_hz);
-	PX4_INFO("low-pass filter cutoff: %.3f Hz", (double)_lowpass_filter.get_cutoff_freq());
+	PX4_INFO("low-pass filter cutoff: %.3f Hz", (double)_lp_filter_velocity.get_cutoff_freq());
 
-	if (_notch_filter.getNotchFreq() > 0.0f) {
+	if (_notch_filter_velocity.getNotchFreq() > 0.0f) {
-		PX4_INFO("notch filter freq: %.3f Hz\tbandwidth: %.3f Hz", (double)_notch_filter.getNotchFreq(),
+		PX4_INFO("notch filter freq: %.3f Hz\tbandwidth: %.3f Hz", (double)_notch_filter_velocity.getNotchFreq(),
-			 (double)_notch_filter.getBandwidth());
+			 (double)_notch_filter_velocity.getBandwidth());
 	}
 }

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -51,6 +51,7 @@
 #include <uORB/topics/sensor_correction.h>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_selection.h>
+#include <uORB/topics/vehicle_angular_acceleration.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 
 class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
@@ -68,7 +69,7 @@ public:
 private:
 	void Run() override;
 
-	void CheckFilters(const matrix::Vector3f &rates);
+	void CheckFilters();
 	void ParametersUpdate(bool force = false);
 	void SensorBiasUpdate(bool force = false);
 	void SensorCorrectionsUpdate(bool force = false);
@@ -82,6 +83,8 @@ private:
 		(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
 		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max,
 
+		(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff,
+
 		(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
 
 		(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
@@ -89,6 +92,7 @@ private:
 		(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
 	)
 
+	uORB::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};
 	uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
 
 	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
@@ -110,12 +114,22 @@ private:
 	matrix::Vector3f _offset{0.f, 0.f, 0.f};
 	matrix::Vector3f _scale{1.f, 1.f, 1.f};
 
+	matrix::Vector3f _angular_acceleration_prev{0.f, 0.f, 0.f};
+	matrix::Vector3f _angular_velocity_prev{0.f, 0.f, 0.f};
+	hrt_abstime _timestamp_sample_prev{0};
+
 	hrt_abstime _last_publish{0};
 	hrt_abstime _filter_check_last{0};
 	static constexpr const float kInitialRateHz{1000.0f}; /**< sensor update rate used for initialization */
 	float _update_rate_hz{kInitialRateHz}; /**< current rate-controller loop update rate in [Hz] */
-	math::LowPassFilter2pVector3f _lowpass_filter{kInitialRateHz, 30};
+
-	math::NotchFilter<matrix::Vector3f> _notch_filter{};
+	// angular velocity filters
+	math::LowPassFilter2pVector3f _lp_filter_velocity{kInitialRateHz, 30.0f};
+	math::NotchFilter<matrix::Vector3f> _notch_filter_velocity{};
+
+	// angular acceleration filter
+	math::LowPassFilter2pVector3f _lp_filter_acceleration{kInitialRateHz, 10.0f};
+
 	float _filter_sample_rate{kInitialRateHz};
 	int _sample_rate_incorrect_count{0};
 

src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c

@@ -94,3 +94,18 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f);
 * @group Sensors
 */
 PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0);
+
+/**
+* Cutoff frequency for angular acceleration
+*
+* The cutoff frequency for the 2nd order butterworth filter used on
+* the time derivative of the measured angular velocity.
+* Set to 0 to disable the filter.
+*
+* @min 0
+* @max 1000
+* @unit Hz
+* @reboot_required true
+* @group Sensors
+*/
+PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 10.0f);