diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fe0585a8d0..231c6fa489 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -197,10 +197,8 @@ 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 ); } @@ -237,7 +235,9 @@ static FAST_RAM filterApplyFnPtr angleSetpointFilterApplyFn = nullFilterApply; static FAST_RAM_ZERO_INIT pt1Filter_t angleSetpointFilter[2]; static FAST_RAM filterApplyFnPtr dtermABGapplyFn = nullFilterApply; static FAST_RAM_ZERO_INIT alphaBetaGammaFilter_t dtermABG[XYZ_AXIS_COUNT]; +#ifdef USE_GYRO_DATA_ANALYSE static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[XYZ_AXIS_COUNT][5]; +#endif #if defined(USE_ITERM_RELAX) static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT]; @@ -673,7 +673,9 @@ static FAST_RAM_ZERO_INIT float stickMovement[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float lastRcDeflectionAbs[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float previousError[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float previousMeasurement[XYZ_AXIS_COUNT]; +#ifdef USE_GYRO_DATA_ANALYSE static FAST_RAM_ZERO_INIT float previousNotchCenterFreq[XYZ_AXIS_COUNT][5]; +#endif static FAST_RAM_ZERO_INIT timeUs_t crashDetectedAtUs; void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index bfe63e31f0..ea9f16d811 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -155,10 +155,8 @@ 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 diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index a748c4aeb4..684b85e213 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1833,37 +1833,20 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) { gyroConfigMutable()->gyro_lowpass_hz[ROLL] = sbufReadU16(src); gyroConfigMutable()->gyro_lowpass_hz[PITCH] = sbufReadU16(src); gyroConfigMutable()->gyro_lowpass_hz[YAW] = sbufReadU16(src); -#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = sbufReadU16(src); gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = sbufReadU16(src); gyroConfigMutable()->gyro_lowpass2_hz[YAW] = sbufReadU16(src); -#else - sbufReadU16(src); - sbufReadU16(src); - sbufReadU16(src); -#endif gyroConfigMutable()->gyro_lowpass_type = sbufReadU8(src); -#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_type = sbufReadU8(src); -#else - sbufReadU8(src); -#endif currentPidProfile->dFilter[ROLL].dLpf2 = sbufReadU16(src); currentPidProfile->dFilter[PITCH].dLpf2 = sbufReadU16(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); @@ -1875,13 +1858,8 @@ 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 diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index cb2690eb58..9af64f98e5 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -394,11 +394,9 @@ 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) } @@ -489,9 +487,7 @@ 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 diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index ab5ea4482e..e8a8269834 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -112,9 +112,7 @@ 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; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index cb2c2c6b03..eefea7e341 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -238,12 +238,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lowpass_hz[ROLL] = 0, .gyro_lowpass_hz[PITCH] = 0, .gyro_lowpass_hz[YAW] = 0, -#ifdef USE_GYRO_LPF2 .gyro_lowpass2_type = FILTER_PT1, .gyro_lowpass2_hz[ROLL] = 0, .gyro_lowpass2_hz[PITCH] = 0, .gyro_lowpass2_hz[YAW] = 0, -#endif .gyro_high_fsr = false, .gyro_use_32khz = true, .gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT, @@ -254,13 +252,11 @@ 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, @@ -293,12 +289,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lowpass_hz[ROLL] = 115, .gyro_lowpass_hz[PITCH] = 115, .gyro_lowpass_hz[YAW] = 105, -#ifdef USE_GYRO_LPF2 .gyro_lowpass2_type = FILTER_PT1, .gyro_lowpass2_hz[ROLL] = 0, .gyro_lowpass2_hz[PITCH] = 0, .gyro_lowpass2_hz[YAW] = 0, -#endif .gyro_high_fsr = false, .gyro_use_32khz = false, .gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT, @@ -314,13 +308,11 @@ 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, diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 403954835e..b81475a75c 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -113,12 +113,10 @@ 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 @@ -132,9 +130,7 @@ typedef struct gyroConfig_s { uint8_t gyro_to_use; uint16_t gyro_lowpass_hz[XYZ_AXIS_COUNT]; -#ifdef USE_GYRO_LPF2 uint16_t gyro_lowpass2_hz[XYZ_AXIS_COUNT]; -#endif uint16_t gyro_ABG_alpha; uint16_t gyro_ABG_boost; @@ -149,21 +145,17 @@ typedef struct gyroConfig_s { // Lowpass primary/secondary uint8_t gyro_lowpass_type; -#ifdef USE_GYRO_LPF2 uint8_t gyro_lowpass2_type; -#endif uint8_t yaw_spin_recovery; 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;