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 #include #include +#include #include #include #include @@ -165,7 +166,7 @@ private: #endif // !CONSTRAINED_FLASH // angular acceleration filter - math::LowPassFilter2p _lp_filter_acceleration[3] {}; + AlphaFilter _lp_filter_acceleration[3] {}; uint32_t _selected_sensor_device_id{0};