diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index dbe7014d1..1e288e9b9 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -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)
diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h
index bdccbb3ef..d0502f4ac 100644
--- a/src/main/sensors/gyro_filter_impl.h
+++ b/src/main/sensors/gyro_filter_impl.h
@@ -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));
diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c
index a5958894b..fd7fc2180 100644
--- a/src/main/sensors/gyroanalyse.c
+++ b/src/main/sensors/gyroanalyse.c
@@ -18,6 +18,11 @@
* If not, see .
*/
+/* 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
#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);
diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h
index b0565ec3f..182d3135c 100644
--- a/src/main/sensors/gyroanalyse.h
+++ b/src/main/sensors/gyroanalyse.h
@@ -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);