rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #6485 from etracer65/dyn_filter_2k 

Fix dynamic filter for gyro loop < 4K
This commit is contained in:
Michael Keller 2018-08-02 13:10:26 +12:00 committed by GitHub
parent 1f1dd981bd 9ba89f6da0
commit b979cb2ea4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 45 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/sensors/gyro.c
View File

@ -527,10 +527,6 @@ bool gyroInit(void)
}
#endif
#ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyseInit();
#endif
switch (debugMode) {
case DEBUG_FFT:
case DEBUG_FFT_FREQ:

62
src/main/sensors/gyroanalyse.c
View File

@ -46,34 +46,59 @@
// Eg [0,31), [31,62), [62, 93) etc
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2)
// compare 1 + this offset FFT bins for peak, ie if 1 start 2.5 * 41.655 or about 104Hz
#define FFT_BIN_OFFSET 1
// analyse up to 666Hz, 16 bins each 41.625 Hz wide
// for gyro loop >= 4KHz, analyse up to 666Hz, 16 bins each 41.625 Hz wide
#define FFT_SAMPLING_RATE_HZ 1333
// centre frequency of bandpass that constrains input to FFT
#define FFT_BPF_HZ (FFT_SAMPLING_RATE_HZ / 4)
// Hz per bin
#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
// 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
// notch centre point will not go below this, must be greater than cutoff, mid of bottom bin
#define DYN_NOTCH_MIN_CENTRE_HZ 125
// 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
// we need 4 steps for each axis
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4)
static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz;
// centre frequency of bandpass that constrains input to FFT
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;
void gyroDataAnalyseInit(void)
void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
{
#ifdef USE_DUAL_GYRO
static bool gyroAnalyseInitialized;
if (gyroAnalyseInitialized) {
return;
}
gyroAnalyseInitialized = true;
#endif
const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
// If we get at least 3 samples then use the default FFT sample frequency
// otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K)
fftSamplingRateHz = MIN((gyroLoopRateHz / 3), FFT_SAMPLING_RATE_HZ);
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
// > 1333hz = 1, 889hz (2.67K) = 2, 666hz (2K) = 3
fftBinOffset = MAX(1, lrintf(FFT_BIN_OFFSET_DESIRED_HZ / fftResolution - 1.5f));
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
}
@ -84,8 +109,10 @@ void gyroDataAnalyseInit(void)
void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
{
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
gyroDataAnalyseInit(targetLooptimeUs);
const uint16_t samplingFrequency = 1000000 / targetLooptimeUs;
state->maxSampleCount = samplingFrequency / FFT_SAMPLING_RATE_HZ;
state->maxSampleCount = samplingFrequency / fftSamplingRateHz;
state->maxSampleCountRcp = 1.f / state->maxSampleCount;
arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE);
@ -93,11 +120,11 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime
// 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_HZ, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
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;
biquadFilterInit(&state->gyroBandpassFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE_HZ, 0.01f * gyroConfig()->dyn_notch_quality, FILTER_BPF);
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);
}
}
@ -231,7 +258,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
bool fftIncreasing = false;
// iterate over fft data and calculate weighted indices
for (int i = 1 + FFT_BIN_OFFSET; i < FFT_BIN_COUNT; i++) {
for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) {
const float data = state->fftData[i];
const float prevData = state->fftData[i - 1];
@ -253,17 +280,18 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
// 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 = DYN_NOTCH_MAX_CENTRE_HZ;
float centerFreq = dynNotchMaxCentreHz;
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 * FFT_RESOLUTION, DYN_NOTCH_MIN_CENTRE_HZ, DYN_NOTCH_MAX_CENTRE_HZ);
centerFreq = constrain(fftMeanIndex * fftResolution, DYN_NOTCH_MIN_CENTRE_HZ, dynNotchMaxCentreHz);
}
centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CENTRE_HZ, dynNotchMaxCentreHz);
state->centerFreq[state->updateAxis] = centerFreq;
if (state->updateAxis == 0) {

1
src/main/sensors/gyroanalyse.h
View File

@ -57,7 +57,6 @@ typedef struct gyroAnalyseState_s {
STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type);
void gyroDataAnalyseInit(void);
void gyroDataAnalyseStateInit(gyroAnalyseState_t *gyroAnalyse, uint32_t targetLooptime);
void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample);
void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn);