enable frequency analysis and automatic, dynamic changing of notch fi…

…lter 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

@@ -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
 
 

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" );

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;

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;

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);

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

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 |

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
 };

src/main/fc/rc_controls.h

@@ -43,7 +43,7 @@ typedef enum {
  BOXGOV,
  BOXOSD,
  BOXTELEMETRY,
- BOXGTUNE,
+ BOXDYNAMICFILTER,
  BOXSONAR,
  BOXSERVO1,
  BOXSERVO2,

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);

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);
  }

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,

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]));
  }
  }
 

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(&currentMeterADCState.mahDrawnState, currentMeterADCState.amperageLatest, lastUpdateAt);
 }