diff --git a/src/core/profile.c b/src/core/profile.c index 4a2311392..8df41aac5 100644 --- 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 index 6b87eb828..b8c74ed3d 100644 --- 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 index 01e46bf12..671ffb1a4 100644 --- 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(¬ch_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(¬ch_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(¬ch_filter[i][p], ¬ch_filter_state[i][p], state.gyro.axis[i]); } - state.gyro.axis[i] = filter_biquad_notch_step(¬ch_filter[i][p], ¬ch_filter_state[i][p], state.gyro.axis[i]); } } }