From 0e8e5b3c16ac5e3afe0f238b9d1ac6cbb302f87d Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Wed, 22 Aug 2018 09:42:08 -0400 Subject: [PATCH] Disable DLPF EXPERIMENTAL mode for MPU60x0; remove experimental support from F3 Officially Invensense lists the experimentl mode as "unsupported" on the MPU60x0 series. Previously it was added to allow testing. It's been determined that it's not a viable gyro filtering operational mode. Also change "experimental" DLPF support available for F4 or higher. Few F3 boards have gyros that can use this mode anyway. Saves 200 bytes. --- src/main/drivers/accgyro/accgyro.h | 16 +++++++++++----- src/main/drivers/accgyro/accgyro_mpu.c | 22 ++++++++++++++-------- src/main/drivers/accgyro/gyro_sync.c | 2 +- src/main/fc/config.c | 4 ++-- src/main/interface/settings.c | 12 +++++++----- src/main/interface/settings.h | 2 +- src/main/target/common_fc_pre.h | 1 + 7 files changed, 37 insertions(+), 22 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index e18a69d7a..8c83cb0e5 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -39,12 +39,18 @@ #define MPU_I2C_INSTANCE I2C_DEVICE #endif -#define GYRO_HARDWARE_LPF_NORMAL 0 -#define GYRO_HARDWARE_LPF_EXPERIMENTAL 1 -#define GYRO_HARDWARE_LPF_1KHZ_SAMPLE 2 +typedef enum { + GYRO_HARDWARE_LPF_NORMAL, + GYRO_HARDWARE_LPF_1KHZ_SAMPLE, +#ifdef USE_GYRO_DLPF_EXPERIMENTAL + GYRO_HARDWARE_LPF_EXPERIMENTAL +#endif +} gyroHardwareLpf_e; -#define GYRO_32KHZ_HARDWARE_LPF_NORMAL 0 -#define GYRO_32KHZ_HARDWARE_LPF_EXPERIMENTAL 1 +typedef enum { + GYRO_32KHZ_HARDWARE_LPF_NORMAL, + GYRO_32KHZ_HARDWARE_LPF_EXPERIMENTAL +} gyro32khzHardwareLpf; typedef enum { GYRO_RATE_1_kHz, diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 9ae8fbfdd..661f61e02 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -359,23 +359,25 @@ void mpuGyroInit(gyroDev_t *gyro) 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 { + uint8_t ret = 0; + + // If gyro is in 32KHz mode then the DLPF bits aren't used + if (gyro->gyroRateKHz <= GYRO_RATE_8_kHz) { switch (gyro->hardware_lpf) { case GYRO_HARDWARE_LPF_NORMAL: ret = 0; break; +#ifdef USE_GYRO_DLPF_EXPERIMENTAL case GYRO_HARDWARE_LPF_EXPERIMENTAL: - ret = 7; + // experimental mode not supported for MPU60x0 family + if ((gyro->gyroHardware != GYRO_MPU6050) && (gyro->gyroHardware != GYRO_MPU6000)) { + ret = 7; + } break; +#endif case GYRO_HARDWARE_LPF_1KHZ_SAMPLE: ret = 1; break; - default: - ret = 0; - break; } } return ret; @@ -384,11 +386,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 } diff --git a/src/main/drivers/accgyro/gyro_sync.c b/src/main/drivers/accgyro/gyro_sync.c index 7b858ce16..55b43f4ec 100644 --- a/src/main/drivers/accgyro/gyro_sync.c +++ b/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; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 9259e5595..8ea1e116b 100644 --- a/src/main/fc/config.c +++ b/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; diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 94830f77e..68c833bc7 100644 --- a/src/main/interface/settings.c +++ b/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) diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index 04483eee9..013e7649e 100644 --- a/src/main/interface/settings.h +++ b/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, diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 4a8278870..d5f576eb1 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -208,6 +208,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