From d575d676b611aa81b49a7fe64461316a9f27b49f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 6 Feb 2018 17:54:19 +1100 Subject: [PATCH] AP_InertialSensor: fixed another transfer without bus semaphore --- .../AP_InertialSensor/AP_InertialSensor_Invensense.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index cb110c01ce..de2a4998bc 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -1109,6 +1109,10 @@ void AP_Invensense_AuxiliaryBus::_configure_slaves() { auto &backend = AP_InertialSensor_Invensense::from(_ins_backend); + if (!backend._dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + return; + } + /* Enable the I2C master to slaves on the auxiliary I2C bus*/ if (!(backend._last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN)) { backend._last_stat_user_ctrl |= BIT_USER_CTRL_I2C_MST_EN; @@ -1127,6 +1131,8 @@ void AP_Invensense_AuxiliaryBus::_configure_slaves() backend._register_write(MPUREG_I2C_MST_DELAY_CTRL, BIT_I2C_SLV0_DLY_EN | BIT_I2C_SLV1_DLY_EN | BIT_I2C_SLV2_DLY_EN | BIT_I2C_SLV3_DLY_EN); + + backend._dev->get_semaphore()->give(); } int AP_Invensense_AuxiliaryBus::_configure_periodic_read(AuxiliaryBusSlave *slave,