AP_InertialSensor: implement a new strategy for fast sampling

this moves to using a 1p filter on the high rate data, followed by
averaging down to 1kHz then a 2p filter to apply configured cutoff
frequency.

It also fixes the FIFO reset to not cause data corruption. We need to
disable all FIFO channels before doing the reset, and wait for the
FIFO to stop in the sensor.

Finally it moves sampling of the MPU6000 and MPU9250 into the main
thread. That significantly improves scheduling performance as we no
longer get long FIFO SPI transfers happening during other tasks. All
transfers happen at the start of the fast loop. That makes timing much
more predictable.

Thanks to Leonard and Paul for help with this design!

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

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);
@@ -422,12 +431,17 @@ void AP_InertialSensor_MPU6000::start()
         AP_HAL::panic("MPU6000: Unable to allocate FIFO buffer");
     }
 
+    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),
+        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;
+        }
+        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 (abs(a.x) > clip_limit ||
-            abs(a.y) > clip_limit ||
-            abs(a.z) > clip_limit) {
+            if (fabsf(a.x) > clip_limit ||
+                fabsf(a.y) > clip_limit ||
+                fabsf(a.z) > clip_limit) {
                 clipped = true;
             }
-        Vector3l g(int16_val(data, 5),
+            _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)

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<AP_HAL::Device> 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;
@@ -111,9 +104,8 @@ 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;
 };
 

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp

@@ -22,6 +22,8 @@
 
 #include <AP_HAL/GPIO.h>
 
+#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);
 
@@ -366,9 +373,13 @@ void AP_InertialSensor_MPU9250::start()
         AP_HAL::panic("MPU9250: Unable to allocate FIFO buffer");
     }
 
+    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));
     }
+}
 
 /*
   update the accel and gyro vectors
@@ -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),
+        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;
+        }
+        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 (abs(a.x) > clip_limit ||
-            abs(a.y) > clip_limit ||
-            abs(a.z) > clip_limit) {
+            if (fabsf(a.x) > clip_limit ||
+                fabsf(a.y) > clip_limit ||
+                fabsf(a.z) > clip_limit) {
                 clipped = true;
             }
-        Vector3l g(int16_val(data, 5),
+            _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)

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<AP_HAL::Device> 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;
 };