diff --git a/src/lib/mathlib/math/filter/NotchFilter.hpp b/src/lib/mathlib/math/filter/NotchFilter.hpp index 5b53825bfc..fbe75df54f 100644 --- a/src/lib/mathlib/math/filter/NotchFilter.hpp +++ b/src/lib/mathlib/math/filter/NotchFilter.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * Copyright (C) 2019-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -70,33 +70,16 @@ public: void setParameters(float sample_freq, float notch_freq, float bandwidth); - /** - * Add a new raw value to the filter using the Direct form II - * - * @return retrieve the filtered result - */ - inline T apply(const T &sample) - { - // Direct Form II implementation - const T delay_element_0{sample - _delay_element_1 *_a1 - _delay_element_2 * _a2}; - const T output{delay_element_0 *_b0 + _delay_element_1 *_b1 + _delay_element_2 * _b2}; - - _delay_element_2 = _delay_element_1; - _delay_element_1 = delay_element_0; - - return output; - } - /** * Add a new raw value to the filter using the Direct Form I * * @return retrieve the filtered result */ - inline T applyDF1(const T &sample) + inline T apply(const T &sample) { // Direct Form I implementation - const T output = _b0 * sample + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 - _a2 * - _delay_element_output_2; + T output = _b0 * sample + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 - _a2 * + _delay_element_output_2; // shift inputs _delay_element_2 = _delay_element_1; @@ -109,6 +92,14 @@ public: return output; } + // Filter array of samples in place using the direct form I + inline void applyArray(T samples[], int num_samples) + { + for (int n = 0; n < num_samples; n++) { + samples[n] = apply(samples[n]); + } + } + float getNotchFreq() const { return _notch_freq; } float getBandwidth() const { return _bandwidth; } @@ -150,53 +141,77 @@ public: _b2 = b[2]; } + void reset(const T &sample) + { + const T input = isFinite(sample) ? sample : T{}; + + _delay_element_1 = _delay_element_2 = input; + _delay_element_output_1 = _delay_element_output_2 = input * (_b0 + _b1 + _b2) / (1 + _a1 + _a2); + + if (!isFinite(_delay_element_1) || !isFinite(_delay_element_2)) { + _delay_element_output_1 = _delay_element_output_2 = {}; + } + } + + void disable() + { + // no filtering + _notch_freq = 0.f; + _bandwidth = 0.f; + _sample_freq = 0.f; + + _delay_element_1 = {}; + _delay_element_2 = {}; + _delay_element_output_1 = {}; + _delay_element_output_2 = {}; + + _b0 = 1.f; + _b1 = 0.f; + _b2 = 0.f; - T reset(const T &sample); + _a1 = 0.f; + _a2 = 0.f; + } protected: - float _notch_freq{}; - float _bandwidth{}; - float _sample_freq{}; + T _delay_element_1{}; + T _delay_element_2{}; + T _delay_element_output_1{}; + T _delay_element_output_2{}; // All the coefficients are normalized by a0, so a0 becomes 1 here - float _a1{}; - float _a2{}; + float _a1{0.f}; + float _a2{0.f}; - float _b0{}; - float _b1{}; - float _b2{}; + float _b0{1.f}; + float _b1{0.f}; + float _b2{0.f}; - T _delay_element_1; - T _delay_element_2; - T _delay_element_output_1; - T _delay_element_output_2; + float _notch_freq{}; + float _bandwidth{}; + float _sample_freq{}; }; /** * Initialises the filter by setting its parameters and coefficients. - * If using the direct form I (applyDF1) method, allows to dynamically + * Using the direct form I method, allows to dynamically * update the filtered frequency, refresh rate and quality factor while * conserving the filter's history */ template void NotchFilter::setParameters(float sample_freq, float notch_freq, float bandwidth) { - _notch_freq = notch_freq; - _bandwidth = bandwidth; - _sample_freq = sample_freq; - - if (notch_freq <= 0.f) { - // no filtering - _b0 = 1.0f; - _b1 = 0.0f; - _b2 = 0.0f; - - _a1 = 0.0f; - _a2 = 0.0f; + if ((sample_freq <= 0.f) || (notch_freq <= 0.f) || (bandwidth <= 0.f) || (notch_freq >= sample_freq / 2) + || !isFinite(sample_freq) || !isFinite(notch_freq) || !isFinite(bandwidth)) { + disable(); return; } + _notch_freq = math::constrain(notch_freq, 5.f, sample_freq / 2); // TODO: min based on actual numerical limit + _bandwidth = math::constrain(bandwidth, 5.f, sample_freq / 2); + _sample_freq = sample_freq; + const float alpha = tanf(M_PI_F * bandwidth / sample_freq); const float beta = -cosf(2.f * M_PI_F * notch_freq / sample_freq); const float a0_inv = 1.f / (alpha + 1.f); @@ -207,23 +222,10 @@ void NotchFilter::setParameters(float sample_freq, float notch_freq, float ba _a1 = _b1; _a2 = (1.f - alpha) * a0_inv; -} -template -T NotchFilter::reset(const T &sample) -{ - T dval = sample; - - if (fabsf(_b0 + _b1 + _b2) > FLT_EPSILON) { - dval = dval / (_b0 + _b1 + _b2); + if (!isFinite(_b0) || !isFinite(_b1) || !isFinite(_b2) || !isFinite(_a1) || !isFinite(_a2)) { + disable(); } - - _delay_element_1 = dval; - _delay_element_2 = dval; - _delay_element_output_1 = {}; - _delay_element_output_2 = {}; - - return apply(sample); } } // namespace math diff --git a/src/lib/mathlib/math/filter/NotchFilterArray.hpp b/src/lib/mathlib/math/filter/NotchFilterArray.hpp deleted file mode 100644 index eff13b868c..0000000000 --- a/src/lib/mathlib/math/filter/NotchFilterArray.hpp +++ /dev/null @@ -1,117 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file NotchFilter.hpp - * - * @brief Notch filter with array input/output - * - * @author Mathieu Bresciani - * @author Samuel Garcin - */ - -#pragma once - -#include "NotchFilter.hpp" - -namespace math -{ -template -class NotchFilterArray : public NotchFilter -{ - using NotchFilter::_delay_element_1; - using NotchFilter::_delay_element_2; - using NotchFilter::_delay_element_output_1; - using NotchFilter::_delay_element_output_2; - using NotchFilter::_a1; - using NotchFilter::_a2; - using NotchFilter::_b0; - using NotchFilter::_b1; - using NotchFilter::_b2; - -public: - - NotchFilterArray() = default; - ~NotchFilterArray() = default; - - // Filter array of samples in place using the Direct form II. - inline void apply(T samples[], uint8_t num_samples) - { - for (int n = 0; n < num_samples; n++) { - // Direct Form II implementation - T delay_element_0{samples[n] - _delay_element_1 *_a1 - _delay_element_2 * _a2}; - - // don't allow bad values to propagate via the filter - if (!isFinite(delay_element_0)) { - delay_element_0 = samples[n]; - } - - samples[n] = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; - - _delay_element_2 = _delay_element_1; - _delay_element_1 = delay_element_0; - } - } - - /** - * Add new raw values to the filter using the Direct form I. - * - * @return retrieve the filtered result - */ - inline void applyDF1(T samples[], uint8_t num_samples) - { - for (int n = 0; n < num_samples; n++) { - // Direct Form II implementation - T output = _b0 * samples[n] + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 - - _a2 * _delay_element_output_2; - - // don't allow bad values to propagate via the filter - if (!isFinite(output)) { - output = samples[n]; - } - - // shift inputs - _delay_element_2 = _delay_element_1; - _delay_element_1 = samples[n]; - - // shift outputs - _delay_element_output_2 = _delay_element_output_1; - _delay_element_output_1 = output; - - // writes value to array - samples[n] = output; - } - } -}; - -} // namespace math diff --git a/src/lib/mathlib/math/filter/NotchFilterTest.cpp b/src/lib/mathlib/math/filter/NotchFilterTest.cpp index f9516b83f4..f528a5995c 100644 --- a/src/lib/mathlib/math/filter/NotchFilterTest.cpp +++ b/src/lib/mathlib/math/filter/NotchFilterTest.cpp @@ -117,7 +117,7 @@ TEST_F(NotchFilterTest, filteringLowSideDF1) for (int i = 0; i < 1000; i++) { float input = sinf(omega * t); float output_expected = sinf(omega * t - phase_delay); - out = _notch_float.applyDF1(input); + out = _notch_float.apply(input); t = i * dt; // Let some time for the filter to settle @@ -167,7 +167,7 @@ TEST_F(NotchFilterTest, filteringHighSideDF1) for (int i = 0; i < 1000; i++) { float input = sinf(omega * t); float output_expected = sinf(omega * t + phase_delay); - out = _notch_float.applyDF1(input); + out = _notch_float.apply(input); t = i * dt; // Let some time for the filter to settle @@ -213,7 +213,7 @@ TEST_F(NotchFilterTest, filterOnNotchDF1) for (int i = 0; i < 1000; i++) { float input = sinf(omega * t); - out = _notch_float.applyDF1(input); + out = _notch_float.apply(input); t = i * dt; // Let some time for the filter to settle @@ -282,7 +282,7 @@ TEST_F(NotchFilterTest, filterVector3fDF1) const Vector3f input(sinf(omega(0) * t), sinf(omega(1) * t), sinf(omega(2) * t)); const Vector3f arg = omega * t + phase_delay; const Vector3f output_expected(sinf(arg(0)), 0.f, sinf(arg(2))); - out = _notch_vector3f.applyDF1(input); + out = _notch_vector3f.apply(input); t = i * dt; // Let some time for the filter to settle diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index cfbcd39bfb..c87d51f9a7 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -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) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index bce91d9771..9a81bc1a02 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include #include @@ -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 _notch_filter_velocity[3] {}; + math::NotchFilter _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 _dynamic_notch_filter_esc_rpm[MAX_NUM_ESC_RPM][MAX_NUM_ESC_RPM_HARMONICS][3] {}; - math::NotchFilterArray _dynamic_notch_filter_fft[MAX_NUM_FFT_PEAKS][3] {}; + math::NotchFilter _dynamic_notch_filter_esc_rpm[MAX_NUM_ESC_RPM][MAX_NUM_ESC_RPM_HARMONICS][3] {}; + math::NotchFilter _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};