Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Filter: adjust attenuation properly at low frequencies 

use sqrt() adjustment from Leonard
This commit is contained in:
Andrew Tridgell 2023-11-06 18:24:43 +11:00
parent 08ccead215
commit 8b9fe4d21d
5 changed files with 73 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/Filter/Filter.h
View File

@ -9,3 +9,12 @@
#include "LowPassFilter.h" #include "LowPassFilter.h"
#include "ModeFilter.h" #include "ModeFilter.h"
#include "Butter.h" #include "Butter.h"
/*
the filter version is logged in the VER message to assist the online
analysis tools, so they can display the right filter formulas for
this version of the code
This should be incremented on significant filtering changes
*/
#define AP_FILTER_VERSION 2

103
libraries/Filter/HarmonicNotchFilter.cpp
View File

@ -34,6 +34,11 @@
#define NOTCH_DEBUG_LOGGING 0 #define NOTCH_DEBUG_LOGGING 0
#endif #endif
/*
point at which the harmonic notch goes to zero attenuation
*/
#define NOTCHFILTER_ATTENUATION_CUTOFF 0.25
// table of user settable parameters // table of user settable parameters
const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = {
@ -141,8 +146,11 @@ HarmonicNotchFilter<T>::~HarmonicNotchFilter() {
the constraints are used to determine attenuation (A) and quality (Q) factors for the filter the constraints are used to determine attenuation (A) and quality (Q) factors for the filter
*/ */
template <class T> template <class T>
void HarmonicNotchFilter<T>::init(float sample_freq_hz, const HarmonicNotchFilterParams &params) void HarmonicNotchFilter<T>::init(float sample_freq_hz, HarmonicNotchFilterParams &_params)
{ {
// keep a copy of the params object
params = &_params;
// sanity check the input // sanity check the input
if (_filters == nullptr || is_zero(sample_freq_hz) || isnan(sample_freq_hz)) { if (_filters == nullptr || is_zero(sample_freq_hz) || isnan(sample_freq_hz)) {
return; return;
@ -150,15 +158,15 @@ void HarmonicNotchFilter<T>::init(float sample_freq_hz, const HarmonicNotchFilte
_sample_freq_hz = sample_freq_hz; _sample_freq_hz = sample_freq_hz;
const float bandwidth_hz = params.bandwidth_hz(); const float bandwidth_hz = params->bandwidth_hz();
const float attenuation_dB = params.attenuation_dB(); const float attenuation_dB = params->attenuation_dB();
float center_freq_hz = params.center_freq_hz(); float center_freq_hz = params->center_freq_hz();
const float nyquist_limit = sample_freq_hz * 0.48f; const float nyquist_limit = sample_freq_hz * 0.48f;
const float bandwidth_limit = bandwidth_hz * 0.52f; const float bandwidth_limit = bandwidth_hz * 0.52f;
// remember the lowest frequency we will have a notch enabled at // remember the lowest frequency we will have a notch enabled at
_minimum_freq = center_freq_hz * params.freq_min_ratio(); _minimum_freq = center_freq_hz * params->freq_min_ratio();
/* /*
adjust the fundamental center frequency we use for the initial adjust the fundamental center frequency we use for the initial
@ -173,8 +181,6 @@ void HarmonicNotchFilter<T>::init(float sample_freq_hz, const HarmonicNotchFilte
// calculate attenuation and quality from the shaping constraints // calculate attenuation and quality from the shaping constraints
NotchFilter<T>::calculate_A_and_Q(center_freq_hz, bandwidth_hz / _composite_notches, attenuation_dB, _A, _Q); NotchFilter<T>::calculate_A_and_Q(center_freq_hz, bandwidth_hz / _composite_notches, attenuation_dB, _A, _Q);
_treat_low_freq_as_min = params.hasOption(HarmonicNotchFilterParams::Options::TreatLowAsMin);
_initialised = true; _initialised = true;
} }
@ -229,7 +235,11 @@ void HarmonicNotchFilter<T>::expand_filter_count(uint16_t total_notches)
} }
/* /*
set the center frequency of a single notch set the center frequency of a single notch harmonic
The spread_mul is the frequency multiplier from the spread of the
double or triple notch. The harmonic_mul is the multiplier for the
frequency for this harmonic
*/ */
template <class T> template <class T>
void HarmonicNotchFilter<T>::set_center_frequency(uint16_t idx, float notch_center, float spread_mul, uint8_t harmonic_mul) void HarmonicNotchFilter<T>::set_center_frequency(uint16_t idx, float notch_center, float spread_mul, uint8_t harmonic_mul)
@ -252,44 +262,53 @@ void HarmonicNotchFilter<T>::set_center_frequency(uint16_t idx, float notch_cent
return; return;
} }
// the minimum frequency for a harmonic is the base minimum frequency // the minimum frequency for a harmonic is the base minimum
// scaled for the harmonic multiplier float harmonic_min_freq = _minimum_freq;
const float harmonic_min_freq = _minimum_freq * harmonic_mul;
// on some vehicles we can adjust the attenuation at low frequencies // we can adjust the attenuation at low frequencies
float A = _A; float A = _A;
// on some vehicles we want to treat zero or very low frequency as // on some vehicles we want to treat zero or very low frequency as
// the minimum frequency, on others we want to disable the // the minimum frequency, on others we want to disable the
// notch. That is controlled by the _treat_low_freq_as_min flag // notch for low frequencies
if (!_treat_low_freq_as_min) { const bool treat_low_freq_as_min = params->hasOption(HarmonicNotchFilterParams::Options::TreatLowAsMin);
if (treat_low_freq_as_min) {
/* /*
disable if the notch is less than half of the min frequency when we are treating low as min we don't want to collapse
the harmonics down to low frequencies
*/
harmonic_min_freq *= harmonic_mul;
} else {
/*
disable if the notch is less than 25% of the min frequency
*/ */
const float disable_threshold = 0.5; const float disable_freq = harmonic_min_freq * NOTCHFILTER_ATTENUATION_CUTOFF;
if (notch_center < disable_threshold * _minimum_freq) { if (notch_center < disable_freq) {
notch.disable(); notch.disable();
return; return;
} }
if (notch_center < _minimum_freq) { if (notch_center < harmonic_min_freq) {
/* /*
scale the attenuation so that we fade out the notch as scale the attenuation so that we fade out the notch as
we get further below the min frequency we get further below the min frequency. The attenuation
factor A goes to 1.0 (which means no attenuation)
TODO: determine if this linear scaling is the right Scaling the attenuation in this way means we don't get a
function. Very likely we will need something more complex glitch at the disable point
*/ */
A = linear_interpolate(1.0, _A, A = linear_interpolate(A, 1.0, notch_center, harmonic_min_freq, disable_freq);
notch_center,
disable_threshold * _minimum_freq,
_minimum_freq);
} }
} }
// never run the center of a notch at a lower frequency than the // don't let the notch go below the min frequency
// minimum specified in the parameters. notch_center = MAX(notch_center, harmonic_min_freq);
notch_center = MAX(notch_center, harmonic_min_freq) * spread_mul;
/* adjust notch center for spread for double and triple notch.
This adjustment is applied last to maintain the symmetry of the
double and triple notches
*/
notch_center *= spread_mul;
notch.init_with_A_and_Q(_sample_freq_hz, notch_center, A, _Q); notch.init_with_A_and_Q(_sample_freq_hz, notch_center, A, _Q);
} }
@ -301,29 +320,7 @@ void HarmonicNotchFilter<T>::set_center_frequency(uint16_t idx, float notch_cent
template <class T> template <class T>
void HarmonicNotchFilter<T>::update(float center_freq_hz) void HarmonicNotchFilter<T>::update(float center_freq_hz)
{ {
if (!_initialised) { update(1, &center_freq_hz);
return;
}
// adjust the fundamental center frequency to be in the allowable range
const float nyquist_limit = _sample_freq_hz * 0.48f;
center_freq_hz = constrain_float(center_freq_hz, 0.0f, nyquist_limit);
_num_enabled_filters = 0;
// update all of the filters using the new center frequency and existing A & Q
for (uint16_t i = 0; i < HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) {
if ((1U<<i) & _harmonics) {
const float notch_center = center_freq_hz;
const uint8_t harmonic_mul = i+1;
if (_composite_notches != 2) {
set_center_frequency(_num_enabled_filters++, notch_center, 1.0, harmonic_mul);
}
if (_composite_notches > 1) {
set_center_frequency(_num_enabled_filters++, notch_center, 1.0 - _notch_spread, harmonic_mul);
set_center_frequency(_num_enabled_filters++, notch_center, 1.0 + _notch_spread, harmonic_mul);
}
}
}
} }
/* /*
@ -358,7 +355,7 @@ void HarmonicNotchFilter<T>::update(uint8_t num_centers, const float center_freq
continue; continue;
} }
const float notch_center = constrain_float(center_freq_hz[center_n] * (harmonic_n+1), 0.0f, nyquist_limit); const float notch_center = constrain_float(center_freq_hz[center_n], 0.0f, nyquist_limit);
const uint8_t harmonic_mul = (harmonic_n+1); const uint8_t harmonic_mul = (harmonic_n+1);
if (_composite_notches != 2) { if (_composite_notches != 2) {
set_center_frequency(_num_enabled_filters++, notch_center, 1.0, harmonic_mul); set_center_frequency(_num_enabled_filters++, notch_center, 1.0, harmonic_mul);

13
libraries/Filter/HarmonicNotchFilter.h
View File

@ -36,7 +36,7 @@ public:
// expand filter bank with new filters // expand filter bank with new filters
void expand_filter_count(uint16_t total_notches); void expand_filter_count(uint16_t total_notches);
// initialize the underlying filters using the provided filter parameters // initialize the underlying filters using the provided filter parameters
void init(float sample_freq_hz, const HarmonicNotchFilterParams &params); void init(float sample_freq_hz, HarmonicNotchFilterParams &params);
// update the underlying filters' center frequencies using center_freq_hz as the fundamental // update the underlying filters' center frequencies using center_freq_hz as the fundamental
void update(float center_freq_hz); void update(float center_freq_hz);
// update all of the underlying center frequencies individually // update all of the underlying center frequencies individually
@ -83,10 +83,8 @@ private:
// minimum frequency (from INS_HNTCH_FREQ * INS_HNTCH_FM_RAT) // minimum frequency (from INS_HNTCH_FREQ * INS_HNTCH_FM_RAT)
float _minimum_freq; float _minimum_freq;
/* // pointer to params object for this filter
flag for treating a very low frequency as the min frequency HarmonicNotchFilterParams *params;
*/
bool _treat_low_freq_as_min;
}; };
// Harmonic notch update mode // Harmonic notch update mode
@ -157,6 +155,11 @@ public:
// save parameters // save parameters
void save_params(); void save_params();
// return the number of composite notches given the options
uint8_t num_composite_notches(void) const {
return hasOption(Options::DoubleNotch) ? 2 : hasOption(Options::TripleNotch) ? 3: 1;
}
private: private:
// configured notch harmonics // configured notch harmonics
AP_Int32 _harmonics; AP_Int32 _harmonics;

6
libraries/Filter/NotchFilter.cpp
View File

@ -59,7 +59,10 @@ template <class T>
void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q) void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q)
{ {
// don't update if no updates required // don't update if no updates required
if (initialised && is_equal(center_freq_hz, _center_freq_hz) && is_equal(sample_freq_hz, _sample_freq_hz)) { if (initialised &&
is_equal(center_freq_hz, _center_freq_hz) &&
is_equal(sample_freq_hz, _sample_freq_hz) &&
is_equal(A, _A)) {
return; return;
} }
@ -91,6 +94,7 @@ void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_h
_center_freq_hz = new_center_freq; _center_freq_hz = new_center_freq;
_sample_freq_hz = sample_freq_hz; _sample_freq_hz = sample_freq_hz;
_A = A;
initialised = true; initialised = true;
} else { } else {
// leave center_freq_hz at last value // leave center_freq_hz at last value

2
libraries/Filter/NotchFilter.h
View File

@ -52,7 +52,7 @@ protected:
bool initialised, need_reset; bool initialised, need_reset;
float b0, b1, b2, a1, a2; float b0, b1, b2, a1, a2;
float _center_freq_hz, _sample_freq_hz; float _center_freq_hz, _sample_freq_hz, _A;
T ntchsig1, ntchsig2, signal2, signal1; T ntchsig1, ntchsig2, signal2, signal1;
}; };