diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index f3ef1a89afa4d503c0b50cb50afe3a3f61baa6f8..e57bf05bd557f7a488b597701aedbccaf4bc7ba3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -686,23 +686,23 @@ AP_InertialSensor::detect_backends(void) hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME), hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_ROLL_180)); } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_TEST_V3) { - _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_PITCH_180)); + _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), false, ROTATION_PITCH_180)); _add_backend(AP_InertialSensor_LSM9DS0::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_EXT_G_NAME), hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_ROLL_180_YAW_270)); - _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270)); + _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), false, ROTATION_YAW_270)); } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXRACER) { _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_NAME), ROTATION_ROLL_180_YAW_90)); - _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90)); + _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), false, ROTATION_ROLL_180_YAW_90)); } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PHMINI) { // PHMINI uses ICM20608 on the ACCEL_MAG device and a MPU9250 on the old MPU6000 CS line _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180)); - _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180)); + _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), false, ROTATION_ROLL_180)); } // also add any PX4 backends (eg. canbus sensors) _add_backend(AP_InertialSensor_PX4::detect(*this)); #elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI && defined(HAL_INS_DEFAULT_ROTATION) - _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), HAL_INS_DEFAULT_ROTATION)); + _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), false, HAL_INS_DEFAULT_ROTATION)); #elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME))); #elif HAL_INS_DEFAULT == HAL_INS_LSM9DS0 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index e409936a5d7d071e47a7ab067a9a22b538c9d881..45b064277b57499f96c00dc9c8543d53d18049e8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -18,6 +18,7 @@ #include <assert.h> #include <utility> +#include <stdio.h> #include <AP_HAL/GPIO.h> @@ -41,9 +42,7 @@ extern const AP_HAL::HAL &hal; #define MPUREG_ZA_OFFS_H 0x0D // Z axis accelerometer offset (high byte) #define MPUREG_ZA_OFFS_L 0x0E // Z axis accelerometer offset (low byte) -// MPU6000 & MPU9250 registers -// not sure if present in MPU9250 -// #define MPUREG_PRODUCT_ID 0x0C // Product ID Register +// MPU9250 registers #define MPUREG_XG_OFFS_USRH 0x13 // X axis gyro offset (high byte) #define MPUREG_XG_OFFS_USRL 0x14 // X axis gyro offset (low byte) #define MPUREG_YG_OFFS_USRH 0x15 // Y axis gyro offset (high byte) @@ -69,11 +68,20 @@ extern const AP_HAL::HAL &hal; # define BITS_GYRO_YGYRO_SELFTEST 0x40 # define BITS_GYRO_XGYRO_SELFTEST 0x80 #define MPUREG_ACCEL_CONFIG 0x1C +#define MPUREG_ACCEL_CONFIG2 0x1D #define MPUREG_MOT_THR 0x1F // detection threshold for Motion interrupt generation. Motion is detected when the absolute value of any of the accelerometer measurements exceeds this #define MPUREG_MOT_DUR 0x20 // duration counter threshold for Motion interrupt generation. The duration counter ticks at 1 kHz, therefore MOT_DUR has a unit of 1 LSB = 1 ms #define MPUREG_ZRMOT_THR 0x21 // detection threshold for Zero Motion interrupt generation. #define MPUREG_ZRMOT_DUR 0x22 // duration counter threshold for Zero Motion interrupt generation. The duration counter ticks at 16 Hz, therefore ZRMOT_DUR has a unit of 1 LSB = 64 ms. #define MPUREG_FIFO_EN 0x23 +# define BIT_TEMP_FIFO_EN 0x80 +# define BIT_XG_FIFO_EN 0x40 +# define BIT_YG_FIFO_EN 0x20 +# define BIT_ZG_FIFO_EN 0x10 +# define BIT_ACCEL_FIFO_EN 0x08 +# define BIT_SLV2_FIFO_EN 0x04 +# define BIT_SLV1_FIFO_EN 0x02 +# define BIT_SLV0_FIFI_EN0 0x01 #define MPUREG_INT_PIN_CFG 0x37 # define BIT_INT_RD_CLEAR 0x10 // clear the interrupt when any read occurs # define BIT_LATCH_INT_EN 0x20 // latch data ready pin @@ -174,12 +182,10 @@ extern const AP_HAL::HAL &hal; #define BITS_DLPF_CFG_5HZ 0x06 #define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 #define BITS_DLPF_CFG_MASK 0x07 - -#define DEFAULT_SMPLRT_DIV MPUREG_SMPLRT_1000HZ -#define DEFAULT_SAMPLE_RATE (1000 / (DEFAULT_SMPLRT_DIV + 1)) +#define BITS_DLPF_FCHOICE_B 0x08 #define MPU9250_SAMPLE_SIZE 14 -#define MPU9250_MAX_FIFO_SAMPLES 3 +#define MPU9250_MAX_FIFO_SAMPLES 20 #define MAX_DATA_READ (MPU9250_MAX_FIFO_SAMPLES * MPU9250_SAMPLE_SIZE) #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) @@ -201,10 +207,13 @@ static const float GYRO_SCALE = 0.0174532f / 16.4f; AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu, AP_HAL::OwnPtr<AP_HAL::Device> dev, + bool fast_sampling, enum Rotation rotation) : AP_InertialSensor_Backend(imu) + , _temp_filter(1000, 1) , _rotation(rotation) , _dev(std::move(dev)) + , _fast_sampling(fast_sampling) { } @@ -221,7 +230,7 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i return nullptr; } AP_InertialSensor_MPU9250 *sensor = - new AP_InertialSensor_MPU9250(imu, std::move(dev), rotation); + new AP_InertialSensor_MPU9250(imu, std::move(dev), false, rotation); if (!sensor || !sensor->_init()) { delete sensor; return nullptr; @@ -234,6 +243,7 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &imu, AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev, + bool fast_sampling, enum Rotation rotation) { if (!dev) { @@ -243,7 +253,7 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i dev->set_read_flag(0x80); - sensor = new AP_InertialSensor_MPU9250(imu, std::move(dev), rotation); + sensor = new AP_InertialSensor_MPU9250(imu, std::move(dev), fast_sampling, rotation); if (!sensor || !sensor->_init()) { delete sensor; return nullptr; @@ -264,9 +274,24 @@ bool AP_InertialSensor_MPU9250::_init() return success; } +void AP_InertialSensor_MPU9250::_fifo_reset() +{ + _register_write(MPUREG_USER_CTRL, 0); + _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_RESET); + _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_EN); +} + +void AP_InertialSensor_MPU9250::_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); + _fifo_reset(); + hal.scheduler->delay(1); +} + bool AP_InertialSensor_MPU9250::_has_auxiliary_bus() { - return _dev->bus_type() != AP_HAL::Device::BUS_TYPE_I2C; + return _dev->bus_type() != AP_HAL::Device::BUS_TYPE_I2C && !_fast_sampling; } void AP_InertialSensor_MPU9250::start() @@ -282,12 +307,19 @@ void AP_InertialSensor_MPU9250::start() _register_write(MPUREG_PWR_MGMT_2, 0x00); hal.scheduler->delay(1); - // disable sensor filtering - _register_write(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2); + // always use FIFO + _fifo_enable(); + if (_fast_sampling) { + // setup for fast sampling + _register_write(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2); + } else { + _register_write(MPUREG_CONFIG, BITS_DLPF_CFG_188HZ); + } + // set sample rate to 1kHz, and use the 2 pole filter to give the // desired rate - _register_write(MPUREG_SMPLRT_DIV, DEFAULT_SMPLRT_DIV); + _register_write(MPUREG_SMPLRT_DIV, 0); hal.scheduler->delay(1); // Gyro scale 2000º/s @@ -297,6 +329,13 @@ void AP_InertialSensor_MPU9250::start() // RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 16g _register_write(MPUREG_ACCEL_CONFIG,3<<3); + if (_fast_sampling) { + // setup ACCEL_FCHOICE for 4kHz sampling + _register_write(MPUREG_ACCEL_CONFIG2, 0x08); + } else { + _register_write(MPUREG_ACCEL_CONFIG2, 0x00); + } + // configure interrupt to fire when new data arrives _register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); @@ -312,12 +351,18 @@ void AP_InertialSensor_MPU9250::start() _dev->get_semaphore()->give(); // grab the used instances - _gyro_instance = _imu.register_gyro(DEFAULT_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_GYR_MPU9250)); - _accel_instance = _imu.register_accel(DEFAULT_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_ACC_MPU9250)); + _gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(DEVTYPE_GYR_MPU9250)); + _accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(DEVTYPE_ACC_MPU9250)); set_gyro_orientation(_gyro_instance, _rotation); set_accel_orientation(_accel_instance, _rotation); + // allocate fifo buffer + _fifo_buffer = new uint8_t[MAX_DATA_READ]; + if (_fifo_buffer == nullptr) { + AP_HAL::panic("MPU9250: Unable to allocate FIFO buffer"); + } + // start the timer process to read samples _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_read_sample, bool)); } @@ -330,6 +375,8 @@ bool AP_InertialSensor_MPU9250::update() update_gyro(_gyro_instance); update_accel(_accel_instance); + _publish_temperature(_accel_instance, _temp_filtered); + return true; } @@ -360,47 +407,140 @@ bool AP_InertialSensor_MPU9250::_data_ready(uint8_t int_status) return (int_status & BIT_RAW_RDY_INT) != 0; } -void AP_InertialSensor_MPU9250::_accumulate(uint8_t *rx) + +void AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples) +{ + for (uint8_t i = 0; i < n_samples; i++) { + uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i; + Vector3f accel, gyro; + + accel = Vector3f(int16_val(data, 1), + int16_val(data, 0), + -int16_val(data, 2)); + accel *= MPU9250_ACCEL_SCALE_1G; + + float temp = int16_val(data, 3); + temp = temp/340 + 36.53; + _last_temp = temp; + + gyro = Vector3f(int16_val(data, 5), + int16_val(data, 4), + -int16_val(data, 6)); + gyro *= GYRO_SCALE; + + _rotate_and_correct_accel(_accel_instance, accel); + _rotate_and_correct_gyro(_gyro_instance, gyro); + + _notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64()); + _notify_new_gyro_raw_sample(_gyro_instance, gyro); + + _temp_filtered = _temp_filter.apply(temp); + } +} + +void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples) { - Vector3f accel, gyro; + Vector3l asum, gsum; + + for (uint8_t i = 0; i < n_samples; i++) { + uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i; + asum += Vector3l(int16_val(data, 1), + int16_val(data, 0), + -int16_val(data, 2)); + gsum += Vector3l(int16_val(data, 5), + int16_val(data, 4), + -int16_val(data, 6)); + + float temp = int16_val(data, 3); + temp = temp/340 + 36.53; + _last_temp = temp; + } - accel = Vector3f(int16_val(rx, 1), - int16_val(rx, 0), - -int16_val(rx, 2)); - accel *= MPU9250_ACCEL_SCALE_1G; - _rotate_and_correct_accel(_accel_instance, accel); - _notify_new_accel_raw_sample(_accel_instance, accel); + float ascale = MPU9250_ACCEL_SCALE_1G / n_samples; + Vector3f accel(asum.x*ascale, asum.y*ascale, asum.z*ascale); - gyro = Vector3f(int16_val(rx, 5), - int16_val(rx, 4), - -int16_val(rx, 6)); - gyro *= GYRO_SCALE; + float gscale = GYRO_SCALE / n_samples; + Vector3f gyro(gsum.x*gscale, gsum.y*gscale, gsum.z*gscale); + + _rotate_and_correct_accel(_accel_instance, accel); _rotate_and_correct_gyro(_gyro_instance, gyro); + + _notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false); _notify_new_gyro_raw_sample(_gyro_instance, gyro); } +/* + * check the FIFO integrity by cross-checking the temperature against + * the last FIFO reading + */ +void AP_InertialSensor_MPU9250::_check_temperature(void) +{ + uint8_t rx[2]; + + if (!_block_read(MPUREG_TEMP_OUT_H, rx, 2)) { + return; + } + float temp = int16_val(rx, 0) / 340 + 36.53; + + if (fabsf(_last_temp - temp) > 2 && !is_zero(_last_temp)) { + // a 2 degree change in one sample is a highly likely + // sign of a FIFO alignment error + printf("FIFO temperature reset: %.2f %.2f\n", + temp, _last_temp); + _last_temp = temp; + _fifo_reset(); + } +} + /* * read from the data registers and update filtered data */ bool AP_InertialSensor_MPU9250::_read_sample() { - /* one register address followed by seven 2-byte registers */ - struct PACKED { - uint8_t int_status; - uint8_t d[14]; - } rx; + uint8_t n_samples; + uint16_t bytes_read; + uint8_t *rx = _fifo_buffer; + + if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) { + hal.console->printf("MPU9250: error in fifo read\n"); + return true; + } + + bytes_read = uint16_val(rx, 0); + n_samples = bytes_read / MPU9250_SAMPLE_SIZE; + + if (n_samples == 0) { + /* Not enough data in FIFO */ + return true; + } - if (!_block_read(MPUREG_INT_STATUS, (uint8_t *) &rx, sizeof(rx))) { - hal.console->printf("MPU9250: error reading sample\n"); + if (n_samples > MPU9250_MAX_FIFO_SAMPLES) { + printf("bytes_read = %u, n_samples %u > %u, dropping samples\n", + bytes_read, n_samples, MPU9250_MAX_FIFO_SAMPLES); + + /* Too many samples, do a FIFO RESET */ + _fifo_reset(); return true; } - if (!_data_ready(rx.int_status)) { + if (!_block_read(MPUREG_FIFO_R_W, rx, n_samples * MPU9250_SAMPLE_SIZE)) { + printf("MPU60x0: error in fifo read %u bytes\n", + n_samples * MPU9250_SAMPLE_SIZE); return true; } - _accumulate(rx.d); + if (_fast_sampling) { + _accumulate_fast_sampling(rx, n_samples); + } else { + _accumulate_fast_sampling(rx, n_samples); + } + + if (_temp_counter++ == 255) { + // check FIFO integrity every 0.25s + _check_temperature(); + } + return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h index 440e30643b3a8cfbb97d12703d516386d9c1f865..c27fa5790e4948d4f33579c5b763a045656f76e3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h @@ -37,6 +37,7 @@ public: static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu, AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev, + bool fast_sampling = false, enum Rotation rotation = ROTATION_NONE); /* update accel and gyro state */ @@ -52,6 +53,7 @@ public: private: AP_InertialSensor_MPU9250(AP_InertialSensor &imu, AP_HAL::OwnPtr<AP_HAL::Device> dev, + bool fast_sampling, enum Rotation rotation); #if MPU9250_DEBUG @@ -67,6 +69,9 @@ private: /* Read a single sample */ bool _read_sample(); + void _fifo_reset(); + void _fifo_enable(); + /* Check if there's data available by reading register */ bool _data_ready(); bool _data_ready(uint8_t int_status); @@ -77,16 +82,31 @@ private: uint8_t _register_read(uint8_t reg); void _register_write(uint8_t reg, uint8_t val); - void _accumulate(uint8_t *sample); + void _accumulate(uint8_t *samples, uint8_t n_samples); + void _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples); + void _check_temperature(void); // instance numbers of accel and gyro data uint8_t _gyro_instance; uint8_t _accel_instance; + float _temp_filtered; + LowPassFilter2pFloat _temp_filter; + AP_HAL::OwnPtr<AP_HAL::Device> _dev; AP_MPU9250_AuxiliaryBus *_auxiliary_bus; enum Rotation _rotation; + + // are we doing more than 1kHz sampling? + bool _fast_sampling; + + // last temperature reading, used to detect FIFO errors + float _last_temp; + uint8_t _temp_counter; + + // buffer for fifo read + uint8_t *_fifo_buffer; }; class AP_MPU9250_AuxiliaryBusSlave : public AuxiliaryBusSlave