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,9 +104,11 @@ 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();
@@ -494,11 +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;
@@ -523,11 +527,13 @@ 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;
@@ -555,11 +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;
@@ -590,11 +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 },
@@ -716,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;
@@ -732,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;
@@ -751,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;
@@ -770,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
                 );
 }
 

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

@@ -874,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/sensors/gyro.c

@@ -250,11 +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,
@@ -306,11 +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,

src/main/sensors/gyro.h

@@ -153,11 +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;