diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 87fa24d615..4cbe266320 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -283,33 +283,6 @@ AP_HAL::Semaphore* AP_MPU9250_BusDriver_SPI::get_semaphore() return _spi->get_semaphore(); } -void AP_MPU9250_BusDriver_SPI::get_driver_state(bool &initialized, bool &working) -{ - AP_HAL::SPIDeviceDriver::State state = _spi->get_state(); - - if (state == AP_HAL::SPIDeviceDriver::State::FAILED) { - initialized = true; - working = false; - return; - } - if (state == AP_HAL::SPIDeviceDriver::State::RUNNING) { - initialized = true; - working = true; - return; - } - - initialized = false; -} - -void AP_MPU9250_BusDriver_SPI::set_driver_state(bool working) -{ - if (working) { - _spi->set_state(AP_HAL::SPIDeviceDriver::State::RUNNING); - } else { - _spi->set_state(AP_HAL::SPIDeviceDriver::State::FAILED); - } -} - bool AP_MPU9250_BusDriver_SPI::has_auxiliary_bus() { return true; @@ -372,96 +345,6 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::_detect(AP_InertialSensor return sensor; } -bool AP_InertialSensor_MPU9250::initialize_driver_state(AP_HAL::SPIDeviceDriver *spi) -{ - if (!spi) - return false; - - AP_MPU9250_BusDriver_SPI *bus = new AP_MPU9250_BusDriver_SPI(spi); - if (!bus) - return false; - - bool initialized = initialize_driver_state(bus); - delete bus; - return initialized; -} - -bool AP_InertialSensor_MPU9250::initialize_driver_state(AP_MPU9250_BusDriver *bus) -{ - bool initialized, working; - - bus->get_driver_state(initialized, working); - if (initialized) { - return working; - } - - bus->set_driver_state(false); - - // initially run the bus at low speed - bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); - - uint8_t whoami = _register_read(bus, MPUREG_WHOAMI); - if (whoami != MPUREG_WHOAMI_MPU9250 && whoami != MPUREG_WHOAMI_MPU9255) { - hal.console->printf("MPU9250: unexpected WHOAMI 0x%x\n", (unsigned)whoami); - goto fail_whoami; - } - - // Chip reset - uint8_t tries; - for (tries = 0; tries < 5; tries++) { - uint8_t user_ctrl = _register_read(bus, MPUREG_USER_CTRL); - - /* First disable the master I2C to avoid hanging the slaves on the - * auxiliary I2C bus */ - if (user_ctrl & BIT_USER_CTRL_I2C_MST_EN) { - _register_write(bus, MPUREG_USER_CTRL, user_ctrl & ~BIT_USER_CTRL_I2C_MST_EN); - hal.scheduler->delay(10); - } - - // reset device - _register_write(bus, 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 - // MPU9250 starts up in sleep mode, and it can take some time - // for it to come out of sleep - _register_write(bus, MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO); - hal.scheduler->delay(5); - - // check it has woken up - if (_register_read(bus, MPUREG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_ZGYRO) { - break; - } - - hal.scheduler->delay(10); - uint8_t status = _register_read(bus, MPUREG_INT_STATUS); - if ((status & BIT_RAW_RDY_INT) != 0) { - break; - } -#if MPU9250_DEBUG - _dump_registers(bus); -#endif - } - - if (tries == 5) { - hal.console->println("Failed to boot MPU9250 5 times"); - goto fail_tries; - } - - bus->set_driver_state(true); - - return true; - -fail_tries: -fail_whoami: - bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); - - return false; -} - /* initialise the sensor */ @@ -484,7 +367,7 @@ bool AP_InertialSensor_MPU9250::_init_sensor() _set_gyro_raw_sample_rate(_gyro_instance, DEFAULT_SAMPLE_RATE); #if MPU9250_DEBUG - _dump_registers(_bus); + _dump_registers(); #endif return true; } @@ -582,31 +465,19 @@ void AP_InertialSensor_MPU9250::_read_data_transaction() /* read an 8 bit register */ -uint8_t AP_InertialSensor_MPU9250::_register_read(AP_MPU9250_BusDriver *bus, - uint8_t reg) +uint8_t AP_InertialSensor_MPU9250::_register_read(uint8_t reg) { uint8_t val; - bus->read8(reg, &val); + _bus->read8(reg, &val); return val; } /* write an 8 bit register */ -void AP_InertialSensor_MPU9250::_register_write(AP_MPU9250_BusDriver *bus, - uint8_t reg, uint8_t val) +void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val) { - bus->write8(reg, val); -} - -inline uint8_t AP_InertialSensor_MPU9250::_register_read(uint8_t reg) -{ - return _register_read(_bus, reg); -} - -inline void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val) -{ - _register_write(_bus, reg, val); + _bus->write8(reg, val); } /* @@ -640,8 +511,59 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void) return false; } - if (!initialize_driver_state(_bus)) - return false; + // initially run the bus at low speed + _bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + + uint8_t whoami = _register_read(MPUREG_WHOAMI); + if (whoami != MPUREG_WHOAMI_MPU9250 && whoami != MPUREG_WHOAMI_MPU9255) { + hal.console->printf("MPU9250: unexpected WHOAMI 0x%x\n", (unsigned)whoami); + goto fail_whoami; + } + + // Chip reset + uint8_t tries; + for (tries = 0; tries < 5; tries++) { + uint8_t user_ctrl = _register_read(MPUREG_USER_CTRL); + + /* First disable the master I2C to avoid hanging the slaves on the + * auxiliary I2C bus */ + if (user_ctrl & BIT_USER_CTRL_I2C_MST_EN) { + _register_write(MPUREG_USER_CTRL, user_ctrl & ~BIT_USER_CTRL_I2C_MST_EN); + hal.scheduler->delay(10); + } + + // 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 + // MPU9250 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; + } + + hal.scheduler->delay(10); + uint8_t status = _register_read(MPUREG_INT_STATUS); + if ((status & BIT_RAW_RDY_INT) != 0) { + break; + } +#if MPU9250_DEBUG + _dump_registers(); +#endif + } + + if (tries == 5) { + hal.console->println("Failed to boot MPU9250 5 times"); + goto fail_tries; + } _register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode @@ -672,6 +594,13 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void) hal.scheduler->resume_timer_procs(); return true; + +fail_tries: +fail_whoami: + _bus_sem->give(); + hal.scheduler->resume_timer_procs(); + _bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + return false; } AuxiliaryBus *AP_InertialSensor_MPU9250::get_auxiliary_bus() diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h index 34e9e68293..be4115d282 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h @@ -29,8 +29,6 @@ public: virtual bool read_data_transaction(uint8_t* samples, uint8_t &n_samples) = 0; virtual AP_HAL::Semaphore* get_semaphore() = 0; - virtual void get_driver_state(bool &initialized, bool &working) = 0; - virtual void set_driver_state(bool working) = 0; virtual bool has_auxiliary_bus() = 0; }; @@ -55,17 +53,6 @@ public: return static_cast(backend); } - /* Put the MPU9250 in a known state so it can be - * used both for the InertialSensor and as for backend of other drivers. - * - * The bus semaphore must be taken and timer_procs suspended. - * - * This method puts the bus in low speed. If the initialization is - * successful the bus is left on low speed so the caller can finish the - * initialization of its driver. - */ - static bool initialize_driver_state(AP_HAL::SPIDeviceDriver *spi); - // detect the sensor static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, AP_HAL::SPIDeviceDriver *spi); @@ -74,13 +61,6 @@ private: AP_MPU9250_BusDriver *bus, int16_t id); - - static bool initialize_driver_state(AP_MPU9250_BusDriver *bus); - - static uint8_t _register_read(AP_MPU9250_BusDriver *bus, uint8_t reg); - static void _register_write(AP_MPU9250_BusDriver *bus, uint8_t reg, - uint8_t val); - bool _init_sensor(); void _read_data_transaction(); bool _data_ready(); @@ -128,7 +108,7 @@ private: enum Rotation _default_rotation; #if MPU9250_DEBUG - static void _dump_registers(AP_MPU9250_BusDriver *bus); + static void _dump_registers(); #endif }; @@ -143,8 +123,6 @@ public: void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed); bool read_data_transaction(uint8_t* samples, uint8_t &n_samples); AP_HAL::Semaphore* get_semaphore(); - void get_driver_state(bool &initialized, bool &working); - void set_driver_state(bool working); bool has_auxiliary_bus(); private: