Low loop rate Dynamic Notch optimization.

spatzengr · Feb 4, 2019 · cff1ea5 · cff1ea5
1 parent f19d333
commit cff1ea5
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 4 deletions.
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
@@ -72,7 +72,7 @@ pidProfile_t *currentPidProfile;
 #define RX_SPI_DEFAULT_PROTOCOL 0
 #endif
 
-#define DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME HZ_TO_INTERVAL_US(2000)
+#define DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME HZ_TO_INTERVAL_US(1333)
 
 PG_REGISTER_WITH_RESET_TEMPLATE(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 1);
 

diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c
@@ -115,8 +115,8 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
     // If we get at least 3 samples then use the default FFT sample frequency
     // otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K)
     const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
-    
-    fftSamplingRateHz = MIN((gyroLoopRateHz / 3), fftSamplingRateHz);
+
+    fftSamplingRateHz = gyroLoopRateHz / MAX(1, gyroLoopRateHz / fftSamplingRateHz);
 
     fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE;
 
@@ -275,7 +275,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
             float dataMax = 0;
             uint8_t binStart = 0;
             uint8_t binMax = 0;
-            //for bins after initial decline, identify start bin and max bin 
+            //for bins after initial decline, identify start bin and max bin
             for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) {
                 if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) {
                     if (!fftIncreased) {