From 77a83c091ae0ad13dcc2c91aa9a5c58dd992df6d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 10 Nov 2016 17:27:22 +1100 Subject: [PATCH] AP_InertialSensor: added register checking for MPU6000/ICM20608 --- .../AP_InertialSensor_MPU6000.cpp | 46 ++++++++++++------- .../AP_InertialSensor_MPU6000.h | 4 +- 2 files changed, 33 insertions(+), 17 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 4cf8365f97..1915301cc4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -331,7 +331,8 @@ void AP_InertialSensor_MPU6000::_fifo_reset() void AP_InertialSensor_MPU6000::_fifo_enable() { _register_write(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN | - BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN); + BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN, + true); _fifo_reset(); hal.scheduler->delay(1); } @@ -362,11 +363,11 @@ void AP_InertialSensor_MPU6000::start() // set sample rate to 1000Hz and apply a software filter // In this configuration, the gyro sample rate is 8kHz - _register_write(MPUREG_SMPLRT_DIV, 0); + _register_write(MPUREG_SMPLRT_DIV, 0, true); hal.scheduler->delay(1); // Gyro scale 2000ยบ/s - _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); + _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS, true); hal.scheduler->delay(1); // read the product ID rev c has 1/2 the sensitivity of rev d @@ -380,18 +381,18 @@ void AP_InertialSensor_MPU6000::start() (product_id == MPU6000_REV_C5))) { // Accel scale 8g (4096 LSB/g) // Rev C has different scaling than rev D - _register_write(MPUREG_ACCEL_CONFIG,1<<3); + _register_write(MPUREG_ACCEL_CONFIG,1<<3, true); _accel_scale = GRAVITY_MSS / 4096.f; } else { // Accel scale 16g (2048 LSB/g) - _register_write(MPUREG_ACCEL_CONFIG,3<<3); + _register_write(MPUREG_ACCEL_CONFIG,3<<3, true); _accel_scale = GRAVITY_MSS / 2048.f; } hal.scheduler->delay(1); if (_is_icm_device) { // this avoids a sensor bug, see description above - _register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE); + _register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true); } // configure interrupt to fire when new data arrives @@ -588,7 +589,7 @@ void AP_InertialSensor_MPU6000::_read_fifo() if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) { hal.console->printf("MPU60x0: error in fifo read\n"); - return; + goto check_registers; } bytes_read = uint16_val(rx, 0); @@ -596,7 +597,7 @@ void AP_InertialSensor_MPU6000::_read_fifo() if (n_samples == 0) { /* Not enough data in FIFO */ - return; + goto check_registers; } if (n_samples > MPU6000_MAX_FIFO_SAMPLES) { @@ -605,13 +606,13 @@ void AP_InertialSensor_MPU6000::_read_fifo() /* Too many samples, do a FIFO RESET */ _fifo_reset(); - return; + goto check_registers; } if (!_block_read(MPUREG_FIFO_R_W, rx, n_samples * MPU6000_SAMPLE_SIZE)) { printf("MPU60x0: error in fifo read %u bytes\n", n_samples * MPU6000_SAMPLE_SIZE); - return; + goto check_registers; } if (_fast_sampling) { @@ -624,6 +625,16 @@ void AP_InertialSensor_MPU6000::_read_fifo() // check FIFO integrity every 0.25s _check_temperature(); } + +check_registers: + if (_reg_check_counter++ == 10) { + _reg_check_counter = 0; + // check next register value for correctness + if (!_dev->check_next_register()) { + _inc_gyro_error_count(_gyro_instance); + _inc_accel_error_count(_accel_instance); + } + } } bool AP_InertialSensor_MPU6000::_block_read(uint8_t reg, uint8_t *buf, @@ -639,9 +650,9 @@ uint8_t AP_InertialSensor_MPU6000::_register_read(uint8_t reg) return val; } -void AP_InertialSensor_MPU6000::_register_write(uint8_t reg, uint8_t val) +void AP_InertialSensor_MPU6000::_register_write(uint8_t reg, uint8_t val, bool checked) { - _dev->write_register(reg, val); + _dev->write_register(reg, val, checked); } /* @@ -666,14 +677,14 @@ void AP_InertialSensor_MPU6000::_set_filter_register(void) // limit to 1kHz if not on SPI config |= BITS_DLPF_CFG_188HZ; } - _register_write(MPUREG_CONFIG, config); + _register_write(MPUREG_CONFIG, config, true); if (_is_icm_device) { if (_fast_sampling) { // setup for 4kHz accels - _register_write(ICMREG_ACCEL_CONFIG2, ICM_ACC_FCHOICE_B); + _register_write(ICMREG_ACCEL_CONFIG2, ICM_ACC_FCHOICE_B, true); } else { - _register_write(ICMREG_ACCEL_CONFIG2, ICM_ACC_DLPF_CFG_218HZ); + _register_write(ICMREG_ACCEL_CONFIG2, ICM_ACC_DLPF_CFG_218HZ, true); } } } @@ -703,6 +714,9 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void) AP_HAL::panic("MPU6000: Unable to get semaphore"); } + // setup for register checking + _dev->setup_checked_registers(7); + // initially run the bus at low speed _dev->set_speed(AP_HAL::Device::SPEED_LOW); @@ -766,7 +780,7 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void) if (_is_icm_device) { // this avoids a sensor bug, see description above - _register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE); + _register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true); } return true; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index bdbf847324..616d2cf233 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -83,7 +83,7 @@ private: * account */ bool _block_read(uint8_t reg, uint8_t *buf, uint32_t size); uint8_t _register_read(uint8_t reg); - void _register_write(uint8_t reg, uint8_t val ); + void _register_write(uint8_t reg, uint8_t val, bool checked=false); void _accumulate(uint8_t *samples, uint8_t n_samples); void _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples); @@ -120,6 +120,8 @@ private: // buffer for fifo read uint8_t *_fifo_buffer; + + uint8_t _reg_check_counter; }; class AP_MPU6000_AuxiliaryBusSlave : public AuxiliaryBusSlave