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..b070586fe6 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -26,11 +26,9 @@
#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 +77,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,6 +105,14 @@ 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
@@ -117,10 +123,50 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
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 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 biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */
float biquadFilterApply(biquadFilter_t *filter, float input)
{
const float result = filter->b0 * input + filter->d1;
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index 4d86bc431e..87928da547 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -33,6 +33,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 +53,8 @@ typedef enum {
typedef enum {
FILTER_LPF,
- FILTER_NOTCH
+ FILTER_NOTCH,
+ FILTER_BPF,
} biquadFilterType_e;
typedef struct firFilter_s {
@@ -71,9 +73,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 biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApply(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/cli.c b/src/main/fc/cli.c
index 873c750320..1946ba4381 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -150,7 +150,7 @@ static const char * const featureNames[] = {
"SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD",
"UNUSED", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
- "SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", NULL
+ "SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", "DYNAMIC_FILTER", NULL
};
// sync this with rxFailsafeChannelMode_e
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_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/settings.c b/src/main/fc/settings.c
index 7ce63c7656..bab752d01a 100644
--- a/src/main/fc/settings.c
+++ b/src/main/fc/settings.c
@@ -204,7 +204,10 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
"STACK",
"ESC_SENSOR_RPM",
"ESC_SENSOR_TMP",
- "ALTITUDE"
+ "ALTITUDE",
+ "FFT",
+ "FFT_TIME",
+ "FFT_FREQ"
};
#ifdef OSD
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/gyro.c b/src/main/sensors/gyro.c
index 5ce67b6d41..6dbdcb1a1e 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -96,6 +96,8 @@ typedef struct gyroSensor_s {
biquadFilter_t notchFilter1[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilter2ApplyFn;
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
+ filterApplyFnPtr notchFilterDynApplyFn;
+ biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
} gyroSensor_t;
static gyroSensor_t gyroSensor0;
@@ -407,7 +409,6 @@ void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz)
{
-
gyroSensor->notchFilter2ApplyFn = nullFilterApply;
const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
if (notchHz && notchHz <= gyroFrequencyNyquist) {
@@ -419,11 +420,21 @@ void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
}
}
+void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
+{
+ gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
+ const float notchQ = filterGetNotchQ(400, 390); //just any init value
+ for (int axis = 0; axis < 3; axis++) {
+ biquadFilterInit(&gyroSensor->notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH);
+ }
+}
+
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
{
gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz);
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
+ gyroInitFilterDynamicNotch(gyroSensor);
}
void gyroInitFilters(void)
@@ -506,6 +517,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;
@@ -527,24 +539,37 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor)
return;
}
+#ifdef USE_GYRO_DATA_ANALYSE
+ gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn);
+#endif
+
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// 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 = gyroSensor->notchFilterDynApplyFn(&gyroSensor->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;
}
-
-#ifdef USE_GYRO_DATA_ANALYSE
- gyroDataAnalyse(&gyroSensor->gyroDev, &gyro);
-#endif
}
void gyroUpdate(void)
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..23f77a7c0c 100644
--- a/src/main/sensors/gyroanalyse.c
+++ b/src/main/sensors/gyroanalyse.c
@@ -1,35 +1,323 @@
+/*
+ * 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 60 // lower cut does not improve the performance much, higher cut makes it worse...
+#define DYN_NOTCH_MIN_CUTOFF 120 // don't cut too deep into low frequencies
+#define DYN_NOTCH_MAX_CUTOFF 200 // don't go above this cutoff (better filtering with "constant" delay at higher center frequencies)
+
+#define BIQUAD_Q 1.0f / sqrtf(2.0f) // quality factor - butterworth
+
+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)
+void initGyroData()
{
- UNUSED(gyroDev);
- UNUSED(gyro);
+ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
+ gyroData[axis][i] = 0;
+ }
+ }
}
-void gyroDataAnalyseUpdate(timeUs_t currentTimeUs)
+static inline int fftFreqToBin(int freq)
{
- UNUSED(currentTimeUs);
+ 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 < XYZ_AXIS_COUNT; axis++) {
+ fftResult[axis].centerFreq = 200; // any init value
+ biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_CHANGERATE, looptime);
+ biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE, BIQUAD_Q, FILTER_BPF);
+ }
+}
+
+// used in OSD
+const gyroFftData_t *gyroFftData(int axis)
+{
+ return &fftResult[axis];
+}
+
+bool isDynamicFilterActive(void)
+{
+ return feature(FEATURE_DYNAMIC_FILTER);
+}
+
+/*
+ * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
+ */
+void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn)
+{
+ if (!isDynamicFilterActive()) {
+ return;
+ }
+
+ // if gyro sampling is > 1kHz, accumulate multiple samples
+ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ fftAcc[axis] += gyroDev->gyroADC[axis];
+ }
+ fftAccCount++;
+
+ // this runs at 1kHz
+ if (fftAccCount == fftSamplingScale) {
+ fftAccCount = 0;
+
+ //calculate mean value of accumulated samples
+ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ float sample = fftAcc[axis] / fftSamplingScale;
+ sample = biquadFilterApply(&fftGyroFilter[axis], sample);
+ gyroData[axis][fftIdx] = sample;
+ if (axis == 0)
+ DEBUG_SET(DEBUG_FFT, 2, lrintf(sample * gyroDev->scale));
+ fftAcc[axis] = 0;
+ }
+
+ fftIdx = (fftIdx + 1) % FFT_WINDOW_SIZE;
+ }
+
+ // calculate FFT and update filters
+ gyroDataAnalyseUpdate(notchFilterDyn);
+}
+
+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(biquadFilter_t *notchFilterDyn)
+{
+ 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++;
+ // fall through
+ }
+ 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++;
+ // fall through
+ }
+ 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 = biquadFilterApply(&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));
+ }
+ }
+
+ 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(¬chFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH);
+ DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+
+ axis = (axis + 1) % 3;
+ step++;
+ // fall through
+ }
+ 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..ac4290738a 100644
--- a/src/main/sensors/gyroanalyse.h
+++ b/src/main/sensors/gyroanalyse.h
@@ -18,9 +18,17 @@
#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 {
+ 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 gyroDataAnalyse(const struct gyroDev_s *gyroDev, biquadFilter_t *notchFilterDyn);
+void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn);
+bool isDynamicFilterActive();
diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h
index f750a7ba26..d7a1b5664c 100644
--- a/src/main/target/OMNIBUS/target.h
+++ b/src/main/target/OMNIBUS/target.h
@@ -20,6 +20,7 @@
#undef TELEMETRY_IBUS //no space left
#undef TELEMETRY_HOTT //no space left
#undef TELEMETRY_JETIEXBUS
+#undef USE_GYRO_DATA_ANALYSE
#define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus
diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h
index 112471a92b..2a31aef325 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
@@ -48,6 +49,7 @@
#define USE_ESC_SENSOR
#define I2C3_OVERCLOCK true
#define TELEMETRY_IBUS
+#define USE_GYRO_DATA_ANALYSE
#endif
#ifdef STM32F7
@@ -56,6 +58,7 @@
#define I2C3_OVERCLOCK true
#define I2C4_OVERCLOCK true
#define TELEMETRY_IBUS
+#define USE_GYRO_DATA_ANALYSE
#endif
#if defined(STM32F4) || defined(STM32F7)