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

Filter: use a define for nyquist cutoff

This commit is contained in:
Andrew Tridgell 2024-03-12 07:30:56 +11:00
parent 90c8359ff6
commit 148c176a59
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/Filter/HarmonicNotchFilter.cpp
View File

@ -38,6 +38,11 @@
#include <stdio.h>
#endif
/*
cutoff proportion of sample rate above which we do not use the notch
*/
#define HARMONIC_NYQUIST_CUTOFF 0.48f
/*
point at which the harmonic notch goes to zero attenuation
*/
@ -166,7 +171,7 @@ void HarmonicNotchFilter<T>::init(float sample_freq_hz, HarmonicNotchFilterParam
const float attenuation_dB = params->attenuation_dB();
float center_freq_hz = params->center_freq_hz();
const float nyquist_limit = sample_freq_hz * 0.48f;
const float nyquist_limit = sample_freq_hz * HARMONIC_NYQUIST_CUTOFF;
const float bandwidth_limit = bandwidth_hz * 0.52f;
// remember the lowest frequency we will have a notch enabled at
@ -248,7 +253,7 @@ void HarmonicNotchFilter<T>::expand_filter_count(uint16_t total_notches)
template <class T>
void HarmonicNotchFilter<T>::set_center_frequency(uint16_t idx, float notch_center, float spread_mul, uint8_t harmonic_mul)
{
const float nyquist_limit = _sample_freq_hz * 0.48f;
const float nyquist_limit = _sample_freq_hz * HARMONIC_NYQUIST_CUTOFF;
auto &notch = _filters[idx];
// scale the notch with the harmonic multiplier
@ -339,7 +344,7 @@ void HarmonicNotchFilter<T>::update(uint8_t num_centers, const float center_freq
}
// adjust the frequencies to be in the allowable range
const float nyquist_limit = _sample_freq_hz * 0.48f;
const float nyquist_limit = _sample_freq_hz * HARMONIC_NYQUIST_CUTOFF;
const uint16_t total_notches = num_centers * _num_harmonics * _composite_notches;
if (total_notches > _num_filters) {