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

AP_InertialSensor: ensure we always have the SPI semaphore for MPU6k

Browse Source
This commit is contained in:
Andrew Tridgell 2013-01-09 20:30:20 +11:00
parent 9b972af307
commit 4ab1cddd15
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_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -317,12 +317,16 @@ bool AP_InertialSensor_MPU6000::_data_ready()
{
if (_drdy_pin) {
return _drdy_pin->read() != 0;
} else {
}
bool got = _spi_sem->take_nonblocking();
if (got) {
uint8_t status;
bool success = _register_read_from_timerprocess(MPUREG_INT_STATUS,
&status);
&status);
_spi_sem->give();
return success && (status & BIT_RAW_RDY_INT) != 0;
}
return false;
}
/**
@ -343,7 +347,7 @@ void AP_InertialSensor_MPU6000::_poll_data(uint32_t now)
*/
void AP_InertialSensor_MPU6000::_read_data_from_timerprocess()
{
static int semfail_ctr = 0;
static uint8_t semfail_ctr = 0;
bool got = _spi_sem->take_nonblocking();
if (!got) {
semfail_ctr++;
@ -434,6 +438,10 @@ void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
(It is not a valid pin under Arduino.) */
_drdy_pin = hal.gpio->channel(70);
if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
}
reset_chip:
// Chip reset
uint8_t tries;
@ -562,14 +570,8 @@ reset_chip:
/* read the first lot of data.
* _read_data_transaction requires the spi semaphore to be taken by
* its caller. */
if (!_spi_sem->take(20)) {
hal.scheduler->panic(PSTR("PANIC: could not take _spi_sem to read "
"first MPU6000 sample"));
return; /* never reached */
}
_last_sample_time_micros = hal.scheduler->micros();
_read_data_transaction();
_spi_sem->give();
// start the timer process to read samples
hal.scheduler->register_timer_process(_poll_data);
@ -577,6 +579,8 @@ reset_chip:
#if MPU6000_DEBUG
_dump_registers();
#endif
_spi_sem->give();
}
float AP_InertialSensor_MPU6000::_temp_to_celsius ( uint16_t regval )