diff --git a/Makefile b/Makefile
index fdcc468d1..e0bdeb14a 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 0508f77e4..63d2e78ff 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 4882b9e96..6df4163ee 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 f3582f213..926891945 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 4d86bc431..d6c6099b5 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 fdb7204b2..4f2ee925b 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 94fd73679..2dff3de8e 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 a34c582f8..7cd4f5487 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 9be4198d2..4eeacb636 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 7fbb9838b..4a080789d 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 260af81f3..b2fb2a0c2 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 69641c492..7f819b6df 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 29b567e7d..d64c38f87 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 da7654357..5552bada6 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 5ce67b6d4..e99447285 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 0b2c0c36a..c4f3aab6b 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 5109c7521..ed8a66d39 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 d63b5023e..0bca01dc4 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 aa9db9dfe..5bf9f5b7d 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 112471a92..16a24e898 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