From 9776629f259082dfe0c15c8cb8a713ede01b35ec Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 7 Mar 2017 15:43:29 +0100 Subject: [PATCH] Revert "[driver/mpu6500] wait until coming out of sleep mode to read WHO_AM_I, directly taken from #2538" This reverts commit 52c104f4b8daa246a3968d4b3a29e358db7657ff. This reverts commit 5572cb057540387bbac2b560c238830b7d0e8c0b. --- src/main/drivers/accgyro_spi_mpu6500.c | 45 +++++++++++--------------- 1 file changed, 18 insertions(+), 27 deletions(-) diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 8b8a2626a..4cd98d824 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -37,8 +37,6 @@ #define DISABLE_MPU6500 IOHi(mpuSpi6500CsPin) #define ENABLE_MPU6500 IOLo(mpuSpi6500CsPin) -#define BIT_SLEEP 0x40 - static IO_t mpuSpi6500CsPin = IO_NONE; bool mpu6500WriteRegister(uint8_t reg, uint8_t data) @@ -81,35 +79,28 @@ static void mpu6500SpiInit(void) static uint8_t mpuDetected = MPU_NONE; uint8_t mpu6500SpiDetect(void) { - uint8_t tmp = 0, detectRetries = 0; + uint8_t tmp; mpu6500SpiInit(); - delayMicroseconds(15); - do { - mpu6500ReadRegister(MPU_RA_PWR_MGMT_1, 1, &tmp); - detectRetries++; - } while (tmp != BIT_SLEEP && detectRetries < 30); + mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp); - if (tmp == BIT_SLEEP) { - mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp); - delayMicroseconds(15); - switch (tmp) { - case MPU6500_WHO_AM_I_CONST: - mpuDetected = MPU_65xx_SPI; - break; - case MPU9250_WHO_AM_I_CONST: - mpuDetected = MPU_9250_SPI; - break; - case ICM20608G_WHO_AM_I_CONST: - mpuDetected = ICM_20608_SPI; - break; - case ICM20602_WHO_AM_I_CONST: - mpuDetected = ICM_20602_SPI; - break; - default: - mpuDetected = MPU_NONE; - } + switch (tmp) { + case MPU6500_WHO_AM_I_CONST: + mpuDetected = MPU_65xx_SPI; + break; + case MPU9250_WHO_AM_I_CONST: + case MPU9255_WHO_AM_I_CONST: + mpuDetected = MPU_9250_SPI; + break; + case ICM20608G_WHO_AM_I_CONST: + mpuDetected = ICM_20608_SPI; + break; + case ICM20602_WHO_AM_I_CONST: + mpuDetected = ICM_20602_SPI; + break; + default: + mpuDetected = MPU_NONE; } return mpuDetected; }