From 212728be340ff7718b5f12ea076d8a6ab4c217d0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 27 Dec 2012 21:28:41 +1100
Subject: [PATCH] InertialSensor: ensure MPU6000 is out of sleep mode before
 configuring

the MPU6000 starts in sleep mode, and can take a while to wakeup
---
 .../AP_InertialSensor_MPU6000.cpp             | 46 ++++++++++++++++---
 .../AP_InertialSensor_MPU6000.h               |  7 +++
 2 files changed, 47 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index 8e28cd5fde..337bfa1b2c 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -391,19 +391,32 @@ void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
 
 reset_chip:
     // Chip reset
-    register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
-    hal.scheduler->delay(100);
+    uint8_t tries;
+    for (tries = 0; tries<5; tries++) {
+        register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
+        hal.scheduler->delay(100);
 
-    // Wake up device and select GyroZ clock (better performance)
-    register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO);
-    hal.scheduler->delay(1);
+        // Wake up device and select GyroZ clock. Note that the
+        // MPU6000 starts up in sleep mode, and it can take some time
+        // for it to come out of sleep
+        register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO);
+        hal.scheduler->delay(5);
+
+        // check it has woken up
+        if (register_read(MPUREG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_ZGYRO) {
+            break;
+        }
+    }
+    if (tries == 5) {
+        hal.console->println_P(PSTR("Failed to boot MPU6000 - retrying"));
+        goto reset_chip;
+    }
 
     register_write(MPUREG_PWR_MGMT_2, 0x00);            // only used for wake-up in accelerometer only low power mode
     hal.scheduler->delay(1);
 
     // Disable I2C bus (recommended on datasheet)
     register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
-
     hal.scheduler->delay(1);
 
     uint8_t rate, filter, default_filter;
@@ -493,6 +506,10 @@ reset_chip:
     // wait for enough time that we should get a sample
     hal.scheduler->delay(msec_per_sample+2);
 
+#if MPU6000_DEBUG
+    _dump_registers();
+#endif
+
     if (_drdy_pin->read() == 0) {
         // we didn't get a sample - run the whole chip setup
         // again. This sometimes happens after a DTR reset or warm
@@ -533,6 +550,23 @@ uint16_t AP_InertialSensor_MPU6000::num_samples_available()
     return _count;
 }
 
+
+#if MPU6000_DEBUG
+// dump all config registers - used for debug
+void AP_InertialSensor_MPU6000::_dump_registers(void)
+{
+    for (uint8_t reg=25; reg<=108; reg++) {
+        uint8_t v = register_read(reg);
+        hal.console->printf_P(PSTR("%02x:%02x "), (unsigned)reg, (unsigned)v);
+        if ((reg - 24) % 16 == 0) {
+            hal.console->println();
+        }
+    }
+    hal.console->println();
+}
+#endif
+
+
 // get_delta_time returns the time period in seconds overwhich the sensor data was collected
 uint32_t AP_InertialSensor_MPU6000::get_delta_time_micros() 
 {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
index f9bcfd1ae0..850b85889a 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
@@ -12,6 +12,9 @@
 #define MPU6000_CS_PIN       53        // APM pin connected to mpu6000's chip select pin
 #define DMP_FIFO_BUFFER_SIZE 72        // DMP FIFO buffer size
 
+// enable debug to see a register dump on startup
+#define MPU6000_DEBUG 0
+
 // DMP memory
 extern const uint8_t        dmpMem[8][16][16] PROGMEM;
 
@@ -105,6 +108,10 @@ public:
     static uint8_t              _fifoCountL;                    // low byte of number of elements in fifo buffer
 
     static bool                 _dmp_initialised;
+
+#if MPU6000_DEBUG
+    void						_dump_registers(void);
+#endif
 };
 
 #endif // __AP_INERTIAL_SENSOR_MPU6000_H__