Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: added register checking for MPU6000/ICM20608

Browse Source
This commit is contained in:
Andrew Tridgell 2016-11-10 17:27:22 +11:00
parent 95a849f472
commit 77a83c091a
2 changed files with 33 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -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;

4
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -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