Merge pull request #10662 from klutvott123/dynamic-notch-update-fix

Allow re-initialising dynamic notch

src/main/flight/gyroanalyse.c

@@ -118,16 +118,9 @@ static uint16_t FAST_DATA_ZERO_INIT   dynNotchMaxFFT;
 static float FAST_DATA_ZERO_INIT      smoothFactor;
 static uint8_t FAST_DATA_ZERO_INIT    numSamples;
 
-void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
+void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
 {
-#ifdef USE_MULTI_GYRO
+    // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
-    static bool gyroAnalyseInitialized;
-    if (gyroAnalyseInitialized) {
-        return;
-    }
-    gyroAnalyseInitialized = true;
-#endif
-
     dynNotchBandwidthHz = gyroConfig()->dyn_notch_bandwidth_hz;
     dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
     dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz);
@@ -152,12 +145,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
     for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, numSamples);
     }
-}
 
-void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
-{
-    // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
-    gyroDataAnalyseInit(targetLooptimeUs);
     state->maxSampleCount = numSamples;
     state->maxSampleCountRcp = 1.0f / state->maxSampleCount;
 

src/main/flight/gyroanalyse.h

@@ -42,7 +42,7 @@ typedef struct gyroAnalyseState_s {
 
 } gyroAnalyseState_t;
 
-void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs);
+void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs);
 void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample);
 void gyroDataAnalyse(gyroAnalyseState_t *state);
 uint16_t getMaxFFT(void);

src/main/sensors/gyro.c

@@ -134,8 +134,8 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
     gyroConfig->dyn_notch_min_hz = 150;
     gyroConfig->gyro_filter_debug_axis = FD_ROLL;
     gyroConfig->dyn_lpf_curve_expo = 5;
-	gyroConfig->simplified_gyro_filter = false;
+    gyroConfig->simplified_gyro_filter = false;
-	gyroConfig->simplified_gyro_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT;
+    gyroConfig->simplified_gyro_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT;
 }
 
 #ifdef USE_GYRO_DATA_ANALYSE

src/main/sensors/gyro.h

@@ -166,11 +166,11 @@ enum {
 };
 
 typedef struct gyroConfig_s {
-    uint8_t  gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
+    uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
-    uint8_t  gyro_hardware_lpf;                // gyro DLPF setting
+    uint8_t gyro_hardware_lpf;                // gyro DLPF setting
 
-    uint8_t  gyro_high_fsr;
+    uint8_t gyro_high_fsr;
-    uint8_t  gyro_to_use;
+    uint8_t gyro_to_use;
 
     uint16_t gyro_lowpass_hz;
     uint16_t gyro_lowpass2_hz;
@@ -179,15 +179,15 @@ typedef struct gyroConfig_s {
     uint16_t gyro_soft_notch_cutoff_1;
     uint16_t gyro_soft_notch_hz_2;
     uint16_t gyro_soft_notch_cutoff_2;
-    int16_t  gyro_offset_yaw;
+    int16_t gyro_offset_yaw;
-    uint8_t  checkOverflow;
+    uint8_t checkOverflow;
 
     // Lowpass primary/secondary
-    uint8_t  gyro_lowpass_type;
+    uint8_t gyro_lowpass_type;
-    uint8_t  gyro_lowpass2_type;
+    uint8_t gyro_lowpass2_type;
 
-    uint8_t  yaw_spin_recovery;
+    uint8_t yaw_spin_recovery;
-    int16_t  yaw_spin_threshold;
+    int16_t yaw_spin_threshold;
 
     uint16_t gyroCalibrationDuration;   // Gyro calibration duration in 1/100 second
 
@@ -195,16 +195,16 @@ typedef struct gyroConfig_s {
     uint16_t dyn_lpf_gyro_max_hz;
 
     uint16_t dyn_notch_max_hz;
-    uint8_t  dyn_notch_count;
+    uint8_t dyn_notch_count;
     uint16_t dyn_notch_bandwidth_hz;
     uint16_t dyn_notch_min_hz;
 
-    uint8_t  gyro_filter_debug_axis;
+    uint8_t gyro_filter_debug_axis;
 
     uint8_t gyrosDetected; // What gyros should detection be attempted for on startup. Automatically set on first startup.
     uint8_t dyn_lpf_curve_expo; // set the curve for dynamic gyro lowpass filter
-    uint8_t  simplified_gyro_filter;
+    uint8_t simplified_gyro_filter;
-    uint8_t  simplified_gyro_filter_multiplier;
+    uint8_t simplified_gyro_filter_multiplier;
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

src/main/sensors/gyro_init.c

@@ -264,7 +264,7 @@ void gyroInitFilters(void)
     dynLpfFilterInit();
 #endif
 #ifdef USE_GYRO_DATA_ANALYSE
-    gyroDataAnalyseStateInit(&gyro.gyroAnalyseState, gyro.targetLooptime);
+    gyroDataAnalyseInit(&gyro.gyroAnalyseState, gyro.targetLooptime);
 #endif
 }