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

Filter: fixed uint8_t bug in total filters and expand_filter_count bug 

this gets the right number of notches on quadplanes, but is still very
bad in fwd flight
This commit is contained in:
Andrew Tridgell 2023-11-07 17:29:55 +11:00
parent 002f1076d7
commit 0d932e8a54
2 changed files with 12 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/Filter/HarmonicNotchFilter.cpp
View File

@ -189,13 +189,8 @@ void HarmonicNotchFilter<T>::allocate_filters(uint8_t num_notches, uint32_t harm
expand the number of filters at runtime, allowing for RPM sources such as lua scripts
*/
template <class T>
void HarmonicNotchFilter<T>::expand_filter_count(uint8_t num_notches)
void HarmonicNotchFilter<T>::expand_filter_count(uint16_t total_notches)
{
uint8_t num_filters = _num_harmonics * num_notches * _composite_notches;
if (num_filters <= _num_filters) {
// enough already
return;
}
if (_alloc_has_failed) {
// we've failed to allocate before, don't try again
return;
@ -204,7 +199,7 @@ void HarmonicNotchFilter<T>::expand_filter_count(uint8_t num_notches)
note that we rely on the semaphore in
AP_InertialSensor_Backend.cpp to make this thread safe
*/
auto filters = new NotchFilter<T>[num_filters];
auto filters = new NotchFilter<T>[total_notches];
if (filters == nullptr) {
_alloc_has_failed = true;
return;
@ -212,7 +207,7 @@ void HarmonicNotchFilter<T>::expand_filter_count(uint8_t num_notches)
memcpy(filters, _filters, sizeof(filters[0])*_num_filters);
auto _old_filters = _filters;
_filters = filters;
_num_filters = num_filters;
_num_filters = total_notches;
delete[] _old_filters;
}
@ -273,15 +268,16 @@ 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;
if (num_centers > _num_filters) {
uint16_t total_notches = num_centers * _num_harmonics * _composite_notches;
if (total_notches > _num_filters) {
// alloc realloc of filters
expand_filter_count(num_centers);
expand_filter_count(total_notches);
}
_num_enabled_filters = 0;
// update all of the filters using the new center frequencies and existing A & Q
for (uint8_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) {
for (uint16_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) {
const uint8_t harmonic_n = i / num_centers;
const uint8_t center_n = i % num_centers;
// the filters are ordered by center and then harmonic so
@ -331,7 +327,7 @@ T HarmonicNotchFilter<T>::apply(const T &sample)
#endif
T output = sample;
for (uint8_t i = 0; i < _num_enabled_filters; i++) {
for (uint16_t i = 0; i < _num_enabled_filters; i++) {
#if NOTCH_DEBUG_LOGGING
if (!_filters[i].initialised) {
::dprintf(dfd, "------- ");
@ -359,7 +355,7 @@ void HarmonicNotchFilter<T>::reset()
return;
}
for (uint8_t i = 0; i < _num_filters; i++) {
for (uint16_t i = 0; i < _num_filters; i++) {
_filters[i].reset();
}
}

6
libraries/Filter/HarmonicNotchFilter.h
View File

@ -32,7 +32,7 @@ public:
// allocate a bank of notch filters for this harmonic notch filter
void allocate_filters(uint8_t num_notches, uint32_t harmonics, uint8_t composite_notches);
// expand filter bank with new filters
void expand_filter_count(uint8_t num_notches);
void expand_filter_count(uint16_t total_notches);
// 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
@ -60,11 +60,11 @@ private:
// number of notches that make up a composite notch
uint8_t _composite_notches;
// number of allocated filters
uint8_t _num_filters;
uint16_t _num_filters;
// pre-calculated number of harmonics
uint8_t _num_harmonics;
// number of enabled filters
uint8_t _num_enabled_filters;
uint16_t _num_enabled_filters;
bool _initialised;
// have we failed to expand filters?