From 5572cb057540387bbac2b560c238830b7d0e8c0b Mon Sep 17 00:00:00 2001
From: nathan <nathan@vertile.com>
Date: Mon, 6 Mar 2017 22:56:41 -0800
Subject: [PATCH] [driver/mpu6500] wait until coming out of sleep mode to read
 WHO_AM_I, directly taken from #2538

---
 src/main/drivers/accgyro_spi_mpu6500.c | 46 ++++++++++++++++----------
 1 file changed, 28 insertions(+), 18 deletions(-)

diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c
index 4cd98d824..4f0c4955f 100755
--- a/src/main/drivers/accgyro_spi_mpu6500.c
+++ b/src/main/drivers/accgyro_spi_mpu6500.c
@@ -37,6 +37,8 @@
 #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)
@@ -79,28 +81,36 @@ static void mpu6500SpiInit(void)
 static uint8_t mpuDetected = MPU_NONE;
 uint8_t mpu6500SpiDetect(void)
 {
-    uint8_t tmp;
+    uint8_t tmp, detectRetries;
 
     mpu6500SpiInit();
 
-    mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp);
+    delayMicroseconds(15);
+    do {
+        mpu6500ReadRegister(MPU_RA_PWR_MGMT_1, 1, &tmp);
+        delayMicroseconds(1);
+        detectRetries++;
+    } while (tmp != BIT_SLEEP && detectRetries < 30);
 
-    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;
+    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;
+        }
     }
     return mpuDetected;
 }