mirror of https://github.com/ArduPilot/ardupilot
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:
parent
edad43611d
commit
212728be34
|
@ -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()
|
||||
{
|
||||
|
|
|
@ -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__
|
||||
|
|
Loading…
Reference in New Issue