fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

angular acceleration: change 2nd order to 1st order LPF

Browse Source 
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.
This commit is contained in:
bresch 2021-10-05 15:14:08 +02:00 committed by Mathieu Bresciani
parent 1b58202b0d
commit d95e387d79
2 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -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];
}

3
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@ -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};