sensors/vehicle_angular_velocity: properly handle filter reset on FIFO data scale changes

For the sake of efficiency (at 8 kHz) all filtering is performed on the raw data before the calibration and rotation is applied to only the final output. As a result we have to be a bit more careful when switching between sensors or in cases where the gyro scale factor changes (eg icm42688p 20 bit data rescaled to fit in int16 output).

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

@@ -93,6 +93,22 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
 	_device_id = device_id.devid;
 }
 
+void PX4Accelerometer::set_scale(float scale)
+{
+	if (fabsf(scale - _scale) > FLT_EPSILON) {
+		// rescale last sample on scale change
+		float rescale = _scale / scale;
+
+		for (auto &s : _last_sample) {
+			s = roundf(s * rescale);
+		}
+
+		_scale = scale;
+
+		UpdateClipLimit();
+	}
+}
+
 void PX4Accelerometer::update(const hrt_abstime &timestamp_sample, float x, float y, float z)
 {
 	// Apply rotation (before scaling)

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -55,7 +55,7 @@ public:
 	void set_error_count(uint32_t error_count) { _error_count = error_count; }
 	void increase_error_count() { _error_count++; }
 	void set_range(float range) { _range = range; UpdateClipLimit(); }
-	void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
+	void set_scale(float scale);
 	void set_temperature(float temperature) { _temperature = temperature; }
 
 	void update(const hrt_abstime &timestamp_sample, float x, float y, float z);

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -80,6 +80,20 @@ void PX4Gyroscope::set_device_type(uint8_t devtype)
 	_device_id = device_id.devid;
 }
 
+void PX4Gyroscope::set_scale(float scale)
+{
+	if (fabsf(scale - _scale) > FLT_EPSILON) {
+		// rescale last sample on scale change
+		float rescale = _scale / scale;
+
+		for (auto &s : _last_sample) {
+			s = roundf(s * rescale);
+		}
+
+		_scale = scale;
+	}
+}
+
 void PX4Gyroscope::update(const hrt_abstime &timestamp_sample, float x, float y, float z)
 {
 	// Apply rotation (before scaling)

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -55,7 +55,7 @@ public:
 	void set_error_count(uint32_t error_count) { _error_count = error_count; }
 	void increase_error_count() { _error_count++; }
 	void set_range(float range) { _range = range; }
-	void set_scale(float scale) { _scale = scale; }
+	void set_scale(float scale);
 	void set_temperature(float temperature) { _temperature = temperature; }
 
 	void update(const hrt_abstime &timestamp_sample, float x, float y, float z);

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -106,10 +106,12 @@ bool VehicleAngularVelocity::UpdateSampleRate()
 	}
 
 	// calculate sensor update rate
-	if (PX4_ISFINITE(sample_rate_hz) && PX4_ISFINITE(publish_rate_hz)) {
+	if ((sample_rate_hz > 0) && PX4_ISFINITE(sample_rate_hz) && (publish_rate_hz > 0) && PX4_ISFINITE(publish_rate_hz)) {
 		// check if sample rate error is greater than 1%
-		if ((fabsf(sample_rate_hz - _filter_sample_rate_hz) / sample_rate_hz) > 0.01f) {
-			PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate_hz, (double)sample_rate_hz);
+		if ((_filter_sample_rate_hz <= FLT_EPSILON) || !PX4_ISFINITE(_filter_sample_rate_hz)
+		    || (fabsf(sample_rate_hz - _filter_sample_rate_hz) / sample_rate_hz) > 0.01f) {
+
+			PX4_DEBUG("updating sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate_hz, (double)sample_rate_hz);
 			_reset_filters = true;
 			_filter_sample_rate_hz = sample_rate_hz;
 
@@ -137,46 +139,46 @@ bool VehicleAngularVelocity::UpdateSampleRate()
 				_publish_interval_min_us = 0;
 			}
 		}
-
-		if (_filter_sample_rate_hz > 0.f) {
-			return true;
-		}
 	}
 
-	return false;
+	return PX4_ISFINITE(_filter_sample_rate_hz) && (_filter_sample_rate_hz > 0);
 }
 
-void VehicleAngularVelocity::ResetFilters()
+void VehicleAngularVelocity::ResetFilters(float new_scale)
 {
-	const Vector3f angular_velocity{GetResetAngularVelocity()};
-	const Vector3f angular_acceleration{GetResetAngularAcceleration()};
-
-	for (int axis = 0; axis < 3; axis++) {
-		// angular velocity low pass
-		_lp_filter_velocity[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_gyro_cutoff.get());
-		_lp_filter_velocity[axis].reset(angular_velocity(axis));
-
-		// angular velocity notch
-		_notch_filter_velocity[axis].setParameters(_filter_sample_rate_hz, _param_imu_gyro_nf_freq.get(),
-				_param_imu_gyro_nf_bw.get());
-		_notch_filter_velocity[axis].reset(angular_velocity(axis));
-
-		// angular acceleration low pass
-		_lp_filter_acceleration[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get());
-		_lp_filter_acceleration[axis].reset(angular_acceleration(axis));
-	}
+	if ((_filter_sample_rate_hz > 0) && PX4_ISFINITE(_filter_sample_rate_hz)) {
+
+		const Vector3f angular_velocity{GetResetAngularVelocity(new_scale)};
+		const Vector3f angular_acceleration{GetResetAngularAcceleration(new_scale)};
+
+		for (int axis = 0; axis < 3; axis++) {
+			// angular velocity low pass
+			_lp_filter_velocity[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_gyro_cutoff.get());
+			_lp_filter_velocity[axis].reset(angular_velocity(axis));
+
+			// angular velocity notch
+			_notch_filter_velocity[axis].setParameters(_filter_sample_rate_hz, _param_imu_gyro_nf_freq.get(),
+					_param_imu_gyro_nf_bw.get());
+			_notch_filter_velocity[axis].reset(angular_velocity(axis));
+
+			// angular acceleration low pass
+			_lp_filter_acceleration[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get());
+			_lp_filter_acceleration[axis].reset(angular_acceleration(axis));
+		}
 
-	// dynamic notch filters, first disable, then force update (if available)
-	DisableDynamicNotchEscRpm();
-	DisableDynamicNotchFFT();
+		// dynamic notch filters, first disable, then force update (if available)
+		DisableDynamicNotchEscRpm();
+		DisableDynamicNotchFFT();
 
-	UpdateDynamicNotchEscRpm(true);
-	UpdateDynamicNotchFFT(true);
+		UpdateDynamicNotchEscRpm(new_scale, true);
+		UpdateDynamicNotchFFT(new_scale, true);
 
-	_angular_velocity_prev = angular_velocity;
+		_angular_velocity_prev = angular_velocity;
+		_last_scale = new_scale;
 
-	_reset_filters = false;
-	perf_count(_filter_reset_perf);
+		_reset_filters = false;
+		perf_count(_filter_reset_perf);
+	}
 }
 
 void VehicleAngularVelocity::SensorBiasUpdate(bool force)
@@ -224,6 +226,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
 						_selected_sensor_device_id = sensor_selection.gyro_device_id;
 						_calibration.set_device_id(sensor_gyro_fifo_sub.get().device_id);
 
+						_timestamp_sample_last = 0;
+						_filter_sample_rate_hz = NAN;
 						_reset_filters = true;
 						_bias.zero();
 						_fifo_available = true;
@@ -231,6 +235,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
 
 						perf_count(_selection_changed_perf);
 
+						PX4_DEBUG("selecting gyro FIFO %d %d", i, _selected_sensor_device_id);
+
 						return true;
 					}
 				}
@@ -249,7 +255,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
 						_calibration.set_device_id(sensor_gyro_sub.get().device_id);
 						_selected_sensor_device_id = sensor_selection.gyro_device_id;
 
-						// clear bias and corrections
+						_timestamp_sample_last = 0;
+						_filter_sample_rate_hz = NAN;
 						_reset_filters = true;
 						_bias.zero();
 						_fifo_available = false;
@@ -257,6 +264,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
 
 						perf_count(_selection_changed_perf);
 
+						PX4_DEBUG("selecting gyro %d %d", i, _selected_sensor_device_id);
+
 						return true;
 					}
 				}
@@ -341,29 +350,37 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 	}
 }
 
-Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const
+Vector3f VehicleAngularVelocity::GetResetAngularVelocity(float new_scale) const
 {
-	if ((_last_publish != 0) && (_last_scale > 0.f)
-	    && PX4_ISFINITE(_angular_velocity(0))
-	    && PX4_ISFINITE(_angular_velocity(1))
-	    && PX4_ISFINITE(_angular_velocity(2))) {
+	if ((_last_publish != 0) && (new_scale > 0.f)) {
 		// angular velocity filtering is performed on raw unscaled data
 		//  start with last valid vehicle body frame angular velocity and compute equivalent raw data (for current sensor selection)
-		return _calibration.Uncorrect(_angular_velocity + _bias) / _last_scale;
+		Vector3f angular_velocity{_calibration.Uncorrect(_angular_velocity + _bias) / new_scale};
+
+		if (PX4_ISFINITE(angular_velocity(0))
+		    && PX4_ISFINITE(angular_velocity(1))
+		    && PX4_ISFINITE(angular_velocity(2))) {
+
+			return angular_velocity;
+		}
 	}
 
 	return Vector3f{0.f, 0.f, 0.f};
 }
 
-Vector3f VehicleAngularVelocity::GetResetAngularAcceleration() const
+Vector3f VehicleAngularVelocity::GetResetAngularAcceleration(float new_scale) const
 {
-	if ((_last_publish != 0) && (_last_scale > 0.f)
-	    && PX4_ISFINITE(_angular_acceleration(0))
-	    && PX4_ISFINITE(_angular_acceleration(1))
-	    && PX4_ISFINITE(_angular_acceleration(2))) {
-		// angular acceleration filtering is performed on raw unscaled data
+	if ((_last_publish != 0) && (new_scale > 0.f)) {
+		// angular acceleration filtering is performed on unscaled angular velocity data
 		//  start with last valid vehicle body frame angular acceleration and compute equivalent raw data (for current sensor selection)
-		return _calibration.rotation().I() * _angular_acceleration / _last_scale;
+		Vector3f angular_acceleration{_calibration.rotation().I() *_angular_acceleration / new_scale};
+
+		if (PX4_ISFINITE(angular_acceleration(0))
+		    && PX4_ISFINITE(angular_acceleration(1))
+		    && PX4_ISFINITE(angular_acceleration(2))) {
+
+			return angular_acceleration;
+		}
 	}
 
 	return Vector3f{0.f, 0.f, 0.f};
@@ -401,7 +418,7 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT()
 #endif // !CONSTRAINED_FLASH
 }
 
-void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
+void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(float new_scale, bool force)
 {
 #if !defined(CONSTRAINED_FLASH)
 	const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::EscRpm;
@@ -434,8 +451,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
 							}
 
 							// only reset if there's sufficient change (> 1%)
-							if (change_percent > 0.01f) {
-								const Vector3f reset_angular_velocity{GetResetAngularVelocity()};
+							if (force || (change_percent > 0.01f)) {
+								const Vector3f reset_angular_velocity{GetResetAngularVelocity(new_scale)};
 
 								for (int axis = 0; axis < 3; axis++) {
 									auto &dnf = _dynamic_notch_filter_esc_rpm[i][harmonic][axis];
@@ -464,7 +481,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
 #endif // !CONSTRAINED_FLASH
 }
 
-void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
+void VehicleAngularVelocity::UpdateDynamicNotchFFT(float new_scale, bool force)
 {
 #if !defined(CONSTRAINED_FLASH)
 	const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT;
@@ -488,13 +505,13 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
 						const float peak_diff_abs = fabsf(dnf.getNotchFreq() - peak_freq);
 						const float change_percent = peak_diff_abs / peak_freq;
 
-						if (change_percent > 0.001f) {
+						if (force || (change_percent > 0.001f)) {
 							// peak frequency changed by at least 0.1%
 							dnf.setParameters(_filter_sample_rate_hz, peak_freq, sensor_gyro_fft.resolution_hz);
 
 							// only reset if there's sufficient change
 							if (peak_diff_abs > sensor_gyro_fft.resolution_hz) {
-								dnf.reset(GetResetAngularVelocity()(axis));
+								dnf.reset(GetResetAngularVelocity(new_scale)(axis));
 							}
 
 							perf_count(_dynamic_notch_filter_fft_update_perf);
@@ -566,22 +583,18 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
 	return data[N - 1];
 }
 
-float VehicleAngularVelocity::FilterAngularAcceleration(int axis, float data[], int N, float dt_s)
+float VehicleAngularVelocity::FilterAngularAcceleration(int axis, float dt_s, float data[], int N)
 {
-	if (N > 0) {
-		// angular acceleration: Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF)
-		float delta_velocity_filtered;
-
-		for (int n = 0; n < N; n++) {
-			const float delta_velocity = (data[n] - _angular_velocity_prev(axis));
-			delta_velocity_filtered = _lp_filter_acceleration[axis].apply(delta_velocity);
-			_angular_velocity_prev(axis) = data[n];
-		}
+	// angular acceleration: Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF)
+	float angular_acceleration_filtered = 0.f;
 
-		return delta_velocity_filtered / dt_s;
+	for (int n = 0; n < N; n++) {
+		const float angular_acceleration = (data[n] - _angular_velocity_prev(axis)) / dt_s;
+		angular_acceleration_filtered = _lp_filter_acceleration[axis].apply(angular_acceleration);
+		_angular_velocity_prev(axis) = data[n];
 	}
 
-	return 0.f;
+	return angular_acceleration_filtered;
 }
 
 void VehicleAngularVelocity::Run()
@@ -589,12 +602,20 @@ void VehicleAngularVelocity::Run()
 	// backup schedule
 	ScheduleDelayed(10_ms);
 
+	ParametersUpdate();
+
 	// update corrections first to set _selected_sensor
 	const bool selection_updated = SensorSelectionUpdate();
 
 	_calibration.SensorCorrectionsUpdate(selection_updated);
 	SensorBiasUpdate(selection_updated);
-	ParametersUpdate();
+
+	if (selection_updated || !PX4_ISFINITE(_filter_sample_rate_hz) || (_filter_sample_rate_hz <= FLT_EPSILON)) {
+		if (!UpdateSampleRate()) {
+			// sensor sample rate required to run
+			return;
+		}
+	}
 
 	if (_fifo_available) {
 		// process all outstanding fifo messages
@@ -604,13 +625,9 @@ void VehicleAngularVelocity::Run()
 			const float dt_s = sensor_fifo_data.dt * 1e-6f;
 			_timestamp_sample_last = sensor_fifo_data.timestamp_sample;
 
+			// in FIFO mode the unscaled raw data is filtered, reset filters on any scale change
 			if (_reset_filters || (fabsf(sensor_fifo_data.scale - _last_scale) > FLT_EPSILON)) {
-				if (UpdateSampleRate()) {
-					// in FIFO mode the unscaled raw data is filtered
-					_last_scale = sensor_fifo_data.scale;
-
-					ResetFilters();
-				}
+				ResetFilters(sensor_fifo_data.scale);
 
 				if (_reset_filters) {
 					continue; // not safe to run until filters configured
@@ -639,14 +656,12 @@ void VehicleAngularVelocity::Run()
 
 					// save last filtered sample
 					angular_velocity_unscaled(axis) = FilterAngularVelocity(axis, data, N);
-					angular_acceleration_unscaled(axis) = FilterAngularAcceleration(axis, data, N, dt_s);
+					angular_acceleration_unscaled(axis) = FilterAngularAcceleration(axis, dt_s, data, N);
 				}
 
 				// Publish
-				if (!_sensor_fifo_sub.updated()) {
-					CalibrateAndPublish(sensor_fifo_data.timestamp_sample, angular_velocity_unscaled, angular_acceleration_unscaled,
-							    sensor_fifo_data.scale);
-				}
+				CalibrateAndPublish(!_sensor_fifo_sub.updated(), sensor_fifo_data.timestamp_sample, angular_velocity_unscaled,
+						    angular_acceleration_unscaled, sensor_fifo_data.scale);
 			}
 		}
 
@@ -655,15 +670,15 @@ void VehicleAngularVelocity::Run()
 		sensor_gyro_s sensor_data;
 
 		while (_sensor_sub.update(&sensor_data)) {
+			if (_timestamp_sample_last == 0) {
+				_timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz;
+			}
+
 			const float dt_s = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) / 1e6f), 0.0002f, 0.02f);
 			_timestamp_sample_last = sensor_data.timestamp_sample;
 
 			if (_reset_filters) {
-				if (UpdateSampleRate()) {
-					// non-FIFO sensor data is already scaled
-					_last_scale = 1.f;
-					ResetFilters();
-				}
+				ResetFilters();
 
 				if (_reset_filters) {
 					continue; // not safe to run until filters configured
@@ -673,8 +688,8 @@ void VehicleAngularVelocity::Run()
 			UpdateDynamicNotchEscRpm();
 			UpdateDynamicNotchFFT();
 
-			Vector3f angular_velocity_unscaled;
-			Vector3f angular_acceleration_unscaled;
+			Vector3f angular_velocity;
+			Vector3f angular_acceleration;
 
 			float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z};
 
@@ -683,19 +698,17 @@ void VehicleAngularVelocity::Run()
 				float data[1] {raw_data_array[axis]};
 
 				// save last filtered sample
-				angular_velocity_unscaled(axis) = FilterAngularVelocity(axis, data, 1);
-				angular_acceleration_unscaled(axis) = FilterAngularAcceleration(axis, data, 1, dt_s);
+				angular_velocity(axis) = FilterAngularVelocity(axis, data);
+				angular_acceleration(axis) = FilterAngularAcceleration(axis, dt_s, data);
 			}
 
 			// Publish
-			if (!_sensor_sub.updated()) {
-				CalibrateAndPublish(sensor_data.timestamp_sample, angular_velocity_unscaled, angular_acceleration_unscaled);
-			}
+			CalibrateAndPublish(!_sensor_sub.updated(), sensor_data.timestamp_sample, angular_velocity, angular_acceleration);
 		}
 	}
 }
 
-void VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime &timestamp_sample,
+void VehicleAngularVelocity::CalibrateAndPublish(bool publish, const hrt_abstime &timestamp_sample,
 		const Vector3f &angular_velocity_unscaled,  const Vector3f &angular_acceleration_unscaled, float scale)
 {
 	// Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias
@@ -704,7 +717,7 @@ void VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime &timestamp_sa
 	// Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation
 	_angular_acceleration = _calibration.rotation() * angular_acceleration_unscaled * scale;
 
-	if (timestamp_sample >= _last_publish + _publish_interval_min_us) {
+	if (publish && (timestamp_sample >= _last_publish + _publish_interval_min_us)) {
 
 		// Publish vehicle_angular_acceleration
 		vehicle_angular_acceleration_s v_angular_acceleration;

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -75,26 +75,26 @@ public:
 private:
 	void Run() override;
 
-	void CalibrateAndPublish(const hrt_abstime &timestamp_sample, const matrix::Vector3f &angular_velocity,
+	void CalibrateAndPublish(bool publish, const hrt_abstime &timestamp_sample, const matrix::Vector3f &angular_velocity,
 				 const matrix::Vector3f &angular_acceleration, float scale = 1.f);
 
-	float FilterAngularVelocity(int axis, float data[], int N);
-	float FilterAngularAcceleration(int axis, float data[], int N, float dt_s);
+	float FilterAngularVelocity(int axis, float data[], int N = 1);
+	float FilterAngularAcceleration(int axis, float dt_s, float data[], int N = 1);
 
 	void DisableDynamicNotchEscRpm();
 	void DisableDynamicNotchFFT();
 	void ParametersUpdate(bool force = false);
 
-	void ResetFilters();
+	void ResetFilters(float new_scale = 1.f);
 	void SensorBiasUpdate(bool force = false);
 	bool SensorSelectionUpdate(bool force = false);
-	void UpdateDynamicNotchEscRpm(bool force = false);
-	void UpdateDynamicNotchFFT(bool force = false);
+	void UpdateDynamicNotchEscRpm(float new_scale = 1.f, bool force = false);
+	void UpdateDynamicNotchFFT(float new_scale = 1.f, bool force = false);
 	bool UpdateSampleRate();
 
 	// scaled appropriately for current FIFO mode
-	matrix::Vector3f GetResetAngularVelocity() const;
-	matrix::Vector3f GetResetAngularAcceleration() const;
+	matrix::Vector3f GetResetAngularVelocity(float new_scale = 1.f) const;
+	matrix::Vector3f GetResetAngularAcceleration(float new_scale = 1.f) const;
 
 	static constexpr int MAX_SENSOR_COUNT = 4;
 
@@ -127,7 +127,7 @@ private:
 	hrt_abstime _publish_interval_min_us{0};
 	hrt_abstime _last_publish{0};
 
-	float _filter_sample_rate_hz{0.f};
+	float _filter_sample_rate_hz{NAN};
 
 	static constexpr const float kInitialRateHz{1000.f}; /**< sensor update rate used for initialization */