sensors/vehicle_angular_velocity: unify filtering for both FIFO and regular use cases

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -146,8 +146,11 @@ bool VehicleAngularVelocity::UpdateSampleRate()
 	return false;
 }
 
-void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, const Vector3f &angular_acceleration)
+void VehicleAngularVelocity::ResetFilters()
 {
+	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());
@@ -161,14 +164,16 @@ void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, cons
 		// 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(true);
+	UpdateDynamicNotchFFT(true);
+
+	_angular_velocity_prev = angular_velocity;
 
 	_reset_filters = false;
 	perf_count(_filter_reset_perf);
@@ -314,6 +319,9 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 			if (_dynamic_notch_filter_esc_rpm_perf == nullptr) {
 				_dynamic_notch_filter_esc_rpm_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch ESC RPM filter");
 			}
+
+		} else {
+			DisableDynamicNotchEscRpm();
 		}
 
 		if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT) {
@@ -324,6 +332,9 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 			if (_dynamic_notch_filter_fft_perf == nullptr) {
 				_dynamic_notch_filter_fft_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch FFT filter");
 			}
+
+		} else {
+			DisableDynamicNotchFFT();
 		}
 
 #endif // !CONSTRAINED_FLASH
@@ -332,11 +343,27 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 
 Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const
 {
-	if (_fifo_available && (_last_scale > 0.f)) {
+	if ((_last_publish != 0) && (_last_scale > 0.f)
+	    && PX4_ISFINITE(_angular_velocity(0))
+	    && PX4_ISFINITE(_angular_velocity(1))
+	    && PX4_ISFINITE(_angular_velocity(2))) {
+		// 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;
+	}
 
-	} else if (!_fifo_available) {
-		return _angular_velocity;
+	return Vector3f{0.f, 0.f, 0.f};
+}
+
+Vector3f VehicleAngularVelocity::GetResetAngularAcceleration() 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
+		//  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;
 	}
 
 	return Vector3f{0.f, 0.f, 0.f};
@@ -490,6 +517,73 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
 #endif // !CONSTRAINED_FLASH
 }
 
+float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int N)
+{
+#if !defined(CONSTRAINED_FLASH)
+
+	// Apply dynamic notch filter from ESC RPM
+	if (_dynamic_notch_esc_rpm_available) {
+		perf_begin(_dynamic_notch_filter_esc_rpm_perf);
+
+		for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
+			for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
+				if (dnf[harmonic][axis].getNotchFreq() > 0.f) {
+					dnf[harmonic][axis].applyDF1(data, N);
+
+				} else {
+					break;
+				}
+			}
+		}
+
+		perf_end(_dynamic_notch_filter_esc_rpm_perf);
+	}
+
+	// Apply dynamic notch filter from FFT
+	if (_dynamic_notch_fft_available) {
+		perf_begin(_dynamic_notch_filter_fft_perf);
+
+		for (auto &dnf : _dynamic_notch_filter_fft) {
+			if (dnf[axis].getNotchFreq() > 0.f) {
+				dnf[axis].applyDF1(data, N);
+			}
+		}
+
+		perf_end(_dynamic_notch_filter_fft_perf);
+	}
+
+#endif // !CONSTRAINED_FLASH
+
+	// Apply general notch filter (IMU_GYRO_NF_FREQ)
+	if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) {
+		_notch_filter_velocity[axis].apply(data, N);
+	}
+
+	// Apply general low-pass filter (IMU_GYRO_CUTOFF)
+	_lp_filter_velocity[axis].apply(data, N);
+
+	// return last filtered sample
+	return data[N - 1];
+}
+
+float VehicleAngularVelocity::FilterAngularAcceleration(int axis, float data[], int N, float dt_s)
+{
+	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];
+		}
+
+		return delta_velocity_filtered / dt_s;
+	}
+
+	return 0.f;
+}
+
 void VehicleAngularVelocity::Run()
 {
 	// backup schedule
@@ -507,30 +601,26 @@ void VehicleAngularVelocity::Run()
 		sensor_gyro_fifo_s sensor_fifo_data;
 
 		while (_sensor_fifo_sub.update(&sensor_fifo_data)) {
-			static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]);
-
-			if ((sensor_fifo_data.samples > 0) && (sensor_fifo_data.samples <= FIFO_SIZE_MAX)) {
-				_timestamp_sample_last = sensor_fifo_data.timestamp_sample;
-
-				const int N = sensor_fifo_data.samples;
-				const float dt_s = sensor_fifo_data.dt * 1e-6f;
+			const float dt_s = sensor_fifo_data.dt * 1e-6f;
+			_timestamp_sample_last = sensor_fifo_data.timestamp_sample;
 
-				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;
-
-						_angular_velocity_prev = GetResetAngularVelocity();
-						Vector3f angular_acceleration{_calibration.rotation().I() *_angular_acceleration / _last_scale};
+			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(_angular_velocity_prev, angular_acceleration);
-					}
+					ResetFilters();
+				}
 
-					if (_reset_filters) {
-						continue; // not safe to run until filters configured
-					}
+				if (_reset_filters) {
+					continue; // not safe to run until filters configured
 				}
+			}
+
+			const int N = sensor_fifo_data.samples;
+			static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]);
 
+			if ((N > 0) && (N <= FIFO_SIZE_MAX)) {
 				UpdateDynamicNotchEscRpm();
 				UpdateDynamicNotchFFT();
 
@@ -547,74 +637,15 @@ void VehicleAngularVelocity::Run()
 						data[n] = raw_data_array[axis][n];
 					}
 
-#if !defined(CONSTRAINED_FLASH)
-
-					// Apply dynamic notch filter from ESC RPM
-					if (_dynamic_notch_esc_rpm_available) {
-						perf_begin(_dynamic_notch_filter_esc_rpm_perf);
-
-						for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
-							for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
-								if (dnf[harmonic][axis].getNotchFreq() > 0.f) {
-									dnf[harmonic][axis].applyDF1(data, N);
-
-								} else {
-									break;
-								}
-							}
-						}
-
-						perf_end(_dynamic_notch_filter_esc_rpm_perf);
-					}
-
-					// Apply dynamic notch filter from FFT
-					if (_dynamic_notch_fft_available) {
-						perf_begin(_dynamic_notch_filter_fft_perf);
-
-						for (auto &dnf : _dynamic_notch_filter_fft) {
-							if (dnf[axis].getNotchFreq() > 0.f) {
-								dnf[axis].applyDF1(data, N);
-							}
-						}
-
-						perf_end(_dynamic_notch_filter_fft_perf);
-					}
-
-#endif // !CONSTRAINED_FLASH
-
-					// Apply general notch filter (IMU_GYRO_NF_FREQ)
-					if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) {
-						_notch_filter_velocity[axis].apply(data, N);
-					}
-
-					// Apply general low-pass filter (IMU_GYRO_CUTOFF)
-					_lp_filter_velocity[axis].apply(data, N);
-
 					// save last filtered sample
-					angular_velocity_unscaled(axis) = data[N - 1];
-
-
-					// 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_unscaled(axis) = delta_velocity_filtered / dt_s;
+					angular_velocity_unscaled(axis) = FilterAngularVelocity(axis, data, N);
+					angular_acceleration_unscaled(axis) = FilterAngularAcceleration(axis, data, N, dt_s);
 				}
 
-				// Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias
-				_angular_velocity = _calibration.Correct(angular_velocity_unscaled * sensor_fifo_data.scale) - _bias;
-
-				// Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation
-				_angular_acceleration = _calibration.rotation() * angular_acceleration_unscaled * sensor_fifo_data.scale;
-
 				// Publish
 				if (!_sensor_fifo_sub.updated()) {
-					Publish(sensor_fifo_data.timestamp_sample);
+					CalibrateAndPublish(sensor_fifo_data.timestamp_sample, angular_velocity_unscaled, angular_acceleration_unscaled,
+							    sensor_fifo_data.scale);
 				}
 			}
 		}
@@ -629,8 +660,9 @@ void VehicleAngularVelocity::Run()
 
 			if (_reset_filters) {
 				if (UpdateSampleRate()) {
-					_angular_velocity_prev = _angular_velocity;
-					ResetFilters(_angular_velocity, _angular_acceleration);
+					// non-FIFO sensor data is already scaled
+					_last_scale = 1.f;
+					ResetFilters();
 				}
 
 				if (_reset_filters) {
@@ -641,70 +673,39 @@ void VehicleAngularVelocity::Run()
 			UpdateDynamicNotchEscRpm();
 			UpdateDynamicNotchFFT();
 
-			// Apply calibration, rotation, and correct for in-run bias errors
-			Vector3f angular_velocity{_calibration.Correct(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z}) - _bias};
-
-			for (int axis = 0; axis < 3; axis++) {
-#if !defined(CONSTRAINED_FLASH)
-
-				// Apply dynamic notch filter from ESC RPM
-				if (_dynamic_notch_esc_rpm_available) {
-					perf_begin(_dynamic_notch_filter_esc_rpm_perf);
-
-					for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
-						for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
-							if (dnf[harmonic][axis].getNotchFreq() > 0.f) {
-								dnf[harmonic][axis].applyDF1(&angular_velocity(axis), 1);
-
-							} else {
-								break;
-							}
-						}
-					}
-
-					perf_end(_dynamic_notch_filter_esc_rpm_perf);
-				}
-
-				// Apply dynamic notch filter from FFT
-				if (_dynamic_notch_fft_available) {
-					perf_begin(_dynamic_notch_filter_fft_perf);
-
-					for (auto &dnf : _dynamic_notch_filter_fft) {
-						if (dnf[axis].getNotchFreq() > 0.f) {
-							dnf[axis].applyDF1(&angular_velocity(axis), 1);
-						}
-					}
-
-					perf_end(_dynamic_notch_filter_fft_perf);
-				}
+			Vector3f angular_velocity_unscaled;
+			Vector3f angular_acceleration_unscaled;
 
-#endif // !CONSTRAINED_FLASH
-
-				// Apply general notch filter (IMU_GYRO_NF_FREQ)
-				_notch_filter_velocity[axis].apply(&angular_velocity(axis), 1);
+			float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z};
 
-				// Apply general low-pass filter (IMU_GYRO_CUTOFF)
-				_lp_filter_velocity[axis].apply(&angular_velocity(axis), 1);
+			for (int axis = 0; axis < 3; axis++) {
+				// copy sensor sample to float array for filtering
+				float data[1] {raw_data_array[axis]};
 
-				// Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF)
-				const float accel = (angular_velocity(axis) - _angular_velocity_prev(axis)) / dt_s;
-				_angular_acceleration(axis) = _lp_filter_acceleration[axis].apply(accel);
-				_angular_velocity_prev(axis) = angular_velocity(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 = angular_velocity;
-
 			// Publish
 			if (!_sensor_sub.updated()) {
-				Publish(sensor_data.timestamp_sample);
+				CalibrateAndPublish(sensor_data.timestamp_sample, angular_velocity_unscaled, angular_acceleration_unscaled);
 			}
 		}
 	}
 }
 
-void VehicleAngularVelocity::Publish(const hrt_abstime &timestamp_sample)
+void VehicleAngularVelocity::CalibrateAndPublish(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
+	_angular_velocity = _calibration.Correct(angular_velocity_unscaled * scale) - _bias;
+
+	// 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) {
+
 		// Publish vehicle_angular_acceleration
 		vehicle_angular_acceleration_s v_angular_acceleration;
 		v_angular_acceleration.timestamp_sample = timestamp_sample;

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -75,11 +75,17 @@ public:
 private:
 	void Run() override;
 
+	void CalibrateAndPublish(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);
+
 	void DisableDynamicNotchEscRpm();
 	void DisableDynamicNotchFFT();
 	void ParametersUpdate(bool force = false);
-	void Publish(const hrt_abstime &timestamp_sample);
-	void ResetFilters(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration);
+
+	void ResetFilters();
 	void SensorBiasUpdate(bool force = false);
 	bool SensorSelectionUpdate(bool force = false);
 	void UpdateDynamicNotchEscRpm(bool force = false);
@@ -88,6 +94,7 @@ private:
 
 	// scaled appropriately for current FIFO mode
 	matrix::Vector3f GetResetAngularVelocity() const;
+	matrix::Vector3f GetResetAngularAcceleration() const;
 
 	static constexpr int MAX_SENSOR_COUNT = 4;