diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index d1f47cded3..034a0ac095 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -269,7 +269,7 @@ void AP_InertialSensor_Backend::_publish_temperature(uint8_t instance, float tem */ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) { - if (!_sem->take_nonblocking()) { + if (!_sem->take(0)) { return; } @@ -292,7 +292,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) */ void AP_InertialSensor_Backend::update_accel(uint8_t instance) { - if (!_sem->take_nonblocking()) { + if (!_sem->take(0)) { return; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index dfc8be6bf6..52ffc471b2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -21,6 +21,8 @@ extern const AP_HAL::HAL& hal; #endif #endif +#define debug(fmt, args ...) do {printf("MPU6000: " fmt "\n", ## args); } while(0) + /* EXT_SYNC allows for frame synchronisation with an external device such as a camera. When enabled the LSB of AccelZ holds the FSYNC bit @@ -64,6 +66,7 @@ extern const AP_HAL::HAL& hal; # define MPUREG_CONFIG_EXT_SYNC_AX 0x05 # define MPUREG_CONFIG_EXT_SYNC_AY 0x06 # define MPUREG_CONFIG_EXT_SYNC_AZ 0x07 +# define MPUREG_CONFIG_FIFO_MODE_STOP 0x40 #define MPUREG_GYRO_CONFIG 0x1B // bit definitions for MPUREG_GYRO_CONFIG # define BITS_GYRO_FS_250DPS 0x00 @@ -313,27 +316,33 @@ bool AP_InertialSensor_MPU6000::_init() bool success = _hardware_init(); -#if MPU6000_DEBUG - _dump_registers(); -#endif - return success; } void AP_InertialSensor_MPU6000::_fifo_reset() { uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0; + _dev->set_speed(AP_HAL::Device::SPEED_LOW); + _register_write(MPUREG_FIFO_EN, 0); _register_write(MPUREG_USER_CTRL, user_ctrl); _register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET); _register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_EN); + _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); + hal.scheduler->delay_microseconds(1); + _dev->set_speed(AP_HAL::Device::SPEED_HIGH); } void AP_InertialSensor_MPU6000::_fifo_enable() { + uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0; + _register_write(MPUREG_FIFO_EN, 0); + hal.scheduler->delay_microseconds(1); + _register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_FIFO_EN); + hal.scheduler->delay_microseconds(1); _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, true); - _fifo_reset(); hal.scheduler->delay(1); } @@ -358,6 +367,10 @@ void AP_InertialSensor_MPU6000::start() // always use FIFO _fifo_enable(); + // grab the used instances + _gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(DEVTYPE_GYR_MPU6000)); + _accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(DEVTYPE_ACC_MPU6000)); + // setup ODR and on-sensor filtering _set_filter_register(); @@ -408,10 +421,6 @@ void AP_InertialSensor_MPU6000::start() _dev->get_semaphore()->give(); - // grab the used instances - _gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(DEVTYPE_GYR_MPU6000)); - _accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(DEVTYPE_ACC_MPU6000)); - // setup sensor rotations from probe() set_gyro_orientation(_gyro_instance, _rotation); set_accel_orientation(_accel_instance, _rotation); @@ -421,13 +430,18 @@ void AP_InertialSensor_MPU6000::start() if (_fifo_buffer == nullptr) { AP_HAL::panic("MPU6000: Unable to allocate FIFO buffer"); } - - // start the timer process to read samples - _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, bool)); + + if (get_sample_rate_hz() >= 400) { + _use_accumulate = true; + } else { + // start the timer process to read samples + _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, bool)); + } } + /* - process any + publish any pending data */ bool AP_InertialSensor_MPU6000::update() { @@ -439,6 +453,17 @@ bool AP_InertialSensor_MPU6000::update() return true; } +/* + accumulate new samples + */ +void AP_InertialSensor_MPU6000::accumulate() +{ + if (_use_accumulate && _dev->get_semaphore()->take(0)) { + _poll_data(); + _dev->get_semaphore()->give(); + } +} + AuxiliaryBus *AP_InertialSensor_MPU6000::get_auxiliary_bus() { if (_auxiliary_bus) { @@ -476,10 +501,10 @@ bool AP_InertialSensor_MPU6000::_poll_data() return true; } -void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples) +bool AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp) { for (uint8_t i = 0; i < n_samples; i++) { - uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i; + const uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i; Vector3f accel, gyro; bool fsync_set = false; @@ -492,9 +517,13 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples) -int16_val(data, 2)); accel *= _accel_scale; - float temp = int16_val(data, 3); - temp = temp/340 + 36.53; - _last_temp = temp; + int16_t t2 = int16_val(data, 3); + if (abs(t2 - raw_temp) > 400) { + debug("temp reset %d %d", raw_temp, t2); + _fifo_reset(); + return false; + } + float temp = t2/340.0 + 36.53; gyro = Vector3f(int16_val(data, 5), int16_val(data, 4), @@ -509,85 +538,83 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples) _temp_filtered = _temp_filter.apply(temp); } + return true; } -void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples) +/* + when doing fast sampling the sensor gives us 8k samples/second. Every 2nd accel sample is a duplicate. + + To filter this we first apply a 1p low pass filter at 188Hz, then we + average over 8 samples to bring the data rate down to 1kHz. This + gives very good aliasing rejection at frequencies well above what + can be handled with 1kHz sample rates. + */ +bool AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp) { - float tsum = 0; + int32_t tsum = 0; const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / _accel_scale; bool clipped = false; + bool ret = true; for (uint8_t i = 0; i < n_samples; i++) { - uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i; - Vector3l a(int16_val(data, 1), - int16_val(data, 0), - -int16_val(data, 2)); - if (abs(a.x) > clip_limit || - abs(a.y) > clip_limit || - abs(a.z) > clip_limit) { - clipped = true; + const uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i; + + // use temperatue to detect FIFO corruption + int16_t t2 = int16_val(data, 3); + if (abs(t2 - raw_temp) > 400) { + debug("temp reset %d %d %d", raw_temp, t2, raw_temp - t2); + _fifo_reset(); + ret = false; + break; } - Vector3l g(int16_val(data, 5), + tsum += t2; + + if ((_accum.count & 1) == 0) { + // accel data is at 4kHz + Vector3f a(int16_val(data, 1), + int16_val(data, 0), + -int16_val(data, 2)); + if (fabsf(a.x) > clip_limit || + fabsf(a.y) > clip_limit || + fabsf(a.z) > clip_limit) { + clipped = true; + } + _accum.accel += _accum.accel_filter.apply(a); + } + + Vector3f g(int16_val(data, 5), int16_val(data, 4), -int16_val(data, 6)); - _accum.accel += a; - _accum.gyro += g; + _accum.gyro += _accum.gyro_filter.apply(g); _accum.count++; - - float temp = int16_val(data, 3); - temp = temp/340 + 36.53; - tsum += temp; - _last_temp = temp; } if (clipped) { increment_clip_count(_accel_instance); } - _temp_filtered = _temp_filter.apply(tsum / n_samples); - + float temp = (tsum/n_samples)/340.0 + 36.53; + _temp_filtered = _temp_filter.apply(temp); + if (_accum.count == MPU6000_MAX_FIFO_SAMPLES) { - float ascale = _accel_scale / _accum.count; - Vector3f accel(_accum.accel.x*ascale, _accum.accel.y*ascale, _accum.accel.z*ascale); + float ascale = _accel_scale / (MPU6000_MAX_FIFO_SAMPLES/2); + _accum.accel *= ascale; - float gscale = GYRO_SCALE / _accum.count; - Vector3f gyro(_accum.gyro.x*gscale, _accum.gyro.y*gscale, _accum.gyro.z*gscale); + float gscale = GYRO_SCALE / MPU6000_MAX_FIFO_SAMPLES; + _accum.gyro *= gscale; - _rotate_and_correct_accel(_accel_instance, accel); - _rotate_and_correct_gyro(_gyro_instance, gyro); + _rotate_and_correct_accel(_accel_instance, _accum.accel); + _rotate_and_correct_gyro(_gyro_instance, _accum.gyro); - _notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false); - _notify_new_gyro_raw_sample(_gyro_instance, gyro); + _notify_new_accel_raw_sample(_accel_instance, _accum.accel, AP_HAL::micros64(), false); + _notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro); _accum.accel.zero(); _accum.gyro.zero(); _accum.count = 0; } - -} - -/* - * check the FIFO integrity by cross-checking the temperature against - * the last FIFO reading - */ -void AP_InertialSensor_MPU6000::_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("MPU6000: FIFO temperature reset: %.2f %.2f\n", - (double)temp, (double)_last_temp); - _last_temp = temp; - _fifo_reset(); - } + return ret; } void AP_InertialSensor_MPU6000::_read_fifo() @@ -595,9 +622,11 @@ void AP_InertialSensor_MPU6000::_read_fifo() uint8_t n_samples; uint16_t bytes_read; uint8_t *rx = _fifo_buffer; + int16_t raw_temp; + uint8_t trx[2]; + bool need_reset = false; if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) { - hal.console->printf("MPU60x0: error in fifo read\n"); goto check_registers; } @@ -609,6 +638,26 @@ void AP_InertialSensor_MPU6000::_read_fifo() goto check_registers; } + /* + fetch temperature in order to detect FIFO sync errors + */ + if (!_block_read(MPUREG_TEMP_OUT_H, trx, 2)) { + return; + } + raw_temp = int16_val(trx, 0); + + /* + testing has shown that if we have more than 32 samples in the + FIFO then some of those samples will be corrupt. It always is + the ones at the end of the FIFO, so clear those with a reset + once we've read the first 24. Reading 24 gives us the normal + number of samples for fast sampling at 400Hz + */ + if (n_samples > 32) { + need_reset = true; + n_samples = 24; + } + while (n_samples > 0) { uint8_t n = MIN(n_samples, MPU6000_MAX_FIFO_SAMPLES - _accum.count); if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU6000_SAMPLE_SIZE)) { @@ -617,23 +666,23 @@ void AP_InertialSensor_MPU6000::_read_fifo() } if (_fast_sampling) { - _accumulate_fast_sampling(rx, n); + if (!_accumulate_fast_sampling(rx, n, raw_temp)) { + debug("stop at %u of %u", n_samples, bytes_read/MPU6000_SAMPLE_SIZE); + break; + } } else { - _accumulate(rx, n); + if (!_accumulate(rx, n, raw_temp)) { + break; + } } n_samples -= n; } - if (bytes_read > MPU6000_SAMPLE_SIZE * 35) { - printf("MPU60x0: fifo reset\n"); + if (need_reset) { + //debug("fifo reset n_samples %u", bytes_read/MPU6000_SAMPLE_SIZE); _fifo_reset(); } - if (_temp_counter++ == 255) { - // check FIFO integrity every 0.25s - _check_temperature(); - } - check_registers: if (_reg_check_counter++ == 10) { _reg_check_counter = 0; @@ -680,6 +729,9 @@ void AP_InertialSensor_MPU6000::_set_filter_register(void) if (enable_fast_sampling(_accel_instance)) { _fast_sampling = (_is_icm_device && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI); + if (_fast_sampling) { + hal.console->printf("MPU6000: enabled fast sampling\n"); + } } if (_fast_sampling) { @@ -689,6 +741,8 @@ void AP_InertialSensor_MPU6000::_set_filter_register(void) // limit to 1kHz if not on SPI config |= BITS_DLPF_CFG_188HZ; } + + config |= MPUREG_CONFIG_FIFO_MODE_STOP; _register_write(MPUREG_CONFIG, config, true); if (_is_icm_device) { @@ -776,10 +830,6 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void) if (_data_ready()) { break; } - -#if MPU6000_DEBUG - _dump_registers(); -#endif } _dev->set_speed(AP_HAL::Device::SPEED_HIGH); @@ -798,28 +848,6 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void) return true; } -#if MPU6000_DEBUG -// dump all config registers - used for debug -void AP_InertialSensor_MPU6000::_dump_registers(void) -{ - hal.console->println("MPU6000 registers"); - if (!_dev->get_semaphore()->take(0)) { - return; - } - - for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) { - uint8_t v = _register_read(reg); - hal.console->printf("%02x:%02x ", (unsigned)reg, (unsigned)v); - if ((reg - (MPUREG_PRODUCT_ID-1)) % 16 == 0) { - hal.console->println(); - } - } - hal.console->println(); - - _dev->get_semaphore()->give(); -} -#endif - AP_MPU6000_AuxiliaryBusSlave::AP_MPU6000_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr, uint8_t instance) : AuxiliaryBusSlave(bus, addr, instance) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index a90ffe9407..145fec923a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -15,9 +15,6 @@ #include "AP_InertialSensor_Backend.h" #include "AuxiliaryBus.h" -// enable debug to see a register dump on startup -#define MPU6000_DEBUG 0 - class AP_MPU6000_AuxiliaryBus; class AP_MPU6000_AuxiliaryBusSlave; @@ -42,7 +39,8 @@ public: enum Rotation rotation = ROTATION_NONE); /* update accel and gyro state */ - bool update(); + bool update() override; + void accumulate() override; /* * Return an AuxiliaryBus if the bus driver allows it @@ -56,10 +54,6 @@ private: AP_HAL::OwnPtr dev, enum Rotation rotation); -#if MPU6000_DEBUG - void _dump_registers(); -#endif - /* Initialize sensor*/ bool _init(); bool _hardware_init(); @@ -85,9 +79,8 @@ private: uint8_t _register_read(uint8_t reg); 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); - void _check_temperature(void); + bool _accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp); + bool _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp); // instance numbers of accel and gyro data uint8_t _gyro_instance; @@ -110,11 +103,10 @@ private: // 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; + // are we using accumulate for sensor reading or a bus callback? + bool _use_accumulate; + // has master i2c been enabled? bool _master_i2c_enable; @@ -123,11 +115,16 @@ private: uint8_t _reg_check_counter; - // accumulators for fast sampling + /* + accumulators for fast sampling + See description in _accumulate_fast_sampling() + */ struct { - Vector3l accel; - Vector3l gyro; + Vector3f accel; + Vector3f gyro; uint8_t count; + LowPassFilterVector3f accel_filter{4000, 188}; + LowPassFilterVector3f gyro_filter{8000, 188}; } _accum; }; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 9a9ecebae6..952af6ed99 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -22,6 +22,8 @@ #include +#define debug(fmt, args ...) do {printf("MPU9250: " fmt "\n", ## args); } while(0) + extern const AP_HAL::HAL &hal; // MPU9250 accelerometer scaling for 16g range @@ -57,6 +59,7 @@ extern const AP_HAL::HAL &hal; # define MPUREG_SMPLRT_100HZ 0x09 # define MPUREG_SMPLRT_50HZ 0x13 #define MPUREG_CONFIG 0x1A +#define MPUREG_CONFIG_FIFO_MODE_STOP 0x40 #define MPUREG_GYRO_CONFIG 0x1B // bit definitions for MPUREG_GYRO_CONFIG # define BITS_GYRO_FS_250DPS 0x00 @@ -263,28 +266,31 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i bool AP_InertialSensor_MPU9250::_init() { bool success = _hardware_init(); - -#if MPU9250_DEBUG - _dump_registers(); -#endif - return success; } void AP_InertialSensor_MPU9250::_fifo_reset() { uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0; + _dev->set_speed(AP_HAL::Device::SPEED_LOW); + _register_write(MPUREG_FIFO_EN, 0); _register_write(MPUREG_USER_CTRL, user_ctrl); _register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET); _register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_EN); + _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); + hal.scheduler->delay_microseconds(1); + _dev->set_speed(AP_HAL::Device::SPEED_HIGH); } void AP_InertialSensor_MPU9250::_fifo_enable() { + uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0; + _register_write(MPUREG_FIFO_EN, 0); + _register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_FIFO_EN); _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, true); - _fifo_reset(); hal.scheduler->delay(1); } @@ -309,15 +315,20 @@ void AP_InertialSensor_MPU9250::start() // always use FIFO _fifo_enable(); + // grab the used instances + _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)); + if (enable_fast_sampling(_accel_instance) && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) { _fast_sampling = true; + hal.console->printf("MPU9250: enabled fast sampling\n"); } if (_fast_sampling) { // setup for fast sampling - _register_write(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2, true); + _register_write(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2 | MPUREG_CONFIG_FIFO_MODE_STOP, true); } else { - _register_write(MPUREG_CONFIG, BITS_DLPF_CFG_188HZ, true); + _register_write(MPUREG_CONFIG, BITS_DLPF_CFG_188HZ | MPUREG_CONFIG_FIFO_MODE_STOP, true); } // set sample rate to 1kHz, and use the 2 pole filter to give the @@ -353,10 +364,6 @@ void AP_InertialSensor_MPU9250::start() _dev->get_semaphore()->give(); - // grab the used instances - _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); @@ -365,9 +372,13 @@ void AP_InertialSensor_MPU9250::start() 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)); + + if (get_sample_rate_hz() >= 400) { + _use_accumulate = true; + } else { + // start the timer process to read samples + _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_read_sample, bool)); + } } /* @@ -383,6 +394,18 @@ bool AP_InertialSensor_MPU9250::update() return true; } +/* + accumulate new samples + */ +void AP_InertialSensor_MPU9250::accumulate() +{ + if (_use_accumulate && _dev->get_semaphore()->take(0)) { + _read_sample(); + _dev->get_semaphore()->give(); + } +} + + AuxiliaryBus *AP_InertialSensor_MPU9250::get_auxiliary_bus() { if (_auxiliary_bus) { @@ -411,10 +434,10 @@ bool AP_InertialSensor_MPU9250::_data_ready(uint8_t int_status) } -void AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples) +bool AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp) { for (uint8_t i = 0; i < n_samples; i++) { - uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i; + const uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i; Vector3f accel, gyro; accel = Vector3f(int16_val(data, 1), @@ -422,9 +445,13 @@ void AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples) -int16_val(data, 2)); accel *= MPU9250_ACCEL_SCALE_1G; - float temp = int16_val(data, 3); - temp = temp/340 + 36.53; - _last_temp = temp; + int16_t t2 = int16_val(data, 3); + if (abs(t2 - raw_temp) > 400) { + debug("temp reset %d %d %d", raw_temp, t2, raw_temp-t2); + _fifo_reset(); + return false; + } + float temp = t2/340 + 36.53; gyro = Vector3f(int16_val(data, 5), int16_val(data, 4), @@ -439,87 +466,85 @@ void AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples) _temp_filtered = _temp_filter.apply(temp); } + return true; } -void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples) +/* + when doing fast sampling the sensor gives us 8k samples/second. Every 2nd accel sample is a duplicate. + + To filter this we first apply a 1p low pass filter at 188Hz, then we + average over 8 samples to bring the data rate down to 1kHz. This + gives very good aliasing rejection at frequencies well above what + can be handled with 1kHz sample rates. + */ +bool AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp) { - float tsum = 0; + int32_t tsum = 0; const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / MPU9250_ACCEL_SCALE_1G; bool clipped = false; + bool ret = true; for (uint8_t i = 0; i < n_samples; i++) { - uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i; - Vector3l a(int16_val(data, 1), - int16_val(data, 0), - -int16_val(data, 2)); - if (abs(a.x) > clip_limit || - abs(a.y) > clip_limit || - abs(a.z) > clip_limit) { - clipped = true; + const uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i; + + // use temperatue to detect FIFO corruption + int16_t t2 = int16_val(data, 3); + if (abs(t2 - raw_temp) > 400) { + debug("temp reset %d %d %d", raw_temp, t2, raw_temp - t2); + _fifo_reset(); + ret = false; + break; } - Vector3l g(int16_val(data, 5), + tsum += t2; + + if ((_accum.count & 1) == 0) { + // accels are at 4kHz not 8kHz + Vector3f a(int16_val(data, 1), + int16_val(data, 0), + -int16_val(data, 2)); + if (fabsf(a.x) > clip_limit || + fabsf(a.y) > clip_limit || + fabsf(a.z) > clip_limit) { + clipped = true; + } + _accum.accel += _accum.accel_filter.apply(a); + } + Vector3f g(int16_val(data, 5), int16_val(data, 4), -int16_val(data, 6)); - _accum.accel += a; - _accum.gyro += g; + _accum.gyro += _accum.gyro_filter.apply(g); _accum.count++; - - float temp = int16_val(data, 3); - temp = temp/340 + 36.53; - tsum += temp; - _last_temp = temp; } if (clipped) { increment_clip_count(_accel_instance); } - _temp_filtered = _temp_filter.apply(tsum / n_samples); + float temp = (tsum/n_samples)/340.0 + 36.53; + _temp_filtered = _temp_filter.apply(temp); if (_accum.count == MPU9250_MAX_FIFO_SAMPLES) { - float ascale = MPU9250_ACCEL_SCALE_1G / _accum.count; - Vector3f accel(_accum.accel.x*ascale, _accum.accel.y*ascale, _accum.accel.z*ascale); + float ascale = MPU9250_ACCEL_SCALE_1G / (MPU9250_MAX_FIFO_SAMPLES/2); + _accum.accel *= ascale; - float gscale = GYRO_SCALE / _accum.count; - Vector3f gyro(_accum.gyro.x*gscale, _accum.gyro.y*gscale, _accum.gyro.z*gscale); + float gscale = GYRO_SCALE / MPU9250_MAX_FIFO_SAMPLES; + _accum.gyro *= gscale; - _rotate_and_correct_accel(_accel_instance, accel); - _rotate_and_correct_gyro(_gyro_instance, gyro); + _rotate_and_correct_accel(_accel_instance, _accum.accel); + _rotate_and_correct_gyro(_gyro_instance, _accum.gyro); - _notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false); - _notify_new_gyro_raw_sample(_gyro_instance, gyro); + _notify_new_accel_raw_sample(_accel_instance, _accum.accel, AP_HAL::micros64(), false); + _notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro); _accum.accel.zero(); _accum.gyro.zero(); _accum.count = 0; } + return ret; } -/* - * 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("MPU9250: FIFO temperature reset: %.2f %.2f\n", - (double)temp, (double)_last_temp); - _last_temp = temp; - _fifo_reset(); - } -} - /* * read from the data registers and update filtered data */ @@ -528,9 +553,11 @@ bool AP_InertialSensor_MPU9250::_read_sample() uint8_t n_samples; uint16_t bytes_read; uint8_t *rx = _fifo_buffer; + int16_t raw_temp; + uint8_t trx[2]; + bool need_reset = false; if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) { - hal.console->printf("MPU9250: error in fifo read\n"); goto check_registers; } @@ -542,6 +569,26 @@ bool AP_InertialSensor_MPU9250::_read_sample() goto check_registers; } + /* + fetch temperature in order to detect FIFO sync errors + */ + if (!_block_read(MPUREG_TEMP_OUT_H, trx, 2)) { + return true; + } + raw_temp = int16_val(trx, 0); + + /* + testing has shown that if we have more than 32 samples in the + FIFO then some of those samples will be corrupt. It always is + the ones at the end of the FIFO, so clear those with a reset + once we've read the first 24. Reading 24 gives us the normal + number of samples for fast sampling at 400Hz + */ + if (n_samples > 32) { + need_reset = true; + n_samples = 24; + } + while (n_samples > 0) { uint8_t n = MIN(MPU9250_MAX_FIFO_SAMPLES - _accum.count, n_samples); if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU9250_SAMPLE_SIZE)) { @@ -550,23 +597,23 @@ bool AP_InertialSensor_MPU9250::_read_sample() } if (_fast_sampling) { - _accumulate_fast_sampling(rx, n); + if (!_accumulate_fast_sampling(rx, n, raw_temp)) { + debug("stop at %u of %u", n_samples, bytes_read/MPU9250_SAMPLE_SIZE); + break; + } } else { - _accumulate(rx, n); + if (!_accumulate(rx, n, raw_temp)) { + break; + } } n_samples -= n; } - if (bytes_read > MPU9250_SAMPLE_SIZE * 35) { - printf("MPU9250: fifo reset\n"); + if (need_reset) { + //debug("fifo reset %u", bytes_read/MPU9250_SAMPLE_SIZE); _fifo_reset(); } - if (_temp_counter++ == 255) { - // check FIFO integrity every 0.25s - _check_temperature(); - } - check_registers: if (_reg_check_counter++ == 10) { _reg_check_counter = 0; @@ -655,10 +702,6 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void) if (_data_ready()) { break; } - -#if MPU9250_DEBUG - _dump_registers(); -#endif } if (tries == 5) { hal.console->println("Failed to boot MPU9250 5 times"); @@ -677,22 +720,6 @@ fail_whoami: return false; } -#if MPU9250_DEBUG -// dump all config registers - used for debug -void AP_InertialSensor_MPU9250::_dump_registers(AP_MPU9250_BusDriver *bus) -{ - hal.console->println("MPU9250 registers"); - for (uint8_t reg=0; reg<=126; reg++) { - uint8_t v = _register_read(bus, reg); - hal.console->printf("%02x:%02x ", (unsigned)reg, (unsigned)v); - if ((reg - (MPUREG_PRODUCT_ID-1)) % 16 == 0) { - hal.console->println(); - } - } - hal.console->println(); -} -#endif - AP_MPU9250_AuxiliaryBusSlave::AP_MPU9250_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr, uint8_t instance) : AuxiliaryBusSlave(bus, addr, instance) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h index e438ca4e6d..f4779e613a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h @@ -16,9 +16,6 @@ class AP_MPU9250_AuxiliaryBus; class AP_MPU9250_AuxiliaryBusSlave; -// enable debug to see a register dump on startup -#define MPU9250_DEBUG 0 - class AP_InertialSensor_MPU9250 : public AP_InertialSensor_Backend { friend AP_MPU9250_AuxiliaryBus; @@ -40,7 +37,8 @@ public: enum Rotation rotation = ROTATION_NONE); /* update accel and gyro state */ - bool update(); + bool update() override; + void accumulate() override; /* * Return an AuxiliaryBus if the bus driver allows it @@ -54,10 +52,6 @@ private: AP_HAL::OwnPtr dev, enum Rotation rotation); -#if MPU9250_DEBUG - static void _dump_registers(); -#endif - bool _init(); bool _hardware_init(); @@ -80,9 +74,8 @@ private: uint8_t _register_read(uint8_t reg); 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); - void _check_temperature(void); + bool _accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp); + bool _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp); // instance numbers of accel and gyro data uint8_t _gyro_instance; @@ -99,23 +92,27 @@ private: // are we doing more than 1kHz sampling? bool _fast_sampling; + // are we using accumulate or timer callback? + bool _use_accumulate; + // has master i2c been enabled? bool _master_i2c_enable; - // last temperature reading, used to detect FIFO errors - float _last_temp; - uint8_t _temp_counter; - // buffer for fifo read uint8_t *_fifo_buffer; uint8_t _reg_check_counter; - // accumulators for fast sampling + /* + accumulators for fast sampling + See description in _accumulate_fast_sampling() + */ struct { - Vector3l accel; - Vector3l gyro; + Vector3f accel; + Vector3f gyro; uint8_t count; + LowPassFilterVector3f accel_filter{8000, 188}; + LowPassFilterVector3f gyro_filter{8000, 188}; } _accum; };