diff --git a/make/source.mk b/make/source.mk
index 45dfb778f0..3a0d90d1e1 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -92,6 +92,7 @@ COMMON_SRC = \
flight/rth_estimator.c \
flight/servos.c \
flight/wind_estimator.c \
+ flight/gyroanalyse.c \
io/beeper.c \
io/lights.c \
io/pwmdriver_i2c.c \
@@ -240,5 +241,22 @@ ifneq ($(filter VCP,$(FEATURES)),)
TARGET_SRC += $(VCP_SRC)
endif
+ifneq ($(DSP_LIB),)
+
+INCLUDE_DIRS += $(DSP_LIB)/Include
+
+TARGET_SRC += $(DSP_LIB)/Source/BasicMathFunctions/arm_mult_f32.c
+TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_rfft_fast_f32.c
+TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_f32.c
+TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_rfft_fast_init_f32.c
+TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_radix8_f32.c
+TARGET_SRC += $(DSP_LIB)/Source/CommonTables/arm_common_tables.c
+
+TARGET_SRC += $(DSP_LIB)/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c
+TARGET_SRC += $(DSP_LIB)/Source/StatisticsFunctions/arm_max_f32.c
+
+TARGET_SRC += $(wildcard $(DSP_LIB)/Source/*/*.S)
+endif
+
# Search path and source files for the ST stdperiph library
VPATH := $(VPATH):$(STDPERIPH_DIR)/src
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 4143d91cd2..b7453e8836 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -71,5 +71,8 @@ typedef enum {
DEBUG_ITERM_RELAX,
DEBUG_D_BOOST,
DEBUG_ANTIGRAVITY,
+ DEBUG_FFT,
+ DEBUG_FFT_TIME,
+ DEBUG_FFT_FREQ,
DEBUG_COUNT
} debugType_e;
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 1a56c0b472..3b8f4cfe16 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -30,7 +30,6 @@
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/
// NULL filter
-
float nullFilterApply(void *filter, float input)
{
UNUSED(filter);
@@ -148,7 +147,8 @@ void biquadRCFIR2FilterInit(biquadFilter_t *filter, uint16_t f_cut, uint32_t sam
}
// zero initial samples
- filter->d1 = filter->d2 = 0;
+ filter->x1 = filter->x2 = 0;
+ filter->y1 = filter->y2 = 0;
}
void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType)
@@ -196,25 +196,59 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
}
// zero initial samples
- filter->d1 = filter->d2 = 0;
+ filter->x1 = filter->x2 = 0;
+ filter->y1 = filter->y2 = 0;
+}
+
+FAST_CODE float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
+{
+ /* compute result */
+ const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2;
+
+ /* shift x1 to x2, input to x1 */
+ filter->x2 = filter->x1;
+ filter->x1 = input;
+
+ /* shift y1 to y2, result to y1 */
+ filter->y2 = filter->y1;
+ filter->y1 = result;
+
+ return result;
}
// Computes a biquad_t filter on a sample
float FAST_CODE NOINLINE biquadFilterApply(biquadFilter_t *filter, float input)
{
- const float result = filter->b0 * input + filter->d1;
- filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;
- filter->d2 = filter->b2 * input - filter->a2 * result;
+ const float result = filter->b0 * input + filter->x1;
+ filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2;
+ filter->x2 = filter->b2 * input - filter->a2 * result;
return result;
}
float biquadFilterReset(biquadFilter_t *filter, float value)
{
- filter->d1 = value - (value * filter->b0);
- filter->d2 = (filter->b2 - filter->a2) * value;
+ filter->x1 = value - (value * filter->b0);
+ filter->x2 = (filter->b2 - filter->a2) * value;
return value;
}
+FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
+{
+ // backup state
+ float x1 = filter->x1;
+ float x2 = filter->x2;
+ float y1 = filter->y1;
+ float y2 = filter->y2;
+
+ biquadFilterInit(filter, filterFreq, refreshRate, Q, filterType);
+
+ // restore state
+ filter->x1 = x1;
+ filter->x2 = x2;
+ filter->y1 = y1;
+ filter->y2 = y2;
+}
+
/*
* FIR filter
*/
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index 491796e434..3ee3d349eb 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -30,7 +30,7 @@ typedef struct pt1Filter_s {
/* this holds the data required to update samples thru a filter */
typedef struct biquadFilter_s {
float b0, b1, b2, a1, a2;
- float d1, d2;
+ float x1, x2, y1, y2;
} biquadFilter_t;
typedef union {
@@ -79,10 +79,11 @@ void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t s
void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType);
float biquadFilterApply(biquadFilter_t *filter, float sample);
float biquadFilterReset(biquadFilter_t *filter, float value);
+float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
+void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);
void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength);
void firFilterUpdate(firFilter_t *filter, float input);
-float firFilterApply(const firFilter_t *filter);
-
+float firFilterApply(const firFilter_t *filter);
\ No newline at end of file
diff --git a/src/main/config/feature.c b/src/main/config/feature.c
index d1479de422..67715427e0 100755
--- a/src/main/config/feature.c
+++ b/src/main/config/feature.c
@@ -22,7 +22,7 @@
#include "config/feature.h"
-static uint32_t activeFeaturesLatch = 0;
+static EXTENDED_FASTRAM uint32_t activeFeaturesLatch = 0;
void latchActiveFeatures(void)
{
@@ -34,7 +34,7 @@ bool featureConfigured(uint32_t mask)
return featureConfig()->enabledFeatures & mask;
}
-bool feature(uint32_t mask)
+bool FAST_CODE NOINLINE feature(uint32_t mask)
{
return activeFeaturesLatch & mask;
}
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index be3a1acc91..a20f4c9c05 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -144,7 +144,7 @@ static bool commandBatchError = false;
// sync this with features_e
static const char * const featureNames[] = {
"THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP",
- "", "SOFTSERIAL", "GPS", "",
+ "DYNAMIC_FILTERS", "SOFTSERIAL", "GPS", "",
"", "TELEMETRY", "CURRENT_METER", "3D", "",
"", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
"BLACKBOX", "", "TRANSPONDER", "AIRMODE",
diff --git a/src/main/fc/config.h b/src/main/fc/config.h
index d79dd49562..62b3fb89e5 100644
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@@ -41,7 +41,7 @@ typedef enum {
FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command
FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3,
FEATURE_MOTOR_STOP = 1 << 4,
- NOT_USED_10 = 1 << 5, // was FEATURE_SERVO_TILT
+ FEATURE_DYNAMIC_FILTERS = 1 << 5, // was FEATURE_SERVO_TILT
FEATURE_SOFTSERIAL = 1 << 6,
FEATURE_GPS = 1 << 7,
FEATURE_UNUSED_3 = 1 << 8, // was FEATURE_FAILSAFE
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index d3387f4dd7..65e47fd1d0 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -78,7 +78,7 @@ tables:
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW",
"FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC", "ITERM_RELAX",
- "D_BOOST", "ANTIGRAVITY"]
+ "D_BOOST", "ANTIGRAVITY", "FFT", "FFT_TIME", "FFT_FREQ"]
- name: async_mode
values: ["NONE", "GYRO", "ALL"]
- name: aux_operator
@@ -129,6 +129,9 @@ tables:
- name: rssi_source
values: ["NONE", "AUTO", "ADC", "CHANNEL", "PROTOCOL", "MSP"]
enum: rssiSource_e
+ - name: dynamicFilterRangeTable
+ values: ["HIGH", "MEDIUM", "LOW"]
+ enum: dynamicFilterRange_e
groups:
- name: PG_GYRO_CONFIG
@@ -179,6 +182,25 @@ groups:
condition: USE_GYRO_BIQUAD_RC_FIR2
min: 0
max: 500
+ - name: dyn_notch_width_percent
+ field: dyn_notch_width_percent
+ condition: USE_DYNAMIC_FILTERS
+ min: 0
+ max: 20
+ - name: dyn_notch_range
+ field: dyn_notch_range
+ condition: USE_DYNAMIC_FILTERS
+ table: dynamicFilterRangeTable
+ - name: dyn_notch_q
+ field: dyn_notch_q
+ condition: USE_DYNAMIC_FILTERS
+ min: 1
+ max: 1000
+ - name: dyn_notch_min_hz
+ field: dyn_notch_min_hz
+ condition: USE_DYNAMIC_FILTERS
+ min: 60
+ max: 1000
- name: gyro_to_use
condition: USE_DUAL_GYRO
min: 0
diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c
new file mode 100644
index 0000000000..0e387e8cd2
--- /dev/null
+++ b/src/main/flight/gyroanalyse.c
@@ -0,0 +1,374 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 software.
+ *
+ * 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"
+
+#ifdef USE_DYNAMIC_FILTERS
+
+#include "build/debug.h"
+
+#include "common/filter.h"
+#include "common/maths.h"
+#include "common/time.h"
+#include "common/utils.h"
+
+#include "drivers/accgyro/accgyro.h"
+#include "drivers/time.h"
+
+#include "sensors/gyro.h"
+#include "fc/config.h"
+
+#include "gyroanalyse.h"
+
+// The FFT splits the frequency domain into an number of bins
+// A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each 31.25Hz wide
+// Eg [0,31), [31,62), [62, 93) etc
+// for gyro loop >= 4KHz, sample rate 2000 defines FFT range to 1000Hz, 16 bins each 62.5 Hz wide
+// NB FFT_WINDOW_SIZE is set to 32 in gyroanalyse.h
+#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2)
+// smoothing frequency for FFT centre frequency
+#define DYN_NOTCH_SMOOTH_FREQ_HZ 50
+// we need 4 steps for each axis
+#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4)
+
+#define DYN_NOTCH_OSD_MIN_THROTTLE 20
+
+static uint16_t EXTENDED_FASTRAM fftSamplingRateHz;
+static float EXTENDED_FASTRAM fftResolution;
+static uint8_t EXTENDED_FASTRAM fftStartBin;
+static uint16_t EXTENDED_FASTRAM dynNotchMaxCtrHz;
+static uint8_t dynamicFilterRange;
+static float EXTENDED_FASTRAM dynNotchQ;
+static float EXTENDED_FASTRAM dynNotch1Ctr;
+static float EXTENDED_FASTRAM dynNotch2Ctr;
+static uint16_t EXTENDED_FASTRAM dynNotchMinHz;
+static bool EXTENDED_FASTRAM dualNotch = true;
+static uint16_t EXTENDED_FASTRAM dynNotchMaxFFT;
+
+// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
+static EXTENDED_FASTRAM float hanningWindow[FFT_WINDOW_SIZE];
+
+void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
+{
+ dynamicFilterRange = gyroConfig()->dyn_notch_range;
+ fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
+ dynNotch1Ctr = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f;
+ dynNotch2Ctr = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f;
+ dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
+ dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
+
+ if (gyroConfig()->dyn_notch_width_percent == 0) {
+ dualNotch = false;
+ }
+
+ if (dynamicFilterRange == DYN_NOTCH_RANGE_HIGH) {
+ fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
+ }
+ else if (dynamicFilterRange == DYN_NOTCH_RANGE_MEDIUM) {
+ fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
+ }
+
+ // 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)
+ const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
+
+ fftSamplingRateHz = MIN((gyroLoopRateHz / 3), fftSamplingRateHz);
+
+ fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE;
+
+ fftStartBin = dynNotchMinHz / lrintf(fftResolution);
+
+ dynNotchMaxCtrHz = fftSamplingRateHz / 2; //Nyquist
+
+ 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)));
+ }
+}
+
+void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
+{
+ // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
+ // *** can this next line be removed ??? ***
+ gyroDataAnalyseInit(targetLooptimeUs);
+
+ const uint16_t samplingFrequency = 1000000 / targetLooptimeUs;
+ state->maxSampleCount = samplingFrequency / fftSamplingRateHz;
+ state->maxSampleCountRcp = 1.f / state->maxSampleCount;
+
+ arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE);
+
+// 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 / fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
+ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ // any init value
+ state->centerFreq[axis] = dynNotchMaxCtrHz;
+ state->prevCenterFreq[axis] = dynNotchMaxCtrHz;
+ biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
+ }
+}
+
+void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample)
+{
+ state->oversampledGyroAccumulator[axis] += sample;
+}
+
+static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2);
+
+/*
+ * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
+ */
+void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
+{
+ // samples should have been pushed by `gyroDataAnalysePush`
+ // if gyro sampling is > 1kHz, accumulate multiple samples
+ state->sampleCount++;
+
+ // this runs at 1kHz
+ if (state->sampleCount == state->maxSampleCount) {
+ state->sampleCount = 0;
+
+ // calculate mean value of accumulated samples
+ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
+ state->downsampledGyroData[axis][state->circularBufferIdx] = sample;
+ if (axis == 0) {
+ DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
+ }
+
+ state->oversampledGyroAccumulator[axis] = 0;
+ }
+
+ state->circularBufferIdx = (state->circularBufferIdx + 1) % FFT_WINDOW_SIZE;
+
+ // We need DYN_NOTCH_CALC_TICKS tick to update all axis with newly sampled value
+ state->updateTicks = DYN_NOTCH_CALC_TICKS;
+ }
+
+ // calculate FFT and update filters
+ if (state->updateTicks > 0) {
+ gyroDataAnalyseUpdate(state, notchFilterDyn, notchFilterDyn2);
+ --state->updateTicks;
+ }
+}
+
+void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut);
+void arm_cfft_radix8by2_f32(arm_cfft_instance_f32 *S, float32_t *p1);
+void arm_cfft_radix8by4_f32(arm_cfft_instance_f32 *S, float32_t *p1);
+void arm_radix8_butterfly_f32(float32_t *pSrc, uint16_t fftLen, const float32_t *pCoef, uint16_t twidCoefModifier);
+void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable);
+
+/*
+ * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
+ */
+static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
+{
+ enum {
+ STEP_ARM_CFFT_F32,
+ STEP_BITREVERSAL,
+ STEP_STAGE_RFFT_F32,
+ STEP_ARM_CMPLX_MAG_F32,
+ STEP_CALC_FREQUENCIES,
+ STEP_UPDATE_FILTERS,
+ STEP_HANNING,
+ STEP_COUNT
+ };
+
+ arm_cfft_instance_f32 *Sint = &(state->fftInstance.Sint);
+
+ uint32_t startTime = 0;
+ if (debugMode == (DEBUG_FFT_TIME)) {
+ startTime = micros();
+ }
+
+ DEBUG_SET(DEBUG_FFT_TIME, 0, state->updateStep);
+ switch (state->updateStep) {
+ case STEP_ARM_CFFT_F32:
+ {
+ switch (FFT_BIN_COUNT) {
+ case 16:
+ // 16us
+ arm_cfft_radix8by2_f32(Sint, state->fftData);
+ break;
+ case 32:
+ // 35us
+ arm_cfft_radix8by4_f32(Sint, state->fftData);
+ break;
+ case 64:
+ // 70us
+ arm_radix8_butterfly_f32(state->fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1);
+ break;
+ }
+ DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+ break;
+ }
+ case STEP_BITREVERSAL:
+ {
+ // 6us
+ arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable);
+ DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+ state->updateStep++;
+ FALLTHROUGH;
+ }
+ case STEP_STAGE_RFFT_F32:
+ {
+ // 14us
+ // this does not work in place => fftData AND rfftData needed
+ stage_rfft_f32(&state->fftInstance, state->fftData, state->rfftData);
+ DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+ break;
+ }
+ case STEP_ARM_CMPLX_MAG_F32:
+ {
+ // 8us
+ arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT);
+ DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime);
+ state->updateStep++;
+ FALLTHROUGH;
+ }
+ case STEP_CALC_FREQUENCIES:
+ {
+ bool fftIncreased = false;
+ float dataMax = 0;
+ uint8_t binStart = 0;
+ uint8_t binMax = 0;
+ //for bins after initial decline, identify start bin and max bin
+ for (int i = fftStartBin; i < FFT_BIN_COUNT; i++) {
+ if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) {
+ if (!fftIncreased) {
+ binStart = i; // first up-step bin
+ fftIncreased = true;
+ }
+ if (state->fftData[i] > dataMax) {
+ dataMax = state->fftData[i];
+ binMax = i; // tallest bin
+ }
+ }
+ }
+ // accumulate fftSum and fftWeightedSum from peak bin, and shoulder bins either side of peak
+ float cubedData = state->fftData[binMax] * state->fftData[binMax] * state->fftData[binMax];
+ float fftSum = cubedData;
+ float fftWeightedSum = cubedData * (binMax + 1);
+ // accumulate upper shoulder
+ for (int i = binMax; i < FFT_BIN_COUNT - 1; i++) {
+ if (state->fftData[i] > state->fftData[i + 1]) {
+ cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i];
+ fftSum += cubedData;
+ fftWeightedSum += cubedData * (i + 1);
+ } else {
+ break;
+ }
+ }
+ // accumulate lower shoulder
+ for (int i = binMax; i > binStart + 1; i--) {
+ if (state->fftData[i] > state->fftData[i - 1]) {
+ cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i];
+ fftSum += cubedData;
+ fftWeightedSum += cubedData * (i + 1);
+ } else {
+ break;
+ }
+ }
+ // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
+ float centerFreq = dynNotchMaxCtrHz;
+ float fftMeanIndex = 0;
+ // idx was shifted by 1 to start at 1, not 0
+ if (fftSum > 0) {
+ fftMeanIndex = (fftWeightedSum / fftSum) - 1;
+ // the index points at the center frequency of each bin so index 0 is actually 16.125Hz
+ centerFreq = fftMeanIndex * fftResolution;
+ } else {
+ centerFreq = state->prevCenterFreq[state->updateAxis];
+ }
+ centerFreq = fmax(centerFreq, dynNotchMinHz);
+ centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
+ state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
+ state->centerFreq[state->updateAxis] = centerFreq;
+
+ dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]);
+
+ if (state->updateAxis == 0) {
+ DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
+ DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]);
+ }
+ if (state->updateAxis == 1) {
+ DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]);
+ }
+ // Debug FFT_Freq carries raw gyro, gyro after first filter set, FFT centre for roll and for pitch
+ DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+ break;
+ }
+ case STEP_UPDATE_FILTERS:
+ {
+ // 7us
+ // calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004)
+ if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
+
+ DEBUG_SET(DEBUG_FFT_FREQ, state->updateAxis + 5, state->centerFreq[state->updateAxis]);
+
+ if (dualNotch) {
+ biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
+ biquadFilterUpdate(¬chFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
+ } else {
+ biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], getLooptime(), dynNotchQ, FILTER_NOTCH);
+ }
+ }
+ DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+
+ state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
+ state->updateStep++;
+ FALLTHROUGH;
+ }
+ case STEP_HANNING:
+ {
+ // 5us
+ // apply hanning window to gyro samples and store result in fftData
+ // hanning starts and ends with 0, could be skipped for minor speed improvement
+ const uint8_t ringBufIdx = FFT_WINDOW_SIZE - state->circularBufferIdx;
+ arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &hanningWindow[0], &state->fftData[0], ringBufIdx);
+ if (state->circularBufferIdx > 0) {
+ arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx);
+ }
+
+ DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+ }
+ }
+
+ state->updateStep = (state->updateStep + 1) % STEP_COUNT;
+}
+
+
+uint16_t getMaxFFT(void) {
+ return dynNotchMaxFFT;
+}
+
+void resetMaxFFT(void) {
+ dynNotchMaxFFT = 0;
+}
+
+#endif // USE_DYNAMIC_FILTERS
diff --git a/src/main/flight/gyroanalyse.h b/src/main/flight/gyroanalyse.h
new file mode 100644
index 0000000000..b4329309e8
--- /dev/null
+++ b/src/main/flight/gyroanalyse.h
@@ -0,0 +1,63 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#ifdef USE_DYNAMIC_FILTERS
+
+#include "arm_math.h"
+#include "common/filter.h"
+
+// max for F3 targets
+#define FFT_WINDOW_SIZE 32
+
+typedef struct gyroAnalyseState_s {
+ // accumulator for oversampled data => no aliasing and less noise
+ uint8_t sampleCount;
+ uint8_t maxSampleCount;
+ float maxSampleCountRcp;
+ float oversampledGyroAccumulator[XYZ_AXIS_COUNT];
+
+ // downsampled gyro data circular buffer for frequency analysis
+ uint8_t circularBufferIdx;
+ float downsampledGyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE];
+
+ // update state machine step information
+ uint8_t updateTicks;
+ uint8_t updateStep;
+ uint8_t updateAxis;
+
+ arm_rfft_fast_instance_f32 fftInstance;
+ float fftData[FFT_WINDOW_SIZE];
+ float rfftData[FFT_WINDOW_SIZE];
+
+ biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT];
+ uint16_t centerFreq[XYZ_AXIS_COUNT];
+ uint16_t prevCenterFreq[XYZ_AXIS_COUNT];
+} gyroAnalyseState_t;
+
+STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type);
+
+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, biquadFilter_t *notchFilterDyn2);
+uint16_t getMaxFFT(void);
+void resetMaxFFT(void);
+#endif
\ No newline at end of file
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index e650f7feb1..1957468db9 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -34,6 +34,7 @@
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
+#include "config/feature.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h"
@@ -66,6 +67,8 @@
#include "sensors/gyro.h"
#include "sensors/sensors.h"
+#include "flight/gyroanalyse.h"
+
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
#endif
@@ -96,7 +99,19 @@ STATIC_FASTRAM filterApplyFnPtr gyroFilterStage2ApplyFn;
STATIC_FASTRAM void *stage2Filter[XYZ_AXIS_COUNT];
#endif
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5);
+#ifdef USE_DYNAMIC_FILTERS
+
+#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
+#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
+
+static EXTENDED_FASTRAM filterApplyFnPtr notchFilterDynApplyFn;
+static EXTENDED_FASTRAM filterApplyFnPtr notchFilterDynApplyFn2;
+static EXTENDED_FASTRAM biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
+static EXTENDED_FASTRAM biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT];
+EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState;
+#endif
+
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 6);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lpf = GYRO_LPF_42HZ, // 42HZ value is defined for Invensense/TDK gyros
@@ -111,7 +126,11 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_soft_notch_cutoff_1 = 1,
.gyro_soft_notch_hz_2 = 0,
.gyro_soft_notch_cutoff_2 = 1,
- .gyro_stage2_lowpass_hz = 0
+ .gyro_stage2_lowpass_hz = 0,
+ .dyn_notch_width_percent = 8,
+ .dyn_notch_range = DYN_NOTCH_RANGE_MEDIUM,
+ .dyn_notch_q = 120,
+ .dyn_notch_min_hz = 150,
);
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
@@ -251,6 +270,33 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
return gyroHardware;
}
+#ifdef USE_DYNAMIC_FILTERS
+bool isDynamicFilterActive(void)
+{
+ return feature(FEATURE_DYNAMIC_FILTERS);
+}
+
+static void gyroInitFilterDynamicNotch(void)
+{
+
+ notchFilterDynApplyFn = nullFilterApply;
+ notchFilterDynApplyFn2 = nullFilterApply;
+
+ if (isDynamicFilterActive()) {
+ notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
+ if(gyroConfig()->dyn_notch_width_percent != 0) {
+ notchFilterDynApplyFn2 = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
+ }
+ const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
+ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ biquadFilterInit(¬chFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, getLooptime(), notchQ, FILTER_NOTCH);
+ biquadFilterInit(¬chFilterDyn2[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, getLooptime(), notchQ, FILTER_NOTCH);
+ }
+ }
+
+}
+#endif
+
bool gyroInit(void)
{
memset(&gyro, 0, sizeof(gyro));
@@ -280,6 +326,10 @@ bool gyroInit(void)
}
gyroInitFilters();
+#ifdef USE_DYNAMIC_FILTERS
+ gyroInitFilterDynamicNotch();
+ gyroDataAnalyseStateInit(&gyroAnalyseState, getLooptime());
+#endif
return true;
}
@@ -429,6 +479,15 @@ void FAST_CODE NOINLINE gyroUpdate()
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
+#ifdef USE_DYNAMIC_FILTERS
+ if (isDynamicFilterActive()) {
+ if (axis == FD_ROLL) {
+ DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
+ DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
+ }
+ }
+#endif
+
if (axis < 2) {
DEBUG_SET(DEBUG_STAGE2, axis, lrintf(gyroADCf));
}
@@ -452,7 +511,25 @@ void FAST_CODE NOINLINE gyroUpdate()
gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
#endif
gyro.gyroADCf[axis] = gyroADCf;
+
+#ifdef USE_DYNAMIC_FILTERS
+ if (isDynamicFilterActive()) {
+ if (axis == FD_ROLL) {
+ DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf));
+ DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf));
+ }
+ gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
+ gyroADCf = notchFilterDynApplyFn((filter_t *)¬chFilterDyn[axis], gyroADCf);
+ gyroADCf = notchFilterDynApplyFn2((filter_t *)¬chFilterDyn2[axis], gyroADCf);
+ }
+#endif
}
+
+#ifdef USE_DYNAMIC_FILTERS
+ if (isDynamicFilterActive()) {
+ gyroDataAnalyse(&gyroAnalyseState, notchFilterDyn, notchFilterDyn2);
+ }
+#endif
}
bool gyroReadTemperature(void)
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index b6802b011c..9c1d627030 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -39,6 +39,16 @@ typedef enum {
GYRO_FAKE
} gyroSensor_e;
+typedef enum {
+ DYN_NOTCH_RANGE_HIGH = 0,
+ DYN_NOTCH_RANGE_MEDIUM,
+ DYN_NOTCH_RANGE_LOW
+} dynamicFilterRange_e;
+
+#define DYN_NOTCH_RANGE_HZ_HIGH 2000
+#define DYN_NOTCH_RANGE_HZ_MEDIUM 1333
+#define DYN_NOTCH_RANGE_HZ_LOW 1000
+
typedef struct gyro_s {
uint32_t targetLooptime;
float gyroADCf[XYZ_AXIS_COUNT];
@@ -60,6 +70,10 @@ typedef struct gyroConfig_s {
uint16_t gyro_soft_notch_hz_2;
uint16_t gyro_soft_notch_cutoff_2;
uint16_t gyro_stage2_lowpass_hz;
+ uint8_t dyn_notch_width_percent;
+ uint8_t dyn_notch_range;
+ uint16_t dyn_notch_q;
+ uint16_t dyn_notch_min_hz;
} gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig);
diff --git a/src/main/target/common.h b/src/main/target/common.h
index e84c13ae26..29d5598c20 100755
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -64,6 +64,7 @@
#endif
#if (FLASH_SIZE > 256)
+#define USE_DYNAMIC_FILTERS
#define USE_EXTENDED_CMS_MENUS
#define USE_UAV_INTERCONNECT
#define USE_RX_UIB
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index 7f0b645222..ca2da612e6 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -50,4 +50,9 @@
#undef USE_SERIALRX_SUMH
#undef USE_SERIALRX_XBUS
#undef USE_SERIALRX_JETIEXBUS
+#endif
+
+#if defined(SIMULATOR_BUILD) || defined(UNIT_TEST)
+// This feature uses 'arm_math.h', which does not exist for x86.
+#undef USE_DYNAMIC_FILTERS
#endif
\ No newline at end of file