From 8f424cdf21d57bf32cfe321bf347f06687d96845 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Jan 2013 20:42:20 +1100 Subject: [PATCH] AP_Compass: ensure we check we got the semaphore --- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 31bd24b3aa..25cab75724 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -112,7 +112,10 @@ void AP_Compass_HMC5843::accumulate(void) return; } - _i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER); + if (!_i2c_sem->take(5)) { + // the bus is busy - try again later + return; + } bool result = read_raw(); _i2c_sem->give(); @@ -164,7 +167,9 @@ AP_Compass_HMC5843::init() hal.scheduler->delay(10); _i2c_sem = hal.i2c->get_semaphore(); - _i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER); + if (!_i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + hal.scheduler->panic(PSTR("Failed to get HMC5843 semaphore")); + } // determine if we are using 5843 or 5883L if (!write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||