From b98bcbf71569d6b6721c8dea6fbd0b0f8925b1c3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 29 Oct 2013 17:42:35 +1100 Subject: [PATCH] AP_InertialSensor: automatically lower bus speed on mpu6k bad reads --- .../AP_InertialSensor_MPU6000.cpp | 153 +++++++----------- .../AP_InertialSensor_MPU6000.h | 27 ++-- 2 files changed, 71 insertions(+), 109 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 758b1a19c9..6aafb29d6a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -154,20 +154,7 @@ extern const AP_HAL::HAL& hal; * RM-MPU-6000A-00.pdf, page 33, section 4.25 lists LSB sensitivity of * gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3) */ -const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532 / 16.4); - -/* pch: I believe the accel and gyro indicies are correct - * but somone else should please confirm. - * - * jamesjb: Y and Z axes are flipped on the PX4FMU - */ -const uint8_t AP_InertialSensor_MPU6000::_gyro_data_index[3] = { 5, 4, 6 }; -const uint8_t AP_InertialSensor_MPU6000::_accel_data_index[3] = { 1, 0, 2 }; - -const int8_t AP_InertialSensor_MPU6000::_gyro_data_sign[3] = { 1, 1, -1 }; -const int8_t AP_InertialSensor_MPU6000::_accel_data_sign[3] = { 1, 1, -1 }; - -const uint8_t AP_InertialSensor_MPU6000::_temp_data_index = 3; +const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532f / 16.4f); /* * RM-MPU-6000A-00.pdf, page 31, section 4.23 lists LSB sensitivity of @@ -180,7 +167,6 @@ const uint8_t AP_InertialSensor_MPU6000::_temp_data_index = 3; AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000() : AP_InertialSensor(), _drdy_pin(NULL), - _temp(0), _initialised(false), _mpu6000_product_id(AP_PRODUCT_ID_NONE) { @@ -203,7 +189,7 @@ uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate ) uint8_t tries = 0; do { - bool success = hardware_init(sample_rate); + bool success = _hardware_init(sample_rate); if (success) { hal.scheduler->delay(5+2); if (!_spi_sem->take(100)) { @@ -230,6 +216,7 @@ uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate ) * _read_data_transaction requires the spi semaphore to be taken by * its caller. */ _last_sample_time_micros = hal.scheduler->micros(); + hal.scheduler->delay(10); _read_data_transaction(); // start the timer process to read samples @@ -241,13 +228,6 @@ uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate ) return _mpu6000_product_id; } -// accumulation in ISR - must be read with interrupts disabled -// the sum of the values since last read -static volatile int32_t _sum[7]; - -// how many values we've accumulated since last read -static volatile uint16_t _count; - /*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ bool AP_InertialSensor_MPU6000::wait_for_sample(uint16_t timeout_ms) @@ -267,10 +247,6 @@ bool AP_InertialSensor_MPU6000::wait_for_sample(uint16_t timeout_ms) bool AP_InertialSensor_MPU6000::update( void ) { - int32_t sum[7]; - float count_scale; - Vector3f accel_scale = _accel_scale.get(); - // wait for at least 1 sample if (!wait_for_sample(1000)) { return false; @@ -278,44 +254,33 @@ bool AP_InertialSensor_MPU6000::update( void ) // disable timer procs for mininum time hal.scheduler->suspend_timer_procs(); - /** ATOMIC SECTION w/r/t TIMER PROCESS */ - { - for (int i=0; i<7; i++) { - sum[i] = _sum[i]; - _sum[i] = 0; - } - - _num_samples = _count; - _count = 0; - } + _gyro = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); + _accel = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z); + _num_samples = _sum_count; + _accel_sum.zero(); + _gyro_sum.zero(); + _sum_count = 0; hal.scheduler->resume_timer_procs(); - count_scale = 1.0f / _num_samples; - - _gyro = Vector3f(_gyro_data_sign[0] * sum[_gyro_data_index[0]], - _gyro_data_sign[1] * sum[_gyro_data_index[1]], - _gyro_data_sign[2] * sum[_gyro_data_index[2]]); _gyro.rotate(_board_orientation); - _gyro *= _gyro_scale * count_scale; + _gyro *= _gyro_scale / _num_samples; _gyro -= _gyro_offset; - _accel = Vector3f(_accel_data_sign[0] * sum[_accel_data_index[0]], - _accel_data_sign[1] * sum[_accel_data_index[1]], - _accel_data_sign[2] * sum[_accel_data_index[2]]); _accel.rotate(_board_orientation); - _accel *= count_scale * MPU6000_ACCEL_SCALE_1G; + _accel *= MPU6000_ACCEL_SCALE_1G / _num_samples; + + Vector3f accel_scale = _accel_scale.get(); _accel.x *= accel_scale.x; _accel.y *= accel_scale.y; _accel.z *= accel_scale.z; _accel -= _accel_offset; - _temp = _temp_to_celsius(sum[_temp_data_index] * count_scale); - if (_last_filter_hz != _mpu6000_filter) { if (_spi_sem->take(10)) { _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); _set_filter_register(_mpu6000_filter, 0); _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + _error_count = 0; _spi_sem->give(); } } @@ -376,32 +341,41 @@ void AP_InertialSensor_MPU6000::_poll_data(void) } } + void AP_InertialSensor_MPU6000::_read_data_transaction() { /* one resister address followed by seven 2-byte registers */ - uint8_t tx[16]; - uint8_t rx[16]; - memset(tx,0,16); - tx[0] = MPUREG_INT_STATUS | 0x80; - rx[1] = 0x42; - if (_error_count > 4) { - _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); - } - _spi->transaction(tx, rx, 16); - - if (rx[1] != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT)) { - // possibly bad bus transaction - _error_count++; - _error_value = rx[1]; - } - - for (uint8_t i = 0; i < 7; i++) { - _sum[i] += (int16_t)(((uint16_t)rx[2*i+2] << 8) | rx[2*i+3]); - } + struct PACKED { + uint8_t cmd; + uint8_t int_status; + uint8_t v[14]; + } rx, tx = { cmd : MPUREG_INT_STATUS | 0x80, }; - _count++; - if (_count == 0) { + _spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx)); + + uint8_t i; + for (i=0; i<14; i++) { + if (rx.v[i] != 0) break; + } + if (rx.int_status != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT) || i != 14) { + // likely a bad bus transaction + if (++_error_count > 4) { + _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + } + } + +#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) + _accel_sum.x += int16_val(rx.v, 1); + _accel_sum.y += int16_val(rx.v, 0); + _accel_sum.z -= int16_val(rx.v, 2); + _gyro_sum.x += int16_val(rx.v, 5); + _gyro_sum.y += int16_val(rx.v, 4); + _gyro_sum.z -= int16_val(rx.v, 6); + _sum_count++; + + if (_sum_count == 0) { // rollover - v unlikely - memset((void*)_sum, 0, sizeof(_sum)); + _accel_sum.zero(); + _gyro_sum.zero(); } } @@ -419,7 +393,7 @@ uint8_t AP_InertialSensor_MPU6000::_register_read( uint8_t reg ) return rx[1]; } -void AP_InertialSensor_MPU6000::register_write(uint8_t reg, uint8_t val) +void AP_InertialSensor_MPU6000::_register_write(uint8_t reg, uint8_t val) { uint8_t tx[2]; uint8_t rx[2]; @@ -457,12 +431,12 @@ 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); + _register_write(MPUREG_CONFIG, filter); } } -bool AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate) +bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate) { if (!_spi_sem->take(100)) { hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore")); @@ -474,13 +448,13 @@ bool AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate) // Chip reset uint8_t tries; for (tries = 0; tries<5; tries++) { - register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET); + _register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET); hal.scheduler->delay(100); // Wake up device and select GyroZ clock. Note that the // MPU6000 starts up in sleep mode, and it can take some time // for it to come out of sleep - register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO); + _register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO); hal.scheduler->delay(5); // check it has woken up @@ -497,11 +471,11 @@ bool AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate) return false; } - register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode + _register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode hal.scheduler->delay(1); // Disable I2C bus (recommended on datasheet) - register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); + _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); hal.scheduler->delay(1); uint8_t default_filter; @@ -532,10 +506,10 @@ bool AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate) // set sample rate to 200Hz, and use _sample_divider to give // the requested rate to the application - register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ); + _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ); hal.scheduler->delay(1); - register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s + _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s hal.scheduler->delay(1); // read the product ID rev c has 1/2 the sensitivity of rev d @@ -546,22 +520,25 @@ bool AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate) (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_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); } else { // Accel scale 8g (4096 LSB/g) - register_write(MPUREG_ACCEL_CONFIG,2<<3); + _register_write(MPUREG_ACCEL_CONFIG,2<<3); } hal.scheduler->delay(1); // configure interrupt to fire when new data arrives - register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); + _register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); hal.scheduler->delay(1); // clear interrupt on any read, and hold the data ready pin high // until we clear the interrupt - register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN); + _register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN); hal.scheduler->delay(1); + // read INT status to clear the initial bits + _register_read(MPUREG_INT_STATUS); + // now that we have initialised, we set the SPI bus speed to high // (8MHz on APM2) _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); @@ -571,12 +548,6 @@ bool AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate) return true; } -float AP_InertialSensor_MPU6000::_temp_to_celsius ( uint16_t regval ) -{ - /* TODO */ - return 20.0; -} - // return the MPU6k gyro drift rate in radian/s/s // note that this is much better than the oilpan gyros float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void) @@ -589,7 +560,7 @@ float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void) bool AP_InertialSensor_MPU6000::sample_available() { _poll_data(); - return (_count >> _sample_shift) > 0; + return (_sum_count >> _sample_shift) > 0; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index d88a2f8ea9..e34a8e7d35 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -37,35 +37,21 @@ protected: uint16_t _init_sensor( Sample_rate sample_rate ); private: + AP_HAL::DigitalSource *_drdy_pin; void _read_data_transaction(); bool _data_ready(); void _poll_data(void); - AP_HAL::DigitalSource *_drdy_pin; uint8_t _register_read( uint8_t reg ); - bool _register_read_from_timerprocess( uint8_t reg, uint8_t *val ); - void register_write( uint8_t reg, uint8_t val ); - bool hardware_init(Sample_rate sample_rate); + void _register_write( uint8_t reg, uint8_t val ); + bool _hardware_init(Sample_rate sample_rate); AP_HAL::SPIDeviceDriver *_spi; AP_HAL::Semaphore *_spi_sem; uint16_t _num_samples; - - float _temp; - - float _temp_to_celsius( uint16_t ); - static const float _gyro_scale; - static const uint8_t _gyro_data_index[3]; - static const int8_t _gyro_data_sign[3]; - - static const uint8_t _accel_data_index[3]; - static const int8_t _accel_data_sign[3]; - - static const uint8_t _temp_data_index; - uint32_t _last_sample_time_micros; // ensure we can't initialise twice @@ -81,7 +67,12 @@ private: void _set_filter_register(uint8_t filter_hz, uint8_t default_filter); uint16_t _error_count; - uint8_t _error_value; + + // accumulation in timer - must be read with timer disabled + // the sum of the values since last read + Vector3l _accel_sum; + Vector3l _gyro_sum; + volatile int16_t _sum_count; public: