366bc06089
This delegates updates to a bank of NotchFilters located at an rpm frequency and harmonics. Center frequency can be updated dynamically. Notch parameters are configurable, including the number of harmonics to filter on. Updates to the filter parameters are optimized across the notch bank. Convert notch bandwidth and frequency to floats. allow all filter harmonics to be controlled. add destructor to harmonic notch. don't allocate sub-filters for harmonic notch if no harmonics set.
71 lines
2.1 KiB
C++
71 lines
2.1 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
#pragma once
|
|
|
|
/*
|
|
notch filter with settable sample rate, center frequency, bandwidth and attenuation
|
|
|
|
Design by Leonard Hall
|
|
*/
|
|
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <cmath>
|
|
#include <inttypes.h>
|
|
#include <AP_Param/AP_Param.h>
|
|
|
|
|
|
template <class T>
|
|
class NotchFilter {
|
|
public:
|
|
// set parameters
|
|
void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB);
|
|
void init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q);
|
|
T apply(const T &sample);
|
|
void reset();
|
|
|
|
// calculate attenuation and quality from provided center frequency and bandwidth
|
|
static void calculate_A_and_Q(float center_freq_hz, float bandwidth_hz, float attenuation_dB, float& A, float& Q);
|
|
|
|
private:
|
|
|
|
bool initialised;
|
|
float b0, b1, b2, a1, a2, a0_inv;
|
|
T ntchsig, ntchsig1, ntchsig2, signal2, signal1;
|
|
};
|
|
|
|
/*
|
|
notch filter enable and filter parameters
|
|
*/
|
|
class NotchFilterParams {
|
|
public:
|
|
NotchFilterParams(void);
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
float center_freq_hz(void) const { return _center_freq_hz; }
|
|
float bandwidth_hz(void) const { return _bandwidth_hz; }
|
|
float attenuation_dB(void) const { return _attenuation_dB; }
|
|
uint8_t enabled(void) const { return _enable; }
|
|
|
|
protected:
|
|
AP_Int8 _enable;
|
|
AP_Float _center_freq_hz;
|
|
AP_Float _bandwidth_hz;
|
|
AP_Float _attenuation_dB;
|
|
};
|
|
|
|
typedef NotchFilter<float> NotchFilterFloat;
|
|
typedef NotchFilter<Vector3f> NotchFilterVector3f;
|
|
|