From ece01da10e5e79a933c69a664c7fa4119290cf6e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 29 Jun 2014 12:11:21 +1000 Subject: [PATCH] AP_InertialSensor: fixed _dump_registers() for MPU6000 need to take the semaphore to prevent bus errors --- .../AP_InertialSensor_MPU6000.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index afa5d97c4a..c2b4fc28c2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -575,14 +575,17 @@ bool AP_InertialSensor_MPU6000::_sample_available() void AP_InertialSensor_MPU6000::_dump_registers(void) { hal.console->println_P(PSTR("MPU6000 registers")); - for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) { - uint8_t v = _register_read(reg); - hal.console->printf_P(PSTR("%02x:%02x "), (unsigned)reg, (unsigned)v); - if ((reg - (MPUREG_PRODUCT_ID-1)) % 16 == 0) { - hal.console->println(); + if (_spi_sem->take(100)) { + for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) { + uint8_t v = _register_read(reg); + hal.console->printf_P(PSTR("%02x:%02x "), (unsigned)reg, (unsigned)v); + if ((reg - (MPUREG_PRODUCT_ID-1)) % 16 == 0) { + hal.console->println(); + } } + hal.console->println(); + _spi_sem->give(); } - hal.console->println(); } #endif