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

AP_InertialSensor: detect bad MPU6000 SPI transactions and lower bus speed

Browse Source 
this uses bad data or bad INT_STATUS values from the MPU6000 to detect
the sensor running too fast and lower bus speed
This commit is contained in:
Andrew Tridgell 2013-10-30 08:54:03 +11:00 committed by Randy Mackay
parent 8e55ab5072
commit ee2fb33567
3 changed files with 13 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -139,6 +139,9 @@ public:
}
}
virtual uint16_t error_count(void) const { return 0; }
virtual bool healthy(void) const { return true; }
protected:
// sensor specific init to be overwritten by descendant classes

13
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -351,12 +351,17 @@ void AP_InertialSensor_MPU6000::_read_data_transaction() {
} rx, tx = { cmd : MPUREG_INT_STATUS | 0x80, };
_spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx));
/*
detect a bad SPI bus transaction by looking for all 14 bytes
zero, or the wrong INT_STATUS register value. This is used to
detect a too high SPI bus speed.
*/
uint8_t i;
for (i=0; i<14; i++) {
if (rx.v[i] != 0) break;
}
if (rx.int_status != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT) || i != 14) {
if ((rx.int_status&~0x6) != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT) || i == 14) {
// likely a bad bus transaction
if (++_error_count > 4) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
@ -534,10 +539,6 @@ bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate)
// clear interrupt on any read, and hold the data ready pin high
// until we clear the interrupt
_register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN);
hal.scheduler->delay(1);
// read INT status to clear the initial bits
_register_read(MPUREG_INT_STATUS);
// now that we have initialised, we set the SPI bus speed to high
// (8MHz on APM2)

3
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -33,6 +33,9 @@ public:
// get_delta_time returns the time period in seconds overwhich the sensor data was collected
float get_delta_time();
uint16_t error_count(void) const { return _error_count; }
bool healthy(void) const { return _error_count <= 4; }
protected:
uint16_t _init_sensor( Sample_rate sample_rate );