allow for disabling the dynamic notch

BossHobby · Aug 12, 2023 · 431ffbc · 431ffbc
1 parent 952bdee
commit 431ffbc
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 12 deletions.
diff --git a/src/core/profile.c b/src/core/profile.c
@@ -258,6 +258,7 @@ const profile_t default_profile = {
 #ifdef DTERM_DYNAMIC_FREQ_MAX
         .dterm_dynamic_max = DTERM_DYNAMIC_FREQ_MAX,
 #endif
+        .gyro_dynamic_notch_enable = 1,
     },
 
     .rate = {

diff --git a/src/core/profile.h b/src/core/profile.h
@@ -312,6 +312,7 @@ typedef struct {
   uint8_t dterm_dynamic_enable;
   float dterm_dynamic_min;
   float dterm_dynamic_max;
+  uint8_t gyro_dynamic_notch_enable;
 } profile_filter_t;
 
 #define FILTER_MEMBERS                                              \
@@ -321,6 +322,7 @@ typedef struct {
   MEMBER(dterm_dynamic_enable, uint8)                               \
   MEMBER(dterm_dynamic_min, float)                                  \
   MEMBER(dterm_dynamic_max, float)                                  \
+  MEMBER(gyro_dynamic_notch_enable, uint8)                          \
   END_STRUCT()
 
 typedef struct {

diff --git a/src/flight/sixaxis.c b/src/flight/sixaxis.c
@@ -146,26 +146,30 @@ void sixaxis_read() {
   state.gyro.axis[2] = state.gyro_raw.axis[2] = -state.gyro_raw.axis[2] * GYRO_RANGE * DEGTORAD;
 
   for (uint32_t i = 0; i < 3; i++) {
-    static uint8_t axis_needs_update = 0;
-    if (sdft_push(&gyro_sdft[i], state.gyro.axis[i]) && axis_needs_update == 0) {
-      axis_needs_update = 3;
-    }
+    if (profile.filter.gyro_dynamic_notch_enable) {
+      static uint8_t axis_needs_update = 0;
+      if (sdft_push(&gyro_sdft[i], state.gyro.axis[i]) && axis_needs_update == 0) {
+        axis_needs_update = 3;
+      }
 
-    if ((axis_needs_update - 1) == i && sdft_update(&gyro_sdft[i])) {
-      for (uint32_t p = 0; p < SDFT_PEAKS; p++) {
-        filter_biquad_notch_coeff(&notch_filter[i][p], gyro_sdft[i].notch_hz[p]);
+      if ((axis_needs_update - 1) == i && sdft_update(&gyro_sdft[i])) {
+        for (uint32_t p = 0; p < SDFT_PEAKS; p++) {
+          filter_biquad_notch_coeff(&notch_filter[i][p], gyro_sdft[i].notch_hz[p]);
+        }
+        axis_needs_update--;
       }
-      axis_needs_update--;
     }
 
     state.gyro.axis[i] = filter_step(profile.filter.gyro[0].type, &filter[0], &filter_state[0][i], state.gyro.axis[i]);
     state.gyro.axis[i] = filter_step(profile.filter.gyro[1].type, &filter[1], &filter_state[1][i], state.gyro.axis[i]);
 
-    for (uint32_t p = 0; p < SDFT_PEAKS; p++) {
-      if (i == 0) {
-        blackbox_set_debug(p, gyro_sdft[i].notch_hz[p]);
+    if (profile.filter.gyro_dynamic_notch_enable) {
+      for (uint32_t p = 0; p < SDFT_PEAKS; p++) {
+        if (i == 0) {
+          blackbox_set_debug(p, gyro_sdft[i].notch_hz[p]);
+        }
+        state.gyro.axis[i] = filter_biquad_notch_step(&notch_filter[i][p], &notch_filter_state[i][p], state.gyro.axis[i]);
       }
-      state.gyro.axis[i] = filter_biquad_notch_step(&notch_filter[i][p], &notch_filter_state[i][p], state.gyro.axis[i]);
     }
   }
 }