From ce0225593253d954c8d8783cdd73dd6c419ecc13 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 25 Oct 2023 13:15:20 -0500 Subject: [PATCH 1/2] add Gyro LPF2 type to OSD --- 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 a8f3804735..39c10a9dd2 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -487,6 +487,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; @@ -515,6 +516,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]; @@ -547,6 +549,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; @@ -575,11 +578,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 974e2f17e9bd4e1aba8be0ce8d1d071e2a30febd Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 25 Oct 2023 15:06:57 -0500 Subject: [PATCH 2/2] 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 5797c373a8..bc265694af 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1227,10 +1227,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 39c10a9dd2..b5b8a6a446 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -487,10 +487,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 static uint16_t gyroConfig_gyro_q; static uint8_t gyroConfig_gyro_notch_count; static uint16_t gyroConfig_gyro_notch_min_hz; @@ -516,10 +518,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 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; @@ -549,10 +553,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 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; diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index b3aebb427d..8173be5278 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); @@ -1811,11 +1821,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 9634ec73c8..f7718b5f2a 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -503,10 +503,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 b77528e9fd..4aa923f603 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, @@ -286,10 +290,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, @@ -743,6 +749,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; @@ -750,6 +757,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; } @@ -888,11 +896,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 0f32c699e1..e7c66fe0cc 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -125,7 +125,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; @@ -140,7 +142,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();