From 04bd9e2bb33b1c0f246024f7e9a2c71a77b8cbbe Mon Sep 17 00:00:00 2001
From: rtlopez <rtlopezdev@gmail.com>
Date: Wed, 2 Aug 2023 00:50:02 +0200
Subject: [PATCH 1/4] handle more debug modes + debug x8 fix

---
 lib/Espfc/src/Actuator.h          |  7 +++++++
 lib/Espfc/src/Blackbox.h          | 19 ++++++++++++++++---
 lib/Espfc/src/Controller.h        | 12 ++++++++++++
 lib/Espfc/src/Input.h             | 12 ++++++++++--
 lib/Espfc/src/ModelState.h        |  1 +
 lib/Espfc/src/Output/Mixer.h      | 13 +++++++++++++
 lib/Espfc/src/Sensor/GyroSensor.h |  3 ++-
 lib/Espfc/src/Stats.h             |  3 ++-
 lib/betaflight/src/platform.h     |  2 +-
 9 files changed, 64 insertions(+), 8 deletions(-)

diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h
index 9a2dac68..525d02fa 100644
--- a/lib/Espfc/src/Actuator.h
+++ b/lib/Espfc/src/Actuator.h
@@ -20,6 +20,7 @@ class Actuator
 
     int update()
     {
+      uint32_t startTime = micros();
       Stats::Measure(_model.state.stats, COUNTER_ACTUATOR);
       updateArmingDisabled();
       updateModeMask();
@@ -28,6 +29,12 @@ class Actuator
       updateScaler();
       updateBuzzer();
       updateDynLpf();
+
+      if(_model.config.debugMode == DEBUG_PIDLOOP)
+      {
+        _model.state.debug[4] = micros() - startTime;
+      }
+
       return 1;
     }
 
diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h
index 00f862a7..fe8c0773 100644
--- a/lib/Espfc/src/Blackbox.h
+++ b/lib/Espfc/src/Blackbox.h
@@ -316,11 +316,19 @@ class Blackbox
       if(!_model.blackboxEnabled()) return 0;
       if(!blackboxSerial) return 0;
       Stats::Measure measure(_model.state.stats, COUNTER_BLACKBOX);
+
+      uint32_t startTime = micros();
       updateArmed();
       updateMode();
       updateData();
       blackboxUpdate(_model.state.loopTimer.last);
       _buffer.flush();
+
+      if(_model.config.debugMode == DEBUG_PIDLOOP)
+      {
+        _model.state.debug[5] = micros() - startTime;
+      }
+
       return 1;
     }
 
@@ -330,7 +338,7 @@ class Blackbox
       for(size_t i = 0; i < 3; i++)
       {
         gyro.gyroADCf[i] = degrees(_model.state.gyro[i]);
-        gyro.gyroADC[i] = degrees(_model.state.gyroSampled[i]);
+        gyro.gyroADC[i] = degrees(_model.state.gyroScaled[i]);
         pidData[i].P = _model.state.innerPid[i].pTerm * 1000.f;
         pidData[i].I = _model.state.innerPid[i].iTerm * 1000.f;
         pidData[i].D = _model.state.innerPid[i].dTerm * 1000.f;
@@ -350,10 +358,15 @@ class Blackbox
       for(size_t i = 0; i < 4; i++)
       {
         motor[i] = Math::clamp(_model.state.outputUs[i], (int16_t)1000, (int16_t)2000);
-        if(_model.state.digitalOutput) {
+        if(_model.state.digitalOutput)
+        {
           motor[i] = PWM_TO_DSHOT(motor[i]);
         }
-        if(_model.config.debugMode != DEBUG_NONE && _model.config.debugMode != DEBUG_BLACKBOX_OUTPUT) {
+      }
+      if(_model.config.debugMode != DEBUG_NONE && _model.config.debugMode != DEBUG_BLACKBOX_OUTPUT)
+      {
+        for(size_t i = 0; i < 8; i++)
+        {
           debug[i] = _model.state.debug[i];
         }
       }
diff --git a/lib/Espfc/src/Controller.h b/lib/Espfc/src/Controller.h
index 5cb75c53..a1bd9f35 100644
--- a/lib/Espfc/src/Controller.h
+++ b/lib/Espfc/src/Controller.h
@@ -42,6 +42,13 @@ class Controller
 
     int update()
     {
+      uint32_t startTime = 0;
+      if(_model.config.debugMode == DEBUG_PIDLOOP)
+      {
+        startTime = micros();
+        _model.state.debug[0] = startTime - _model.state.loopTimer.last;
+      }
+
       {
         Stats::Measure(_model.state.stats, COUNTER_OUTER_PID);
         resetIterm();
@@ -67,6 +74,11 @@ class Controller
         }
       }
 
+      if(_model.config.debugMode == DEBUG_PIDLOOP)
+      {
+        _model.state.debug[2] = micros() - startTime;
+      }
+
       return 1;
     }
 
diff --git a/lib/Espfc/src/Input.h b/lib/Espfc/src/Input.h
index 1a675ee9..09af602e 100644
--- a/lib/Espfc/src/Input.h
+++ b/lib/Espfc/src/Input.h
@@ -100,11 +100,19 @@ class Input
     {
       if(!_device) return 0;
 
+      uint32_t startTime = micros();
+
       InputStatus status = readInputs();
 
-      if(failsafe(status)) return 1;
+      if(!failsafe(status))
+      {
+        filterInputs(status);
+      }
 
-      filterInputs(status);
+      if(_model.config.debugMode == DEBUG_PIDLOOP)
+      {
+        _model.state.debug[1] = micros() - startTime;
+      }
 
       return 1;
     }
diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h
index f7433773..98e57528 100644
--- a/lib/Espfc/src/ModelState.h
+++ b/lib/Espfc/src/ModelState.h
@@ -139,6 +139,7 @@ struct ModelState
 
   VectorInt16 gyroRaw;
   VectorFloat gyroSampled;
+  VectorFloat gyroScaled;
   VectorFloat gyroDynNotch;
   VectorFloat gyroImu;
 
diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h
index 7ef88105..0fff92e2 100644
--- a/lib/Espfc/src/Output/Mixer.h
+++ b/lib/Espfc/src/Output/Mixer.h
@@ -76,12 +76,25 @@ class Mixer
 
     int update()
     {
+      uint32_t startTime = micros();
+
       float outputs[OUTPUT_CHANNELS];
       const MixerConfig& mixer = _model.state.currentMixer;
 
       updateMixer(mixer, outputs);
       writeOutput(mixer, outputs);
 
+      if(_model.config.debugMode == DEBUG_PIDLOOP)
+      {
+        _model.state.debug[3] = micros() - startTime;
+      }
+
+      if(_model.config.debugMode == DEBUG_CYCLETIME)
+      {
+        _model.state.debug[0] = _model.state.stats.loopTime();
+        _model.state.debug[1] = lrintf(_model.state.stats.getCpuLoad());
+      }
+
       return 1;
     }
 
diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h
index fb810e32..5ff94f5c 100644
--- a/lib/Espfc/src/Sensor/GyroSensor.h
+++ b/lib/Espfc/src/Sensor/GyroSensor.h
@@ -103,6 +103,7 @@ class GyroSensor: public BaseSensor
 
       calibrate();
 
+      _model.state.gyroScaled = _model.state.gyro;
 
       // filtering
       for(size_t i = 0; i < 3; ++i)
@@ -113,7 +114,7 @@ class GyroSensor: public BaseSensor
         }
         if(_model.config.debugMode == DEBUG_GYRO_SCALED)
         {
-          _model.state.debug[i] = lrintf(degrees(_model.state.gyro[i]));
+          _model.state.debug[i] = lrintf(degrees(_model.state.gyroScaled[i]));
         }
         if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == _model.config.debugAxis)
         {
diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h
index 9f371ee8..b97f98f8 100644
--- a/lib/Espfc/src/Stats.h
+++ b/lib/Espfc/src/Stats.h
@@ -80,7 +80,8 @@ class Stats
     {
       uint32_t now = micros();
       uint32_t diff = now - _loop_last;
-      _loop_time += (((int32_t)diff - _loop_time + 8) >> 4);
+      _loop_time = diff;
+      //_loop_time += (((int32_t)diff - _loop_time + 8) >> 4);
       _loop_last = now;
     }
 
diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h
index c61708e1..d4c985ff 100644
--- a/lib/betaflight/src/platform.h
+++ b/lib/betaflight/src/platform.h
@@ -69,7 +69,7 @@ extern const char * boardIdentifier;
 #define CONCAT3(_1,_2,_3)  CONCAT(CONCAT(_1, _2), _3)
 #define CONCAT4(_1,_2,_3,_4)  CONCAT(CONCAT3(_1, _2, _3), _4)
 #define XYZ_AXIS_COUNT 3
-#define DEBUG16_VALUE_COUNT 4
+#define DEBUG16_VALUE_COUNT 8
 #define DEBUG_SET(mode, index, value) {if (debugMode == (mode)) {debug[(index)] = (value);}}
 
 #define LOG2_8BIT(v)  (8 - 90/(((v)/4+14)|1) - 2/((v)/2+1))

From ee22bf6fa6c7df9961105033cf61448172bb0997 Mon Sep 17 00:00:00 2001
From: rtlopez <rtlopezdev@gmail.com>
Date: Wed, 2 Aug 2023 23:20:51 +0200
Subject: [PATCH 2/4] bblog sample rate fix

---
 lib/Espfc/src/Blackbox.h          | 2 +-
 lib/Espfc/src/Sensor/GyroSensor.h | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h
index fe8c0773..1cb3e1c0 100644
--- a/lib/Espfc/src/Blackbox.h
+++ b/lib/Espfc/src/Blackbox.h
@@ -266,7 +266,7 @@ class Blackbox
       if(_model.magActive()) enabledSensors |= SENSOR_MAG;
       if(_model.baroActive()) enabledSensors |= SENSOR_BARO;
 
-      gyro.sampleLooptime = 125; //_model.state.gyroTimer.interval;
+      gyro.sampleLooptime = _model.state.gyroTimer.interval;
       targetPidLooptime = _model.state.loopTimer.interval;
       activePidLoopDenom = _model.config.loopSync;
 
diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h
index 5ff94f5c..48c3df53 100644
--- a/lib/Espfc/src/Sensor/GyroSensor.h
+++ b/lib/Espfc/src/Sensor/GyroSensor.h
@@ -189,7 +189,7 @@ class GyroSensor: public BaseSensor
                 _model.state.debug[0] = lrintf(_fft[i].peaks[0].freq);
                 _model.state.debug[1] = lrintf(_fft[i].peaks[1].freq);
                 _model.state.debug[2] = lrintf(_fft[i].peaks[2].freq);
-                _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i]));
+                _model.state.debug[3] = lrintf(_fft[i].peaks[3].freq);
               }
             }
             if(dynamicFilterEnabled && dynamicFilterUpdate)
@@ -216,7 +216,7 @@ class GyroSensor: public BaseSensor
             if(dynamicFilterDebug)
             {
               _model.state.debug[i] = lrintf(freq);
-              if(i == _model.config.debugAxis) _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i]));
+              if(i == _model.config.debugAxis) _model.state.debug[3] = lrintf(degrees(_model.state.gyroDynNotch[i]));
             }
             if(dynamicFilterEnabled && dynamicFilterUpdate)
             {

From 0a015329c58036b5296bcdcf04bb88933b3f044a Mon Sep 17 00:00:00 2001
From: rtlopez <rtlopezdev@gmail.com>
Date: Fri, 4 Aug 2023 00:52:49 +0200
Subject: [PATCH 3/4] more debug + optimize fft

---
 lib/Espfc/src/Espfc.h             |   6 +-
 lib/Espfc/src/Input.h             |  31 +++++++-
 lib/Espfc/src/Math/FFTAnalyzer.h  | 114 ++++++++++++++++++++----------
 lib/Espfc/src/Sensor/GyroSensor.h |  12 +++-
 4 files changed, 116 insertions(+), 47 deletions(-)

diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h
index 59b2b376..49f60990 100644
--- a/lib/Espfc/src/Espfc.h
+++ b/lib/Espfc/src/Espfc.h
@@ -59,10 +59,10 @@ class Espfc
     int update()
     {
 #if defined(ESPFC_MULTI_CORE)
-      if(!_model.state.appQueue.isEmpty())
+      /*if(!_model.state.appQueue.isEmpty())
       {
         return 0;
-      }
+      }*/
 
       if(!_model.state.gyroTimer.check())
       {
@@ -82,7 +82,7 @@ class Espfc
         _buzzer.update();
         _model.state.stats.update();
       }
-      _model.state.appQueue.send(Event(EVENT_IDLE));
+      //_model.state.appQueue.send(Event(EVENT_IDLE));
 
       return 1;
 #else
diff --git a/lib/Espfc/src/Input.h b/lib/Espfc/src/Input.h
index 09af602e..a590e945 100644
--- a/lib/Espfc/src/Input.h
+++ b/lib/Espfc/src/Input.h
@@ -120,9 +120,15 @@ class Input
     InputStatus readInputs()
     {
       Stats::Measure readMeasure(_model.state.stats, COUNTER_INPUT_READ);
+      uint32_t startTime = micros();
 
       InputStatus status = _device->update();
 
+      if(_model.config.debugMode == DEBUG_RX_TIMING)
+      {
+        _model.state.debug[0] = micros() - startTime;
+      }
+
       if(status == INPUT_IDLE) return status;
 
       _model.state.inputRxLoss = (status == INPUT_LOST || status == INPUT_FAILSAFE);
@@ -148,6 +154,8 @@ class Input
     {
       if(_model.state.inputFrameCount < 5) return; // ignore few first frames that might be garbage
 
+      uint32_t startTime = micros();
+
       uint16_t channels[INPUT_CHANNELS];
       _device->get(channels, _model.state.inputChannelCount);
 
@@ -187,6 +195,11 @@ class Input
         _model.state.inputBufferPrevious[c] = _model.state.inputBuffer[c];
         _model.state.inputBuffer[c] = v;
       }
+
+      if(_model.config.debugMode == DEBUG_RX_TIMING)
+      {
+        _model.state.debug[2] = micros() - startTime;
+      }
     }
 
     bool failsafe(InputStatus status)
@@ -259,6 +272,7 @@ class Input
     void filterInputs(InputStatus status)
     {
       Stats::Measure filterMeasure(_model.state.stats, COUNTER_INPUT_FILTER);
+      uint32_t startTime = micros();
 
       const bool newFrame = status != INPUT_IDLE;
       const bool interpolation = _model.config.input.interpolationMode != INPUT_INTERPOLATION_OFF && _model.config.input.filterType == INPUT_INTERPOLATION;
@@ -284,6 +298,11 @@ class Input
         }
         setInput((Axis)c, v, newFrame);
       }
+
+      if(_model.config.debugMode == DEBUG_RX_TIMING)
+      {
+        _model.state.debug[3] = micros() - startTime;
+      }
     }
 
     void updateFrameRate()
@@ -303,7 +322,8 @@ class Input
 
       if(_model.config.debugMode == DEBUG_RC_SMOOTHING_RATE)
       {
-        _model.state.debug[0] = _model.state.inputFrameRate;
+        _model.state.debug[0] = _model.state.inputFrameDelta / 10;
+        _model.state.debug[1] = _model.state.inputFrameRate;
       }
 
       // auto cutoff input freq
@@ -313,8 +333,8 @@ class Input
         _model.state.inputAutoFreq += 0.25f * (freq - _model.state.inputAutoFreq);
         if(_model.config.debugMode == DEBUG_RC_SMOOTHING_RATE)
         {
-          _model.state.debug[1] = lrintf(freq);
-          _model.state.debug[2] = lrintf(_model.state.inputAutoFreq);
+          _model.state.debug[2] = lrintf(freq);
+          _model.state.debug[3] = lrintf(_model.state.inputAutoFreq);
         }
         FilterConfig conf((FilterType)_model.config.input.filter.type, _model.state.inputAutoFreq);
         FilterConfig confDerivative((FilterType)_model.config.input.filterDerivative.type, _model.state.inputAutoFreq);
@@ -330,6 +350,11 @@ class Input
           }
         }
       }
+
+      if(_model.config.debugMode == DEBUG_RX_TIMING)
+      {
+        _model.state.debug[1] = micros() - now;
+      }
     }
 
     Device::InputDevice * getInputDevice()
diff --git a/lib/Espfc/src/Math/FFTAnalyzer.h b/lib/Espfc/src/Math/FFTAnalyzer.h
index 613b39d4..dab9c6e4 100644
--- a/lib/Espfc/src/Math/FFTAnalyzer.h
+++ b/lib/Espfc/src/Math/FFTAnalyzer.h
@@ -7,18 +7,25 @@
 #include "Filter.h"
 #include "dsps_fft4r.h"
 #include "dsps_wind_hann.h"
+#include <algorithm>
 
 namespace Espfc {
 
 namespace Math {
 
+enum FFTPhase {
+  PHASE_COLLECT,
+  PHASE_FFT,
+  PHASE_PEAKS
+};
+
 template<size_t SAMPLES>
 class FFTAnalyzer
 {
 public:
-  FFTAnalyzer(): _idx(0) {}
+  FFTAnalyzer(): _idx(0), _phase(PHASE_COLLECT) {}
 
-  int begin(int16_t rate, const DynamicFilterConfig& config)
+  int begin(int16_t rate, const DynamicFilterConfig& config, size_t axis)
   {
     int16_t nyquistLimit = rate / 2;
     _rate = rate;
@@ -26,63 +33,86 @@ class FFTAnalyzer
     _freq_max = std::min(config.max_freq, nyquistLimit);
     _peak_count = std::min((size_t)config.width, PEAKS_MAX);
 
-    _idx = 0;
+    _idx = axis * SAMPLES / 3;
     _bin_width = (float)_rate / SAMPLES; // no need to dived by 2 as we next process `SAMPLES / 2` results
 
-    dsps_fft4r_init_fc32(NULL, BINS);
+    _begin = (_freq_min / _bin_width) + 1;
+    _end = std::min(BINS - 1, (size_t)(_freq_max / _bin_width)) - 1;
+
+    // init fft tables
+    dsps_fft4r_init_fc32(nullptr, BINS);
 
     // Generate hann window
-    dsps_wind_hann_f32(_wind, SAMPLES);
+    dsps_wind_hann_f32(_win, SAMPLES);
 
     clearPeaks();
 
+    for(size_t i = 0; i < SAMPLES; i++) _in[i] = 0;
+    //std::fill(_in, _in + SAMPLES, 0);
+
     return 1;
   }
 
   // calculate fft and find noise peaks
   int update(float v)
   {
-    _samples[_idx] = v;
-
-    if(++_idx < SAMPLES) return 0; // not enough samples
+    _in[_idx] = v;
 
-    _idx = 0;
-
-    // apply window function
-    for (size_t j = 0; j < SAMPLES; j++)
+    if(++_idx >= SAMPLES)
     {
-      _samples[j] *= _wind[j]; // real
+      _idx = 0;
+      _phase = PHASE_FFT;
     }
 
-    // FFT Radix-4
-    dsps_fft4r_fc32(_samples, BINS);
+    switch(_phase)
+    {
+      case PHASE_COLLECT:
+        return 0;
 
-    // Bit reverse 
-    dsps_bit_rev4r_fc32(_samples, BINS);
+      case PHASE_FFT: // 32us
+        // apply window function
+        for (size_t j = 0; j < SAMPLES; j++)
+        {
+          _out[j] = _in[j] * _win[j]; // real
+        }
 
-    // Convert one complex vector with length SAMPLES/2 to one real spectrum vector with length SAMPLES/2
-    dsps_cplx2real_fc32(_samples, BINS);
+        // FFT Radix-4
+        dsps_fft4r_fc32(_out, BINS);
 
-    // calculate magnitude
-    for (size_t j = 0; j < BINS; j++)
-    {
-      size_t k = j * 2;
-      _samples[j] = _samples[k] * _samples[k] + _samples[k + 1] * _samples[k + 1];
-      //_samples[j] = sqrt(_samples[j]);
-    }
+        // Bit reverse
+        dsps_bit_rev4r_fc32(_out, BINS);
 
-    clearPeaks();
-    const size_t begin = (_freq_min / _bin_width) + 1;
-    const size_t end = std::min(BINS - 1, (size_t)(_freq_max / _bin_width)) - 1;
+        // Convert one complex vector with length SAMPLES/2 to one real spectrum vector with length SAMPLES/2
+        dsps_cplx2real_fc32(_out, BINS);
 
-    Math::peakDetect(_samples, begin, end, _bin_width, peaks, _peak_count);
+        _phase = PHASE_PEAKS;
+        return 0;
 
-    // sort peaks by freq
-    Math::peakSort(peaks, _peak_count);
-  
-    return 1;
+      case PHASE_PEAKS: // 12us + 22us sqrt()
+        // calculate magnitude
+        for (size_t j = 0; j < BINS; j++)
+        {
+          size_t k = j * 2;
+          //_out[j] = _out[k] * _out[k] + _out[k + 1] * _out[k + 1];
+          _out[j] = sqrt(_out[k] * _out[k] + _out[k + 1] * _out[k + 1]);
+        }
+
+        clearPeaks();
+
+        Math::peakDetect(_out, _begin, _end, _bin_width, peaks, _peak_count);
+
+        // sort peaks by freq
+        Math::peakSort(peaks, _peak_count);
+
+        _phase = PHASE_COLLECT;
+        return 1;
+
+      default:
+        _phase = PHASE_COLLECT;
+        return 0;
+    }
   }
-  
+
   static const size_t PEAKS_MAX = 8;
   Peak peaks[PEAKS_MAX];
 
@@ -90,6 +120,7 @@ class FFTAnalyzer
   void clearPeaks()
   {
     for(size_t i = 0; i < PEAKS_MAX; i++) peaks[i] = Peak();
+    //std::fill(peaks, peaks + PEAKS_MAX, Peak());
   }
 
   static const size_t BINS = SAMPLES >> 1;
@@ -100,12 +131,19 @@ class FFTAnalyzer
   int16_t _peak_count;
 
   size_t _idx;
+  FFTPhase _phase;
+  size_t _begin;
+  size_t _end;
   float _bin_width;
 
-  // fft input and output
-  __attribute__((aligned(16))) float _samples[SAMPLES];
+  // fft input
+  __attribute__((aligned(16))) float _in[SAMPLES];
+
+  // fft output
+  __attribute__((aligned(16))) float _out[SAMPLES];
+
   // Window coefficients
-  __attribute__((aligned(16))) float _wind[SAMPLES];
+  __attribute__((aligned(16))) float _win[SAMPLES];
 };
 
 }
diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h
index 48c3df53..dae7648b 100644
--- a/lib/Espfc/src/Sensor/GyroSensor.h
+++ b/lib/Espfc/src/Sensor/GyroSensor.h
@@ -49,7 +49,7 @@ class GyroSensor: public BaseSensor
 #ifdef ESPFC_DSP
       for(size_t i = 0; i < 3; i++)
       {
-        _fft[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.dynamicFilter);
+        _fft[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.dynamicFilter, i);
       }
 #endif
 
@@ -166,7 +166,7 @@ class GyroSensor: public BaseSensor
     {
       bool dynamicFilterEnabled = _model.isActive(FEATURE_DYNAMIC_FILTER);
       bool dynamicFilterFeed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0;
-      bool dynamicFilterDebug = _model.config.debugMode == DEBUG_FFT_FREQ;
+      bool dynamicFilterDebug = _model.config.debugMode == DEBUG_FFT_FREQ || _model.config.debugMode == DEBUG_FFT_TIME;
       bool dynamicFilterUpdate = dynamicFilterEnabled && _model.state.dynamicFilterTimer.check();
       const float q = _model.config.dynamicFilter.q * 0.01;
 
@@ -180,11 +180,17 @@ class GyroSensor: public BaseSensor
           const size_t peakCount = _model.config.dynamicFilter.width;
           if(dynamicFilterFeed)
           {
+            uint32_t startTime = micros();
             int status = _fft[i].update(_model.state.gyroDynNotch[i]);
+            if(_model.config.debugMode == DEBUG_FFT_TIME)
+            {
+              if(i == 0) _model.state.debug[0] = status;
+              _model.state.debug[i + 1] = micros() - startTime;
+            }
             dynamicFilterUpdate = dynamicFilterEnabled && status;
             if(dynamicFilterDebug)
             {
-              if(i == _model.config.debugAxis)
+              if(_model.config.debugMode == DEBUG_FFT_FREQ && i == _model.config.debugAxis)
               {
                 _model.state.debug[0] = lrintf(_fft[i].peaks[0].freq);
                 _model.state.debug[1] = lrintf(_fft[i].peaks[1].freq);

From fc95cd45c354efc6414266abb31d0a936686affb Mon Sep 17 00:00:00 2001
From: rtlopez <rtlopezdev@gmail.com>
Date: Tue, 8 Aug 2023 13:02:34 +0200
Subject: [PATCH 4/4] anti gravity gain off for UI

---
 lib/Espfc/src/Msp/MspProcessor.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h
index 9bd24306..dc6dcd2c 100644
--- a/lib/Espfc/src/Msp/MspProcessor.h
+++ b/lib/Espfc/src/Msp/MspProcessor.h
@@ -1092,7 +1092,7 @@ class MspProcessor
           r.writeU8(_model.config.angleLimit); // levelAngleLimit;
           r.writeU8(0); // was pidProfile.levelSensitivity
           r.writeU16(0); // itermThrottleThreshold;
-          r.writeU16(0); // itermAcceleratorGain; anti_gravity_gain
+          r.writeU16(1000); // itermAcceleratorGain; anti_gravity_gain, 0 in 1.45+
           r.writeU16(_model.config.dtermSetpointWeight);
           r.writeU8(0); // iterm rotation
           r.writeU8(0); // smart feed forward
@@ -1140,8 +1140,8 @@ class MspProcessor
               m.readU8(); // was pidProfile.levelSensitivity
           }
           if (m.remain() >= 4) {
-              m.readU16();
-              m.readU16();
+              m.readU16(); // itermThrottleThreshold;
+              m.readU16(); // itermAcceleratorGain; anti_gravity_gain
           }
           if (m.remain() >= 2) {
             _model.config.dtermSetpointWeight = m.readU16();