From d9909b91d3cf359405cd52c2023f814b8ebcc695 Mon Sep 17 00:00:00 2001 From: rav Date: Thu, 11 May 2017 16:10:29 +0200 Subject: [PATCH] enable frequency analysis and automatic, dynamic changing of notch filter frequencies change F3 from CM1 to CM4 add debug flags for FFT add bandpass filter add different filtering apply function add feature DYNAMIC_FILTER replace mode GTUNE with DYNAMIC FILTER move gyro frequency analysis into gyro loop instead of own task --- Makefile | 14 +- .../startup/TrueSTUDIO/startup_stm32f302x8.s | 0 .../startup/TrueSTUDIO/startup_stm32f303xc.s | 0 .../startup/TrueSTUDIO/startup_stm32f30x.s | 0 .../startup/TrueSTUDIO/startup_stm32f334x8.s | 0 .../startup/arm/startup_stm32f302x8.s | 0 .../startup/arm/startup_stm32f303xc.s | 0 .../STM32F30x/startup/arm/startup_stm32f30x.s | 0 .../startup/arm/startup_stm32f334x8.s | 0 .../startup/gcc_ride7/startup_stm32f30x.s | 0 .../startup/iar/startup_stm32f302x8.s | 0 .../startup/iar/startup_stm32f303xc.s | 0 .../STM32F30x/startup/iar/startup_stm32f30x.s | 0 .../startup/iar/startup_stm32f334x8.s | 0 .../DeviceSupport/ST/STM32F30x/stm32f30x.h | 0 .../ST/STM32F30x/stm32f30x_conf.h | 0 .../ST/STM32F30x/stm32f30x_gpio.c | 0 .../ST/STM32F30x/stm32f30x_gpio.h | 0 .../ST/STM32F30x/stm32f30x_rcc.c | 0 .../ST/STM32F30x/stm32f30x_rcc.h | 0 src/main/build/atomic.h | 2 +- src/main/build/debug.h | 3 + src/main/common/filter.c | 78 +++-- src/main/common/filter.h | 14 +- src/main/fc/config.h | 1 + src/main/fc/fc_msp.c | 8 +- src/main/fc/fc_tasks.c | 13 - src/main/fc/rc_controls.h | 2 +- src/main/flight/pid.c | 4 +- src/main/flight/servos.c | 2 +- src/main/scheduler/scheduler.h | 3 - src/main/sensors/acceleration.c | 2 +- src/main/sensors/current.c | 2 +- src/main/sensors/gyro.c | 45 ++- src/main/sensors/gyro.h | 1 + src/main/sensors/gyroanalyse.c | 304 +++++++++++++++++- src/main/sensors/gyroanalyse.h | 16 +- src/main/sensors/voltage.c | 4 +- src/main/target/common_fc_pre.h | 1 + 39 files changed, 449 insertions(+), 70 deletions(-) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f302x8.s (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f303xc.s (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f30x.s (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f334x8.s (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f302x8.s (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f303xc.s (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f30x.s (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f334x8.s (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/startup/gcc_ride7/startup_stm32f30x.s (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f302x8.s (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f303xc.s (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f30x.s (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f334x8.s (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/stm32f30x.h (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/stm32f30x_conf.h (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/stm32f30x_gpio.c (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/stm32f30x_gpio.h (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/stm32f30x_rcc.c (100%) rename lib/main/CMSIS/{CM1 => CM4}/DeviceSupport/ST/STM32F30x/stm32f30x_rcc.h (100%) diff --git a/Makefile b/Makefile index fdcc468d1d..e0bdeb14a3 100644 --- a/Makefile +++ b/Makefile @@ -276,14 +276,14 @@ STARTUP_SRC = startup_stm32f30x_md_gcc.S STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC)) DEVICE_STDPERIPH_SRC = $(STDPERIPH_SRC) -VPATH := $(VPATH):$(CMSIS_DIR)/CM1/CoreSupport:$(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x -CMSIS_SRC = $(notdir $(wildcard $(CMSIS_DIR)/CM1/CoreSupport/*.c \ - $(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x/*.c)) +VPATH := $(VPATH):$(CMSIS_DIR)/CM4/CoreSupport:$(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F30x +CMSIS_SRC = $(notdir $(wildcard $(CMSIS_DIR)/CM4/CoreSupport/*.c \ + $(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F30x/*.c)) INCLUDE_DIRS := $(INCLUDE_DIRS) \ $(STDPERIPH_DIR)/inc \ - $(CMSIS_DIR)/CM1/CoreSupport \ - $(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x + $(CMSIS_DIR)/CM4/CoreSupport \ + $(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F30x ifneq ($(filter VCP, $(FEATURES)),) INCLUDE_DIRS := $(INCLUDE_DIRS) \ @@ -1023,12 +1023,13 @@ else ifeq ($(TARGET),$(filter $(TARGET),$(SITL_TARGETS))) SRC := $(TARGET_SRC) $(SITL_SRC) $(VARIANT_SRC) endif -ifneq ($(filter $(TARGET),$(F4_TARGETS) $(F7_TARGETS)),) +ifneq ($(filter $(TARGET),$(F3_TARGETS) $(F4_TARGETS) $(F7_TARGETS)),) DSPLIB := $(ROOT)/lib/main/DSP_Lib DEVICE_FLAGS += -DARM_MATH_CM4 -DARM_MATH_MATRIX_CHECK -DARM_MATH_ROUNDING -D__FPU_PRESENT=1 -DUNALIGNED_SUPPORT_DISABLE INCLUDE_DIRS += $(DSPLIB)/Include +SRC += $(DSPLIB)/Source/BasicMathFunctions/arm_mult_f32.c SRC += $(DSPLIB)/Source/TransformFunctions/arm_rfft_fast_f32.c SRC += $(DSPLIB)/Source/TransformFunctions/arm_cfft_f32.c SRC += $(DSPLIB)/Source/TransformFunctions/arm_rfft_fast_init_f32.c @@ -1039,6 +1040,7 @@ SRC += $(DSPLIB)/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c SRC += $(DSPLIB)/Source/StatisticsFunctions/arm_max_f32.c SRC += $(wildcard $(DSPLIB)/Source/*/*.S) + endif diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f302x8.s b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f302x8.s similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f302x8.s rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f302x8.s diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f303xc.s b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f303xc.s similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f303xc.s rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f303xc.s diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f30x.s b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f30x.s similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f30x.s rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f30x.s diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f334x8.s b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f334x8.s similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f334x8.s rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/TrueSTUDIO/startup_stm32f334x8.s diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f302x8.s b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f302x8.s similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f302x8.s rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f302x8.s diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f303xc.s b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f303xc.s similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f303xc.s rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f303xc.s diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f30x.s b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f30x.s similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f30x.s rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f30x.s diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f334x8.s b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f334x8.s similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f334x8.s rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/arm/startup_stm32f334x8.s diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/gcc_ride7/startup_stm32f30x.s b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/gcc_ride7/startup_stm32f30x.s similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/gcc_ride7/startup_stm32f30x.s rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/gcc_ride7/startup_stm32f30x.s diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f302x8.s b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f302x8.s similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f302x8.s rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f302x8.s diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f303xc.s b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f303xc.s similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f303xc.s rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f303xc.s diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f30x.s b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f30x.s similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f30x.s rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f30x.s diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f334x8.s b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f334x8.s similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f334x8.s rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/startup/iar/startup_stm32f334x8.s diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/stm32f30x.h b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/stm32f30x.h similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/stm32f30x.h rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/stm32f30x.h diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/stm32f30x_conf.h b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/stm32f30x_conf.h similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/stm32f30x_conf.h rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/stm32f30x_conf.h diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/stm32f30x_gpio.c b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/stm32f30x_gpio.c similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/stm32f30x_gpio.c rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/stm32f30x_gpio.c diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/stm32f30x_gpio.h b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/stm32f30x_gpio.h similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/stm32f30x_gpio.h rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/stm32f30x_gpio.h diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/stm32f30x_rcc.c b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/stm32f30x_rcc.c similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/stm32f30x_rcc.c rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/stm32f30x_rcc.c diff --git a/lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/stm32f30x_rcc.h b/lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/stm32f30x_rcc.h similarity index 100% rename from lib/main/CMSIS/CM1/DeviceSupport/ST/STM32F30x/stm32f30x_rcc.h rename to lib/main/CMSIS/CM4/DeviceSupport/ST/STM32F30x/stm32f30x_rcc.h diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h index 0508f77e4f..63d2e78ffe 100644 --- a/src/main/build/atomic.h +++ b/src/main/build/atomic.h @@ -31,7 +31,7 @@ __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX_nb(uint3 __ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) ); } -#if !defined(STM32F4) && !defined(STM32F7) /* already defined in /lib/main/CMSIS/CM4/CoreSupport/core_cmFunc.h for F4 targets */ +#if !defined(STM32F3) && !defined(STM32F4) && !defined(STM32F7) /* already defined in /lib/main/CMSIS/CM4/CoreSupport/core_cmFunc.h for F4 targets */ __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX(uint32_t basePri) { __ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) : "memory" ); diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 4882b9e96b..6df4163eee 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -65,5 +65,8 @@ typedef enum { DEBUG_ESC_SENSOR_RPM, DEBUG_ESC_SENSOR_TMP, DEBUG_ALTITUDE, + 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 f3582f213e..926891945d 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -24,13 +24,6 @@ #include "common/maths.h" #include "common/utils.h" -#define M_LN2_FLOAT 0.69314718055994530942f -#define M_PI_FLOAT 3.14159265358979323846f - -#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */ -#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/ - - // NULL filter float nullFilterApply(void *filter, float input) @@ -79,22 +72,22 @@ void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refr { biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF); } + void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType) { // setup variables - const float sampleRate = 1 / ((float)refreshRate * 0.000001f); - const float omega = 2 * M_PI_FLOAT * filterFreq / sampleRate; + const float omega = 2.0f * M_PI_FLOAT * filterFreq * refreshRate * 0.000001f; const float sn = sinf(omega); const float cs = cosf(omega); - const float alpha = sn / (2 * Q); + const float alpha = sn / (2.0f * Q); float b0 = 0, b1 = 0, b2 = 0, a0 = 0, a1 = 0, a2 = 0; switch (filterType) { case FILTER_LPF: - b0 = (1 - cs) / 2; + b0 = (1 - cs) * 0.5f; b1 = 1 - cs; - b2 = (1 - cs) / 2; + b2 = (1 - cs) * 0.5f; a0 = 1 + alpha; a1 = -2 * cs; a2 = 1 - alpha; @@ -107,21 +100,70 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh a1 = -2 * cs; a2 = 1 - alpha; break; + case FILTER_BPF: + b0 = alpha; + b1 = 0; + b2 = -alpha; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; + break; } // precompute the coefficients - filter->b0 = b0 / a0; - filter->b1 = b1 / a0; - filter->b2 = b2 / a0; - filter->a1 = a1 / a0; - filter->a2 = a2 / a0; + a0 = 1.0f / a0; + filter->b0 = b0 * a0; + filter->b1 = b1 * a0; + filter->b2 = b2 * a0; + filter->a1 = a1 * a0; + filter->a2 = a2 * a0; // zero initial samples + filter->x1 = filter->x2 = 0; + filter->y1 = filter->y2 = 0; filter->d1 = filter->d2 = 0; } -/* Computes a biquadFilter_t filter on a sample */ +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; + float d1 = filter->d1; + float d2 = filter->d2; + + biquadFilterInit(filter, filterFreq, refreshRate, Q, filterType); + + // restore state + filter->x1 = x1; + filter->x2 = x2; + filter->y1 = y1; + filter->y2 = y2; + filter->d1 = d1; + filter->d2 = d2; +} + +/* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */ float biquadFilterApply(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 biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */ +float biquadFilterApplyDF2(biquadFilter_t *filter, float input) { const float result = filter->b0 * input + filter->d1; filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2; diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 4d86bc431e..d6c6099b5d 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -23,6 +23,11 @@ #define MAX_FIR_DENOISE_WINDOW_SIZE 120 #endif +#define M_LN2_FLOAT 0.69314718055994530942f +#define M_PI_FLOAT 3.14159265358979323846f +#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */ +#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/ + typedef struct pt1Filter_s { float state; float k; @@ -33,6 +38,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 x1, x2, y1, y2; float d1, d2; } biquadFilter_t; @@ -52,7 +58,8 @@ typedef enum { typedef enum { FILTER_LPF, - FILTER_NOTCH + FILTER_NOTCH, + FILTER_BPF, } biquadFilterType_e; typedef struct firFilter_s { @@ -71,9 +78,14 @@ float nullFilterApply(void *filter, float input); void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); +void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); float biquadFilterApply(biquadFilter_t *filter, float input); +float biquadFilterApplyDF2(biquadFilter_t *filter, float input); float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff); +// not exactly correct, but very very close and much much faster +#define filterGetNotchQApprox(centerFreq, cutoff) ((float)(cutoff * centerFreq) / ((float)(centerFreq - cutoff) * (float)(centerFreq + cutoff))) + void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT); float pt1FilterApply(pt1Filter_t *filter, float input); float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index fdb7204b20..4f2ee925b2 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -60,6 +60,7 @@ typedef enum { FEATURE_SOFTSPI = 1 << 26, FEATURE_ESC_SENSOR = 1 << 27, FEATURE_ANTI_GRAVITY = 1 << 28, + FEATURE_DYNAMIC_FILTER = 1 << 29, } features_e; #define MAX_NAME_LENGTH 16 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 94fd736792..2dff3de8ee 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -135,7 +135,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXGOV, "GOVERNOR", 18 }, { BOXOSD, "OSD SW", 19 }, { BOXTELEMETRY, "TELEMETRY", 20 }, - { BOXGTUNE, "GTUNE", 21 }, + { BOXDYNAMICFILTER, "DYNAMIC FILTER", 21 }, { BOXSONAR, "SONAR", 22 }, { BOXSERVO1, "SERVO1", 23 }, { BOXSERVO2, "SERVO2", 24 }, @@ -306,6 +306,10 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXANTIGRAVITY; } + if (!feature(FEATURE_DYNAMIC_FILTER)) { + activeBoxIds[activeBoxIdCount++] = BOXDYNAMICFILTER; + } + if (sensors(SENSOR_ACC)) { activeBoxIds[activeBoxIdCount++] = BOXANGLE; activeBoxIds[activeBoxIdCount++] = BOXHORIZON; @@ -418,7 +422,7 @@ static uint32_t packFlightModeFlags(void) IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXDYNAMICFILTER)) << BOXDYNAMICFILTER | IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR | IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index a34c582f8f..7cd4f5487e 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -75,7 +75,6 @@ #include "sensors/battery.h" #include "sensors/compass.h" #include "sensors/gyro.h" -#include "sensors/gyroanalyse.h" #include "sensors/sonar.h" #include "sensors/esc_sensor.h" @@ -352,9 +351,6 @@ void fcTasksInit(void) setTaskEnabled(TASK_VTXCTRL, true); #endif #endif -#ifdef USE_GYRO_DATA_ANALYSE - setTaskEnabled(TASK_GYRO_DATA_ANALYSE, true); -#endif } #endif @@ -597,14 +593,5 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_IDLE, }, #endif - -#ifdef USE_GYRO_DATA_ANALYSE - [TASK_GYRO_DATA_ANALYSE] = { - .taskName = "GYROFFT", - .taskFunc = gyroDataAnalyseUpdate, - .desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz, 10ms - .staticPriority = TASK_PRIORITY_MEDIUM, - }, -#endif #endif }; diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 9be4198d2a..4eeacb636b 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -43,7 +43,7 @@ typedef enum { BOXGOV, BOXOSD, BOXTELEMETRY, - BOXGTUNE, + BOXDYNAMICFILTER, BOXSONAR, BOXSERVO1, BOXSERVO2, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 7fbb9838b8..4a080789de 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -171,7 +171,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) if (pidProfile->dterm_notch_hz == 0 || pidProfile->dterm_notch_hz > pidFrequencyNyquist) { dtermNotchFilterApplyFn = nullFilterApply; } else { - dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; + dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2; const float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { dtermFilterNotch[axis] = &biquadFilterNotch[axis]; @@ -194,7 +194,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) } break; case FILTER_BIQUAD: - dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApply; + dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2; for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { dtermFilterLpf[axis] = &biquadFilter[axis]; biquadFilterInitLPF(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 260af81f36..b2fb2a0c21 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -514,7 +514,7 @@ static void filterServos(void) servoFilterIsSet = true; } - servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx])); + servo[servoIdx] = lrintf(biquadFilterApplyDF2(&servoFilter[servoIdx], (float)servo[servoIdx])); // Sanity check servo[servoIdx] = constrain(servo[servoIdx], servoParams(servoIdx)->min, servoParams(servoIdx)->max); } diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 69641c4920..7f819b6df6 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -110,9 +110,6 @@ typedef enum { #ifdef VTX_CONTROL TASK_VTXCTRL, #endif -#ifdef USE_GYRO_DATA_ANALYSE - TASK_GYRO_DATA_ANALYSE, -#endif /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 29b567e7dc..d64c38f870 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -465,7 +465,7 @@ void accUpdate(rollAndPitchTrims_t *rollAndPitchTrims) if (accLpfCutHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - acc.accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)acc.accSmooth[axis])); + acc.accSmooth[axis] = lrintf(biquadFilterApplyDF2(&accFilter[axis], (float)acc.accSmooth[axis])); } } diff --git a/src/main/sensors/current.c b/src/main/sensors/current.c index da7654357c..5552bada66 100644 --- a/src/main/sensors/current.c +++ b/src/main/sensors/current.c @@ -135,7 +135,7 @@ void currentMeterADCRefresh(int32_t lastUpdateAt) { const uint16_t iBatSample = adcGetChannel(ADC_CURRENT); currentMeterADCState.amperageLatest = currentMeterADCToCentiamps(iBatSample); - currentMeterADCState.amperage = currentMeterADCToCentiamps(biquadFilterApply(&adciBatFilter, iBatSample)); + currentMeterADCState.amperage = currentMeterADCToCentiamps(biquadFilterApplyDF2(&adciBatFilter, iBatSample)); updateCurrentmAhDrawnState(¤tMeterADCState.mahDrawnState, currentMeterADCState.amperageLatest, lastUpdateAt); } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5ce67b6d41..e994472858 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -102,6 +102,9 @@ static gyroSensor_t gyroSensor0; static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); +static filterApplyFnPtr notchFilterDynApplyFn; +biquadFilter_t *notchFilterDyn[3]; + #define DEBUG_GYRO_CALIBRATION 3 #ifdef STM32F10X @@ -367,7 +370,7 @@ void gyroInitFilterLpf(gyroSensor_t *gyroSensor, uint8_t lpfHz) if (lpfHz && lpfHz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known switch (gyroConfig()->gyro_soft_lpf_type) { case FILTER_BIQUAD: - gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; + gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2; for (int axis = 0; axis < 3; axis++) { gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->softLpfFilter.gyroFilterLpfState[axis]; biquadFilterInitLPF(&gyroSensor->softLpfFilter.gyroFilterLpfState[axis], lpfHz, gyro.targetLooptime); @@ -397,7 +400,7 @@ void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n gyroSensor->notchFilter1ApplyFn = nullFilterApply; const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed if (notchHz && notchHz <= gyroFrequencyNyquist) { - gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; + gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2; const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); for (int axis = 0; axis < 3; axis++) { biquadFilterInit(&gyroSensor->notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH); @@ -411,7 +414,7 @@ void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n gyroSensor->notchFilter2ApplyFn = nullFilterApply; const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed if (notchHz && notchHz <= gyroFrequencyNyquist) { - gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; + gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2; const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); for (int axis = 0; axis < 3; axis++) { biquadFilterInit(&gyroSensor->notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH); @@ -426,9 +429,23 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); } +void gyroInitFilterDynamicNotch() +{ + static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT]; + + notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApply; //must be this function, do not change + + const float notchQ = filterGetNotchQ(400, 390); //just any init value + for (int axis = 0; axis < 3; axis++) { + notchFilterDyn[axis] = &gyroFilterNotch[axis]; + biquadFilterInit(notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH); + } +} + void gyroInitFilters(void) { gyroInitSensorFilters(&gyroSensor0); + gyroInitFilterDynamicNotch(); } bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor) @@ -506,6 +523,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t void gyroUpdateSensor(gyroSensor_t *gyroSensor) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { + return; } gyroSensor->gyroDev.dataReady = false; @@ -531,14 +549,27 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor) // scale gyro output to degrees per second float gyroADCf = (float)gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; +#ifdef USE_GYRO_DATA_ANALYSE + // Apply Dynamic Notch filtering + if (axis == 0) + DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data + + if (isDynamicFilterActive()) + gyroADCf = notchFilterDynApplyFn(notchFilterDyn[axis], gyroADCf); + + if (axis == 0) + DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch +#endif + + // Apply Static Notch filtering + DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf)); + gyroADCf = gyroSensor->notchFilter1ApplyFn(&gyroSensor->notchFilter1[axis], gyroADCf); + gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf); + // Apply LPF DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf); - // Apply Notch filtering - DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf)); - gyroADCf = gyroSensor->notchFilter1ApplyFn(&gyroSensor->notchFilter1[axis], gyroADCf); - gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf); gyro.gyroADCf[axis] = gyroADCf; } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 0b2c0c36a4..c4f3aab6bf 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -66,6 +66,7 @@ typedef struct gyroConfig_s { PG_DECLARE(gyroConfig_t, gyroConfig); bool gyroInit(void); + void gyroInitFilters(void); void gyroUpdate(void); const busDevice_t *gyroSensorBus(void); diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index 5109c75213..ed8a66d391 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -1,35 +1,321 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it 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 is distributed in the hope that it 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 Cleanflight. If not, see . + */ + #include #include "platform.h" #ifdef USE_GYRO_DATA_ANALYSE - #include "arm_math.h" #include "build/debug.h" +#include "common/filter.h" #include "common/maths.h" #include "common/time.h" #include "common/utils.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/accgyro/accgyro.h" +#include "drivers/system.h" + +#include "fc/config.h" +#include "fc/rc_controls.h" #include "sensors/gyro.h" #include "sensors/gyroanalyse.h" +#include "common/filter.h" -void gyroDataAnalyseInit(void) -{ +// 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 with a width 31.25Hz +// Eg [0,31), [31,62), [62, 93) etc + +#define FFT_WINDOW_SIZE 32 // max for f3 targets +#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_BPF_HZ 200 // use a bandpass on gyro data to ignore extreme low and extreme high frequencies +#define DYN_NOTCH_WIDTH 100 // just an orientation and start value +#define DYN_NOTCH_CHANGERATE 40 // 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) + +static uint16_t samplingFrequency; // gyro rate +static uint8_t fftBinCount; +static float fftResolution; // hz per bin +static float gyroData[3][FFT_WINDOW_SIZE]; // gyro data used for frequency analysis + +static arm_rfft_fast_instance_f32 fftInstance; +static float fftData[FFT_WINDOW_SIZE]; +static float rfftData[FFT_WINDOW_SIZE]; +static gyroFftData_t fftResult[3]; +static uint16_t fftMaxFreq = 0; // nyquist rate +static uint16_t fftIdx = 0; // use a circular buffer for the last FFT_WINDOW_SIZE samples + + +// accumulator for oversampled data => no aliasing and less noise +static float fftAcc[3] = {0, 0, 0}; +static int fftAccCount = 0; +static int fftSamplingScale; + +// bandpass filter gyro data +static biquadFilter_t fftGyroFilter[3]; + +// filter for smoothing frequency estimation +static biquadFilter_t fftFreqFilter[3]; + +// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window +static float hanningWindow[FFT_WINDOW_SIZE]; + +void initHanning() { + for (int i = 0; i < FFT_WINDOW_SIZE; ++i) { + hanningWindow[i] = (0.5 - 0.5 * cosf(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1))); + } } -void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro) -{ - UNUSED(gyroDev); +void initGyroData() { + for (int axis = 0; axis < 3; ++axis) { + for (int i = 0; i < FFT_WINDOW_SIZE; ++i) { + gyroData[axis][i] = 0; + } + } +} + +static inline int fftFreqToBin(int freq) { + return ((FFT_WINDOW_SIZE / 2 - 1) * freq) / (fftMaxFreq); +} + +void gyroDataAnalyseInit(uint32_t targetLooptimeUs) { + // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later + samplingFrequency = 1000000 / targetLooptimeUs; + fftSamplingScale = samplingFrequency / FFT_SAMPLING_RATE; + fftMaxFreq = FFT_SAMPLING_RATE / 2; + fftBinCount = fftFreqToBin(fftMaxFreq) + 1; + fftResolution = FFT_SAMPLING_RATE / FFT_WINDOW_SIZE; + arm_rfft_fast_init_f32(&fftInstance, FFT_WINDOW_SIZE); + + initGyroData(); + initHanning(); + + // recalculation of filters takes 4 calls per axis => each filter gets updated every 3 * 4 = 12 calls + // at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms + float looptime = targetLooptimeUs * 4 * 3; + for (int axis = 0; axis < 3; ++axis) { + fftResult[axis].centerFreq = 200; + biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_CHANGERATE, looptime); + biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE, BIQUAD_Q, FILTER_BPF); + } +} + +// used in OSD +const gyroFftData_t *gyroFftData(int axis) { + return &fftResult[axis]; +} + +bool isDynamicFilterActive(void) { + return (IS_RC_MODE_ACTIVE(BOXDYNAMICFILTER) || feature(FEATURE_DYNAMIC_FILTER)); +} + +/* + * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function + */ +void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro) { + if (!isDynamicFilterActive()) { + return; + } UNUSED(gyro); + + // if gyro sampling is > 1kHz, accumulate multiple samples + for (int axis = 0; axis < 3; ++axis) { + fftAcc[axis] += gyroDev->gyroADC[axis] * gyroDev->scale; + } + fftAccCount++; + + // this runs at 1kHz + if (fftAccCount == fftSamplingScale) { + fftAccCount = 0; + + //calculate mean value of accumulated samples + for (int axis = 0; axis < 3; ++axis) { + float sample = fftAcc[axis] / fftSamplingScale; + sample = biquadFilterApplyDF2(&fftGyroFilter[axis], sample); + gyroData[axis][fftIdx] = sample; + if (axis == 0) + DEBUG_SET(DEBUG_FFT, 2, lrintf(sample)); + fftAcc[axis] = 0; + } + + fftIdx = (fftIdx + 1) % FFT_WINDOW_SIZE; + } + + // calculate FFT and update filters + gyroDataAnalyseUpdate(); } -void gyroDataAnalyseUpdate(timeUs_t currentTimeUs) -{ - UNUSED(currentTimeUs); +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); + +typedef 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 +} UpdateStep_e; + +/* + * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds + */ +void gyroDataAnalyseUpdate() { + static int axis = 0; + static int step = 0; + arm_cfft_instance_f32 * Sint = &(fftInstance.Sint); + + uint32_t startTime = 0; + if (debugMode == (DEBUG_FFT_TIME)) + startTime = micros(); + + DEBUG_SET(DEBUG_FFT_TIME, 0, step); + switch (step) { + case STEP_ARM_CFFT_F32: + { + switch (FFT_WINDOW_SIZE / 2) { + case 16: + // 16us + arm_cfft_radix8by2_f32(Sint, fftData); + break; + case 32: + // 35us + arm_cfft_radix8by4_f32(Sint, fftData); + break; + case 64: + // 70us + arm_radix8_butterfly_f32(fftData, FFT_WINDOW_SIZE / 2, Sint->pTwiddle, 1); + break; + } + DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); + break; + } + case STEP_BITREVERSAL: + { + // 6us + arm_bitreversal_32((uint32_t*) fftData, Sint->bitRevLength, Sint->pBitRevTable); + DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); + step++; + //break; + } + case STEP_STAGE_RFFT_F32: + { + // 14us + // this does not work in place => fftData AND rfftData needed + stage_rfft_f32(&fftInstance, fftData, rfftData); + DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); + break; + } + case STEP_ARM_CMPLX_MAG_F32: + { + // 8us + arm_cmplx_mag_f32(rfftData, fftData, fftBinCount); + DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime); + step++; + //break; + } + case STEP_CALC_FREQUENCIES: + { + // 13us + float fftSum = 0; + float fftWeightedSum = 0; + + fftResult[axis].maxVal = 0; + // iterate over fft data and calculate weighted indexes + float squaredData; + for (int i = 0; i < fftBinCount; ++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 * fftResolution, DYN_NOTCH_MIN_CUTOFF + 10, fftMaxFreq); + centerFreq = biquadFilterApplyDF2(&fftFreqFilter[axis], centerFreq); + centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CUTOFF + 10, fftMaxFreq); + fftResult[axis].centerFreq = centerFreq; + if (axis == 0) { + DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100)); + } + } + + // copy data for display in OSD + const float scaleFactor = 255.0 / MIN(1, fftResult[axis].maxVal); + const int count = MIN(GYRO_FFT_BIN_COUNT, fftBinCount); + for (int ii = 0; ii < count; ++ii) { + fftResult[axis].bins[ii] = fftData[ii] * scaleFactor; + } + + + DEBUG_SET(DEBUG_FFT_FREQ, axis, fftResult[axis].centerFreq); + DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); + break; + } + 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); + float notchQ = filterGetNotchQApprox(fftResult[axis].centerFreq, cutoffFreq); + biquadFilterUpdate(notchFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH); + DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); + + axis = (axis + 1) % 3; + step++; + } + 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 + uint8_t ringBufIdx = FFT_WINDOW_SIZE - fftIdx; + arm_mult_f32(&gyroData[axis][fftIdx], &hanningWindow[0], &fftData[0], ringBufIdx); + if (fftIdx > 0) + arm_mult_f32(&gyroData[axis][0], &hanningWindow[ringBufIdx], &fftData[ringBufIdx], fftIdx); + + DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); + } + } + + step = (step + 1) % STEP_COUNT; } + #endif // USE_GYRO_DATA_ANALYSE diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h index d63b5023e7..0bca01dc4a 100644 --- a/src/main/sensors/gyroanalyse.h +++ b/src/main/sensors/gyroanalyse.h @@ -18,9 +18,21 @@ #pragma once #include "common/time.h" +#include "common/filter.h" -void gyroDataAnalyseInit(void); +#define GYRO_FFT_BIN_COUNT 16 // FFT_WINDOW_SIZE / 2 +typedef struct gyroFftData_s { + uint8_t bins[GYRO_FFT_BIN_COUNT]; + float maxVal; + uint16_t centerFreq; +} gyroFftData_t; + +void gyroDataAnalyseInit(uint32_t targetLooptime); +const gyroFftData_t *gyroFftData(int axis); struct gyroDev_s; struct gyro_s; void gyroDataAnalyse(const struct gyroDev_s *gyroDev, const struct gyro_s *gyro); -void gyroDataAnalyseUpdate(timeUs_t currentTimeUs); +void gyroDataAnalyseUpdate(); +bool isDynamicFilterActive(); + +extern biquadFilter_t *notchFilterDyn[3]; diff --git a/src/main/sensors/voltage.c b/src/main/sensors/voltage.c index aa9db9dfeb..5bf9f5b7d4 100644 --- a/src/main/sensors/voltage.c +++ b/src/main/sensors/voltage.c @@ -153,7 +153,7 @@ void voltageMeterADCRefresh(void) uint8_t channel = voltageMeterAdcChannelMap[i]; uint16_t rawSample = adcGetChannel(channel); - uint16_t filteredSample = biquadFilterApply(&state->filter, rawSample); + uint16_t filteredSample = biquadFilterApplyDF2(&state->filter, rawSample); // always calculate the latest voltage, see getLatestVoltage() which does the calculation on demand. state->voltageFiltered = voltageAdcToVoltage(filteredSample, config); @@ -210,7 +210,7 @@ void voltageMeterESCRefresh(void) #ifdef USE_ESC_SENSOR escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); voltageMeterESCState.voltageUnfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage / 10 : 0; - voltageMeterESCState.voltageFiltered = biquadFilterApply(&voltageMeterESCState.filter, voltageMeterESCState.voltageUnfiltered); + voltageMeterESCState.voltageFiltered = biquadFilterApplyDF2(&voltageMeterESCState.filter, voltageMeterESCState.voltageUnfiltered); #endif } diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 112471a92b..16a24e8986 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -41,6 +41,7 @@ #ifdef STM32F3 #define MINIMAL_CLI #define USE_DSHOT +#define USE_GYRO_DATA_ANALYSE #endif #ifdef STM32F4