InertialSensor: ensure MPU6000 is out of sleep mode before configuring

the MPU6000 starts in sleep mode, and can take a while to wakeup
This commit is contained in:
Andrew Tridgell 2012-12-27 21:28:41 +11:00
parent edad43611d
commit 212728be34
2 changed files with 47 additions and 6 deletions

View File

@ -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()
{

View File

@ -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__