Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass_AK8963: fix sem handling

Browse Source 
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..."
This commit is contained in:
Julien BERAUD 2015-07-31 16:57:12 +02:00 committed by Randy Mackay
parent 5b133934db
commit de7cf019ef
1 changed files with 13 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -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()