From de7cf019ef99c2f71613c2ccfdbd8b8b3de18cd7 Mon Sep 17 00:00:00 2001 From: Julien BERAUD Date: Fri, 31 Jul 2015 16:57:12 +0200 Subject: [PATCH] AP_Compass_AK8963: fix sem handling In case of error or zeroed data, the i2c semaphore wasn't given. It happened at first startup on Bebop and caused a failure: "PANIC: failed to take _bus->sem 100 times in a row..." --- libraries/AP_Compass/AP_Compass_AK8963.cpp | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 80cce66667..470c044b8a 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -237,30 +237,31 @@ void AP_Compass_AK8963::_make_factory_sensitivity_adjustment(Vector3f& field) co void AP_Compass_AK8963::_update() { + struct AP_AK8963_SerialBus::raw_value rv; + float mag_x, mag_y, mag_z; + if (hal.scheduler->micros() - _last_update_timestamp < 10000) { - return; + goto end; } if (!_sem_take_nonblocking()) { - return; + goto end; } - struct AP_AK8963_SerialBus::raw_value rv; - _bus->read_raw(&rv); /* Check for overflow. See AK8963's datasheet, section * 6.4.3.6 - Magnetic Sensor Overflow. */ if ((rv.st2 & 0x08)) { - return; + goto fail; } - float mag_x = (float) rv.val[0]; - float mag_y = (float) rv.val[1]; - float mag_z = (float) rv.val[2]; + mag_x = (float) rv.val[0]; + mag_y = (float) rv.val[1]; + mag_z = (float) rv.val[2]; if (is_zero(mag_x) && is_zero(mag_y) && is_zero(mag_z)) { - return; + goto fail; } _mag_x_accum += mag_x; @@ -275,7 +276,10 @@ void AP_Compass_AK8963::_update() } _last_update_timestamp = hal.scheduler->micros(); +fail: _sem_give(); +end: + return; } bool AP_Compass_AK8963::_check_id()