Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 13:38:38 -04:00
Code Issues Packages Projects Releases Wiki Activity

Filter: fixed filter expansion for other than motors based RPM sources

Browse Source 
this also fixes the uint8_t storage of a number than can be greater
than 256. Max total notches in a single HarmonicNotchFilter is
currently 12*16*3 for 12 ESCs, with INS_HNTCH_HMNCS=0xFFFF and triple
notch
This commit is contained in:
Andrew Tridgell 2023-11-06 16:51:02 +11:00
parent 304890fc7d
commit ae75ea94eb
2 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/Filter/HarmonicNotchFilter.cpp
View File

@ -204,6 +204,10 @@ void HarmonicNotchFilter<T>::allocate_filters(uint8_t num_notches, uint32_t harm
template <class T>
void HarmonicNotchFilter<T>::expand_filter_count(uint16_t total_notches)
{
if (total_notches <= _num_filters) {
// enough already
return;
}
if (_alloc_has_failed) {
// we've failed to allocate before, don't try again
return;
@ -228,7 +232,7 @@ void HarmonicNotchFilter<T>::expand_filter_count(uint16_t total_notches)
set the center frequency of a single notch
*/
template <class T>
void HarmonicNotchFilter<T>::set_center_frequency(uint8_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)
{
const float nyquist_limit = _sample_freq_hz * 0.48f;
auto &notch = _filters[idx];
@ -307,7 +311,7 @@ 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; i < HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) {
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;
@ -336,7 +340,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;
uint16_t total_notches = num_centers * _num_harmonics * _composite_notches;
const uint16_t total_notches = num_centers * _num_harmonics * _composite_notches;
if (total_notches > _num_filters) {
// alloc realloc of filters
expand_filter_count(total_notches);

2
libraries/Filter/HarmonicNotchFilter.h
View File

@ -47,7 +47,7 @@ public:
spread_mul is a scale factor for spreading of double or triple notch
harmonic_mul is the multiplier for harmonics, 1 is for the fundamental
*/
void set_center_frequency(uint8_t idx, float center_freq_hz, float spread_mul, uint8_t harmonic_mul);
void set_center_frequency(uint16_t idx, float center_freq_hz, float spread_mul, uint8_t harmonic_mul);
// apply a sample to each of the underlying filters in turn
T apply(const T &sample);