Increase Number of Available Dynamic Notches to 7 (betaflight#13750)

HThuren · Jul 28, 2024 · 1fb73b3 · 1fb73b3
1 parent 5e40ec8
commit 1fb73b3
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 6 deletions.
diff --git a/src/main/flight/dyn_notch_filter.c b/src/main/flight/dyn_notch_filter.c
@@ -19,11 +19,11 @@
  */
 
 /* original work by Rav
- * 
+ *
  * 2018_07 updated by ctzsnooze to post filter, wider Q, different peak detection
  * coding assistance and advice from DieHertz, Rav, eTracer
  * test pilots icr4sh, UAV Tech, Flint723
- * 
+ *
  * 2021_02 updated by KarateBrot: switched FFT with SDFT, multiple notches per axis
  * test pilots: Sugar K, bizmar
  */
@@ -375,8 +375,9 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
             }
 
             if (state.axis == gyro.gyroDebugAxis) {
-                for (int p = 0; p < dynNotch.count && p < 3; p++) {
-                    DEBUG_SET(DEBUG_FFT_FREQ, p, lrintf(dynNotch.centerFreq[state.axis][p]));
+                for (int p = 0; p < dynNotch.count && p < DYN_NOTCH_COUNT_MAX; p++) {
+                    // debug channel 0 is reserved for pre DN gyro
+                    DEBUG_SET(DEBUG_FFT_FREQ, p + 1, lrintf(dynNotch.centerFreq[state.axis][p]));
                 }
                 DEBUG_SET(DEBUG_DYN_LPF, 1, lrintf(dynNotch.centerFreq[state.axis][0]));
             }

diff --git a/src/main/flight/dyn_notch_filter.h b/src/main/flight/dyn_notch_filter.h
@@ -26,7 +26,7 @@
 
 #include "pg/dyn_notch.h"
 
-#define DYN_NOTCH_COUNT_MAX 5
+#define DYN_NOTCH_COUNT_MAX 7
 
 void dynNotchInit(const dynNotchConfig_t *config, const timeUs_t targetLooptimeUs);
 void dynNotchPush(const int axis, const float sample);

diff --git a/src/main/sensors/gyro_filter_impl.c b/src/main/sensors/gyro_filter_impl.c
@@ -68,7 +68,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
         if (isDynNotchActive()) {
             if (axis == gyro.gyroDebugAxis) {
                 GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
-                GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
+                GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 0, lrintf(gyroADCf));
                 GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf));
             }