fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 22:50:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

mathlib: Notchfilter updates

Browse Source 
- merge NotchFilterArray into regular NotchFilter (apply vs applyArray)
 - only use direct form 1 to prevent reset confusion
 - safe default field initialization
 - update VehicleAngularVelocity usage
This commit is contained in:
Daniel Agar
2021-05-25 12:14:24 -04:00
parent 055b5404c3
commit f25a70a674
5 changed files with 76 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+3 -3
View File
@@ -545,7 +545,7 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
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);
dnf[harmonic][axis].applyArray(data, N);
} else {
break;
@@ -562,7 +562,7 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
for (auto &dnf : _dynamic_notch_filter_fft) {
if (dnf[axis].getNotchFreq() > 0.f) {
dnf[axis].applyDF1(data, N);
dnf[axis].applyArray(data, N);
}
}
@@ -573,7 +573,7 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
// Apply general notch filter (IMU_GYRO_NF_FREQ)
if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) {
_notch_filter_velocity[axis].apply(data, N);
_notch_filter_velocity[axis].applyArray(data, N);
}
// Apply general low-pass filter (IMU_GYRO_CUTOFF)
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
+4 -4
View File
@@ -38,7 +38,7 @@
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
#include <lib/mathlib/math/filter/NotchFilterArray.hpp>
#include <lib/mathlib/math/filter/NotchFilter.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
@@ -133,7 +133,7 @@ private:
// angular velocity filters
math::LowPassFilter2pArray _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
math::NotchFilterArray<float> _notch_filter_velocity[3] {};
math::NotchFilter<float> _notch_filter_velocity[3] {};
#if !defined(CONSTRAINED_FLASH)
@@ -148,8 +148,8 @@ private:
static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(
sensor_gyro_fft_s::peak_frequencies_x[0]);
math::NotchFilterArray<float> _dynamic_notch_filter_esc_rpm[MAX_NUM_ESC_RPM][MAX_NUM_ESC_RPM_HARMONICS][3] {};
math::NotchFilterArray<float> _dynamic_notch_filter_fft[MAX_NUM_FFT_PEAKS][3] {};
math::NotchFilter<float> _dynamic_notch_filter_esc_rpm[MAX_NUM_ESC_RPM][MAX_NUM_ESC_RPM_HARMONICS][3] {};
math::NotchFilter<float> _dynamic_notch_filter_fft[MAX_NUM_FFT_PEAKS][3] {};
perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr};