Rework gyro sample rate and DLPF configuration and expose additional filter cutoffs (#5483)

The old gyro_lpf setting was based on the DLPF_CFG values for the MPU6050 gyro and the enumeration was inaccurate and misleading.  For example, the default "OFF" setting did not disable the DLPF, but actually set it to around 250hz.  The actual cutoff frequency for each setting varies by gyro hardware so the literal frequencies in the enumeration were also incorrect.

Removed gyro_lpf and replaced it with gyro_hardware_lpf (8KHz) and gyro_32khz_hardware_lpf (32KHz).  The parameters were renamed to indicate that they are hardware filtering options to differentiate from the many software lowpass filtering options.

gyro_hardware_lpf - This parameter sets the filtering and sample rate options for 8KHz gyros (or 32KHz capable gyros running in 8KHz mode).

- NORMAL - default setting that is equivalent to the previous "OFF" setting.  Configures 8KHz sampling with ~250Hz filter cutoff.
- EXPERIMENTAL - 8KHz sampling with a higher frequency filter cutoff (around 3000hz).  Considerably more noisy and requires additional software filtering.  Note that for the MPU6000 Invensense doesn't officially document the filter cutoff frequency for this selection and simply lists it as "reserved".  In testing it's clear that a higher frequency filter cutoff is being selected due to the increased noise, but the actual cutoff frequency is unknown.
- 1KHZ_SAMPLING - 1KHz sample rate with and approximate 188Hz filter cutoff.

Note that the following additional 1KHz sample rate options with lower filter cutoffs have been eliminated - "98HZ", "42HZ", "20HZ", "10HZ", "5HZ".  It seems unlikely that these are still needed are probably no longer viable and flight performance would be very poor.

gyro_32khz_hardware_lpf - This parameter sets the filtering options while running in 32KHz mode on capable gyros.  It also exposes a new high frequency filter cutoff mode.

- NORMAL - The default and matches the current settings used for 32KHz mode.  Provides a filter cutoff around 3000Hz.
- EXPERIMENTAL - Selects a filter cutoff around 8000Hz.  This is a very noisy setting and will require substantial software filtering.

The default values for both 8KHz and 32KHz sample rates were chosen to match the previous defaults and users should not experience any performance differences.

Normalized the gyro initialization.  Previously there was little consistency on how the initialization was performed and the settings interpreted.  For example, MPU9250 used a completely different logic tree when configuring the registers.

Disconnected the literal parameter value from the gyro initialization.  The gyro_lpf parameter contained a number from 0-7 that was literally applied to the configuration register during the gyro initialization.  This caused some older gyro initializations to be incorrect as they used a different register layout (MPU3050 and L3G4200D).  By transitioning to a logical selection the actual value applied to the hardware register is abstracted.  This will better future-proof the design as new gyros may have a different register structure that may be incompatible with the old method.

Added a gyroregisters command to the CLI that is used to read the current register settings from the gyro and dump them to the CLI.  This is used to verify the configuration in comparison to the datasheets for the various gyros.  Testing empirically by looking at the relative noise from the gyros can give a rough estimate whether the different options are selecting correctly, but it's not very precise.  The code for the gyroregisters CLI command is wrapped inside #ifdef USE_GYRO_REGISTER_DUMP blocks to allow easy disabling.  It's currently enabled for all targets but we may decide to disable before release or only limit to targets with more available space (>=F4).

src/main/blackbox/blackbox.c

@@ -1289,7 +1289,11 @@ static bool blackboxWriteSysinfo(void)
 
         BLACKBOX_PRINT_HEADER_LINE("deadband", "%d",                        rcControlsConfig()->deadband);
         BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d",                    rcControlsConfig()->yaw_deadband);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d",                        gyroConfig()->gyro_lpf);
+
+        BLACKBOX_PRINT_HEADER_LINE("gyro_hardware_lpf", "%d",               gyroConfig()->gyro_hardware_lpf);
+#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
+        BLACKBOX_PRINT_HEADER_LINE("gyro_32khz_hardware_lpf", "%d",         gyroConfig()->gyro_32khz_hardware_lpf);
+#endif
         BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_type", "%d",               gyroConfig()->gyro_lowpass_type);
         BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz", "%d",                 gyroConfig()->gyro_lowpass_hz);
         BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_type", "%d",              gyroConfig()->gyro_lowpass2_type);

src/main/drivers/accgyro/accgyro.h

@@ -35,14 +35,12 @@
 #define MPU_I2C_INSTANCE I2C_DEVICE
 #endif
 
-#define GYRO_LPF_256HZ      0
+#define GYRO_HARDWARE_LPF_NORMAL       0
-#define GYRO_LPF_188HZ      1
+#define GYRO_HARDWARE_LPF_EXPERIMENTAL 1
-#define GYRO_LPF_98HZ       2
+#define GYRO_HARDWARE_LPF_1KHZ_SAMPLE  2
-#define GYRO_LPF_42HZ       3
+
-#define GYRO_LPF_20HZ       4
+#define GYRO_32KHZ_HARDWARE_LPF_NORMAL       0
-#define GYRO_LPF_10HZ       5
+#define GYRO_32KHZ_HARDWARE_LPF_EXPERIMENTAL 1
-#define GYRO_LPF_5HZ        6
-#define GYRO_LPF_NONE       7
 
 typedef enum {
     GYRO_RATE_1_kHz,
@@ -75,10 +73,11 @@ typedef struct gyroDev_s {
     gyroRateKHz_e gyroRateKHz;
     bool dataReady;
     bool gyro_high_fsr;
-    uint8_t lpf;
+    uint8_t hardware_lpf;
+    uint8_t hardware_32khz_lpf;
     uint8_t mpuDividerDrops;
     ioTag_t mpuIntExtiTag;
-    uint8_t filler[3];
+    uint8_t filler[2];
 } gyroDev_t;
 
 typedef struct accDev_s {

src/main/drivers/accgyro/accgyro_l3g4200d.c

@@ -15,6 +15,8 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+// NOTE: This gyro is considered obsolete and may be removed in the future.
+
 #include <stdbool.h>
 #include <stdint.h>
 
@@ -60,23 +62,9 @@ static void l3g4200dInit(gyroDev_t *gyro)
 {
     bool ack;
 
-    uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
+    // Removed lowpass filter selection and just default to 32Hz regardless of gyro->hardware_lpf
-
+    // The previous selection was broken anyway as the old gyro->lpf values ranged from 0-7 and
-    switch (gyro->lpf) {
+    // the switch statement would have always taken the default and used L3G4200D_DLPF_32HZ
-        default:
-            case 32:
-            mpuLowPassFilter = L3G4200D_DLPF_32HZ;
-            break;
-        case 54:
-            mpuLowPassFilter = L3G4200D_DLPF_54HZ;
-            break;
-        case 78:
-            mpuLowPassFilter = L3G4200D_DLPF_78HZ;
-            break;
-        case 93:
-            mpuLowPassFilter = L3G4200D_DLPF_93HZ;
-            break;
-    }
 
     delay(100);
 
@@ -85,7 +73,9 @@ static void l3g4200dInit(gyroDev_t *gyro)
         failureMode(FAILURE_ACC_INIT);
 
     delay(5);
-    i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
+    i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | L3G4200D_DLPF_32HZ);
+
+    UNUSED(gyro);
 }
 
 // Read 3 gyro values into user-provided buffer. No overrun checking is done.

src/main/drivers/accgyro/accgyro_mpu.c

@@ -354,3 +354,54 @@ void mpuGyroInit(gyroDev_t *gyro)
     UNUSED(gyro);
 #endif
 }
+
+uint8_t mpuGyroDLPF(gyroDev_t *gyro)
+{
+    uint8_t ret;
+    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
+    } else {
+        switch (gyro->hardware_lpf) {
+            case GYRO_HARDWARE_LPF_NORMAL:
+                ret = 0;
+                break;
+            case GYRO_HARDWARE_LPF_EXPERIMENTAL:
+                ret = 7;
+                break;
+            case GYRO_HARDWARE_LPF_1KHZ_SAMPLE:
+                ret = 1;
+                break;
+            default:
+                ret = 0;
+                break;
+        }
+    }
+    return ret;
+}
+
+uint8_t mpuGyroFCHOICE(gyroDev_t *gyro)
+{
+    if (gyro->gyroRateKHz > GYRO_RATE_8_kHz) {
+        if (gyro->hardware_32khz_lpf == GYRO_32KHZ_HARDWARE_LPF_EXPERIMENTAL) {
+            return FCB_8800_32;
+        } else {
+            return FCB_3600_32;
+        }
+    } else {
+        return FCB_DISABLED;  // Not in 32KHz mode, set FCHOICE to select 8KHz sampling
+    }
+}
+
+#ifdef USE_GYRO_REGISTER_DUMP
+uint8_t mpuGyroReadRegister(const busDevice_t *bus, uint8_t reg)
+{
+    uint8_t data;
+    const bool ack = busReadRegisterBuffer(bus, reg, &data, 1);
+    if (ack) {
+        return data;
+    } else {
+        return 0;
+    }
+
+}
+#endif

src/main/drivers/accgyro/accgyro_mpu.h

@@ -211,6 +211,9 @@ void mpuGyroInit(struct gyroDev_s *gyro);
 bool mpuGyroRead(struct gyroDev_s *gyro);
 bool mpuGyroReadSPI(struct gyroDev_s *gyro);
 void mpuDetect(struct gyroDev_s *gyro);
+uint8_t mpuGyroDLPF(struct gyroDev_s *gyro);
+uint8_t mpuGyroFCHOICE(struct gyroDev_s *gyro);
+uint8_t mpuGyroReadRegister(const busDevice_t *bus, uint8_t reg);
 
 struct accDev_s;
 bool mpuAccRead(struct accDev_s *acc);

src/main/drivers/accgyro/accgyro_mpu3050.c

@@ -15,6 +15,8 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+// NOTE: This gyro is considered obsolete and may be removed in the future.
+
 #include <stdbool.h>
 #include <stdint.h>
 
@@ -48,6 +50,17 @@
 #define MPU3050_USER_RESET      0x01
 #define MPU3050_CLK_SEL_PLL_GX  0x01
 
+static uint8_t mpu3050GetDLPF(uint8_t lpf)
+{
+    uint8_t ret;
+    if (lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
+        ret = MPU3050_DLPF_188HZ;
+    } else {
+        ret = MPU3050_DLPF_256HZ;
+    }
+    return ret;
+}
+
 static void mpu3050Init(gyroDev_t *gyro)
 {
     delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
@@ -57,7 +70,7 @@ static void mpu3050Init(gyroDev_t *gyro)
         failureMode(FAILURE_ACC_INIT);
     }
 
-    busWriteRegister(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
+    busWriteRegister(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpu3050GetDLPF(gyro->hardware_lpf));
     busWriteRegister(&gyro->bus, MPU3050_INT_CFG, 0);
     busWriteRegister(&gyro->bus, MPU3050_USER_CTRL, MPU3050_USER_RESET);
     busWriteRegister(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);

src/main/drivers/accgyro/accgyro_mpu6050.c

@@ -81,7 +81,7 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
     busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
     busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
     delay(15); //PLL Settling time when changing CLKSEL is max 10ms.  Use 15ms to be sure
-    busWriteRegister(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
+    busWriteRegister(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro)); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
     busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);   //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec
 
     // ACC Init stuff.

src/main/drivers/accgyro/accgyro_mpu6500.c

@@ -61,12 +61,11 @@ void mpu6500GyroInit(gyroDev_t *gyro)
     delay(100);
     busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
     delay(15);
-    const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
+    busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | mpuGyroFCHOICE(gyro));
-    busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
     delay(15);
     busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
     delay(15);
-    busWriteRegister(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
+    busWriteRegister(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro));
     delay(15);
     busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops
     delay(100);

src/main/drivers/accgyro/accgyro_spi_icm20649.c

@@ -131,8 +131,14 @@ void icm20649GyroInit(gyroDev_t *gyro)
     spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
     delay(15);
     const uint8_t gyro_fsr = gyro->gyro_high_fsr ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS;
+
+    // If hardware_lpf is either GYRO_HARDWARE_LPF_NORMAL or GYRO_HARDWARE_LPF_EXPERIMENTAL then the
+    // gyro is running in 9KHz sample mode and GYRO_FCHOICE should be 0, otherwise we're in 1.1KHz sample
+    // mode and GYRO_FCHOICE = 1.  When in 1.1KHz mode select the 196.6Hz DLPF (GYRO_DLPFCFG = 0)
+    // Unfortunately we can't configure any difference in DLPF based on NORMAL vs. EXPERIMENTAL because
+    // the ICM20649 only has a single 9KHz DLPF cutoff.
     uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1100_Hz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips)
-    raGyroConfigData |= gyro_fsr << 1 | gyro->lpf << 3;
+    raGyroConfigData |= gyro_fsr << 1;
     spiBusWriteRegister(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData);
     delay(15);
     spiBusWriteRegister(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops

src/main/drivers/accgyro/accgyro_spi_icm20689.c

@@ -129,21 +129,11 @@ void icm20689GyroInit(gyroDev_t *gyro)
 //    delay(100);
     spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
     delay(15);
-    uint8_t raGyroConfigData = INV_FSR_2000DPS << 3;
+    spiBusWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | mpuGyroFCHOICE(gyro));
-    if (gyro->gyroRateKHz > GYRO_RATE_8_kHz) {
-        // use otherwise redundant LPF value to configure FCHOICE_B
-        // see REGISTER 27 – GYROSCOPE CONFIGURATION in datasheet
-        if (gyro->lpf==GYRO_LPF_NONE) {
-            raGyroConfigData |= FCB_8800_32;
-        } else {
-            raGyroConfigData |= FCB_3600_32;
-        }
-    }
-    spiBusWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
     delay(15);
     spiBusWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
     delay(15);
-    spiBusWriteRegister(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
+    spiBusWriteRegister(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro));
     delay(15);
     spiBusWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops
     delay(100);

src/main/drivers/accgyro/accgyro_spi_mpu6000.c

@@ -106,7 +106,7 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro)
     spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATON);
 
     // Accel and Gyro DLPF Setting
-    spiBusWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf);
+    spiBusWriteRegister(&gyro->bus, MPU6000_CONFIG, mpuGyroDLPF(gyro));
     delayMicroseconds(1);
 
     spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_FAST);  // 18 MHz SPI clock

src/main/drivers/accgyro/accgyro_spi_mpu9250.c

@@ -142,26 +142,9 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {
 
     mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
 
-    //Fchoice_b defaults to 00 which makes fchoice 11
+    mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | mpuGyroFCHOICE(gyro));
-    uint8_t raGyroConfigData = INV_FSR_2000DPS << 3;
-    if (gyro->gyroRateKHz > GYRO_RATE_8_kHz) {
-        // use otherwise redundant LPF value to configure FCHOICE_B
-        // see REGISTER 27 – GYROSCOPE CONFIGURATION in datasheet
-        if (gyro->lpf==GYRO_LPF_NONE) {
-            raGyroConfigData |= FCB_8800_32;
-        } else {
-            raGyroConfigData |= FCB_3600_32;
-        }
-    }
-    mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
 
-    if (gyro->lpf == 4) {
+    mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro));
-        mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF
-    } else if (gyro->lpf < 4) {
-        mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
-    } else if (gyro->lpf > 4) {
-        mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF
-    }
 
     mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops);
 

src/main/drivers/accgyro/gyro_sync.c

@@ -31,7 +31,7 @@ uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenomin
 {
     float gyroSamplePeriod;
 
-    if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) {
+    if (lpf == GYRO_HARDWARE_LPF_NORMAL || lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL) {
         if (gyro_use_32khz) {
             gyro->gyroRateKHz = GYRO_RATE_32_kHz;
             gyroSamplePeriod = 31.5f;

src/main/fc/config.c

@@ -347,7 +347,7 @@ void validateAndFixGyroConfig(void)
         gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
     }
 
-    if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
+    if (gyroConfig()->gyro_hardware_lpf != GYRO_HARDWARE_LPF_NORMAL && gyroConfig()->gyro_hardware_lpf != GYRO_HARDWARE_LPF_EXPERIMENTAL) {
         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;
@@ -378,7 +378,7 @@ void validateAndFixGyroConfig(void)
         samplingTime = 0.000125f;
         break;
     }
-    if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
+    if (gyroConfig()->gyro_hardware_lpf != GYRO_HARDWARE_LPF_NORMAL && gyroConfig()->gyro_hardware_lpf != GYRO_HARDWARE_LPF_EXPERIMENTAL) {
         switch (gyroMpuDetectionResult()->sensor) {
         case ICM_20649_SPI:
             samplingTime = 1.0f / 1100.0f;

src/main/interface/cli.c

@@ -2343,6 +2343,17 @@ static void cliGpsPassthrough(char *cmdline)
 }
 #endif
 
+#if defined(USE_GYRO_REGISTER_DUMP) && !defined(SIMULATOR_BUILD)
+static void cliDumpGyroRegisters(char *cmdline)
+{
+    tfp_printf("# WHO_AM_I    0x%X\r\n", gyroReadRegister(MPU_RA_WHO_AM_I));
+    tfp_printf("# CONFIG      0x%X\r\n", gyroReadRegister(MPU_RA_CONFIG));
+    tfp_printf("# GYRO_CONFIG 0x%X\r\n", gyroReadRegister(MPU_RA_GYRO_CONFIG));
+    UNUSED(cmdline);
+}
+#endif
+
+
 static int parseOutputIndex(char *pch, bool allowAllEscs) {
     int outputIndex = atoi(pch);
     if ((outputIndex >= 0) && (outputIndex < getMotorCount())) {
@@ -3804,6 +3815,9 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
 #ifdef USE_GPS
     CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough),
+#endif
+#if defined(USE_GYRO_REGISTER_DUMP) && !defined(SIMULATOR_BUILD)
+    CLI_COMMAND_DEF("gyroregisters", "dump gyro config registers contents", NULL, cliDumpGyroRegisters),
 #endif
     CLI_COMMAND_DEF("help", NULL, NULL, cliHelp),
 #ifdef USE_LED_STRIP

src/main/interface/settings.c

@@ -218,16 +218,18 @@ static const char * const lookupTableRxSpi[] = {
 };
 #endif
 
-static const char * const lookupTableGyroLpf[] = {
+static const char * const lookupTableGyroHardwareLpf[] = {
-    "OFF",
+    "NORMAL",
-    "188HZ",
+    "EXPERIMENTAL",
-    "98HZ",
+    "1KHZ_SAMPLING"
-    "42HZ",
+};
-    "20HZ",
+
-    "10HZ",
+#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
-    "5HZ",
+static const char * const lookupTableGyro32khzHardwareLpf[] = {
+    "NORMAL",
     "EXPERIMENTAL"
 };
+#endif
 
 #ifdef USE_CAMERA_CONTROL
 static const char * const lookupTableCameraControlMode[] = {
@@ -340,7 +342,10 @@ const lookupTableEntry_t lookupTables[] = {
 #ifdef USE_RX_SPI
     { lookupTableRxSpi, sizeof(lookupTableRxSpi) / sizeof(char *) },
 #endif
-    { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) },
+    { lookupTableGyroHardwareLpf, sizeof(lookupTableGyroHardwareLpf) / sizeof(char *) },
+#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
+    { lookupTableGyro32khzHardwareLpf, sizeof(lookupTableGyro32khzHardwareLpf) / sizeof(char *) },
+#endif
     { lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) },
 #ifdef USE_BARO
     { lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) },
@@ -384,7 +389,10 @@ const lookupTableEntry_t lookupTables[] = {
 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_lpf",                   VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_LPF }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf) },
+    { "gyro_hardware_lpf",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_HARDWARE_LPF }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_hardware_lpf) },
+#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
+    { "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)
     { "gyro_high_range",            VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_high_fsr) },
 #endif

src/main/interface/settings.h

@@ -45,7 +45,10 @@ typedef enum {
 #ifdef USE_RX_SPI
     TABLE_RX_SPI,
 #endif
-    TABLE_GYRO_LPF,
+    TABLE_GYRO_HARDWARE_LPF,
+#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
+    TABLE_GYRO_32KHZ_HARDWARE_LPF,
+#endif
     TABLE_ACC_HARDWARE,
 #ifdef USE_BARO
     TABLE_BARO_HARDWARE,

src/main/sensors/gyro.c

@@ -179,7 +179,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_align = ALIGN_DEFAULT,
     .gyroMovementCalibrationThreshold = 48,
     .gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT,
-    .gyro_lpf = GYRO_LPF_256HZ,
+    .gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL,
+    .gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL,
     .gyro_lowpass_type = FILTER_PT1,
     .gyro_lowpass_hz = 90,
     .gyro_lowpass_order = 1,
@@ -443,8 +444,9 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor)
     }
 
     // Must set gyro targetLooptime before gyroDev.init and initialisation of filters
-    gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
+    gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_hardware_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
-    gyroSensor->gyroDev.lpf = gyroConfig()->gyro_lpf;
+    gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf;
+    gyroSensor->gyroDev.hardware_32khz_lpf = gyroConfig()->gyro_32khz_hardware_lpf;
     gyroSensor->gyroDev.initFn(&gyroSensor->gyroDev);
     if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
         gyroSensor->gyroDev.gyroAlign = gyroConfig()->gyro_align;
@@ -1160,3 +1162,10 @@ uint16_t gyroAbsRateDps(int axis)
 {
     return fabsf(gyro.gyroADCf[axis]);
 }
+
+#ifdef USE_GYRO_REGISTER_DUMP
+uint8_t gyroReadRegister(uint8_t reg)
+{
+    return mpuGyroReadRegister(gyroSensorBus(), reg);
+}
+#endif

src/main/sensors/gyro.h

@@ -70,7 +70,8 @@ typedef struct gyroConfig_s {
     sensor_align_e 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.
     uint8_t  gyro_sync_denom;                  // Gyro sample divider
-    uint8_t  gyro_lpf;                         // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
+    uint8_t  gyro_hardware_lpf;                // gyro DLPF setting
+    uint8_t  gyro_32khz_hardware_lpf;          // gyro 32khz DLPF setting
 
     bool     gyro_high_fsr;
     bool     gyro_use_32khz;
@@ -120,3 +121,4 @@ int16_t gyroGetTemperature(void);
 int16_t gyroRateDps(int axis);
 bool gyroOverflowDetected(void);
 uint16_t gyroAbsRateDps(int axis);
+uint8_t gyroReadRegister(uint8_t reg);

src/main/target/COLIBRI_RACE/i2c_bst.c

@@ -438,7 +438,14 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
             bstWrite8(rcControlsConfig()->yaw_deadband);
             break;
         case BST_FC_FILTERS:
-            bstWrite16(constrain(gyroConfig()->gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values
+            switch (gyroConfig()->gyro_hardware_lpf) { // Extra safety to prevent OSD setting corrupt values
+                case GYRO_HARDWARE_LPF_1KHZ_SAMPLE:
+                    bstWrite16(1);
+                    break;
+                default:
+                    bstWrite16(0);
+                    break;
+            }
             break;
         default:
             // we do not know how to handle the (valid) message, indicate error BST
@@ -632,7 +639,14 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
             rcControlsConfigMutable()->yaw_deadband = bstRead8();
             break;
         case BST_SET_FC_FILTERS:
-            gyroConfigMutable()->gyro_lpf = bstRead16();
+            switch (bstRead16()) {
+                case 1:
+                    gyroConfigMutable()->gyro_hardware_lpf = GYRO_HARDWARE_LPF_1KHZ_SAMPLE;
+                    break;
+                default:
+                    gyroConfigMutable()->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
+                    break;
+            }
             break;
 
         default:

src/main/target/NAZE/config.c

@@ -56,7 +56,7 @@ void targetConfiguration(void)
 
     motorConfigMutable()->minthrottle = 1049;
 
-    gyroConfigMutable()->gyro_lpf = GYRO_LPF_188HZ;
+    gyroConfigMutable()->gyro_hardware_lpf = GYRO_HARDWARE_LPF_1KHZ_SAMPLE;
     gyroConfigMutable()->gyro_soft_lpf_hz = 100;
     gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
     gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;

src/main/target/common_fc_pre.h

@@ -103,6 +103,7 @@
 
 
 #define USE_CLI
+#define USE_GYRO_REGISTER_DUMP  // Adds gyroregisters command to cli to dump configured register values
 #define USE_PPM
 #define USE_PWM
 #define USE_SERIAL_RX

src/test/unit/blackbox_unittest.cc

@@ -71,14 +71,14 @@ TEST(BlackboxTest, TestInitIntervals)
     EXPECT_EQ(4096, blackboxSInterval);
 
     // 1kHz PIDloop
-    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_LPF_188HZ, 1, false);
+    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_HARDWARE_LPF_1KHZ_SAMPLE, 1, false);
     blackboxInit();
     EXPECT_EQ(32, blackboxIInterval);
     EXPECT_EQ(1, blackboxPInterval);
     EXPECT_EQ(8192, blackboxSInterval);
 
     // 2kHz PIDloop
-    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_LPF_256HZ, 4, false);
+    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_HARDWARE_LPF_NORMAL, 4, false);
     EXPECT_EQ(500, gyro.targetLooptime);
     blackboxInit();
     EXPECT_EQ(64, blackboxIInterval);
@@ -86,7 +86,7 @@ TEST(BlackboxTest, TestInitIntervals)
     EXPECT_EQ(16384, blackboxSInterval);
 
     // 4kHz PIDloop
-    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_LPF_256HZ, 2, false);
+    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_HARDWARE_LPF_NORMAL, 2, false);
     EXPECT_EQ(250, gyro.targetLooptime);
     blackboxInit();
     EXPECT_EQ(128, blackboxIInterval);
@@ -94,7 +94,7 @@ TEST(BlackboxTest, TestInitIntervals)
     EXPECT_EQ(32768, blackboxSInterval);
 
     // 8kHz PIDloop
-    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_LPF_256HZ, 1, false);
+    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_HARDWARE_LPF_NORMAL, 1, false);
     EXPECT_EQ(125, gyro.targetLooptime);
     gyro.targetLooptime = 125;
     blackboxInit();
@@ -103,7 +103,7 @@ TEST(BlackboxTest, TestInitIntervals)
     EXPECT_EQ(65536, blackboxSInterval);
 
     // 16kHz PIDloop
-    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_LPF_256HZ, 2, true);
+    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_HARDWARE_LPF_NORMAL, 2, true);
     EXPECT_EQ(63, gyro.targetLooptime);
     gyro.targetLooptime = 63; // rounded from 62.5
     blackboxInit();
@@ -112,7 +112,7 @@ TEST(BlackboxTest, TestInitIntervals)
     EXPECT_EQ(131072, blackboxSInterval);
 
     // 32kHz PIDloop
-    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_LPF_256HZ, 1, true);
+    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_HARDWARE_LPF_NORMAL, 1, true);
     EXPECT_EQ(31, gyro.targetLooptime);
     gyro.targetLooptime = 31; // rounded from 31.25
     blackboxInit();