Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +03:00
Code Issues Wiki Activity

Dynamic Filter Update

Browse source 
Improves on earlier 3.5RC1 dynamic filter with:
- better FFT tracking performance overall, even with a narrower default notch width
- can reach 850Hz for high kV 2.5-3" and 6S quads
- works better with 32k gyros
- can be applied pre- (location 1) and post- (location 2) static filters. Pre-static filter location works best, but post-static may work well in 32k modes or with PT1 option
- option to use a PT1 filter rather than the classical notch filter, perhaps useful for quads with a lot of noise above their peak.
- ability to totally bypass the pre-FFT bandpass filter, by setting Q=0, maximising the range of responsiveness
- "ignore" value, absolute FFT bin amplitude below which the bin will be excluded from consideration. May be tweaked to optimise peak detection; primarily for advanced tuners. If too high, only the very peaks will be included, if too low, noise may clutter peak detection.
- "threshold" value for peak detection, which, when divided by 10, is the multiple by which one bin must exceed the previous one for peak detection to commence. If too low, FFT detection becomes more random, if too high, no peaks are detected.
This commit is contained in:
ctzsnooze 2018-08-05 17:51:28 +10:00
parent 40a4ab77fe
commit 1e960c95eb
7 changed files with 179 additions and 65 deletions
Download patch file Download diff file Expand all files Collapse all files

127
src/main/sensors/gyroanalyse.c
View file

@ -45,19 +45,26 @@
// 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_BIN_COUNT (FFT_WINDOW_SIZE / 2)
// for gyro loop >= 4KHz, analyse up to 666Hz, 16 bins each 41.625 Hz wide
#define FFT_SAMPLING_RATE_HZ 1333
// for gyro loop >= 4KHz, analyse up to 1000Hz, 16 bins each 62.5 Hz wide
#define FFT_SAMPLING_RATE_HZ 2000
#define FFT_RESOLUTION ((float)FFT_SAMPLING_RATE_HZ / FFT_WINDOW_SIZE)
// following bin must be at least 2 times previous to indicate start of peak
#define FFT_MIN_BIN_RISE 2
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2)
// the desired approimate lower frequency when calculating bin offset
#define FFT_BIN_OFFSET_DESIRED_HZ 90
// lowpass frequency for smoothing notch centre point
#define DYN_NOTCH_SMOOTH_FREQ_HZ 60
// centre frequency of bandpass that constrains input to FFT
#if FFT_SAMPLING_RATE_HZ == 2000
#define FFT_BPF_HZ 350
#elif FFT_SAMPLING_RATE_HZ < 2000
#define FFT_BPF_HZ (FFT_SAMPLING_RATE_HZ / 4)
#endif
#define DYN_NOTCH_SMOOTH_FREQ_HZ 50
// notch centre point will not go below this, must be greater than cutoff, mid of bottom bin
#define DYN_NOTCH_MIN_CENTRE_HZ 125
#define DYN_NOTCH_MIN_CENTRE_HZ 140
// maximum notch centre frequency limited by Nyquist
#define DYN_NOTCH_MAX_CENTRE_HZ (FFT_SAMPLING_RATE_HZ / 2)
// lowest allowed notch cutoff frequency
#define DYN_NOTCH_MIN_CUTOFF_HZ 105
#define DYN_NOTCH_MIN_CUTOFF_HZ 110
// we need 4 steps for each axis
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4)
@ -66,13 +73,14 @@ static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz;
static uint16_t FAST_RAM_ZERO_INIT fftBpfHz;
// Hz per bin
static float FAST_RAM_ZERO_INIT fftResolution;
// maximum notch centre frequency limited by Nyquist
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxCentreHz;
static uint8_t FAST_RAM_ZERO_INIT fftBinOffset;
// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE];
static FAST_RAM_ZERO_INIT float dynamicNotchCutoff;
static FAST_RAM_ZERO_INIT float dynamicFilterCutoffFactor;
static FAST_RAM_ZERO_INIT uint8_t dynamicFilterType;
static FAST_RAM_ZERO_INIT float dynamicFilterThreshold;
static FAST_RAM_ZERO_INIT float dynamicFilterIgnore;
void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
{
@ -92,7 +100,6 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
fftBpfHz = fftSamplingRateHz / 4;
fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE;
dynNotchMaxCentreHz = fftSamplingRateHz / 2;
// Calculate the FFT bin offset to try and get the lowest bin used
// in the center calc close to 90hz
@ -103,7 +110,10 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
}
dynamicNotchCutoff = (100.0f - gyroConfig()->dyn_notch_width_percent) / 100;
dynamicFilterCutoffFactor = (100.0f - gyroConfig()->dyn_filter_width_percent) / 100;
dynamicFilterType = gyroConfig()->dyn_filter_type;
dynamicFilterThreshold = gyroConfig()->dyn_filter_threshold / 10;
dynamicFilterIgnore = gyroConfig()->dyn_filter_ignore / 10;
}
void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
@ -123,7 +133,8 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime
const float looptime = MAX(1000000u / fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// any init value
state->centerFreq[axis] = 200;
state->centerFreq[axis] = DYN_NOTCH_MAX_CENTRE_HZ;
state->prevCenterFreq[axis] = DYN_NOTCH_MAX_CENTRE_HZ;
biquadFilterInit(&state->gyroBandpassFilter[axis], fftBpfHz, 1000000 / fftSamplingRateHz, 0.01f * gyroConfig()->dyn_notch_quality, FILTER_BPF);
biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
}
@ -134,12 +145,12 @@ void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float
state->oversampledGyroAccumulator[axis] += sample;
}
static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn);
static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter);
/*
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
*/
void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
void gyroDataAnalyse(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter)
{
// samples should have been pushed by `gyroDataAnalysePush`
// if gyro sampling is > 1kHz, accumulate multiple samples
@ -152,8 +163,9 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
// calculate mean value of accumulated samples
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
sample = biquadFilterApply(&state->gyroBandpassFilter[axis], sample);
if (gyroConfig()->dyn_notch_quality > 4){
sample = biquadFilterApply(&state->gyroBandpassFilter[axis], sample);
}
state->downsampledGyroData[axis][state->circularBufferIdx] = sample;
if (axis == 0) {
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
@ -170,7 +182,7 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
// calculate FFT and update filters
if (state->updateTicks > 0) {
gyroDataAnalyseUpdate(state, notchFilterDyn);
gyroDataAnalyseUpdate(state, dynFilter);
--state->updateTicks;
}
}
@ -184,7 +196,7 @@ void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t
/*
* Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
*/
static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter)
{
enum {
STEP_ARM_CFFT_F32,
@ -255,59 +267,88 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
// calculate FFT centreFreq
float fftSum = 0;
float fftWeightedSum = 0;
float dataAvg = 0;
bool fftIncreasing = false;
//get simple average of bin amplitudes
for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) {
dataAvg += state->fftData[i];
}
dataAvg = dataAvg / FFT_BIN_COUNT;
// iterate over fft data and calculate weighted indices
for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) {
const float data = state->fftData[i];
const float prevData = state->fftData[i - 1];
if (fftIncreasing || data > prevData * FFT_MIN_BIN_RISE) {
float cubedData = data * data * data;
// add previous bin before first rise
if (!fftIncreasing) {
cubedData += prevData * prevData * prevData;
fftIncreasing = true;
// only consider bins above ignore multiple of average size
if (data > dynamicFilterIgnore * dataAvg) {
// only consider bins after > threshold step up from previous
if (fftIncreasing || data > prevData * dynamicFilterThreshold) {
float cubedData = data * data * data;
// add previous bin before first rise
if (!fftIncreasing) {
cubedData += prevData * prevData * prevData;
fftIncreasing = true;
}
fftSum += cubedData;
// calculate weighted index starting at 1, not 0
fftWeightedSum += cubedData * (i + 1);
}
fftSum += cubedData;
// calculate weighted index starting at 1, not 0
fftWeightedSum += cubedData * (i + 1);
}
}
// get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
// if no peak, go to highest point to minimise delay
float centerFreq = dynNotchMaxCentreHz;
float centerFreq = DYN_NOTCH_MAX_CENTRE_HZ;
float fftMeanIndex = 0;
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 * fftResolution, DYN_NOTCH_MIN_CENTRE_HZ, dynNotchMaxCentreHz);
centerFreq = fftMeanIndex * FFT_RESOLUTION;
} else {
centerFreq = state->prevCenterFreq[state->updateAxis];
}
state->prevCenterFreq[state->updateAxis] = centerFreq;
centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CENTRE_HZ, DYN_NOTCH_MAX_CENTRE_HZ);
centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CENTRE_HZ, dynNotchMaxCentreHz);
centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CENTRE_HZ, DYN_NOTCH_MAX_CENTRE_HZ);
state->centerFreq[state->updateAxis] = centerFreq;
if (state->updateAxis == 0) {
DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
}
DEBUG_SET(DEBUG_FFT_FREQ, state->updateAxis, state->centerFreq[state->updateAxis]);
if (state->updateAxis == 0 || state->updateAxis == 1) {
DEBUG_SET(DEBUG_FFT_FREQ, state->updateAxis, state->centerFreq[state->updateAxis]);
}
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break;
}
case STEP_UPDATE_FILTERS:
{
// 7us
// calculate cutoffFreq and notch Q, update notch filter
const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicNotchCutoff, DYN_NOTCH_MIN_CUTOFF_HZ);
const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq);
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH);
switch (dynamicFilterType) {
case FILTER_PT1: {
const int cutoffFreq = state->centerFreq[state->updateAxis] * dynamicFilterCutoffFactor;
const float gyroDt = gyro.targetLooptime * 1e-6f;
const float gain = pt1FilterGain(cutoffFreq, gyroDt);
pt1FilterUpdateCutoff(&dynFilter[state->updateAxis].pt1FilterState, gain);
break;
}
case FILTER_BIQUAD: {
// calculate cutoffFreq and notch Q, update notch filter
const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicFilterCutoffFactor, DYN_NOTCH_MIN_CUTOFF_HZ);
const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq);
biquadFilterUpdate(&dynFilter[state->updateAxis].biquadFilterState, state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH);
break;
}
}
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;