aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

AP_InertialSensor: try to lower SPI bus speed on errors

Browse Source
This commit is contained in:
Andrew Tridgell 2013-10-28 18:20:47 +11:00 committed by Randy Mackay
parent 0bda21a3fc
commit f049d19aa3
2 changed files with 50 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -206,12 +206,17 @@ uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate )
bool success = hardware_init(sample_rate);
if (success) {
hal.scheduler->delay(5+2);
if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
}
if (_data_ready()) {
_spi_sem->give();
break;
} else {
hal.console->println_P(
PSTR("MPU6000 startup failed: no data ready"));
}
_spi_sem->give();
}
if (tries++ > 5) {
hal.scheduler->panic(PSTR("PANIC: failed to boot MPU6000 5 times"));
@ -331,29 +336,8 @@ bool AP_InertialSensor_MPU6000::_data_ready()
if (_drdy_pin) {
return _drdy_pin->read() != 0;
}
if (hal.scheduler->in_timerprocess()) {
bool got = _spi_sem->take_nonblocking();
if (got) {
uint8_t status = _register_read(MPUREG_INT_STATUS);
_spi_sem->give();
return (status & BIT_RAW_RDY_INT) != 0;
} else {
return false;
}
} else {
bool got = _spi_sem->take(10);
if (got) {
uint8_t status = _register_read(MPUREG_INT_STATUS);
_spi_sem->give();
return (status & BIT_RAW_RDY_INT) != 0;
} else {
hal.scheduler->panic(
PSTR("PANIC: AP_InertialSensor_MPU6000::_data_ready failed to "
"take SPI semaphore synchronously"));
}
}
return false;
}
/**
@ -361,33 +345,7 @@ bool AP_InertialSensor_MPU6000::_data_ready()
*/
void AP_InertialSensor_MPU6000::_poll_data(void)
{
if (_data_ready()) {
if (hal.scheduler->in_timerprocess()) {
_read_data_from_timerprocess();
} else {
/* Synchronous read - take semaphore */
bool got = _spi_sem->take(10);
if (got) {
_last_sample_time_micros = hal.scheduler->micros();
_read_data_transaction();
_spi_sem->give();
} else {
hal.scheduler->panic(
PSTR("PANIC: AP_InertialSensor_MPU6000::_poll_data "
"failed to take SPI semaphore synchronously"));
}
}
}
}
/*
* this is called from the _poll_data, in the timer process context.
* when the MPU6000 has new sensor data available and add it to _sum[] to
* ensure this is the case, these other devices must perform their spi reads
* after being called by the AP_TimerProcess.
*/
void AP_InertialSensor_MPU6000::_read_data_from_timerprocess()
{
if (!_spi_sem->take_nonblocking()) {
/*
the semaphore being busy is an expected condition when the
@ -397,23 +355,47 @@ void AP_InertialSensor_MPU6000::_read_data_from_timerprocess()
*/
return;
}
if (_data_ready()) {
_last_sample_time_micros = hal.scheduler->micros();
_read_data_transaction();
}
_spi_sem->give();
} else {
/* Synchronous read - take semaphore */
if (_spi_sem->take(10)) {
if (_data_ready()) {
_last_sample_time_micros = hal.scheduler->micros();
_read_data_transaction();
}
_spi_sem->give();
} else {
hal.scheduler->panic(
PSTR("PANIC: AP_InertialSensor_MPU6000::_poll_data "
"failed to take SPI semaphore synchronously"));
}
}
}
void AP_InertialSensor_MPU6000::_read_data_transaction() {
/* one resister address followed by seven 2-byte registers */
uint8_t tx[15];
uint8_t rx[15];
memset(tx,0,15);
tx[0] = MPUREG_ACCEL_XOUT_H | 0x80;
_spi->transaction(tx, rx, 15);
uint8_t tx[16];
uint8_t rx[16];
memset(tx,0,16);
tx[0] = MPUREG_INT_STATUS | 0x80;
rx[1] = 0x42;
if (_error_count > 4) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
}
_spi->transaction(tx, rx, 16);
if (rx[1] != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT)) {
// possibly bad bus transaction
_error_count++;
_error_value = rx[1];
}
for (uint8_t i = 0; i < 7; i++) {
_sum[i] += (int16_t)(((uint16_t)rx[2*i+1] << 8) | rx[2*i+2]);
_sum[i] += (int16_t)(((uint16_t)rx[2*i+2] << 8) | rx[2*i+3]);
}
_count++;

4
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -38,7 +38,6 @@ protected:
private:
void _read_data_from_timerprocess();
void _read_data_transaction();
bool _data_ready();
void _poll_data(void);
@ -81,6 +80,9 @@ private:
void _set_filter_register(uint8_t filter_hz, uint8_t default_filter);
uint16_t _error_count;
uint8_t _error_value;
public:
#if MPU6000_DEBUG