Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

Filter: fixed uint8_t bug in total filters and expand_filter_count bug

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