From 3ee112843c094c78d5e18182c649d540fde13ee2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 2 Jan 2018 12:50:00 +1100 Subject: [PATCH] AP_InertialSensor: configure bypass for 20789 like we do for AK8963 --- .../AP_InertialSensor_Invensense.cpp | 16 +++------------- 1 file changed, 3 insertions(+), 13 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index 0b30f21994..399a581851 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -499,14 +499,6 @@ void AP_InertialSensor_Invensense::start() _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 - uint8_t v = _register_read(MPUREG_INT_PIN_CFG) | BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN; - if (_mpu_type == Invensense_ICM20789) { - v &= BIT_BYPASS_EN; - } - _register_write(MPUREG_INT_PIN_CFG, v); - // now that we have initialised, we set the bus speed to high _dev->set_speed(AP_HAL::Device::SPEED_HIGH); @@ -962,11 +954,9 @@ bool AP_InertialSensor_Invensense::_hardware_init(void) } /* bus-dependent initialization */ - if ((_dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C) && (_mpu_type == Invensense_MPU9250)) { - /* Enable I2C bypass to access internal AK8963 */ - if (_mpu_type != Invensense_ICM20789) { - _register_write(MPUREG_INT_PIN_CFG, BIT_BYPASS_EN); - } + if ((_dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C) && (_mpu_type == Invensense_MPU9250 || _mpu_type == Invensense_ICM20789)) { + /* Enable I2C bypass to access internal device */ + _register_write(MPUREG_INT_PIN_CFG, BIT_BYPASS_EN); }