Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 06:45:16 +03:00
Code Issues Wiki Activity

Merge pull request #10098 from etracer65/improve_gyro_detect

Browse source 
Speed up gyro device detection; fix MPU6000 initialization
This commit is contained in:
Michael Keller 2020-08-17 22:24:36 +12:00 • committed by GitHub
commit 511ebd4bc2
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 64 additions and 75 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/main/drivers/accgyro/accgyro_mpu.c
View file

@ -210,6 +210,9 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
#ifdef USE_GYRO_SPI_ICM20689 #ifdef USE_GYRO_SPI_ICM20689
icm20689SpiDetect, // icm20689SpiDetect detects ICM20602 and ICM20689 icm20689SpiDetect, // icm20689SpiDetect detects ICM20602 and ICM20689
#endif #endif
#ifdef USE_ACCGYRO_LSM6DSO
lsm6dsoDetect,
#endif
#ifdef USE_ACCGYRO_BMI160 #ifdef USE_ACCGYRO_BMI160
bmi160Detect, bmi160Detect,
#endif #endif
@ -224,9 +227,6 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
#endif #endif
#ifdef USE_GYRO_L3GD20 #ifdef USE_GYRO_L3GD20
l3gd20Detect, l3gd20Detect,
#endif
#ifdef USE_ACCGYRO_LSM6DSO
lsm6dsoDetect,
#endif #endif
NULL // Avoid an empty array NULL // Avoid an empty array
}; };

64
src/main/drivers/accgyro/accgyro_spi_icm20689.c
View file

@ -57,37 +57,35 @@ uint8_t icm20689SpiDetect(const busDevice_t *bus)
spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); //low speed spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); //low speed
// reset the device configuration
spiBusWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); spiBusWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
delay(100);
uint8_t icmDetected = MPU_NONE; // reset the device signal paths
uint8_t attemptsRemaining = 20; spiBusWriteRegister(bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
do { delay(100);
delay(150);
const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I); uint8_t icmDetected;
switch (whoAmI) {
case ICM20601_WHO_AM_I_CONST: const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
icmDetected = ICM_20601_SPI;
break; switch (whoAmI) {
case ICM20602_WHO_AM_I_CONST: case ICM20601_WHO_AM_I_CONST:
icmDetected = ICM_20602_SPI; icmDetected = ICM_20601_SPI;
break; break;
case ICM20608G_WHO_AM_I_CONST: case ICM20602_WHO_AM_I_CONST:
icmDetected = ICM_20608_SPI; icmDetected = ICM_20602_SPI;
break; break;
case ICM20689_WHO_AM_I_CONST: case ICM20608G_WHO_AM_I_CONST:
icmDetected = ICM_20689_SPI; icmDetected = ICM_20608_SPI;
break; break;
default: case ICM20689_WHO_AM_I_CONST:
icmDetected = MPU_NONE; icmDetected = ICM_20689_SPI;
break; break;
} default:
if (icmDetected != MPU_NONE) { icmDetected = MPU_NONE;
break; break;
} }
if (!attemptsRemaining) {
return MPU_NONE;
}
} while (attemptsRemaining--);
spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD); spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD);
@ -121,12 +119,8 @@ void icm20689GyroInit(gyroDev_t *gyro)
spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION);
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); // Device was already reset during detection so proceed with configuration
delay(100);
spiBusWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
delay(100);
// spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
// delay(100);
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15); delay(15);
spiBusWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); spiBusWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);

69
src/main/drivers/accgyro/accgyro_spi_mpu6000.c
View file

@ -129,55 +129,50 @@ uint8_t mpu6000SpiDetect(const busDevice_t *bus)
spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION);
// reset the device configuration
spiBusWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); spiBusWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
delay(100); // datasheet specifies a 100ms delay after reset
uint8_t attemptsRemaining = 5; // reset the device signal paths
do { spiBusWriteRegister(bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
delay(150); delay(100); // datasheet specifies a 100ms delay after signal path reset
const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
if (whoAmI == MPU6000_WHO_AM_I_CONST) { const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
break; uint8_t detectedSensor = MPU_NONE;
if (whoAmI == MPU6000_WHO_AM_I_CONST) {
const uint8_t productID = spiBusReadRegister(bus, MPU_RA_PRODUCT_ID);
/* look for a product ID we recognise */
// verify product revision
switch (productID) {
case MPU6000ES_REV_C4:
case MPU6000ES_REV_C5:
case MPU6000_REV_C4:
case MPU6000_REV_C5:
case MPU6000ES_REV_D6:
case MPU6000ES_REV_D7:
case MPU6000ES_REV_D8:
case MPU6000_REV_D6:
case MPU6000_REV_D7:
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
detectedSensor = MPU_60x0_SPI;
} }
if (!attemptsRemaining) {
return MPU_NONE;
}
} while (attemptsRemaining--);
const uint8_t productID = spiBusReadRegister(bus, MPU_RA_PRODUCT_ID);
/* look for a product ID we recognise */
// verify product revision
switch (productID) {
case MPU6000ES_REV_C4:
case MPU6000ES_REV_C5:
case MPU6000_REV_C4:
case MPU6000_REV_C5:
case MPU6000ES_REV_D6:
case MPU6000ES_REV_D7:
case MPU6000ES_REV_D8:
case MPU6000_REV_D6:
case MPU6000_REV_D7:
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
return MPU_60x0_SPI;
} }
return MPU_NONE; spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD);
return detectedSensor;
} }
static void mpu6000AccAndGyroInit(gyroDev_t *gyro) static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
{ {
spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION);
// Device Reset // Device was already reset during detection so proceed with configuration
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
delay(150);
spiBusWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
delay(150);
// Clock Source PPL with Z axis gyro reference // Clock Source PPL with Z axis gyro reference
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);