From 2ffb8d1583befc9200ace065f23f638151a36189 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 6 Feb 2018 17:44:22 +1100 Subject: [PATCH] AP_InertialSensor: fixed a bug where bus semaphore not held during init of the invensense driver we could do a transfer without the bus semaphore held. That violates the locking rules for the bus --- libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index dc5754df89..cb110c01ce 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -966,10 +966,10 @@ bool AP_InertialSensor_Invensense::_hardware_init(void) } _dev->set_speed(AP_HAL::Device::SPEED_HIGH); - _dev->get_semaphore()->give(); if (tries == 5) { hal.console->printf("Failed to boot Invensense 5 times\n"); + _dev->get_semaphore()->give(); return false; } @@ -978,6 +978,7 @@ bool AP_InertialSensor_Invensense::_hardware_init(void) // this avoids a sensor bug, see description above _register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true); } + _dev->get_semaphore()->give(); return true; }