Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +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
icm20689SpiDetect, // icm20689SpiDetect detects ICM20602 and ICM20689
#endif
#ifdef USE_ACCGYRO_LSM6DSO
lsm6dsoDetect,
#endif
#ifdef USE_ACCGYRO_BMI160
bmi160Detect,
#endif
@ -224,9 +227,6 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
#endif
#ifdef USE_GYRO_L3GD20
l3gd20Detect,
#endif
#ifdef USE_ACCGYRO_LSM6DSO
lsm6dsoDetect,
#endif
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
// reset the device configuration
spiBusWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
delay(100);
uint8_t icmDetected = MPU_NONE;
uint8_t attemptsRemaining = 20;
do {
delay(150);
const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
switch (whoAmI) {
case ICM20601_WHO_AM_I_CONST:
icmDetected = ICM_20601_SPI;
break;
case ICM20602_WHO_AM_I_CONST:
icmDetected = ICM_20602_SPI;
break;
case ICM20608G_WHO_AM_I_CONST:
icmDetected = ICM_20608_SPI;
break;
case ICM20689_WHO_AM_I_CONST:
icmDetected = ICM_20689_SPI;
break;
default:
icmDetected = MPU_NONE;
break;
}
if (icmDetected != MPU_NONE) {
break;
}
if (!attemptsRemaining) {
return MPU_NONE;
}
} while (attemptsRemaining--);
// reset the device signal paths
spiBusWriteRegister(bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
delay(100);
uint8_t icmDetected;
const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
switch (whoAmI) {
case ICM20601_WHO_AM_I_CONST:
icmDetected = ICM_20601_SPI;
break;
case ICM20602_WHO_AM_I_CONST:
icmDetected = ICM_20602_SPI;
break;
case ICM20608G_WHO_AM_I_CONST:
icmDetected = ICM_20608_SPI;
break;
case ICM20689_WHO_AM_I_CONST:
icmDetected = ICM_20689_SPI;
break;
default:
icmDetected = MPU_NONE;
break;
}
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);
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
delay(100);
spiBusWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
delay(100);
// spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
// delay(100);
// Device was already reset during detection so proceed with configuration
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
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);
// reset the device configuration
spiBusWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
delay(100); // datasheet specifies a 100ms delay after reset
uint8_t attemptsRemaining = 5;
do {
delay(150);
// reset the device signal paths
spiBusWriteRegister(bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
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) {
break;
const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
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)
{
spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION);
// Device Reset
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);
// Device was already reset during detection so proceed with configuration
// Clock Source PPL with Z axis gyro reference
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);