From 1fb73b36f413ba42bebcbba305a1ddd39ba244c3 Mon Sep 17 00:00:00 2001
From: Brian White <brian.white@queensu.ca>
Date: Sun, 28 Jul 2024 16:10:36 -0400
Subject: [PATCH] Increase Number of Available Dynamic Notches to 7 (#13750)

---
 src/main/flight/dyn_notch_filter.c  | 9 +++++----
 src/main/flight/dyn_notch_filter.h  | 2 +-
 src/main/sensors/gyro_filter_impl.c | 2 +-
 3 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/src/main/flight/dyn_notch_filter.c b/src/main/flight/dyn_notch_filter.c
index e6ba5e895..695a5a729 100644
--- 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
index 3789aeea6..a7d760170 100644
--- 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
index f5992e011..629b42eb0 100644
--- 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));
             }
 
-- 
2.11.4.GIT