From 6ba54464405346ac88ab9376e8f603129c973fa0 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 25 Oct 2023 15:17:08 -0500 Subject: [PATCH 1/4] 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 | 3 +++ src/main/cms/cms_menu_imu.c | 27 ++++++++++++++++++++++++++- src/main/flight/pid.c | 4 +++- src/main/flight/pid.h | 2 ++ src/main/interface/msp.c | 24 ++++++++++++++++++++++++ src/main/interface/settings.c | 14 +++++++++++++- src/main/interface/settings.h | 3 +++ src/main/sensors/gyro.c | 8 +++++++- src/main/sensors/gyro.h | 10 ++++++++++ 9 files changed, 91 insertions(+), 4 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 5797c373a8..672d39fc3a 100644 --- a/src/main/blackbox/blackbox.c +++ b/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); diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index a8f3804735..e479a1fa17 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/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 }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 19b51d18f7..fe0585a8d0 100644 --- a/src/main/flight/pid.c +++ b/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); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index ea9f16d811..bfe63e31f0 100644 --- a/src/main/flight/pid.h +++ b/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 diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index b3aebb427d..9b3dd142da 100644 --- a/src/main/interface/msp.c +++ b/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 diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 9634ec73c8..19f857c663 100644 --- a/src/main/interface/settings.c +++ b/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) }, diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index f351a2aa12..ab5ea4482e 100644 --- a/src/main/interface/settings.h +++ b/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; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index b77528e9fd..b957ac7ef3 100644 --- a/src/main/sensors/gyro.c +++ b/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); } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 0f32c699e1..84bee04419 100644 --- a/src/main/sensors/gyro.h +++ b/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; From de71a3a275047d084d3ba7da584c0286ff906248 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 26 Oct 2023 08:23:46 -0500 Subject: [PATCH 2/4] add Gyro LPF2 type to OSD (#934) --- src/main/cms/cms_menu_imu.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index e479a1fa17..64be1292d8 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -493,6 +493,7 @@ static uint8_t gyroConfig_gyro_lowpass1_type; static uint16_t gyroConfig_gyro_lowpass_hz_roll; static uint16_t gyroConfig_gyro_lowpass_hz_pitch; static uint16_t gyroConfig_gyro_lowpass_hz_yaw; +static uint8_t gyroConfig_gyro_lowpass2_type; static uint16_t gyroConfig_gyro_lowpass2_hz_roll; static uint16_t gyroConfig_gyro_lowpass2_hz_pitch; static uint16_t gyroConfig_gyro_lowpass2_hz_yaw; @@ -524,6 +525,7 @@ static long cmsx_menuGyro_onEnter(void) { gyroConfig_gyro_lowpass_hz_roll = gyroConfig()->gyro_lowpass_hz[ROLL]; gyroConfig_gyro_lowpass_hz_pitch = gyroConfig()->gyro_lowpass_hz[PITCH]; gyroConfig_gyro_lowpass_hz_yaw = gyroConfig()->gyro_lowpass_hz[YAW]; + gyroConfig_gyro_lowpass2_type = gyroConfig()->gyro_lowpass2_type; 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]; @@ -558,6 +560,7 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) { gyroConfigMutable()->gyro_lowpass_hz[ROLL] = gyroConfig_gyro_lowpass_hz_roll; gyroConfigMutable()->gyro_lowpass_hz[PITCH] = gyroConfig_gyro_lowpass_hz_pitch; gyroConfigMutable()->gyro_lowpass_hz[YAW] = gyroConfig_gyro_lowpass_hz_yaw; + gyroConfigMutable()->gyro_lowpass2_type = gyroConfig_gyro_lowpass2_type; 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; @@ -589,11 +592,12 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) { static OSD_Entry cmsx_menuFilterGlobalEntries[] = { { "-- FILTER GLB --", OME_Label, NULL, NULL, 0 }, + { "GYRO LPF TYPE", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &gyroConfig_gyro_lowpass1_type, 4, cms_FilterType }, 0 }, { "GYRO LPF ROLL", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass_hz_roll, 0, 16000, 1 }, 0 }, { "GYRO LPF PITCH", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass_hz_pitch, 0, 16000, 1 }, 0 }, { "GYRO LPF YAW", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass_hz_yaw, 0, 16000, 1 }, 0 }, - { "GYRO LPF TYPE", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &gyroConfig_gyro_lowpass1_type, 4, cms_FilterType }, 0 }, #ifdef USE_GYRO_LPF2 + { "GYRO LPF2 TYPE", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &gyroConfig_gyro_lowpass2_type, 4, cms_FilterType }, 0 }, { "GYRO LPF2 ROLL", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz_roll, 0, 16000, 1 }, 0 }, { "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 }, From db57386cd614604c83db951e16d36d224641918a Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 26 Oct 2023 09:49:06 -0500 Subject: [PATCH 3/4] fix/add USE_GYRO_LPF2 gating (#935) * fix/add USE_GYRO_LPF2 gating --- src/main/blackbox/blackbox.c | 2 ++ src/main/cms/cms_menu_imu.c | 6 ++++++ src/main/interface/msp.c | 20 ++++++++++++++++++++ src/main/interface/settings.c | 2 ++ src/main/sensors/gyro.c | 10 ++++++++++ src/main/sensors/gyro.h | 4 ++++ src/main/sensors/gyro_filter_impl.h | 2 ++ src/main/target/NBDBBBLV2/config.c | 2 ++ src/main/target/NBDBBBLV3/config.c | 2 ++ src/main/target/NBDHMBF41S/config.c | 2 ++ src/main/target/NBDHMBF4PRO/config.c | 2 ++ src/test/unit/sensor_gyro_unittest.cc | 2 ++ 12 files changed, 56 insertions(+) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 672d39fc3a..436871abb4 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1229,10 +1229,12 @@ static bool blackboxWriteSysinfo(void) { BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz_roll", "%d", gyroConfig()->gyro_lowpass_hz[ROLL]); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz_pitch", "%d", gyroConfig()->gyro_lowpass_hz[PITCH]); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz_yaw", "%d", gyroConfig()->gyro_lowpass_hz[YAW]); +#ifdef USE_GYRO_LPF2 BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_type", "%d", gyroConfig()->gyro_lowpass2_type); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_hz_roll", "%d", gyroConfig()->gyro_lowpass2_hz[ROLL]); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_hz_pitch", "%d", gyroConfig()->gyro_lowpass2_hz[PITCH]); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_hz_yaw", "%d", gyroConfig()->gyro_lowpass2_hz[YAW]); +#endif BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_hz_2); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1, diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 64be1292d8..50c0bfc589 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -493,10 +493,12 @@ static uint8_t gyroConfig_gyro_lowpass1_type; static uint16_t gyroConfig_gyro_lowpass_hz_roll; static uint16_t gyroConfig_gyro_lowpass_hz_pitch; static uint16_t gyroConfig_gyro_lowpass_hz_yaw; +#ifdef USE_GYRO_LPF2 static uint8_t gyroConfig_gyro_lowpass2_type; static uint16_t gyroConfig_gyro_lowpass2_hz_roll; static uint16_t gyroConfig_gyro_lowpass2_hz_pitch; static uint16_t gyroConfig_gyro_lowpass2_hz_yaw; +#endif #ifdef USE_GYRO_DATA_ANALYSE static uint16_t gyroConfig_gyro_q; static uint8_t gyroConfig_gyro_notch_count; @@ -525,10 +527,12 @@ static long cmsx_menuGyro_onEnter(void) { gyroConfig_gyro_lowpass_hz_roll = gyroConfig()->gyro_lowpass_hz[ROLL]; gyroConfig_gyro_lowpass_hz_pitch = gyroConfig()->gyro_lowpass_hz[PITCH]; gyroConfig_gyro_lowpass_hz_yaw = gyroConfig()->gyro_lowpass_hz[YAW]; +#ifdef USE_GYRO_LPF2 gyroConfig_gyro_lowpass2_type = gyroConfig()->gyro_lowpass2_type; 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]; +#endif #ifdef USE_GYRO_DATA_ANALYSE gyroConfig_gyro_q = gyroConfig()->dyn_notch_q; gyroConfig_gyro_notch_count = gyroConfig()->dyn_notch_count; @@ -560,10 +564,12 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) { gyroConfigMutable()->gyro_lowpass_hz[ROLL] = gyroConfig_gyro_lowpass_hz_roll; gyroConfigMutable()->gyro_lowpass_hz[PITCH] = gyroConfig_gyro_lowpass_hz_pitch; gyroConfigMutable()->gyro_lowpass_hz[YAW] = gyroConfig_gyro_lowpass_hz_yaw; +#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_type = gyroConfig_gyro_lowpass2_type; 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; +#endif #ifdef USE_GYRO_DATA_ANALYSE gyroConfigMutable()->dyn_notch_q = gyroConfig_gyro_q; gyroConfigMutable()->dyn_notch_count = gyroConfig_gyro_notch_count; diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 9b3dd142da..a748c4aeb4 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1194,11 +1194,21 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) { sbufWriteU16(dst, gyroConfig()->gyro_lowpass_hz[ROLL]); sbufWriteU16(dst, gyroConfig()->gyro_lowpass_hz[PITCH]); sbufWriteU16(dst, gyroConfig()->gyro_lowpass_hz[YAW]); +#ifdef USE_GYRO_LPF2 sbufWriteU16(dst, gyroConfig()->gyro_lowpass2_hz[ROLL]); sbufWriteU16(dst, gyroConfig()->gyro_lowpass2_hz[PITCH]); sbufWriteU16(dst, gyroConfig()->gyro_lowpass2_hz[YAW]); +#else + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); +#endif sbufWriteU8(dst, gyroConfig()->gyro_lowpass_type); +#ifdef USE_GYRO_LPF2 sbufWriteU8(dst, gyroConfig()->gyro_lowpass2_type); +#else + sbufWriteU8(dst, 0); +#endif sbufWriteU16(dst, currentPidProfile->dFilter[ROLL].dLpf2); sbufWriteU16(dst, currentPidProfile->dFilter[PITCH].dLpf2); sbufWriteU16(dst, currentPidProfile->dFilter[YAW].dLpf2); @@ -1823,11 +1833,21 @@ 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); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 19f857c663..cb2690eb58 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -513,10 +513,12 @@ const clivalue_t valueTable[] = { { "gyro_lowpass_hz_pitch", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz[PITCH]) }, { "gyro_lowpass_hz_yaw", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz[YAW]) }, +#ifdef USE_GYRO_LPF2 { "gyro_lowpass2_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FILTER_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_type) }, { "gyro_lowpass2_hz_roll", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz[ROLL]) }, { "gyro_lowpass2_hz_pitch", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz[PITCH]) }, { "gyro_lowpass2_hz_yaw", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz[YAW]) }, +#endif { "gyro_abg_alpha", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_ABG_alpha) }, { "gyro_abg_boost", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 2000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_ABG_boost) }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index b957ac7ef3..cb2c2c6b03 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -158,8 +158,10 @@ typedef struct gyroSensor_s { gyroLowpassFilter_t lowpassFilter[XYZ_AXIS_COUNT]; // lowpass2 gyro soft filter +#ifdef USE_GYRO_LPF2 filterApplyFnPtr lowpass2FilterApplyFn; gyroLowpassFilter_t lowpass2Filter[XYZ_AXIS_COUNT]; +#endif // ABG filter filterApplyFnPtr gyroABGFilterApplyFn; @@ -236,10 +238,12 @@ 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, @@ -289,10 +293,12 @@ 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, @@ -749,6 +755,7 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type) { lpfHz[PITCH] = gyroConfig()->gyro_lowpass_hz[PITCH]; lpfHz[YAW] = gyroConfig()->gyro_lowpass_hz[YAW]; break; +#ifdef USE_GYRO_LPF2 case FILTER_LOWPASS2: lowpassFilterApplyFn = &gyroSensor->lowpass2FilterApplyFn; lowpassFilter = gyroSensor->lowpass2Filter; @@ -756,6 +763,7 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type) { lpfHz[PITCH] = gyroConfig()->gyro_lowpass2_hz[PITCH]; lpfHz[YAW] = gyroConfig()->gyro_lowpass2_hz[YAW]; break; +#endif default: return; } @@ -894,11 +902,13 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) { FILTER_LOWPASS, gyroConfig()->gyro_lowpass_type ); +#ifdef USE_GYRO_LPF2 gyroInitLowpassFilterLpf( gyroSensor, FILTER_LOWPASS2, gyroConfig()->gyro_lowpass2_type ); +#endif gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); #ifdef USE_GYRO_DATA_ANALYSE diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 84bee04419..403954835e 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -132,7 +132,9 @@ 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; @@ -147,7 +149,9 @@ 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; diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h index 2008434496..4bf1060e19 100644 --- a/src/main/sensors/gyro_filter_impl.h +++ b/src/main/sensors/gyro_filter_impl.h @@ -36,7 +36,9 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) { #endif // apply static notch filters and software lowpass filters +#ifdef USE_GYRO_LPF2 gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); +#endif gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); diff --git a/src/main/target/NBDBBBLV2/config.c b/src/main/target/NBDBBBLV2/config.c index 2c735aec79..407f0f6f97 100644 --- a/src/main/target/NBDBBBLV2/config.c +++ b/src/main/target/NBDBBBLV2/config.c @@ -168,9 +168,11 @@ void targetConfiguration(void) gyroConfigMutable()->gyro_lowpass_hz[ROLL] = 200; gyroConfigMutable()->gyro_lowpass_hz[PITCH] = 200; gyroConfigMutable()->gyro_lowpass_hz[YAW] = 200; +#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = 250; gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = 250; gyroConfigMutable()->gyro_lowpass2_hz[YAW] = 250; +#endif gyroConfigMutable()->yaw_spin_threshold = 1950; rxConfigMutable()->mincheck = 1075; rxConfigMutable()->maxcheck = 1900; diff --git a/src/main/target/NBDBBBLV3/config.c b/src/main/target/NBDBBBLV3/config.c index 27f7b894ad..0774446586 100644 --- a/src/main/target/NBDBBBLV3/config.c +++ b/src/main/target/NBDBBBLV3/config.c @@ -168,9 +168,11 @@ void targetConfiguration(void) gyroConfigMutable()->gyro_lowpass_hz[ROLL] = 200; gyroConfigMutable()->gyro_lowpass_hz[PITCH] = 200; gyroConfigMutable()->gyro_lowpass_hz[YAW] = 200; +#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = 250; gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = 250; gyroConfigMutable()->gyro_lowpass2_hz[YAW] = 250; +#endif gyroConfigMutable()->yaw_spin_threshold = 1950; rxConfigMutable()->mincheck = 1075; rxConfigMutable()->maxcheck = 1900; diff --git a/src/main/target/NBDHMBF41S/config.c b/src/main/target/NBDHMBF41S/config.c index 493da570e9..92fe2d8b70 100644 --- a/src/main/target/NBDHMBF41S/config.c +++ b/src/main/target/NBDHMBF41S/config.c @@ -114,7 +114,9 @@ void targetConfiguration(void) gyroConfigMutable()->gyro_lowpass_type = FILTER_PT1; gyroConfigMutable()->gyro_lowpass_hz = 200; +#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_hz = 200; +#endif gyroConfigMutable()->yaw_spin_threshold = 1950; gyroConfigMutable()->dyn_lpf_gyro_min_hz = 160; gyroConfigMutable()->dyn_lpf_gyro_max_hz = 400; diff --git a/src/main/target/NBDHMBF4PRO/config.c b/src/main/target/NBDHMBF4PRO/config.c index 42862bed35..fd309465bc 100644 --- a/src/main/target/NBDHMBF4PRO/config.c +++ b/src/main/target/NBDHMBF4PRO/config.c @@ -149,9 +149,11 @@ void targetConfiguration(void) { gyroConfigMutable()->gyro_lowpass_hz[ROLL] = 200; gyroConfigMutable()->gyro_lowpass_hz[PITCH] = 200; gyroConfigMutable()->gyro_lowpass_hz[YAW] = 200; +#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = 250; gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = 250; gyroConfigMutable()->gyro_lowpass2_hz[YAW] = 250; +#endif gyroConfigMutable()->yaw_spin_threshold = 1950; // gyroConfigMutable()->dyn_lpf_gyro_min_hz = 200; // gyroConfigMutable()->dyn_lpf_gyro_max_hz = 500; diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index fc2dbc20e4..0ee0877b84 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -116,7 +116,9 @@ TEST(SensorGyro, Update) pgResetAll(); // turn off filters gyroConfigMutable()->gyro_lowpass_hz[ROLL] = 0; +#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = 0; +#endif gyroConfigMutable()->gyro_soft_notch_hz_1 = 0; gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; gyroInit(); From b2117eb9c6606d804c7361a3b6a08b1c9f6b5b95 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Fri, 27 Oct 2023 10:54:51 -0500 Subject: [PATCH 4/4] revert some USE_GYRO_DATA_ANALYSE and USE_GYRO_LPF2 gating (#936) revert some USE_GYRO_DATA_ANALYSE and USE_GYRO_LPF2 gating because C compiler sucks --- src/main/flight/pid.c | 6 ++++-- src/main/flight/pid.h | 2 -- src/main/interface/msp.c | 22 ---------------------- src/main/interface/settings.c | 4 ---- src/main/interface/settings.h | 2 -- src/main/sensors/gyro.c | 8 -------- src/main/sensors/gyro.h | 8 -------- 7 files changed, 4 insertions(+), 48 deletions(-) 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;