Merge pull request #6617 from etracer65/gyro_lpf_updates

Disable DLPF EXPERIMENTAL mode for MPU60x0; remove experimental support from F3

src/main/drivers/accgyro/accgyro.h

@@ -39,12 +39,18 @@
 #define MPU_I2C_INSTANCE I2C_DEVICE
 #endif
 
-#define GYRO_HARDWARE_LPF_NORMAL       0
+typedef enum {
-#define GYRO_HARDWARE_LPF_EXPERIMENTAL 1
+    GYRO_HARDWARE_LPF_NORMAL,
-#define GYRO_HARDWARE_LPF_1KHZ_SAMPLE  2
+    GYRO_HARDWARE_LPF_1KHZ_SAMPLE,
+#ifdef USE_GYRO_DLPF_EXPERIMENTAL
+    GYRO_HARDWARE_LPF_EXPERIMENTAL
+#endif
+} gyroHardwareLpf_e;
 
-#define GYRO_32KHZ_HARDWARE_LPF_NORMAL       0
+typedef enum {
-#define GYRO_32KHZ_HARDWARE_LPF_EXPERIMENTAL 1
+    GYRO_32KHZ_HARDWARE_LPF_NORMAL,
+    GYRO_32KHZ_HARDWARE_LPF_EXPERIMENTAL
+} gyro32khzHardwareLpf;
 
 typedef enum {
     GYRO_RATE_1_kHz,

src/main/drivers/accgyro/accgyro_mpu.c

@@ -359,20 +359,27 @@ void mpuGyroInit(gyroDev_t *gyro)
 
 uint8_t mpuGyroDLPF(gyroDev_t *gyro)
 {
-    uint8_t ret;
+    uint8_t ret = 0;
-    if (gyro->gyroRateKHz > GYRO_RATE_8_kHz) {
+    
-        ret = 0;  // If gyro is in 32KHz mode then the DLPF bits aren't used - set to 0
+    // If gyro is in 32KHz mode then the DLPF bits aren't used
-    } else {
+    if (gyro->gyroRateKHz <= GYRO_RATE_8_kHz) {
         switch (gyro->hardware_lpf) {
-            case GYRO_HARDWARE_LPF_NORMAL:
+#ifdef USE_GYRO_DLPF_EXPERIMENTAL
-                ret = 0;
-                break;
             case GYRO_HARDWARE_LPF_EXPERIMENTAL:
+                // experimental mode not supported for MPU60x0 family
+                if ((gyro->gyroHardware != GYRO_MPU6050) && (gyro->gyroHardware != GYRO_MPU6000)) {
                     ret = 7;
+                } else {
+                    ret = 0;
+                }
                 break;
+#endif
+
             case GYRO_HARDWARE_LPF_1KHZ_SAMPLE:
                 ret = 1;
                 break;
+                
+            case GYRO_HARDWARE_LPF_NORMAL:
             default:
                 ret = 0;
                 break;
@@ -384,11 +391,15 @@ uint8_t mpuGyroDLPF(gyroDev_t *gyro)
 uint8_t mpuGyroFCHOICE(gyroDev_t *gyro)
 {
     if (gyro->gyroRateKHz > GYRO_RATE_8_kHz) {
+#ifdef USE_GYRO_DLPF_EXPERIMENTAL
         if (gyro->hardware_32khz_lpf == GYRO_32KHZ_HARDWARE_LPF_EXPERIMENTAL) {
             return FCB_8800_32;
         } else {
             return FCB_3600_32;
         }
+#else
+        return FCB_3600_32;
+#endif
     } else {
         return FCB_DISABLED;  // Not in 32KHz mode, set FCHOICE to select 8KHz sampling
     }

src/main/drivers/accgyro/gyro_sync.c

@@ -51,7 +51,7 @@ uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenomin
 {
     float gyroSamplePeriod;
 
-    if (lpf == GYRO_HARDWARE_LPF_NORMAL || lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL) {
+    if (lpf != GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
         if (gyro_use_32khz) {
             gyro->gyroRateKHz = GYRO_RATE_32_kHz;
             gyroSamplePeriod = 31.25f;

src/main/fc/config.c

@@ -427,7 +427,7 @@ void validateAndFixGyroConfig(void)
         gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
     }
 
-    if (gyroConfig()->gyro_hardware_lpf != GYRO_HARDWARE_LPF_NORMAL && gyroConfig()->gyro_hardware_lpf != GYRO_HARDWARE_LPF_EXPERIMENTAL) {
+    if (gyroConfig()->gyro_hardware_lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
         pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
         gyroConfigMutable()->gyro_sync_denom = 1;
         gyroConfigMutable()->gyro_use_32khz = false;
@@ -458,7 +458,7 @@ void validateAndFixGyroConfig(void)
         samplingTime = 0.000125f;
         break;
     }
-    if (gyroConfig()->gyro_hardware_lpf != GYRO_HARDWARE_LPF_NORMAL && gyroConfig()->gyro_hardware_lpf != GYRO_HARDWARE_LPF_EXPERIMENTAL) {
+    if (gyroConfig()->gyro_hardware_lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
         switch (gyroMpuDetectionResult()->sensor) {
         case ICM_20649_SPI:
             samplingTime = 1.0f / 1100.0f;

src/main/interface/settings.c

@@ -229,11 +229,13 @@ static const char * const lookupTableRxSpi[] = {
 
 static const char * const lookupTableGyroHardwareLpf[] = {
     "NORMAL",
-    "EXPERIMENTAL",
+    "1KHZ_SAMPLING",
-    "1KHZ_SAMPLING"
+#ifdef USE_GYRO_DLPF_EXPERIMENTAL
+    "EXPERIMENTAL"
+#endif
 };
 
-#ifdef USE_32K_CAPABLE_GYRO
+#if defined(USE_32K_CAPABLE_GYRO) && defined(USE_GYRO_DLPF_EXPERIMENTAL)
 static const char * const lookupTableGyro32khzHardwareLpf[] = {
     "NORMAL",
     "EXPERIMENTAL"
@@ -416,7 +418,7 @@ const lookupTableEntry_t lookupTables[] = {
     LOOKUP_TABLE_ENTRY(lookupTableRxSpi),
 #endif
     LOOKUP_TABLE_ENTRY(lookupTableGyroHardwareLpf),
-#ifdef USE_32K_CAPABLE_GYRO
+#if defined(USE_32K_CAPABLE_GYRO) && defined(USE_GYRO_DLPF_EXPERIMENTAL)
     LOOKUP_TABLE_ENTRY(lookupTableGyro32khzHardwareLpf),
 #endif
     LOOKUP_TABLE_ENTRY(lookupTableAccHardware),
@@ -491,7 +493,7 @@ const clivalue_t valueTable[] = {
 // PG_GYRO_CONFIG
     { "align_gyro",                 VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_align) },
     { "gyro_hardware_lpf",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_HARDWARE_LPF }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_hardware_lpf) },
-#ifdef USE_32K_CAPABLE_GYRO
+#if defined(USE_32K_CAPABLE_GYRO) && defined(USE_GYRO_DLPF_EXPERIMENTAL)
     { "gyro_32khz_hardware_lpf",    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_32KHZ_HARDWARE_LPF }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_32khz_hardware_lpf) },
 #endif
 #if defined(USE_GYRO_SPI_ICM20649)

src/main/interface/settings.h

@@ -52,7 +52,7 @@ typedef enum {
     TABLE_RX_SPI,
 #endif
     TABLE_GYRO_HARDWARE_LPF,
-#ifdef USE_32K_CAPABLE_GYRO
+#if defined(USE_32K_CAPABLE_GYRO) && defined(USE_GYRO_DLPF_EXPERIMENTAL)
     TABLE_GYRO_32KHZ_HARDWARE_LPF,
 #endif
     TABLE_ACC_HARDWARE,

src/main/sensors/gyro.c

@@ -178,7 +178,7 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ
 #define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980  // 97.5% full scale (1950dps for 2000dps gyro)
 #define GYRO_OVERFLOW_RESET_THRESHOLD 30340    // 92.5% full scale (1850dps for 2000dps gyro)
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 4);
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5);
 
 #ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
 #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1

src/main/target/common_fc_pre.h

@@ -207,6 +207,7 @@
 #define USE_GPS_NMEA
 #define USE_GPS_UBLOX
 #define USE_GPS_RESCUE
+#define USE_GYRO_DLPF_EXPERIMENTAL
 #define USE_OSD
 #define USE_OSD_OVER_MSP_DISPLAYPORT
 #define USE_OSD_ADJUSTMENTS