diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h index f6f9570cdb..eb90ee217f 100644 --- a/src/main/drivers/accgyro.h +++ b/src/main/drivers/accgyro.h @@ -26,10 +26,6 @@ #define MPU_I2C_INSTANCE I2C_DEVICE #endif -#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) -#define GYRO_SUPPORTS_32KHZ -#endif - #define GYRO_LPF_256HZ 0 #define GYRO_LPF_188HZ 1 #define GYRO_LPF_98HZ 2 diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 15c791c175..acc0668eaf 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -1047,17 +1047,12 @@ void validateAndFixGyroConfig(void) float samplingTime = 0.000125f; if (gyroConfig()->gyro_use_32khz) { -#ifdef GYRO_SUPPORTS_32KHZ samplingTime = 0.00003125; - // F1 and F3 can't handle high pid speed. + // F1 and F3 can't handle high pid speed #if defined(STM32F1) pidConfig()->pid_process_denom = constrain(pidConfig()->pid_process_denom, 16, 16); -#endif -#if defined(STM32F3) +#elif defined(STM32F3) pidConfig()->pid_process_denom = constrain(pidConfig()->pid_process_denom, 4, 16); -#endif -#else - gyroConfig()->gyro_use_32khz = false; #endif } diff --git a/src/main/fc/serial_cli.c b/src/main/fc/serial_cli.c index 9c9626207b..63a786f9a9 100755 --- a/src/main/fc/serial_cli.c +++ b/src/main/fc/serial_cli.c @@ -617,9 +617,7 @@ const clivalue_t valueTable[] = { #if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL) { "gyro_isr_update", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_isr_update, .config.lookup = { TABLE_OFF_ON } }, #endif -#ifdef GYRO_SUPPORTS_32KHZ { "gyro_use_32khz", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_use_32khz, .config.lookup = { TABLE_OFF_ON } }, -#endif { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 6570b862a8..9d0a488809 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -242,6 +242,22 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse) if (!gyroDetect(&gyro.dev)) { return false; } + + switch (detectedSensors[SENSOR_INDEX_GYRO]) { + default: + // gyro does not support 32kHz + // cast away constness, legitimate as this is cross-validation + ((gyroConfig_t*)gyroConfig)->gyro_use_32khz = false; + break; + case GYRO_MPU6500: + case GYRO_MPU9250: + case GYRO_ICM20689: + case GYRO_ICM20608G: + case GYRO_ICM20602: + // do nothing, as gyro supports 32kHz + break; + } + // Must set gyro sample rate before initialisation gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom, gyroConfig->gyro_use_32khz); gyro.dev.lpf = gyroConfig->gyro_lpf;