Dynamic Notch Filter Update
Improves performance of the dynamic notch filter, increasing peak accuracy over a wider band of frequencies, and generally using a narrower, higher notch. Details: - FFT now operates on gyro data *after* gyro notches and lowpasses - FFT bandpass Q changed from 0.707 to 0.05, to 'open up' the FFT to a greater range of incoming frequencies - FFT centre output now ranges from about 130 to 666Hz. - ignore the lowest couple of FFT bins going into centre frequency calculation - analyse FFT bins from low to high, keep ignoring bins until a bin is found that exceeds its previous bin by a factor of 2; then start examining bins from the bin before that (stops the FFT from being biased low, or going to the lowest value if there is no notch at all). - if no bin exceeds previous by more than 2 times, ie no obvious peak, smoothly go to maximum allowed notch frequency to avoid delay (might be better to bypass filter altogether?) - dominant bin emphasised by cubing bin height before calculating mean - maximum cutoff frequency is half the highest allowable centre frequency - default notch width is +/-25% of centre, narrower than before most of the time - code tidied up - thanks to rav, Flint, UAV Tech, icr4sh, diehertz and everyone else who helped with this.
This commit is contained in:
ctzsnooze
parent
039c74c896
commit
ad9197ca6a
|
|
@ -520,6 +520,7 @@ bool gyroInit(void)
|
|||
|
||||
switch (debugMode) {
|
||||
case DEBUG_FFT:
|
||||
case DEBUG_FFT_FREQ:
|
||||
case DEBUG_GYRO_RAW:
|
||||
case DEBUG_GYRO_SCALED:
|
||||
case DEBUG_GYRO_FILTERED:
|
||||
|
|
@ -1042,12 +1043,6 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
|
|||
return;
|
||||
}
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
if (isDynamicFilterActive()) {
|
||||
gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn);
|
||||
}
|
||||
#endif
|
||||
|
||||
const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs;
|
||||
accumulationLastTimeSampledUs = currentTimeUs;
|
||||
accumulatedMeasurementTimeUs += sampleDeltaUs;
|
||||
|
|
@ -1069,6 +1064,11 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
|
|||
} else {
|
||||
filterGyroDebug(gyroSensor, sampleDeltaUs);
|
||||
}
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
if (isDynamicFilterActive()) {
|
||||
gyroDataAnalyse(gyroSensor->notchFilterDyn);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
|
||||
|
|
|
|||
|
|
@ -7,11 +7,18 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe
|
|||
// DEBUG_GYRO_SCALED records the unfiltered, scaled gyro output
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroADCf));
|
||||
|
||||
// apply static notch filters and software lowpass filters
|
||||
gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
|
||||
gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
|
||||
gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf);
|
||||
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
// apply dynamic notch filter
|
||||
if (isDynamicFilterActive()) {
|
||||
gyroDataAnalysePush(axis, gyroADCf);
|
||||
if (axis == X) {
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); // store raw data
|
||||
}
|
||||
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
|
||||
if (axis == X) {
|
||||
|
|
@ -19,11 +26,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe
|
|||
}
|
||||
}
|
||||
#endif
|
||||
// apply static notch filters and software lowpass filters
|
||||
gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
|
||||
gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
|
||||
gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf);
|
||||
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
|
||||
|
||||
// DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));
|
||||
|
||||
|
|
|
|||
|
|
@ -18,6 +18,11 @@
|
|||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/* original work by Rav
|
||||
* 2018_07 updated by ctzsnooze to post filter, wider Q, different peak detection
|
||||
* coding assistance and advice from DieHertz, Rav, eTracer
|
||||
* test pilots icr4sh, UAV Tech, Flint723
|
||||
*/
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
|
@ -42,22 +47,23 @@
|
|||
// A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each with a width 31.25Hz
|
||||
// Eg [0,31), [31,62), [62, 93) etc
|
||||
|
||||
#define FFT_WINDOW_SIZE 32 // max for f3 targets
|
||||
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2)
|
||||
#define FFT_MIN_FREQ 100 // not interested in filtering frequencies below 100Hz
|
||||
#define FFT_SAMPLING_RATE 1000 // allows analysis up to 500Hz which is more than motors create
|
||||
#define FFT_MAX_FREQUENCY (FFT_SAMPLING_RATE / 2) // nyquist rate
|
||||
#define FFT_BPF_HZ 200 // use a bandpass on gyro data to ignore extreme low and extreme high frequencies
|
||||
#define FFT_RESOLUTION ((float)FFT_SAMPLING_RATE / FFT_WINDOW_SIZE) // hz per bin
|
||||
#define DYN_NOTCH_WIDTH 100 // just an orientation and start value
|
||||
#define DYN_NOTCH_CHANGERATE 60 // lower cut does not improve the performance much, higher cut makes it worse...
|
||||
#define DYN_NOTCH_MIN_CUTOFF 120 // don't cut too deep into low frequencies
|
||||
#define DYN_NOTCH_MAX_CUTOFF 200 // don't go above this cutoff (better filtering with "constant" delay at higher center frequencies)
|
||||
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) // we need 4 steps for each axis
|
||||
#define FFT_WINDOW_SIZE 32 // max for f3 targets
|
||||
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2)
|
||||
#define FFT_BIN_OFFSET 1 // compare 1 + this offset FFT bins for peak, ie if 1 start 2.5 * 41.655 or about 104Hz
|
||||
#define FFT_SAMPLING_RATE_HZ 1333 // analyse up to 666Hz, 16 bins each 41.655z wide
|
||||
#define FFT_BPF_HZ (FFT_SAMPLING_RATE_HZ / 4) // centre frequency of bandpass that constrains input to FFT
|
||||
#define FFT_BIQUAD_Q 0.05f // bandpass quality factor, 0.1 for steep sided bandpass
|
||||
#define FFT_RESOLUTION ((float)FFT_SAMPLING_RATE_HZ / FFT_WINDOW_SIZE) // hz per bin
|
||||
#define FFT_MIN_BIN_RISE 2 // following bin must be at least 2 times previous to indicate start of peak
|
||||
#define DYN_NOTCH_SMOOTH_FREQ_HZ 60 // lowpass frequency for smoothing notch centre point
|
||||
#define DYN_NOTCH_MIN_CENTRE_HZ 125 // notch centre point will not go below this, must be greater than cutoff, mid of bottom bin
|
||||
#define DYN_NOTCH_MAX_CENTRE_HZ (FFT_SAMPLING_RATE_HZ / 2) // maximum notch centre frequency limited by nyquist
|
||||
#define DYN_NOTCH_WIDTH_PERCENT 25 // maybe adjustable via CLI?
|
||||
#define DYN_NOTCH_CUTOFF ((float)(100 - DYN_NOTCH_WIDTH_PERCENT) / 100)
|
||||
#define DYN_NOTCH_MIN_CUTOFF_HZ 105 // lowest allowed notch cutoff frequency
|
||||
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) // we need 4 steps for each axis
|
||||
|
||||
#define BIQUAD_Q 1.0f / sqrtf(2.0f) // quality factor - butterworth
|
||||
|
||||
static FAST_RAM_ZERO_INIT uint16_t fftSamplingScale;
|
||||
static FAST_RAM_ZERO_INIT uint16_t fftSamplingCount;
|
||||
|
||||
// gyro data used for frequency analysis
|
||||
static float FAST_RAM_ZERO_INIT gyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE];
|
||||
|
|
@ -99,7 +105,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
|
|||
{
|
||||
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
|
||||
const uint16_t samplingFrequency = 1000000 / targetLooptimeUs;
|
||||
fftSamplingScale = samplingFrequency / FFT_SAMPLING_RATE;
|
||||
fftSamplingCount = samplingFrequency / FFT_SAMPLING_RATE_HZ;
|
||||
arm_rfft_fast_init_f32(&fftInstance, FFT_WINDOW_SIZE);
|
||||
|
||||
initGyroData();
|
||||
|
|
@ -108,11 +114,11 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
|
|||
// recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls
|
||||
// at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms
|
||||
// for gyro rate > 16kHz, we have update frequency of 1kHz => 1ms
|
||||
const float looptime = MAX(1000000u / FFT_SAMPLING_RATE, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
|
||||
const float looptime = MAX(1000000u / FFT_SAMPLING_RATE_HZ, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
fftResult[axis].centerFreq = 200; // any init value
|
||||
biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_CHANGERATE, looptime);
|
||||
biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE, BIQUAD_Q, FILTER_BPF);
|
||||
biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
|
||||
biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE_HZ, FFT_BIQUAD_Q, FILTER_BPF);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -122,34 +128,36 @@ const gyroFftData_t *gyroFftData(int axis)
|
|||
return &fftResult[axis];
|
||||
}
|
||||
|
||||
static FAST_RAM_ZERO_INIT float fftAcc[XYZ_AXIS_COUNT];
|
||||
void gyroDataAnalysePush(const int axis, const float sample)
|
||||
{
|
||||
fftAcc[axis] += sample;
|
||||
}
|
||||
|
||||
/*
|
||||
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
|
||||
*/
|
||||
void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn)
|
||||
void gyroDataAnalyse(biquadFilter_t *notchFilterDyn)
|
||||
{
|
||||
// accumulator for oversampled data => no aliasing and less noise
|
||||
static FAST_RAM_ZERO_INIT float fftAcc[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT uint32_t fftAccCount;
|
||||
|
||||
static FAST_RAM_ZERO_INIT uint32_t gyroDataAnalyseUpdateTicks;
|
||||
|
||||
// if gyro sampling is > 1kHz, accumulate multiple samples
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
fftAcc[axis] += gyroDev->gyroADC[axis];
|
||||
}
|
||||
// samples should have been pushed by `gyroDataAnalysePush`
|
||||
// if gyro sampling is > 1kHz, accumulate multiple samples
|
||||
fftAccCount++;
|
||||
|
||||
// this runs at 1kHz
|
||||
if (fftAccCount == fftSamplingScale) {
|
||||
if (fftAccCount == fftSamplingCount) {
|
||||
fftAccCount = 0;
|
||||
|
||||
//calculate mean value of accumulated samples
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
float sample = fftAcc[axis] / fftSamplingScale;
|
||||
float sample = fftAcc[axis] / fftSamplingCount;
|
||||
sample = biquadFilterApply(&fftGyroFilter[axis], sample);
|
||||
gyroData[axis][fftIdx] = sample;
|
||||
if (axis == 0)
|
||||
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample * gyroDev->scale));
|
||||
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
|
||||
fftAcc[axis] = 0;
|
||||
}
|
||||
|
||||
|
|
@ -244,37 +252,42 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
|
|||
case STEP_CALC_FREQUENCIES:
|
||||
{
|
||||
// 13us
|
||||
// calculate FFT centreFreq
|
||||
float fftSum = 0;
|
||||
float fftWeightedSum = 0;
|
||||
|
||||
fftResult[axis].maxVal = 0;
|
||||
bool fftIncreasing = false;
|
||||
float cubedData;
|
||||
// iterate over fft data and calculate weighted indexes
|
||||
float squaredData;
|
||||
for (int i = 0; i < FFT_BIN_COUNT; i++) {
|
||||
squaredData = fftData[i] * fftData[i]; //more weight on higher peaks
|
||||
fftResult[axis].maxVal = MAX(fftResult[axis].maxVal, squaredData);
|
||||
fftSum += squaredData;
|
||||
fftWeightedSum += squaredData * (i + 1); // calculate weighted index starting at 1, not 0
|
||||
}
|
||||
|
||||
// get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
|
||||
if (fftSum > 0) {
|
||||
// idx was shifted by 1 to start at 1, not 0
|
||||
float fftMeanIndex = (fftWeightedSum / fftSum) - 1;
|
||||
// the index points at the center frequency of each bin so index 0 is actually 16.125Hz
|
||||
// fftMeanIndex += 0.5;
|
||||
|
||||
// don't go below the minimal cutoff frequency + 10 and don't jump around too much
|
||||
float centerFreq;
|
||||
centerFreq = constrain(fftMeanIndex * FFT_RESOLUTION, DYN_NOTCH_MIN_CUTOFF + 10, FFT_MAX_FREQUENCY);
|
||||
centerFreq = biquadFilterApply(&fftFreqFilter[axis], centerFreq);
|
||||
centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CUTOFF + 10, FFT_MAX_FREQUENCY);
|
||||
fftResult[axis].centerFreq = centerFreq;
|
||||
if (axis == 0) {
|
||||
DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
|
||||
for (int i = 1 + FFT_BIN_OFFSET; i < FFT_BIN_COUNT; i++) {
|
||||
if (!fftIncreasing && (fftData[i] < fftData[i-1] * FFT_MIN_BIN_RISE)) {
|
||||
// do nothing unless has increased at some point
|
||||
} else {
|
||||
cubedData = fftData[i] * fftData[i] * fftData[i]; //more weight on higher peaks
|
||||
if (!fftIncreasing){
|
||||
cubedData += fftData[i-1] * fftData[i-1] * fftData[i-1]; //add previous bin before first rise
|
||||
}
|
||||
fftSum += cubedData;
|
||||
fftWeightedSum += cubedData * (i + 1); // calculate weighted index starting at 1, not 0
|
||||
fftIncreasing = true;
|
||||
}
|
||||
}
|
||||
|
||||
// get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
|
||||
float centerFreq;
|
||||
float fftMeanIndex;
|
||||
if (fftSum > 0) {
|
||||
// idx was shifted by 1 to start at 1, not 0
|
||||
fftMeanIndex = (fftWeightedSum / fftSum) - 1;
|
||||
// the index points at the center frequency of each bin so index 0 is actually 16.125Hz
|
||||
centerFreq = constrain(fftMeanIndex * FFT_RESOLUTION, DYN_NOTCH_MIN_CENTRE_HZ, DYN_NOTCH_MAX_CENTRE_HZ);
|
||||
} else {
|
||||
centerFreq = DYN_NOTCH_MAX_CENTRE_HZ; // if no peak, go to highest point to minimise delay
|
||||
}
|
||||
centerFreq = biquadFilterApply(&fftFreqFilter[axis], centerFreq);
|
||||
fftResult[axis].centerFreq = centerFreq;
|
||||
if (axis == 0) {
|
||||
DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
|
||||
}
|
||||
DEBUG_SET(DEBUG_FFT_FREQ, axis, fftResult[axis].centerFreq);
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
break;
|
||||
|
|
@ -282,8 +295,8 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
|
|||
case STEP_UPDATE_FILTERS:
|
||||
{
|
||||
// 7us
|
||||
// calculate new filter coefficients
|
||||
float cutoffFreq = constrain(fftResult[axis].centerFreq - DYN_NOTCH_WIDTH, DYN_NOTCH_MIN_CUTOFF, DYN_NOTCH_MAX_CUTOFF);
|
||||
// calculate cutoffFreq and notch Q, update notch filter
|
||||
float cutoffFreq = fmax(fftResult[axis].centerFreq * DYN_NOTCH_CUTOFF, DYN_NOTCH_MIN_CUTOFF_HZ);
|
||||
float notchQ = filterGetNotchQ(fftResult[axis].centerFreq, cutoffFreq);
|
||||
biquadFilterUpdate(¬chFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH);
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
|
|
|
|||
|
|
@ -31,5 +31,6 @@ typedef struct gyroFftData_s {
|
|||
void gyroDataAnalyseInit(uint32_t targetLooptime);
|
||||
const gyroFftData_t *gyroFftData(int axis);
|
||||
struct gyroDev_s;
|
||||
void gyroDataAnalyse(const struct gyroDev_s *gyroDev, biquadFilter_t *notchFilterDyn);
|
||||
void gyroDataAnalysePush(int axis, float sample);
|
||||
void gyroDataAnalyse(biquadFilter_t *notchFilterDyn);
|
||||
void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn);
|
||||
|
|
|
|||
Loading…
Reference in New Issue