aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Filter: Decouple notch filter configuration from the filter itself.

Browse Source 
Allow multiple notch filters to be instantiated using the same configuration.
This commit is contained in:
Andy Piper 2019-05-17 16:59:54 +01:00 committed by Andrew Tridgell
parent 18d403c3ac
commit 1a8475cf9f
2 changed files with 26 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/Filter/NotchFilter.cpp
View File

@ -55,15 +55,22 @@ T NotchFilter<T>::apply(const T &sample)
return output;
}
template <class T>
void NotchFilter<T>::reset()
{
ntchsig2 = ntchsig1 = T();
signal2 = signal1 = T();
}
// table of user settable parameters
const AP_Param::GroupInfo NotchFilterVector3fParam::var_info[] = {
const AP_Param::GroupInfo NotchFilterParams::var_info[] = {
// @Param: ENABLE
// @DisplayName: Enable
// @Description: Enable notch filter
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO_FLAGS("ENABLE", 1, NotchFilterVector3fParam, enable, 0, AP_PARAM_FLAG_ENABLE),
AP_GROUPINFO_FLAGS("ENABLE", 1, NotchFilterParams, _enable, 0, AP_PARAM_FLAG_ENABLE),
// @Param: FREQ
// @DisplayName: Frequency
@ -71,7 +78,7 @@ const AP_Param::GroupInfo NotchFilterVector3fParam::var_info[] = {
// @Range: 10 200
// @Units: Hz
// @User: Advanced
AP_GROUPINFO("FREQ", 2, NotchFilterVector3fParam, center_freq_hz, 80),
AP_GROUPINFO("FREQ", 2, NotchFilterParams, _center_freq_hz, 80),
// @Param: BW
// @DisplayName: Bandwidth
@ -79,7 +86,7 @@ const AP_Param::GroupInfo NotchFilterVector3fParam::var_info[] = {
// @Range: 5 50
// @Units: Hz
// @User: Advanced
AP_GROUPINFO("BW", 3, NotchFilterVector3fParam, bandwidth_hz, 20),
AP_GROUPINFO("BW", 3, NotchFilterParams, _bandwidth_hz, 20),
// @Param: ATT
// @DisplayName: Attenuation
@ -87,7 +94,7 @@ const AP_Param::GroupInfo NotchFilterVector3fParam::var_info[] = {
// @Range: 5 30
// @Units: dB
// @User: Advanced
AP_GROUPINFO("ATT", 4, NotchFilterVector3fParam, attenuation_dB, 15),
AP_GROUPINFO("ATT", 4, NotchFilterParams, _attenuation_dB, 15),
AP_GROUPEND
};
@ -95,46 +102,11 @@ const AP_Param::GroupInfo NotchFilterVector3fParam::var_info[] = {
/*
a notch filter with enable and filter parameters - constructor
*/
NotchFilterVector3fParam::NotchFilterVector3fParam(void)
NotchFilterParams::NotchFilterParams(void)
{
AP_Param::setup_object_defaults(this, var_info);
}
/*
initialise filter
*/
void NotchFilterVector3fParam::init(float _sample_freq_hz)
{
filter.init(_sample_freq_hz, center_freq_hz, bandwidth_hz, attenuation_dB);
sample_freq_hz = _sample_freq_hz;
last_center_freq = center_freq_hz;
last_bandwidth = bandwidth_hz;
last_attenuation = attenuation_dB;
}
/*
apply a filter sample
*/
Vector3f NotchFilterVector3fParam::apply(const Vector3f &sample)
{
if (!enable) {
// when not enabled it is a simple pass-through
return sample;
}
// check for changed parameters
if (!is_equal(center_freq_hz.get(), last_center_freq) ||
!is_equal(bandwidth_hz.get(), last_bandwidth) ||
!is_equal(attenuation_dB.get(), last_attenuation)) {
if (!is_zero(sample_freq_hz)) {
init(sample_freq_hz);
}
}
return filter.apply(sample);
}
/*
instantiate template classes
*/

31
libraries/Filter/NotchFilter.h
View File

@ -32,6 +32,7 @@ public:
// set parameters
void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB);
T apply(const T &sample);
void reset();
private:
bool initialised;
@ -40,29 +41,23 @@ private:
};
/*
a notch filter with enable and filter parameters
notch filter enable and filter parameters
*/
class NotchFilterVector3fParam {
class NotchFilterParams {
public:
NotchFilterVector3fParam(void);
void init(float sample_freq_hz);
Vector3f apply(const Vector3f &sample);
NotchFilterParams(void);
static const struct AP_Param::GroupInfo var_info[];
uint16_t center_freq_hz(void) const { return _center_freq_hz; }
uint8_t bandwidth_hz(void) const { return _bandwidth_hz; }
float attenuation_dB(void) const { return _attenuation_dB; }
uint8_t enabled(void) const { return _enable; }
private:
AP_Int8 enable;
AP_Float center_freq_hz;
AP_Float bandwidth_hz;
AP_Float attenuation_dB;
float sample_freq_hz;
float last_center_freq;
float last_bandwidth;
float last_attenuation;
NotchFilter<Vector3f> filter;
AP_Int8 _enable;
AP_Int16 _center_freq_hz;
AP_Int8 _bandwidth_hz;
AP_Float _attenuation_dB;
};
typedef NotchFilter<float> NotchFilterFloat;