Merge remote-tracking branch 'emuflight/master' into 20231023_disable…

…_icm426xx_AFSR
emuflight · Oct 26, 2023 · b9d9e70 · b9d9e70
2 parents c9dbf62 + db57386
commit b9d9e70
Show file tree

Hide file tree

Showing 12 changed files with 61 additions and 1 deletion.
diff --git 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
@@ -493,9 +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;
@@ -524,9 +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;
@@ -558,9 +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;
@@ -589,11 +598,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 },

diff --git 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
@@ -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
@@ -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,13 +755,15 @@ 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;
         lpfHz[ROLL] = gyroConfig()->gyro_lowpass2_hz[ROLL];
         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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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();