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

Filter: HarmonicNotch: convert harmonics to int32 param

Browse Source
This commit is contained in:
Iampete1 2023-07-14 18:35:01 +01:00 committed by Randy Mackay
parent fa0f089fe1
commit b68ece1007
2 changed files with 32 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/Filter/HarmonicNotchFilter.cpp
View File

@ -22,7 +22,6 @@
#include <GCS_MAVLink/GCS.h>
#define HNF_MAX_FILTERS HAL_HNF_MAX_FILTERS // must be even for double-notch filters
#define HNF_MAX_HARMONICS 8
// table of user settable parameters
const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = {
@ -61,7 +60,22 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = {
// @Param: HMNCS
// @DisplayName: Harmonic Notch Filter harmonics
// @Description: Bitmask of harmonic frequencies to apply Harmonic Notch Filter to. This option takes effect on the next reboot. A value of 0 disables this filter. The first harmonic refers to the base frequency.
// @Bitmask: 0:1st harmonic,1:2nd harmonic,2:3rd harmonic,3:4th hamronic,4:5th harmonic,5:6th harmonic,6:7th harmonic,7:8th harmonic
// @Bitmask: 0: 1st harmonic
// @Bitmask: 1: 2nd harmonic
// @Bitmask: 2: 3rd harmonic
// @Bitmask: 3: 4th harmonic
// @Bitmask: 4: 5th harmonic
// @Bitmask: 5: 6th harmonic
// @Bitmask: 6: 7th harmonic
// @Bitmask: 7: 8th harmonic
// @Bitmask: 8: 9th harmonic
// @Bitmask: 9: 10th harmonic
// @Bitmask: 10: 11th harmonic
// @Bitmask: 11: 12th harmonic
// @Bitmask: 12: 13th harmonic
// @Bitmask: 13: 14th harmonic
// @Bitmask: 14: 15th harmonic
// @Bitmask: 15: 16th harmonic
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("HMNCS", 5, HarmonicNotchFilterParams, _harmonics, 3),
@ -143,7 +157,7 @@ void HarmonicNotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, fl
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 num_notches, uint8_t harmonics, uint8_t composite_notches)
void HarmonicNotchFilter<T>::allocate_filters(uint8_t num_notches, uint32_t harmonics, uint8_t composite_notches)
{
_composite_notches = MIN(composite_notches, 3);
_num_harmonics = __builtin_popcount(harmonics);
@ -327,6 +341,11 @@ HarmonicNotchFilterParams::HarmonicNotchFilterParams(void)
AP_Param::setup_object_defaults(this, var_info);
}
void HarmonicNotchFilterParams::init()
{
_harmonics.convert_parameter_width(AP_PARAM_INT8);
}
/*
save changed parameters
*/

17
libraries/Filter/HarmonicNotchFilter.h
View File

@ -19,7 +19,7 @@
#include <AP_Param/AP_Param.h>
#include "NotchFilter.h"
#define HNF_MAX_HARMONICS 8
#define HNF_MAX_HARMONICS 16
/*
a filter that manages a set of notch filters targetted at a fundamental center frequency
@ -30,7 +30,7 @@ class HarmonicNotchFilter {
public:
~HarmonicNotchFilter();
// allocate a bank of notch filters for this harmonic notch filter
void allocate_filters(uint8_t num_notches, uint8_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
void expand_filter_count(uint8_t num_notches);
// initialize the underlying filters using the provided filter parameters
@ -56,7 +56,7 @@ private:
// quality factor of each filter
float _Q;
// a bitmask of the harmonics to use
uint8_t _harmonics;
uint32_t _harmonics;
// number of notches that make up a composite notch
uint8_t _composite_notches;
// number of allocated filters
@ -95,16 +95,19 @@ public:
};
HarmonicNotchFilterParams(void);
void init();
// set the fundamental center frequency of the harmonic notch
void set_center_freq_hz(float center_freq) { _center_freq_hz.set(center_freq); }
// set the bandwidth of the harmonic notch
void set_bandwidth_hz(float bandwidth_hz) { _bandwidth_hz.set(bandwidth_hz); }
// harmonics enabled on the harmonic notch
uint8_t harmonics(void) const { return _harmonics; }
uint32_t harmonics(void) const { return _harmonics; }
// set the harmonics value
void set_harmonics(uint8_t hmncs) { _harmonics.set(hmncs); }
void set_harmonics(uint32_t hmncs) { _harmonics.set(hmncs); }
// has the user set the harmonics value
void set_default_harmonics(uint8_t hmncs) { _harmonics.set_default(hmncs); }
void set_default_harmonics(uint32_t hmncs) { _harmonics.set_default(hmncs); }
// reference value of the harmonic notch
float reference(void) const { return _reference; }
void set_reference(float ref) { _reference.set(ref); }
@ -125,7 +128,7 @@ public:
private:
// configured notch harmonics
AP_Int8 _harmonics;
AP_Int32 _harmonics;
// notch reference value
AP_Float _reference;
// notch dynamic tracking mode