aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Filter: add support for double harmonic notches to more effectively target wider noise peaks

Browse Source
This commit is contained in:
Andy Piper 2019-12-27 12:06:21 +00:00 committed by Andrew Tridgell
parent cf6f5502a4
commit 1dbde3af09
2 changed files with 65 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
libraries/Filter/HarmonicNotchFilter.cpp
View File

@ -16,7 +16,8 @@
#include "HarmonicNotchFilter.h"
#include <GCS_MAVLink/GCS.h>
#define HNF_MAX_FILTERS 3
#define HNF_MAX_FILTERS 6 // must be even for double-notch filters
#define HNF_MAX_HARMONICS 8
// table of user settable parameters
const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = {
@ -76,6 +77,14 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = {
// @User: Advanced
AP_GROUPINFO("MODE", 7, HarmonicNotchFilterParams, _tracking_mode, 1),
// @Param: OPTS
// @DisplayName: Harmonic Notch Filter options
// @Description: Harmonic Notch Filter options.
// @Bitmask: 0:Double notch
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("OPTS", 8, HarmonicNotchFilterParams, _options, 0),
AP_GROUPEND
};
@ -102,39 +111,41 @@ void HarmonicNotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, fl
}
_sample_freq_hz = sample_freq_hz;
// Calculate spread required to achieve an equivalent single notch using two notches with Bandwidth/2
_notch_spread = bandwidth_hz / (32 * center_freq_hz);
const float nyquist_limit = sample_freq_hz * 0.48f;
const float bandwidth_limit = bandwidth_hz * 0.52f;
// adjust the fundamental center frequency to be in the allowable range
center_freq_hz = constrain_float(center_freq_hz, bandwidth_hz * 0.52f, nyquist_limit);
center_freq_hz = constrain_float(center_freq_hz, bandwidth_limit, nyquist_limit);
// calculate attenuation and quality from the shaping constraints
NotchFilter<T>::calculate_A_and_Q(center_freq_hz, bandwidth_hz, attenuation_dB, _A, _Q);
_num_enabled_filters = 0;
// initialize all the configured filters with the same A & Q and multiples of the center frequency
for (uint8_t i = 0, filt = 0; i < HNF_MAX_HARMONICS && filt < _num_filters; i++) {
const float notch_center = center_freq_hz * (i+1);
if ((1U<<i) & _harmonics) {
// only enable the filter if its center frequency is below the nyquist frequency
if (notch_center < nyquist_limit) {
_filters[filt].init_with_A_and_Q(sample_freq_hz, notch_center, _A, _Q);
_num_enabled_filters++;
}
filt++;
}
if (_double_notch) {
// position the individual notches so that the attenuation is no worse than a single notch
// calculate attenuation and quality from the shaping constraints
NotchFilter<T>::calculate_A_and_Q(center_freq_hz, bandwidth_hz * 0.5, attenuation_dB, _A, _Q);
} else {
// calculate attenuation and quality from the shaping constraints
NotchFilter<T>::calculate_A_and_Q(center_freq_hz, bandwidth_hz, attenuation_dB, _A, _Q);
}
_initialised = true;
update(center_freq_hz);
}
/*
allocate a collection of, at most HNF_MAX_FILTERS, notch filters to be managed by this harmonic notch filter
*/
template <class T>
void HarmonicNotchFilter<T>::allocate_filters(uint8_t harmonics)
void HarmonicNotchFilter<T>::allocate_filters(uint8_t harmonics, bool double_notch)
{
_double_notch = double_notch;
for (uint8_t i = 0; i < HNF_MAX_HARMONICS && _num_filters < HNF_MAX_FILTERS; i++) {
if ((1U<<i) & harmonics) {
_num_filters++;
if (_double_notch) {
_num_filters++;
}
}
}
if (_num_filters > 0) {
@ -166,12 +177,29 @@ void HarmonicNotchFilter<T>::update(float center_freq_hz)
_num_enabled_filters = 0;
// update all of the filters using the new center frequency and existing A & Q
for (uint8_t i = 0, filt = 0; i < HNF_MAX_HARMONICS && filt < _num_filters; i++) {
const float notch_center = center_freq_hz * (i+1);
if ((1U<<i) & _harmonics) {
// only enable the filter if its center frequency is below the nyquist frequency
if (notch_center < nyquist_limit) {
_filters[filt].init_with_A_and_Q(_sample_freq_hz, notch_center, _A, _Q);
_num_enabled_filters++;
const float notch_center = center_freq_hz * (i+1);
if (!_double_notch) {
// only enable the filter if its center frequency is below the nyquist frequency
if (notch_center < nyquist_limit) {
_filters[filt].init_with_A_and_Q(_sample_freq_hz, notch_center, _A, _Q);
_num_enabled_filters++;
}
} else {
float notch_center_double;
// only enable the filter if its center frequency is below the nyquist frequency
notch_center_double = notch_center * (1.0 - _notch_spread);
if (notch_center_double < nyquist_limit) {
_filters[filt].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q);
_num_enabled_filters++;
}
filt++;
// only enable the filter if its center frequency is below the nyquist frequency
notch_center_double = notch_center * (1.0 + _notch_spread);
if (notch_center_double < nyquist_limit) {
_filters[filt].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q);
_num_enabled_filters++;
}
}
filt++;
}

14
libraries/Filter/HarmonicNotchFilter.h
View File

@ -30,7 +30,7 @@ class HarmonicNotchFilter {
public:
~HarmonicNotchFilter();
// allocate a bank of notch filters for this harmonic notch filter
void allocate_filters(uint8_t harmonics);
void allocate_filters(uint8_t harmonics, bool double_notch);
// initialize the underlying filters using the provided filter parameters
void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB);
// update the underlying filters' center frequencies using center_freq_hz as the fundamental
@ -45,12 +45,16 @@ private:
NotchFilter<T>* _filters;
// sample frequency for each filter
float _sample_freq_hz;
// base double notch bandwidth for each filter
float _notch_spread;
// attenuation for each filter
float _A;
// quality factor of each filter
float _Q;
// a bitmask of the harmonics to use
uint8_t _harmonics;
// whether to use double-notches
bool _double_notch;
// number of allocated filters
uint8_t _num_filters;
// number of enabled filters
@ -72,6 +76,10 @@ enum class HarmonicNotchDynamicMode {
*/
class HarmonicNotchFilterParams : public NotchFilterParams {
public:
enum class Options {
DoubleNotch = 1<<0,
};
HarmonicNotchFilterParams(void);
// set the fundamental center frequency of the harmonic notch
void set_center_freq_hz(float center_freq) { _center_freq_hz.set(center_freq); }
@ -79,6 +87,8 @@ public:
uint8_t harmonics(void) const { return _harmonics; }
// reference value of the harmonic notch
float reference(void) const { return _reference; }
// notch options
bool hasOption(Options option) const { return _options & uint16_t(option); }
// notch dynamic tracking mode
HarmonicNotchDynamicMode tracking_mode(void) const { return HarmonicNotchDynamicMode(_tracking_mode.get()); }
static const struct AP_Param::GroupInfo var_info[];
@ -90,6 +100,8 @@ private:
AP_Float _reference;
// notch dynamic tracking mode
AP_Int8 _tracking_mode;
// notch options
AP_Int16 _options;
};
typedef HarmonicNotchFilter<Vector3f> HarmonicNotchFilterVector3f;