From d95e387d798edbdb061e69a21371996bb74bdd29 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Tue, 5 Oct 2021 15:14:08 +0200
Subject: [PATCH] angular acceleration: change 2nd order to 1st order LPF

Having a 2nd order low-pass filter in the derivative loop reduces
stability at low cutoff values as too much phase is lost through
the filter. Using a 1st order filter avoids this issue because its
maximum phase loss is 90 degrees instead of 180 degrees for a 2nd order
lpf.
---
 .../vehicle_angular_velocity/VehicleAngularVelocity.cpp     | 6 +++---
 .../vehicle_angular_velocity/VehicleAngularVelocity.hpp     | 3 ++-
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
index 79b447e94c..b861e93c2e 100644
--- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
@@ -162,7 +162,7 @@ void VehicleAngularVelocity::ResetFilters()
 			_notch_filter_velocity[axis].reset(angular_velocity_uncalibrated(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].setCutoffFreq(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get());
 			_lp_filter_acceleration[axis].reset(angular_acceleration_uncalibrated(axis));
 		}
 
@@ -305,7 +305,7 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 
 		// gyro derivative low pass cutoff changed
 		for (auto &lp : _lp_filter_acceleration) {
-			if (fabsf(lp.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > 0.01f) {
+			if (fabsf(lp.getCutoffFreq() - _param_imu_dgyro_cutoff.get()) > 0.01f) {
 				_reset_filters = true;
 				break;
 			}
@@ -630,7 +630,7 @@ float VehicleAngularVelocity::FilterAngularAcceleration(int axis, float dt_s, fl
 
 	for (int n = 0; n < N; n++) {
 		const float angular_acceleration = (data[n] - _angular_velocity_raw_prev(axis)) / dt_s;
-		angular_acceleration_filtered = _lp_filter_acceleration[axis].apply(angular_acceleration);
+		angular_acceleration_filtered = _lp_filter_acceleration[axis].update(angular_acceleration);
 		_angular_velocity_raw_prev(axis) = data[n];
 	}
 
diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
index 6b893a659d..115af7ad4f 100644
--- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
+++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
@@ -37,6 +37,7 @@
 #include <lib/sensor_calibration/Gyroscope.hpp>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
 #include <lib/mathlib/math/filter/NotchFilter.hpp>
 #include <px4_platform_common/log.h>
@@ -165,7 +166,7 @@ private:
 #endif // !CONSTRAINED_FLASH
 
 	// angular acceleration filter
-	math::LowPassFilter2p<float> _lp_filter_acceleration[3] {};
+	AlphaFilter<float> _lp_filter_acceleration[3] {};
 
 	uint32_t _selected_sensor_device_id{0};