From 3379ddd51f71ed0358088f065d14a247ae2d3880 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Thu, 3 Jul 2014 12:09:34 +0200 Subject: [PATCH] AP_InertialSensor_MPU6000: Add _register_write_check method. This private method allows to check whether the value written and the posterior value readed are the same. Should be used only for debuging purposes, for release versions use _register_write instead. --- .../AP_InertialSensor_MPU6000.cpp | 17 ++++++++++++++++- .../AP_InertialSensor_MPU6000.h | 1 + 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 6a85650f92..3c0af4ce60 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -413,6 +413,22 @@ void AP_InertialSensor_MPU6000::_register_write(uint8_t reg, uint8_t val) _spi->transaction(tx, rx, 2); } +/* + useful when debugging SPI bus errors + */ +void AP_InertialSensor_MPU6000::_register_write_check(uint8_t reg, uint8_t val) +{ + uint8_t readed; + _register_write(reg, val); + readed = _register_read(reg); + if (readed != val){ + hal.console->printf_P(PSTR("Values doesn't match; written: %02x; read: %02x "), val, readed); + } +#if MPU6000_DEBUG + hal.console->printf_P(PSTR("Values written: %02x; readed: %02x "), val, readed); +#endif +} + /* set the DLPF filter frequency. Assumes caller has taken semaphore */ @@ -440,7 +456,6 @@ void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz, uint8_t if (filter != 0) { _last_filter_hz = filter_hz; - _register_write(MPUREG_CONFIG, filter); } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index 3ec0be621a..f6567d849d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -45,6 +45,7 @@ private: void _poll_data(void); uint8_t _register_read( uint8_t reg ); void _register_write( uint8_t reg, uint8_t val ); + void _register_write_check(uint8_t reg, uint8_t val); bool _hardware_init(Sample_rate sample_rate); AP_HAL::SPIDeviceDriver *_spi;