From 3cb6f391d455fd6b2a291413227bc291159fabe6 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Wed, 5 Aug 2015 13:29:35 -0300 Subject: [PATCH] AP_InertialSensor: MPU6000: split detection and initialization --- .../AP_InertialSensor_MPU6000.cpp | 262 ++++++++++-------- .../AP_InertialSensor_MPU6000.h | 14 +- 2 files changed, 153 insertions(+), 123 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 4817631240..3ac1707bfb 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -180,12 +180,17 @@ AP_MPU6000_BusDriver_SPI::AP_MPU6000_BusDriver_SPI(void) : _spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000); } -void AP_MPU6000_BusDriver_SPI::init(bool &fifo_mode, uint8_t &max_samples) +void AP_MPU6000_BusDriver_SPI::init() +{ + /* Disable I2C bus if SPI selected (Recommended in Datasheet to be done + * just after the device is reset) */ + write8(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); +} + +void AP_MPU6000_BusDriver_SPI::start(bool &fifo_mode, uint8_t &max_samples) { fifo_mode = false; _error_count = 0; - // Disable I2C bus if SPI selected (Recommended in Datasheet - write8(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); /* maximum number of samples read by a burst * a sample is an array containing : * gyro_x @@ -291,7 +296,11 @@ AP_MPU6000_BusDriver_I2C::AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8 _i2c_sem(NULL) {} -void AP_MPU6000_BusDriver_I2C::init(bool &fifo_mode, uint8_t &max_samples) +void AP_MPU6000_BusDriver_I2C::init() +{ +} + +void AP_MPU6000_BusDriver_I2C::start(bool &fifo_mode, uint8_t &max_samples) { // enable fifo mode fifo_mode = true; @@ -431,7 +440,7 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect_spi(AP_InertialSens AP_MPU6000_BusDriver *bus = new AP_MPU6000_BusDriver_SPI(); if (!bus) return nullptr; - return _detect(imu, bus); + return _detect(imu, bus, HAL_INS_MPU60XX_SPI); } /* Detect the sensor on the specified I2C bus and address */ @@ -448,7 +457,8 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect_i2c(AP_InertialSens /* Common detection method - it takes ownership of the bus, freeing it if it's * not possible to return an AP_InertialSensor_Backend */ AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::_detect(AP_InertialSensor &_imu, - AP_MPU6000_BusDriver *bus) + AP_MPU6000_BusDriver *bus, + int16_t id) { AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu, bus); if (sensor == NULL) { @@ -460,6 +470,8 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::_detect(AP_InertialSensor return nullptr; } + sensor->_id = id; + return sensor; } @@ -473,42 +485,126 @@ bool AP_InertialSensor_MPU6000::_init_sensor(void) #endif hal.scheduler->suspend_timer_procs(); + bool success = _hardware_init(); + hal.scheduler->resume_timer_procs(); - uint8_t tries = 0; - do { - bool success = _hardware_init(); - if (success) { - hal.scheduler->delay(5+2); - if (!_bus_sem->take(100)) { - return false; - } - if (_data_ready()) { - _bus_sem->give(); - break; - } - _bus_sem->give(); - } - if (tries++ > 5) { - hal.console->print_P(PSTR("failed to boot MPU6000 5 times")); - return false; - } - } while (1); +#if MPU6000_DEBUG + _dump_registers(); +#endif + + return success; +} + +void AP_InertialSensor_MPU6000::start() +{ + uint8_t max_samples; + + hal.scheduler->suspend_timer_procs(); + + if (!_bus_sem->take(100)) { + hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore")); + } + + // initially run the bus at low speed + _bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + + // only used for wake-up in accelerometer only low power mode + _register_write(MPUREG_PWR_MGMT_2, 0x00); + hal.scheduler->delay(1); + + _bus->start(_fifo_mode, max_samples); + + /* each sample is on 16 bits */ + _samples = new uint8_t[max_samples * MPU6000_SAMPLE_SIZE]; + hal.scheduler->delay(1); + +#if MPU6000_FAST_SAMPLING + _sample_count = 1; +#else + // sample rate and filtering + // to minimise the effects of aliasing we choose a filter + // that is less than half of the sample rate + switch (_imu.get_sample_rate()) { + case AP_InertialSensor::RATE_50HZ: + // this is used for plane and rover, where noise resistance is + // more important than update rate. Tests on an aerobatic plane + // show that 10Hz is fine, and makes it very noise resistant + _sample_count = 4; + break; + case AP_InertialSensor::RATE_100HZ: + _sample_count = 2; + break; + case AP_InertialSensor::RATE_200HZ: + _sample_count = 1; + break; + default: + return; + } +#endif + +#if MPU6000_FAST_SAMPLING + // disable sensor filtering + _set_filter_register(256); + + // set sample rate to 1000Hz and apply a software filter + // In this configuration, the gyro sample rate is 8kHz + // Therefore the sample rate value is 8kHz/(SMPLRT_DIV + 1) + // So we have to set it to 7 to have a 1kHz sampling + // rate on the gyro + _register_write(MPUREG_SMPLRT_DIV, 7); +#else + _set_filter_register(_accel_filter_cutoff()); + + // set sample rate to 200Hz, and use _sample_divider to give + // the requested rate to the application + _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ); +#endif + hal.scheduler->delay(1); + + _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s + hal.scheduler->delay(1); + + // read the product ID rev c has 1/2 the sensitivity of rev d + _product_id = _register_read(MPUREG_PRODUCT_ID); + //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id); + + if ((_product_id == MPU6000ES_REV_C4) || + (_product_id == MPU6000ES_REV_C5) || + (_product_id == MPU6000_REV_C4) || + (_product_id == MPU6000_REV_C5)) { + // Accel scale 8g (4096 LSB/g) + // Rev C has different scaling than rev D + _register_write(MPUREG_ACCEL_CONFIG,1<<3); + } else { + // Accel scale 8g (4096 LSB/g) + _register_write(MPUREG_ACCEL_CONFIG,2<<3); + } + hal.scheduler->delay(1); + + // configure interrupt to fire when new data arrives + _register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); + hal.scheduler->delay(1); + + // clear interrupt on any read, and hold the data ready pin high + // until we clear the interrupt + _register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN); + + // now that we have initialised, we set the SPI bus speed to high + _bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + + _bus_sem->give(); // grab the used instances _gyro_instance = _imu.register_gyro(); _accel_instance = _imu.register_accel(); hal.scheduler->resume_timer_procs(); - - // start the timer process to read samples - hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, void)); - -#if MPU6000_DEBUG - _dump_registers(); -#endif - return true; + // start the timer process to read samples + hal.scheduler->register_timer_process( + FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, void)); } + /* process any */ @@ -713,8 +809,6 @@ void AP_InertialSensor_MPU6000::_set_filter_register(uint16_t filter_hz) bool AP_InertialSensor_MPU6000::_hardware_init(void) { - uint8_t max_samples; - if (!_bus_sem->take(100)) { hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore")); } @@ -725,9 +819,13 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void) // Chip reset uint8_t tries; for (tries = 0; tries<5; tries++) { + /* reset device */ _register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET); hal.scheduler->delay(100); + /* bus-dependent initialization*/ + _bus->init(); + // 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 @@ -738,102 +836,28 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void) if (_register_read(MPUREG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_ZGYRO) break; + hal.scheduler->delay(10); + if (_data_ready()) + break; + #if MPU6000_DEBUG _dump_registers(); #endif } if (tries == 5) { hal.console->println_P(PSTR("Failed to boot MPU6000 5 times")); - _bus_sem->give(); - return false; + goto fail_tries; } - _register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode - hal.scheduler->delay(1); - - _bus->init(_fifo_mode, max_samples); - /* each sample is on 16 bits */ - _samples = new uint8_t[max_samples * MPU6000_SAMPLE_SIZE]; - hal.scheduler->delay(1); - -#if MPU6000_FAST_SAMPLING - _sample_count = 1; -#else - // sample rate and filtering - // to minimise the effects of aliasing we choose a filter - // that is less than half of the sample rate - switch (_imu.get_sample_rate()) { - case AP_InertialSensor::RATE_50HZ: - // this is used for plane and rover, where noise resistance is - // more important than update rate. Tests on an aerobatic plane - // show that 10Hz is fine, and makes it very noise resistant - _sample_count = 4; - break; - case AP_InertialSensor::RATE_100HZ: - _sample_count = 2; - break; - case AP_InertialSensor::RATE_200HZ: - _sample_count = 1; - break; - default: - return false; - } -#endif - -#if MPU6000_FAST_SAMPLING - // disable sensor filtering - _set_filter_register(256); - - // set sample rate to 1000Hz and apply a software filter - // In this configuration, the gyro sample rate is 8kHz - // Therefore the sample rate value is 8kHz/(SMPLRT_DIV + 1) - // So we have to set it to 7 to have a 1kHz sampling - // rate on the gyro - _register_write(MPUREG_SMPLRT_DIV, 7); -#else - _set_filter_register(_accel_filter_cutoff()); - - // set sample rate to 200Hz, and use _sample_divider to give - // the requested rate to the application - _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ); -#endif - hal.scheduler->delay(1); - - _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s - hal.scheduler->delay(1); - - // read the product ID rev c has 1/2 the sensitivity of rev d - _product_id = _register_read(MPUREG_PRODUCT_ID); - //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id); - - if ((_product_id == MPU6000ES_REV_C4) || - (_product_id == MPU6000ES_REV_C5) || - (_product_id == MPU6000_REV_C4) || - (_product_id == MPU6000_REV_C5)) { - // Accel scale 8g (4096 LSB/g) - // Rev C has different scaling than rev D - _register_write(MPUREG_ACCEL_CONFIG,1<<3); - } else { - // Accel scale 8g (4096 LSB/g) - _register_write(MPUREG_ACCEL_CONFIG,2<<3); - } - hal.scheduler->delay(1); - - // configure interrupt to fire when new data arrives - _register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); - hal.scheduler->delay(1); - - // clear interrupt on any read, and hold the data ready pin high - // until we clear the interrupt - _register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN); - - // now that we have initialised, we set the SPI bus speed to high - // (8MHz on APM2) _bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); - _bus_sem->give(); return true; + +fail_tries: + _bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + _bus_sem->give(); + return false; } #if MPU6000_DEBUG diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index e10b0c6846..4253036fad 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -32,7 +32,8 @@ class AP_MPU6000_BusDriver { public: virtual ~AP_MPU6000_BusDriver() { }; - virtual void init(bool &fifo_mode, uint8_t &max_samples) = 0; + virtual void init() = 0; + virtual void start(bool &fifo_mode, uint8_t &max_samples) = 0; virtual void read8(uint8_t reg, uint8_t *val) = 0; /// Copy data from the device to @p buf starting at @p reg with @size @@ -62,9 +63,12 @@ public: bool gyro_sample_available(void) { return _sum_count >= _sample_count; } bool accel_sample_available(void) { return _sum_count >= _sample_count; } + void start() override; + private: static AP_InertialSensor_Backend *_detect(AP_InertialSensor &_imu, - AP_MPU6000_BusDriver *bus); + AP_MPU6000_BusDriver *bus, + int16_t id = -1); #if MPU6000_DEBUG void _dump_registers(void); @@ -123,7 +127,8 @@ class AP_MPU6000_BusDriver_SPI : public AP_MPU6000_BusDriver { public: AP_MPU6000_BusDriver_SPI(void); - void init(bool &fifo_mode, uint8_t &max_samples); + void init(); + void start(bool &fifo_mode, uint8_t &max_samples); void read8(uint8_t reg, uint8_t *val); void read_block(uint8_t reg, uint8_t *buf, uint32_t size) override; void write8(uint8_t reg, uint8_t val); @@ -144,7 +149,8 @@ class AP_MPU6000_BusDriver_I2C : public AP_MPU6000_BusDriver { public: AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr); - void init(bool &fifo_mode, uint8_t &max_samples); + void init(); + void start(bool &fifo_mode, uint8_t &max_samples); void read8(uint8_t reg, uint8_t *val); void read_block(uint8_t reg, uint8_t *buf, uint32_t size) override; void write8(uint8_t reg, uint8_t val);