AP_InertialSensor: added INS_FAST_SAMPLE parameter

this allows enable/disable of fast sampling per IMU, making
experimentation easier.

It also fixes the fast sampling to always average over 8 samples, and
fixes the 9250 to use the correct accumulator when not doing fast
sampling

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -418,11 +418,16 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("ACC3_ID", 35, AP_InertialSensor, _accel_id[2], 0),
 
+    // @Param: FAST_SAMPLE
+    // @DisplayName: Fast sampling mask
+    // @Description: Mask of IMUs to enable fast sampling on, if available
+    // @User: Advanced
+    AP_GROUPINFO("FAST_SAMPLE",  36, AP_InertialSensor, _fast_sampling_mask,   0),
+
     /*
       NOTE: parameter indexes have gaps above. When adding new
       parameters check for conflicts carefully
      */
-
     AP_GROUPEND
 };
 
@@ -691,32 +696,32 @@ AP_InertialSensor::detect_backends(void)
 
     } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {
         // older Pixhawk2 boards have the MPU6000 instead of MPU9250
-        if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), true, ROTATION_PITCH_180))) {
+        if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_PITCH_180))) {
             _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_EXT_NAME), 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));
-        if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), false, ROTATION_YAW_270))) {
+        if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270))) {
             _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), 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), false, 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));
 
     } 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), false, ROTATION_ROLL_180));
+        _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
 
     } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PH2SLIM) {
-        _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), true, ROTATION_YAW_270));        
+        _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
     }
     // 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), false, HAL_INS_DEFAULT_ROTATION));
+    _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), 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

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -352,6 +352,9 @@ class AP_InertialSensor : AP_AccelCal_Client
     // use for attitude, velocity, position estimates
     AP_Int8     _use[INS_MAX_INSTANCES];
 
+    // control enable of fast sampling
+    AP_Int8     _fast_sampling_mask;
+
     // board orientation from AHRS
     enum Rotation _board_orientation;
 

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -186,6 +186,11 @@ class AP_InertialSensor_Backend
     void increment_clip_count(uint8_t instance) {
         _imu._accel_clip_count[instance]++;
     }
+
+    // should fast sampling be enabled on this IMU?
+    bool enable_fast_sampling(uint8_t instance) {
+        return (_imu._fast_sampling_mask & (1U<<instance)) != 0;
+    }
 
     // note that each backend is also expected to have a static detect()
     // function which instantiates an instance of the backend sensor

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -513,7 +513,6 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
 
 void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
 {
-    Vector3l asum, gsum;
     float tsum = 0;
     const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / _accel_scale;
     bool clipped = false;
@@ -528,10 +527,13 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
             abs(a.z) > clip_limit) {
             clipped = true;
         }
-        asum += a;
-        gsum += Vector3l(int16_val(data, 5),
-                         int16_val(data, 4),
-                         -int16_val(data, 6));
+        Vector3l g(int16_val(data, 5),
+                   int16_val(data, 4),
+                   -int16_val(data, 6));
+
+        _accum.accel += a;
+        _accum.gyro += g;
+        _accum.count++;
 
         float temp = int16_val(data, 3);
         temp = temp/340 + 36.53;
@@ -542,20 +544,27 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
     if (clipped) {
         increment_clip_count(_accel_instance);
     }
-
-    float ascale = _accel_scale / n_samples;
-    Vector3f accel(asum.x*ascale, asum.y*ascale, asum.z*ascale);
 
-    float gscale = GYRO_SCALE / n_samples;
-    Vector3f gyro(gsum.x*gscale, gsum.y*gscale, gsum.z*gscale);
+    _temp_filtered = _temp_filter.apply(tsum / n_samples);
+
+    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 gscale = GYRO_SCALE / _accum.count;
+        Vector3f gyro(_accum.gyro.x*gscale, _accum.gyro.y*gscale, _accum.gyro.z*gscale);
 
-    _rotate_and_correct_accel(_accel_instance, accel);
-    _rotate_and_correct_gyro(_gyro_instance, gyro);
+        _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);
+        _notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false);
+        _notify_new_gyro_raw_sample(_gyro_instance, gyro);
+
+        _accum.accel.zero();
+        _accum.gyro.zero();
+        _accum.count = 0;
+    }
 
-    _temp_filtered = _temp_filter.apply(tsum / n_samples);
 }
 
 /*
@@ -601,7 +610,7 @@ void AP_InertialSensor_MPU6000::_read_fifo()
     }
 
     while (n_samples > 0) {
-        uint8_t n = MIN(n_samples, MPU6000_MAX_FIFO_SAMPLES);
+        uint8_t n = MIN(n_samples, MPU6000_MAX_FIFO_SAMPLES - _accum.count);
         if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU6000_SAMPLE_SIZE)) {
             printf("MPU60x0: error in fifo read %u bytes\n", n * MPU6000_SAMPLE_SIZE);
             goto check_registers;
@@ -663,10 +672,10 @@ void AP_InertialSensor_MPU6000::_set_filter_register(void)
     config = 0;
 #endif
 
-#if 0
-    // temporarily disable fast sampling
-    _fast_sampling = (_is_icm_device && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI);
-#endif
+
+    if (enable_fast_sampling(_accel_instance)) {
+        _fast_sampling = (_is_icm_device && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI);
+    }
 
     if (_fast_sampling) {
         // this gives us 8kHz sampling on gyros and 4kHz on accels

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -122,6 +122,13 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend
     uint8_t *_fifo_buffer;
 
     uint8_t _reg_check_counter;
+
+    // accumulators for fast sampling
+    struct {
+        Vector3l accel;
+        Vector3l gyro;
+        uint8_t count;
+    } _accum;
 };
 
 class AP_MPU6000_AuxiliaryBusSlave : public AuxiliaryBusSlave

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp

@@ -207,14 +207,11 @@ 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 disabled for now
-    , _fast_sampling(false)
 {
 }
 
@@ -231,7 +228,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), false, rotation);
+        new AP_InertialSensor_MPU9250(imu, std::move(dev), rotation);
     if (!sensor || !sensor->_init()) {
         delete sensor;
         return nullptr;
@@ -244,7 +241,6 @@ 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) {
@@ -254,7 +250,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), fast_sampling, rotation);
+    sensor = new AP_InertialSensor_MPU9250(imu, std::move(dev), rotation);
     if (!sensor || !sensor->_init()) {
         delete sensor;
         return nullptr;
@@ -313,6 +309,10 @@ void AP_InertialSensor_MPU9250::start()
     // always use FIFO
     _fifo_enable();
 
+    if (enable_fast_sampling(_accel_instance) && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
+        _fast_sampling = true;
+    }
+
     if (_fast_sampling) {
         // setup for fast sampling
         _register_write(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2, true);
@@ -458,10 +458,13 @@ void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint
             abs(a.z) > clip_limit) {
             clipped = true;
         }
-        asum += a;
-        gsum += Vector3l(int16_val(data, 5),
-                         int16_val(data, 4),
-                         -int16_val(data, 6));
+        Vector3l g(int16_val(data, 5),
+                   int16_val(data, 4),
+                   -int16_val(data, 6));
+
+        _accum.accel += a;
+        _accum.gyro += g;
+        _accum.count++;
 
         float temp = int16_val(data, 3);
         temp = temp/340 + 36.53;
@@ -472,20 +475,26 @@ void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint
     if (clipped) {
         increment_clip_count(_accel_instance);
     }
-
-    float ascale = MPU9250_ACCEL_SCALE_1G / n_samples;
-    Vector3f accel(asum.x*ascale, asum.y*ascale, asum.z*ascale);
 
-    float gscale = GYRO_SCALE / n_samples;
-    Vector3f gyro(gsum.x*gscale, gsum.y*gscale, gsum.z*gscale);
+    _temp_filtered = _temp_filter.apply(tsum / n_samples);
+
+    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 gscale = GYRO_SCALE / _accum.count;
+        Vector3f gyro(_accum.gyro.x*gscale, _accum.gyro.y*gscale, _accum.gyro.z*gscale);
 
-    _rotate_and_correct_accel(_accel_instance, accel);
-    _rotate_and_correct_gyro(_gyro_instance, gyro);
+        _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);
+        _notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false);
+        _notify_new_gyro_raw_sample(_gyro_instance, gyro);
 
-    _temp_filtered = _temp_filter.apply(tsum / n_samples);
+        _accum.accel.zero();
+        _accum.gyro.zero();
+        _accum.count = 0;
+    }
 }
 
 
@@ -535,7 +544,7 @@ bool AP_InertialSensor_MPU9250::_read_sample()
     }
 
     while (n_samples > 0) {
-        uint8_t n = MIN(MPU9250_MAX_FIFO_SAMPLES, n_samples);
+        uint8_t n = MIN(MPU9250_MAX_FIFO_SAMPLES - _accum.count, n_samples);
         if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU9250_SAMPLE_SIZE)) {
             printf("MPU60x0: error in fifo read %u bytes\n", n * MPU9250_SAMPLE_SIZE);
             goto check_registers;
@@ -544,7 +553,7 @@ bool AP_InertialSensor_MPU9250::_read_sample()
         if (_fast_sampling) {
             _accumulate_fast_sampling(rx, n);
         } else {
-            _accumulate_fast_sampling(rx, n);
+            _accumulate(rx, n);
         }
         n_samples -= n;
     }

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h

@@ -37,7 +37,6 @@ class AP_InertialSensor_MPU9250 : public AP_InertialSensor_Backend
 
     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 */
@@ -53,7 +52,6 @@ class AP_InertialSensor_MPU9250 : public AP_InertialSensor_Backend
 private:
     AP_InertialSensor_MPU9250(AP_InertialSensor &imu,
                               AP_HAL::OwnPtr<AP_HAL::Device> dev,
-                              bool fast_sampling,
                               enum Rotation rotation);
 
 #if MPU9250_DEBUG
@@ -112,6 +110,13 @@ class AP_InertialSensor_MPU9250 : public AP_InertialSensor_Backend
     uint8_t *_fifo_buffer;
 
     uint8_t _reg_check_counter;
+
+    // accumulators for fast sampling
+    struct {
+        Vector3l accel;
+        Vector3l gyro;
+        uint8_t count;
+    } _accum;
 };
 
 class AP_MPU9250_AuxiliaryBusSlave : public AuxiliaryBusSlave