From 77a83c091ae0ad13dcc2c91aa9a5c58dd992df6d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
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