From cff1ea5450589e59672ea86c7a3a8c0aeecaa5e5 Mon Sep 17 00:00:00 2001 From: UAV Tech Date: Sun, 3 Feb 2019 23:49:47 -0500 Subject: [PATCH] Low loop rate Dynamic Notch optimization. --- src/main/fc/config.c | 2 +- src/main/sensors/gyroanalyse.c | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index e45dfdd885a..1f66d0b7e29 100644 --- 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 index 0c8a95e591d..6289feb786b 100644 --- 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) {