option to apply dynamic-notches to RP vs normal/default RPY (#881)

* add dynamicFilter mode. RP or RPY
* dynamicFilter mode - OSD
* dynamicFilter mode - rename mode to axis
* dynamicFilter mode (axis) - #ifdef USE_GYRO_DATA_ANALYSE gating

src/main/blackbox/blackbox.c

@@ -1180,8 +1180,10 @@ static bool blackboxWriteSysinfo(void) {
         BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass2_hz_roll", "%d",          currentPidProfile->dFilter[ROLL].dLpf2);
         BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass2_hz_pitch", "%d",         currentPidProfile->dFilter[PITCH].dLpf2);
         BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass2_hz_yaw", "%d",           currentPidProfile->dFilter[YAW].dLpf2);
+#ifdef USE_GYRO_DATA_ANALYSE
         BLACKBOX_PRINT_HEADER_LINE("dterm_dyn_notch_enable", "%d",          currentPidProfile->dtermDynNotch);
         BLACKBOX_PRINT_HEADER_LINE("dterm_dyn_notch_q", "%d",               currentPidProfile->dterm_dyn_notch_q);
+#endif
         BLACKBOX_PRINT_HEADER_LINE("dterm_ABG_alpha", "%d",                 currentPidProfile->dterm_ABG_alpha);
         BLACKBOX_PRINT_HEADER_LINE("dterm_ABG_boost", "%d",                 currentPidProfile->dterm_ABG_boost);
         BLACKBOX_PRINT_HEADER_LINE("dterm_ABG_half_life", "%d",             currentPidProfile->dterm_ABG_half_life);
@@ -1240,6 +1242,7 @@ static bool blackboxWriteSysinfo(void) {
         BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_count", "%d",        gyroConfig()->dyn_notch_count);
         BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_min_hz", "%d",       gyroConfig()->dyn_notch_min_hz);
         BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_max_hz", "%d",       gyroConfig()->dyn_notch_max_hz);
+        BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_axis", "%d",       gyroConfig()->dyn_notch_axis);
 #endif
         BLACKBOX_PRINT_HEADER_LINE("gyro_ABG_alpha", "%d",                  gyroConfig()->gyro_ABG_alpha);
         BLACKBOX_PRINT_HEADER_LINE("gyro_ABG_boost", "%d",                  gyroConfig()->gyro_ABG_boost);

src/main/cms/cms_menu_imu.c

@@ -104,6 +104,12 @@ static const char * const cms_FilterType[] = {
     "PT1", "BIQUAD", "PT2", "PT3", "PT4",
 };
 
+#ifdef USE_GYRO_DATA_ANALYSE
+static const char * const cms_dynNotchAxisType[] = {
+    "RP", "RPY"
+};
+#endif
+
 static long cmsx_menuImu_onEnter(void) {
     pidProfileIndex = getCurrentPidProfileIndex();
     tmpPidProfileIndex = pidProfileIndex + 1;
@@ -490,10 +496,13 @@ static uint16_t gyroConfig_gyro_lowpass_hz_yaw;
 static uint16_t gyroConfig_gyro_lowpass2_hz_roll;
 static uint16_t gyroConfig_gyro_lowpass2_hz_pitch;
 static uint16_t gyroConfig_gyro_lowpass2_hz_yaw;
+#ifdef USE_GYRO_DATA_ANALYSE
 static uint16_t gyroConfig_gyro_q;
 static uint8_t gyroConfig_gyro_notch_count;
 static uint16_t gyroConfig_gyro_notch_min_hz;
 static uint16_t gyroConfig_gyro_notch_max_hz;
+static uint8_t gyroConfig_gyro_notch_axis;
+#endif
 static uint16_t gyroConfig_gyro_abg_alpha;
 static uint16_t gyroConfig_gyro_abg_boost;
 static uint8_t gyroConfig_gyro_abg_half_life;
@@ -518,14 +527,16 @@ static long cmsx_menuGyro_onEnter(void) {
     gyroConfig_gyro_lowpass2_hz_roll =  gyroConfig()->gyro_lowpass2_hz[ROLL];
     gyroConfig_gyro_lowpass2_hz_pitch =  gyroConfig()->gyro_lowpass2_hz[PITCH];
     gyroConfig_gyro_lowpass2_hz_yaw =  gyroConfig()->gyro_lowpass2_hz[YAW];
+#ifdef USE_GYRO_DATA_ANALYSE
     gyroConfig_gyro_q = gyroConfig()->dyn_notch_q;
     gyroConfig_gyro_notch_count = gyroConfig()->dyn_notch_count;
     gyroConfig_gyro_notch_min_hz = gyroConfig()->dyn_notch_min_hz;
     gyroConfig_gyro_notch_max_hz = gyroConfig()->dyn_notch_max_hz;
+    gyroConfig_gyro_notch_axis = gyroConfig()->dyn_notch_axis;
+#endif
     gyroConfig_gyro_abg_alpha = gyroConfig()->gyro_ABG_alpha;
     gyroConfig_gyro_abg_boost = gyroConfig()->gyro_ABG_boost;
     gyroConfig_gyro_abg_half_life = gyroConfig()->gyro_ABG_half_life;
-
 #ifndef USE_GYRO_IMUF9001
     gyroConfig_imuf_roll_q = gyroConfig()->imuf_roll_q;
     gyroConfig_imuf_pitch_q = gyroConfig()->imuf_pitch_q;
@@ -550,10 +561,13 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) {
     gyroConfigMutable()->gyro_lowpass2_hz[ROLL] =  gyroConfig_gyro_lowpass2_hz_roll;
     gyroConfigMutable()->gyro_lowpass2_hz[PITCH] =  gyroConfig_gyro_lowpass2_hz_pitch;
     gyroConfigMutable()->gyro_lowpass2_hz[YAW] =  gyroConfig_gyro_lowpass2_hz_yaw;
+#ifdef USE_GYRO_DATA_ANALYSE
     gyroConfigMutable()->dyn_notch_q = gyroConfig_gyro_q;
     gyroConfigMutable()->dyn_notch_count = gyroConfig_gyro_notch_count;
     gyroConfigMutable()->dyn_notch_min_hz = gyroConfig_gyro_notch_min_hz;
     gyroConfigMutable()->dyn_notch_max_hz = gyroConfig_gyro_notch_max_hz;
+    gyroConfigMutable()->dyn_notch_axis = gyroConfig_gyro_notch_axis;
+#endif
     gyroConfigMutable()->gyro_ABG_alpha = gyroConfig_gyro_abg_alpha;
     gyroConfigMutable()->gyro_ABG_boost = gyroConfig_gyro_abg_boost;
     gyroConfigMutable()->gyro_ABG_half_life = gyroConfig_gyro_abg_half_life;
@@ -584,10 +598,13 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] = {
     { "GYRO LPF2 PITCH",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz_pitch,   0, 16000, 1 }, 0 },
     { "GYRO LPF2 YAW",    OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz_yaw,     0, 16000, 1 }, 0 },
 #endif
+#ifdef USE_GYRO_DATA_ANALYSE
     { "DYN NOTCH Q",         OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_q,        0, 1000, 1 }, 0 },
     { "DYN NOTCH COUNT",     OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroConfig_gyro_notch_count,        1, 5, 1 }, 0 },
     { "DYN NOTCH MIN HZ",    OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_notch_min_hz,       30, 1000, 1 }, 0 },
     { "DYN NOTCH MAX HZ",    OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_notch_max_hz,       200, 1000, 1 }, 0 },
+    { "DYN NOTCH AXIS",      OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &gyroConfig_gyro_notch_axis, 1, cms_dynNotchAxisType }, 0 },
+#endif
 #ifndef USE_GYRO_IMUF9001
     { "IMUF W",           OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_w,                   0, 512,     1 }, 0 },
     { "ROLL Q",           OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_q,              100, 16000, 100 }, 0 },
@@ -709,8 +726,10 @@ static uint16_t cmsx_dterm_lowpass2_type;
 static uint16_t cmsx_dterm_lowpass2_hz_roll;
 static uint16_t cmsx_dterm_lowpass2_hz_pitch;
 static uint16_t cmsx_dterm_lowpass2_hz_yaw;
+#ifdef USE_GYRO_DATA_ANALYSE
 static uint8_t cmsx_dterm_dyn_notch_enable ;
 static uint16_t cmsx_dterm_dyn_notch_q;
+#endif
 static uint16_t cmsx_dterm_abg_alpha;
 static uint16_t cmsx_dterm_abg_boost;
 static uint8_t cmsx_dterm_abg_half_life;
@@ -725,8 +744,10 @@ static long cmsx_FilterPerProfileRead(void) {
     cmsx_dterm_lowpass2_hz_roll  = pidProfile->dFilter[ROLL].dLpf2;
     cmsx_dterm_lowpass2_hz_pitch = pidProfile->dFilter[PITCH].dLpf2;
     cmsx_dterm_lowpass2_hz_yaw   = pidProfile->dFilter[YAW].dLpf2;
+#ifdef USE_GYRO_DATA_ANALYSE
     cmsx_dterm_dyn_notch_enable  = pidProfile->dtermDynNotch;
     cmsx_dterm_dyn_notch_q       = pidProfile->dterm_dyn_notch_q;
+#endif
     cmsx_dterm_abg_alpha = pidProfile->dterm_ABG_alpha;
     cmsx_dterm_abg_boost = pidProfile->dterm_ABG_boost;
     cmsx_dterm_abg_half_life = pidProfile->dterm_ABG_half_life;
@@ -744,8 +765,10 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self) {
     pidProfile->dFilter[ROLL].dLpf2  = cmsx_dterm_lowpass2_hz_roll;
     pidProfile->dFilter[PITCH].dLpf2 = cmsx_dterm_lowpass2_hz_pitch;
     pidProfile->dFilter[YAW].dLpf2   = cmsx_dterm_lowpass2_hz_yaw;
+#ifdef USE_GYRO_DATA_ANALYSE
     pidProfile->dtermDynNotch        = cmsx_dterm_dyn_notch_enable;
     pidProfile->dterm_dyn_notch_q    = cmsx_dterm_dyn_notch_q;
+#endif
     pidProfile->dterm_ABG_alpha = cmsx_dterm_abg_alpha;
     pidProfile->dterm_ABG_boost = cmsx_dterm_abg_boost;
     pidProfile->dterm_ABG_half_life = cmsx_dterm_abg_half_life;
@@ -763,8 +786,10 @@ static OSD_Entry cmsx_menuFilterPerProfileEntries[] = {
     { "DTERM LPF2 ROLL", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz_roll,    0, 500, 1 }, 0 },
     { "DTERM LPF2 PITCH", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz_pitch,    0, 500, 1 }, 0 },
     { "DTERM LPF2 YAW", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz_yaw,    0, 500, 1 }, 0 },
+#ifdef USE_GYRO_DATA_ANALYSE
     { "DTERM DYN ENABLE",   OME_TAB,   NULL, &(OSD_TAB_t)   { (uint8_t *) &cmsx_dterm_dyn_notch_enable, 1, cms_offOnLabels }, 0 },
     { "DTERM DYN NOT Q",    OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_dyn_notch_q,    0, 2000, 1 }, 0 },
+#endif
     { "DTERM ABG ALPHA",    OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_abg_alpha,       0, 1000, 1 }, 0 },
     { "DTERM ABG BOOST",    OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_abg_boost,       0, 2000, 1 }, 0 },
     { "DTERM ABG HL",       OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_dterm_abg_half_life,   0, 250, 1 }, 0 },

src/main/flight/pid.c

@@ -197,8 +197,10 @@ void resetPidProfile(pidProfile_t *pidProfile) {
     .dterm_ABG_half_life = 50,
     .emuGravityGain = 50,
     .angle_filter = 100,
+#ifdef USE_GYRO_DATA_ANALYSE
     .dtermDynNotch = false,
     .dterm_dyn_notch_q = 400,
+#endif
                 );
 }
 
@@ -811,7 +813,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
             float dDelta = ((feathered_pids * pureMeasurement) + ((1 - feathered_pids) * pureError)) * pidFrequency; //calculating the dterm determine how much is calculated using measurement vs error
             //filter the dterm
 #ifdef USE_GYRO_DATA_ANALYSE
-            if (isDynamicFilterActive() && pidProfile->dtermDynNotch) {
+            if (isDynamicFilterActive() && pidProfile->dtermDynNotch && axis <= gyroConfig()->dyn_notch_axis+1) {
                 for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
                     if (getCenterFreq(axis, p) != previousNotchCenterFreq[axis][p]) {
                         previousNotchCenterFreq[axis][p] = getCenterFreq(axis, p);

src/main/flight/pid.h

@@ -155,8 +155,10 @@ typedef struct pidProfile_s {
     uint16_t dterm_ABG_alpha;
     uint16_t dterm_ABG_boost;
     uint8_t dterm_ABG_half_life;
+#ifdef USE_GYRO_DATA_ANALYSE
     uint8_t dtermDynNotch;
     uint16_t dterm_dyn_notch_q;
+#endif
 } pidProfile_t;
 
 #ifndef USE_OSD_SLAVE

src/main/interface/msp.c

@@ -1204,10 +1204,17 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) {
         sbufWriteU16(dst, currentPidProfile->dFilter[YAW].dLpf2);
         //MSP 1.51 removes SmartDTermSmoothing and WitchCraft
         //MSP 1.51 adds and refactors dynamic_filter
+#ifdef USE_GYRO_DATA_ANALYSE
         sbufWriteU8(dst, gyroConfig()->dyn_notch_count);    //dynamic_gyro_notch_count
         sbufWriteU16(dst, gyroConfig()->dyn_notch_q);
         sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz);
         sbufWriteU16(dst, gyroConfig()->dyn_notch_max_hz);   //dynamic_gyro_notch_max_hz
+#else
+        sbufWriteU8(dst, 0);
+        sbufWriteU16(dst, 0);
+        sbufWriteU16(dst, 0);
+        sbufWriteU16(dst, 0);
+#endif
         //end MSP 1.51 add/refactor dynamic filter
         //MSP 1.51
         sbufWriteU16(dst, gyroConfig()->gyro_ABG_alpha);
@@ -1219,8 +1226,13 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) {
         sbufWriteU8(dst, currentPidProfile->dterm_ABG_half_life);
         //end MSP 1.51
         //MSP 1.51 dynamic dTerm notch
+#ifdef USE_GYRO_DATA_ANALYSE
         sbufWriteU8(dst, currentPidProfile->dtermDynNotch);        //dterm_dyn_notch_enable
         sbufWriteU16(dst, currentPidProfile->dterm_dyn_notch_q);   //dterm_dyn_notch_q
+#else
+        sbufWriteU8(dst, 0);
+        sbufWriteU16(dst, 0);
+#endif
         //end MSP 1.51 dynamic dTerm notch
         break;
     /*#ifndef USE_GYRO_IMUF9001
@@ -1821,10 +1833,17 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) {
             currentPidProfile->dFilter[YAW].dLpf2 = sbufReadU16(src);
             //MSP 1.51 removes SmartDTermSmoothing and WitchCraft
             //MSP 1.51 adds and refactors dynamic_filter
+#ifdef USE_GYRO_DATA_ANALYSE
             gyroConfigMutable()->dyn_notch_count = sbufReadU8(src); //dynamic_gyro_notch_count
             gyroConfigMutable()->dyn_notch_q = sbufReadU16(src);
             gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
             gyroConfigMutable()->dyn_notch_max_hz = sbufReadU16(src); //dynamic_gyro_notch_max_hz
+#else
+            sbufReadU8(src);
+            sbufReadU16(src);
+            sbufReadU16(src);
+            sbufReadU16(src);
+#endif
             //end 1.51 add/refactor dynamic_filter
             //MSP 1.51
             gyroConfigMutable()->gyro_ABG_alpha = sbufReadU16(src);
@@ -1836,8 +1855,13 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) {
             currentPidProfile->dterm_ABG_half_life = sbufReadU8(src);
             //end MSP 1.51
             //MSP 1.51 dynamic dTerm notch
+#ifdef USE_GYRO_DATA_ANALYSE
             currentPidProfile->dtermDynNotch = sbufReadU8(src);         //dterm_dyn_notch_enable
             currentPidProfile->dterm_dyn_notch_q = sbufReadU16(src);    //dterm_dyn_notch_q
+#else
+            sbufReadU8(src);
+            sbufReadU16(src);
+#endif
             //end MSP 1.51 dynamic dTerm notch
         }
         // reinitialize the gyro filters with the new values

src/main/interface/settings.c

@@ -293,6 +293,7 @@ static const char * const lookupTableRcInterpolation[] = {
 static const char * const lookupTableRcInterpolationChannels[] = {
     "RP", "RPY", "RPYT", "T", "RPT",
 };
+
 static const char * const lookupTableFilterType[] = {
     "PT1", "BIQUAD", "PT2", "PT3", "PT4",
 };
@@ -393,6 +394,12 @@ static const char *const lookupTableMixerImplType[] = {
     "LEGACY", "SMOOTH", "2PASS"
 };
 
+#ifdef USE_GYRO_DATA_ANALYSE
+static const char *const lookupTableDynNotchAxisType[] = {
+    "RP", "RPY"
+};
+#endif
+
 #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
 
 const lookupTableEntry_t lookupTables[] = {
@@ -482,6 +489,9 @@ const lookupTableEntry_t lookupTables[] = {
     LOOKUP_TABLE_ENTRY(lookupTableOsdLogoOnArming),
 #endif
     LOOKUP_TABLE_ENTRY(lookupTableMixerImplType),
+#ifdef USE_GYRO_DATA_ANALYSE
+    LOOKUP_TABLE_ENTRY(lookupTableDynNotchAxisType),
+#endif
 };
 
 #undef LOOKUP_TABLE_ENTRY
@@ -553,6 +563,7 @@ const clivalue_t valueTable[] = {
     { "gyro_to_use",                VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
 #endif
 #if defined(USE_GYRO_DATA_ANALYSE)
+    { "dynamic_gyro_notch_axis",    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYN_NOTCH_AXIS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_axis) },
     { "dynamic_gyro_notch_q",       VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) },
     { "dynamic_gyro_notch_count",   VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, 5 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_count) },
     { "dynamic_gyro_notch_min_hz",  VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) },
@@ -863,9 +874,10 @@ const clivalue_t valueTable[] = {
     { "runaway_takeoff_deactivate_throttle_percent",  VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_deactivate_throttle) }, // minimum throttle percentage during deactivation phase
 #endif
 // PG_PID_PROFILE
+#ifdef USE_GYRO_DATA_ANALYSE
     { "dterm_dyn_notch_enable",     VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermDynNotch) },
     { "dterm_dyn_notch_q",          VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_dyn_notch_q) },
-
+#endif
     { "dterm_abg_alpha",            VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_alpha) },
     { "dterm_abg_boost",            VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_boost) },
     { "dterm_abg_half_life",        VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_half_life) },

src/main/interface/settings.h

@@ -112,6 +112,9 @@ typedef enum {
     TABLE_OSD_LOGO_ON_ARMING,
 #endif
     TABLE_MIXER_IMPL_TYPE,
+#ifdef USE_GYRO_DATA_ANALYSE
+    TABLE_DYN_NOTCH_AXIS_TYPE,
+#endif
     LOOKUP_TABLE_COUNT
 } lookupTableIndex_e;
 

src/main/sensors/gyro.c

@@ -250,10 +250,13 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
                   .checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
                   .yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO,
                   .yaw_spin_threshold = 1950,
+#ifdef USE_GYRO_DATA_ANALYSE
+                  .dyn_notch_axis = RPY,
                   .dyn_notch_q = 400,
                   .dyn_notch_count = 3, // default of 3 is similar to the matrix filter.
                   .dyn_notch_min_hz = 150,
                   .dyn_notch_max_hz = 600,
+#endif
                   .imuf_mode = GTBCM_GYRO_ACC_FILTER_F,
                   .imuf_rate = IMUF_RATE_16K,
                   .imuf_roll_q = 6000,
@@ -305,10 +308,13 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
                   .gyro_offset_yaw = 0,
                   .yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO,
                   .yaw_spin_threshold = 1950,
+#ifdef USE_GYRO_DATA_ANALYSE
+                  .dyn_notch_axis = RPY,
                   .dyn_notch_q = 350,
                   .dyn_notch_count = 3, // default of 3 is similar to the matrix filter.
                   .dyn_notch_min_hz = 150,
                   .dyn_notch_max_hz = 600,
+#endif
                   .gyro_ABG_alpha = 0,
                   .gyro_ABG_boost = 275,
                   .gyro_ABG_half_life = 50,
@@ -843,7 +849,7 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) {
     gyroSensor->notchFilterDynApplyFn = nullFilterApply;
     if (isDynamicFilterActive()) {
         gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
-        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        for (int axis = 0; axis < gyroConfig()->dyn_notch_axis+1; axis++) {
             for (int axis2 = 0; axis2 < gyroConfig()->dyn_notch_count; axis2++) {
                 biquadFilterInit(&gyroSensor->gyroAnalyseState.notchFilterDyn[axis][axis2], 400, gyro.targetLooptime, gyroConfig()->dyn_notch_q / 100.0f, FILTER_NOTCH);
             }

src/main/sensors/gyro.h

@@ -113,6 +113,13 @@ typedef struct smithPredictor_s {
 } smithPredictor_t;
 #endif // USE_SMITH_PREDICTOR
 
+#ifdef USE_GYRO_DATA_ANALYSE
+typedef enum {
+    RP = 0,
+    RPY = 1
+} dynamicGyroAxisType_e;
+#endif
+
 typedef struct gyroConfig_s {
     uint8_t  gyro_align;                       // gyro alignment
     uint8_t  gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
@@ -146,10 +153,13 @@ typedef struct gyroConfig_s {
     int16_t  yaw_spin_threshold;
 
     uint16_t gyroCalibrationDuration;  // Gyro calibration duration in 1/100 second
+#if defined(USE_GYRO_DATA_ANALYSE)
+    uint8_t dyn_notch_axis;
     uint16_t dyn_notch_q;
     uint8_t dyn_notch_count;
     uint16_t dyn_notch_min_hz;
     uint16_t dyn_notch_max_hz;
+#endif
 #if defined(USE_GYRO_IMUF9001)
     uint16_t imuf_mode;
     uint16_t imuf_rate;