diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index d87a10b571..018482e44b 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -37,6 +37,9 @@ #pragma GCC diagnostic warning "-Wpadded" #endif +#define GYRO_SCALE_2000DPS (2000.0f / (1 << 15)) // 16.384 dps/lsb scalefactor for 2000dps sensors +#define GYRO_SCALE_4000DPS (4000.0f / (1 << 15)) // 8.192 dps/lsb scalefactor for 4000dps sensors + typedef enum { GYRO_NONE = 0, GYRO_DEFAULT, diff --git a/src/main/drivers/accgyro/accgyro_fake.c b/src/main/drivers/accgyro/accgyro_fake.c index 279f179edd..a6dd371178 100644 --- a/src/main/drivers/accgyro/accgyro_fake.c +++ b/src/main/drivers/accgyro/accgyro_fake.c @@ -94,7 +94,7 @@ bool fakeGyroDetect(gyroDev_t *gyro) gyro->readFn = fakeGyroRead; gyro->temperatureFn = fakeGyroReadTemperature; #if defined(SIMULATOR_BUILD) - gyro->scale = 1.0f / 16.4f; + gyro->scale = GYRO_SCALE_2000DPS; #else gyro->scale = 1.0f; #endif diff --git a/src/main/drivers/accgyro/accgyro_mpu3050.c b/src/main/drivers/accgyro/accgyro_mpu3050.c index f3d8ab3cee..a52b1a7984 100644 --- a/src/main/drivers/accgyro/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro/accgyro_mpu3050.c @@ -107,8 +107,7 @@ bool mpu3050Detect(gyroDev_t *gyro) gyro->readFn = mpu3050GyroRead; gyro->temperatureFn = mpu3050ReadTemperature; - // 16.4 dps/lsb scalefactor - gyro->scale = 1.0f / 16.4f; + gyro->scale = GYRO_SCALE_2000DPS; return true; } diff --git a/src/main/drivers/accgyro/accgyro_mpu6050.c b/src/main/drivers/accgyro/accgyro_mpu6050.c index c5e8d71e94..29f51296ed 100644 --- a/src/main/drivers/accgyro/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro/accgyro_mpu6050.c @@ -107,8 +107,7 @@ bool mpu6050GyroDetect(gyroDev_t *gyro) gyro->initFn = mpu6050GyroInit; gyro->readFn = mpuGyroRead; - // 16.4 dps/lsb scalefactor - gyro->scale = 1.0f / 16.4f; + gyro->scale = GYRO_SCALE_2000DPS; return true; } diff --git a/src/main/drivers/accgyro/accgyro_mpu6500.c b/src/main/drivers/accgyro/accgyro_mpu6500.c index 12db679680..1b2dcda17e 100644 --- a/src/main/drivers/accgyro/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_mpu6500.c @@ -104,8 +104,7 @@ bool mpu6500GyroDetect(gyroDev_t *gyro) gyro->initFn = mpu6500GyroInit; gyro->readFn = mpuGyroRead; - // 16.4 dps/lsb scalefactor - gyro->scale = 1.0f / 16.4f; + gyro->scale = GYRO_SCALE_2000DPS; return true; } diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.c b/src/main/drivers/accgyro/accgyro_spi_bmi160.c index ad28ea87fa..4e46aea316 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi160.c @@ -350,7 +350,7 @@ bool bmi160SpiGyroDetect(gyroDev_t *gyro) gyro->initFn = bmi160SpiGyroInit; gyro->readFn = bmi160GyroRead; - gyro->scale = 1.0f / 16.4f; + gyro->scale = GYRO_SCALE_2000DPS; return true; } diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index 687c0f720b..1d94ef91f5 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -440,7 +440,7 @@ bool bmi270SpiGyroDetect(gyroDev_t *gyro) gyro->initFn = bmi270SpiGyroInit; gyro->readFn = bmi270GyroRead; - gyro->scale = 1.0f / 16.4f; + gyro->scale = GYRO_SCALE_2000DPS; return true; } diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20649.c b/src/main/drivers/accgyro/accgyro_spi_icm20649.c index ee1935d41e..f793142938 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20649.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20649.c @@ -166,9 +166,9 @@ bool icm20649SpiGyroDetect(gyroDev_t *gyro) gyro->initFn = icm20649GyroInit; gyro->readFn = icm20649GyroReadSPI; - // 16.4 dps/lsb 2kDps - // 8.2 dps/lsb 4kDps - gyro->scale = 1.0f / (gyro->gyro_high_fsr ? 8.2f : 16.4f); + // 16.384 dps/lsb scalefactor for 2000dps sensors + // 8.192 dps/lsb scalefactor for 4000dps sensors + gyro->scale = (gyro->gyro_high_fsr ? GYRO_SCALE_4000DPS : GYRO_SCALE_2000DPS); return true; } diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index 0dd542cbb6..4e709c009c 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -166,8 +166,7 @@ bool icm20689SpiGyroDetect(gyroDev_t *gyro) gyro->initFn = icm20689GyroInit; gyro->readFn = mpuGyroReadSPI; - // 16.4 dps/lsb scalefactor - gyro->scale = 1.0f / 16.4f; + gyro->scale = GYRO_SCALE_2000DPS; return true; } diff --git a/src/main/drivers/accgyro/accgyro_spi_icm42605.c b/src/main/drivers/accgyro/accgyro_spi_icm42605.c index cea9d0ac27..62535d5008 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm42605.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm42605.c @@ -278,8 +278,7 @@ bool icm42605SpiGyroDetect(gyroDev_t *gyro) gyro->initFn = icm42605GyroInit; gyro->readFn = icm42605GyroReadSPI; - // 16.4 dps/lsb scalefactor - gyro->scale = 1.0f / 16.4f; + gyro->scale = GYRO_SCALE_2000DPS; return true; } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index 308be55b53..b9df455ea8 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c @@ -235,8 +235,7 @@ bool mpu6000SpiGyroDetect(gyroDev_t *gyro) gyro->initFn = mpu6000SpiGyroInit; gyro->readFn = mpuGyroReadSPI; - // 16.4 dps/lsb scalefactor - gyro->scale = 1.0f / 16.4f; + gyro->scale = GYRO_SCALE_2000DPS; return true; } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c index 1664255d53..8980830d6e 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c @@ -126,11 +126,10 @@ bool mpu6500SpiGyroDetect(gyroDev_t *gyro) case MPU_9250_SPI: case ICM_20608_SPI: case ICM_20602_SPI: - // 16.4 dps/lsb scalefactor - gyro->scale = 1.0f / 16.4f; + gyro->scale = GYRO_SCALE_2000DPS; break; case ICM_20601_SPI: - gyro->scale = 1.0f / (gyro->gyro_high_fsr ? 8.2f : 16.4f); + gyro->scale = (gyro->gyro_high_fsr ? GYRO_SCALE_4000DPS : GYRO_SCALE_2000DPS); break; default: return false; diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c index e1c0b23c8c..91ef27ad4c 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c @@ -186,8 +186,7 @@ bool mpu9250SpiGyroDetect(gyroDev_t *gyro) gyro->initFn = mpu9250SpiGyroInit; gyro->readFn = mpuGyroReadSPI; - // 16.4 dps/lsb scalefactor - gyro->scale = 1.0f / 16.4f; + gyro->scale = GYRO_SCALE_2000DPS; return true; }