Merge pull request #7292 from kmitchel/fft_osd

Add max FFT to OSD Summary

src/main/fc/core.c

@@ -51,7 +51,9 @@
 #include "sensors/boardalignment.h"
 #include "sensors/gyro.h"
 #include "sensors/sensors.h"
-
+#if (!defined(SIMULATOR_BUILD) && !defined(UNIT_TEST))
+#include "sensors/gyroanalyse.h"
+#endif
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/core.h"
@@ -437,6 +439,10 @@ void tryArm(void)
         }
         imuQuaternionHeadfreeOffsetSet();
 
+#if (!defined(SIMULATOR_BUILD) && !defined(UNIT_TEST)  && defined(USE_GYRO_DATA_ANALYSE))
+        resetMaxFFT();
+#endif
+
         disarmAt = currentTimeUs + armingConfig()->auto_disarm_delay * 1e6;   // start disarm timeout, will be extended when throttle is nonzero
 
         lastArmingDisabledReason = 0;

src/main/interface/settings.c

@@ -1127,6 +1127,10 @@ const clivalue_t valueTable[] = {
     { "osd_stat_max_esc_rpm",       VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_ESC_RPM,     PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
     { "osd_stat_min_link_quality",  VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MIN_LINK_QUALITY,PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
     { "osd_stat_flight_dist",       VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_FLIGHT_DISTANCE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
+#ifdef USE_GYRO_DATA_ANALYSE
+    { "osd_stat_max_fft",           VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_FFT, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
+#endif
+
 #ifdef USE_OSD_PROFILES
     { "osd_profile",                VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, OSD_PROFILE_COUNT }, PG_OSD_CONFIG, offsetof(osdConfig_t, osdProfileIndex) },
 #endif

src/main/io/osd.c

@@ -95,6 +95,9 @@
 #include "sensors/battery.h"
 #include "sensors/esc_sensor.h"
 #include "sensors/sensors.h"
+#if (!defined(SIMULATOR_BUILD) && !defined(UNIT_TEST)  && defined(USE_GYRO_DATA_ANALYSE))
+#include "sensors/gyroanalyse.h"
+#endif
 
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
@@ -1821,6 +1824,18 @@ static void osdShowStats(uint16_t endBatteryVoltage)
     }
 #endif
 
+#if (!defined(SIMULATOR_BUILD) && !defined(UNIT_TEST)  && defined(USE_GYRO_DATA_ANALYSE))
+    if (osdStatGetState(OSD_STAT_MAX_FFT) && featureIsEnabled(FEATURE_DYNAMIC_FILTER)) {
+        int value = getMaxFFT();
+        if (value > 0) {
+            tfp_sprintf(buff, "%dHZ", value);
+            osdDisplayStatisticLabel(top++, "PEAK FFT", buff);
+        } else {
+            osdDisplayStatisticLabel(top++, "PEAK FFT", "THRT<20%");
+        }
+    }
+#endif
+
 }
 
 static void osdShowArmed(void)

src/main/io/osd.h

@@ -157,6 +157,7 @@ typedef enum {
     OSD_STAT_MAX_ESC_RPM,
     OSD_STAT_MIN_LINK_QUALITY,
     OSD_STAT_FLIGHT_DISTANCE,
+    OSD_STAT_MAX_FFT,
     OSD_STAT_COUNT // MUST BE LAST
 } osd_stats_e;
 

src/main/sensors/gyroanalyse.c

@@ -41,6 +41,8 @@
 #include "sensors/gyro.h"
 #include "sensors/gyroanalyse.h"
 
+#include "fc/core.h"
+
 // 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 31.25Hz wide
 // Eg [0,31), [31,62), [62, 93) etc
@@ -54,6 +56,8 @@
 // we need 4 steps for each axis
 #define DYN_NOTCH_CALC_TICKS      (XYZ_AXIS_COUNT * 4)
 
+#define DYN_NOTCH_OSD_MIN_THROTTLE 20
+
 static uint16_t FAST_RAM_ZERO_INIT   fftSamplingRateHz;
 static float FAST_RAM_ZERO_INIT      fftResolution;
 static uint8_t FAST_RAM_ZERO_INIT    fftBinOffset;
@@ -64,6 +68,7 @@ static float FAST_RAM_ZERO_INIT      dynNotch1Ctr;
 static float FAST_RAM_ZERO_INIT      dynNotch2Ctr;
 static uint16_t FAST_RAM_ZERO_INIT   dynNotchMinHz;
 static bool FAST_RAM dualNotch = true;
+static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxFFT;
 
 // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
 static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE];
@@ -323,6 +328,10 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
             state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
             state->centerFreq[state->updateAxis] = centerFreq;
 
+            if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) {
+                dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]);
+            }
+
             if (state->updateAxis == 0) {
                 DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
                 DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]);
@@ -371,4 +380,13 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
     state->updateStep = (state->updateStep + 1) % STEP_COUNT;
 }
 
+
+uint16_t getMaxFFT(void) {
+    return dynNotchMaxFFT;
+}
+
+void resetMaxFFT(void) {
+    dynNotchMaxFFT = 0;
+}
+
 #endif // USE_GYRO_DATA_ANALYSE

src/main/sensors/gyroanalyse.h

@@ -59,3 +59,5 @@ STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlyi
 void gyroDataAnalyseStateInit(gyroAnalyseState_t *gyroAnalyse, uint32_t targetLooptime);
 void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample);
 void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2);
+uint16_t getMaxFFT(void);
+void resetMaxFFT(void);