From ab4ff72cc2e4122d9a6594b9c0a900314a6104fe Mon Sep 17 00:00:00 2001 From: Kenneth Mitchell Date: Thu, 20 Dec 2018 20:58:19 -0500 Subject: [PATCH] Add max FFT to OSD Summary --- src/main/fc/core.c | 8 +++++++- src/main/interface/settings.c | 4 ++++ src/main/io/osd.c | 15 +++++++++++++++ src/main/io/osd.h | 1 + src/main/sensors/gyroanalyse.c | 18 ++++++++++++++++++ src/main/sensors/gyroanalyse.h | 2 ++ 6 files changed, 47 insertions(+), 1 deletion(-) diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 8b80f258c4..93dec31d4e 100644 --- a/src/main/fc/core.c +++ b/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; diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 9802e8b6fb..e3ff077386 100644 --- a/src/main/interface/settings.c +++ b/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 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 316a7745c0..35872bebb9 100644 --- a/src/main/io/osd.c +++ b/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" @@ -1829,6 +1832,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) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index a0bdf9c0ec..02309935a0 100644 --- a/src/main/io/osd.h +++ b/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; diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index 4295f93dc7..0c8a95e591 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/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 diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h index c708429724..de5ba174fe 100644 --- a/src/main/sensors/gyroanalyse.h +++ b/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);