AP_InertialSensor: make the backend fast gyro rate configurable

raise gyro rate default on F7 and H7
clean up gyro rate docs and output startup banner

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -40,6 +40,8 @@
 
 extern const AP_HAL::HAL& hal;
 
+
+
 #if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
 #define DEFAULT_GYRO_FILTER  20
 #define DEFAULT_ACCEL_FILTER 20
@@ -54,6 +56,12 @@ extern const AP_HAL::HAL& hal;
 #define DEFAULT_STILL_THRESH 0.1f
 #endif
 
+#if defined(STM32H7) || defined(STM32F7)
+#define MPU_FIFO_FASTSAMPLE_DEFAULT 1
+#else
+#define MPU_FIFO_FASTSAMPLE_DEFAULT 0
+#endif
+
 #define SAMPLE_UNIT 1
 
 #define GYRO_INIT_MAX_DIFF_DPS 0.1f
@@ -493,6 +501,14 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
     // @Path: ../Filter/HarmonicNotchFilter.cpp
     AP_SUBGROUPINFO(_harmonic_notch_filter, "HNTCH_",  41, AP_InertialSensor, HarmonicNotchFilterParams),
 
+    // @Param: GYRO_RATE
+    // @DisplayName: Gyro rate for IMUs with Fast Sampling enabled
+    // @Description: Gyro rate for IMUs with fast sampling enabled. The gyro rate is the sample rate at which the IMU filters operate and needs to be at least double the maximum filter frequency. If the sensor does not support the selected rate the next highest supported rate will be used. For IMUs which do not support fast sampling this setting is ignored and the default gyro rate of 1Khz is used.
+    // @User: Advanced
+    // @Values: 0:1 kHz,1:2 kHz,3:4 kHz
+    // @RebootRequired: True
+    AP_GROUPINFO("GYRO_RATE",  42, AP_InertialSensor, _fast_sampling_rate, MPU_FIFO_FASTSAMPLE_DEFAULT),
+
     /*
       NOTE: parameter indexes have gaps above. When adding new
       parameters check for conflicts carefully
@@ -681,11 +697,11 @@ bool AP_InertialSensor::set_gyro_window_size(uint16_t size) {
 }
 
 void
-AP_InertialSensor::init(uint16_t sample_rate)
+AP_InertialSensor::init(uint16_t loop_rate)
 {
     // remember the sample rate
-    _sample_rate = sample_rate;
+    _loop_rate = loop_rate;
-    _loop_delta_t = 1.0f / sample_rate;
+    _loop_delta_t = 1.0f / loop_rate;
 
     // we don't allow deltat values greater than 10x the normal loop
     // time to be exposed outside of INS. Large deltat values can
@@ -709,7 +725,7 @@ AP_InertialSensor::init(uint16_t sample_rate)
         init_gyro();
     }
 
-    _sample_period_usec = 1000*1000UL / _sample_rate;
+    _sample_period_usec = 1000*1000UL / _loop_rate;
 
     // establish the baseline time between samples
     _delta_time = 0;
@@ -979,6 +995,16 @@ AP_InertialSensor::init_gyro()
     _save_gyro_calibration();
 }
 
+// output GCS startup messages
+bool AP_InertialSensor::get_output_banner(uint8_t backend_id, char* banner, uint8_t banner_len)
+{
+    if (backend_id >= INS_MAX_BACKENDS || _backends[backend_id] == nullptr) {
+        return false;
+    }
+
+    return _backends[backend_id]->get_output_banner(banner, banner_len);
+}
+
 // accelerometer clipping reporting
 uint32_t AP_InertialSensor::get_accel_clip_count(uint8_t instance) const
 {
@@ -1055,7 +1081,7 @@ bool AP_InertialSensor::calibrate_trim(float &trim_roll, float &trim_pitch)
 
     _calibrating = true;
 
-    const uint8_t update_dt_milliseconds = (uint8_t)(1000.0f/get_sample_rate()+0.5f);
+    const uint8_t update_dt_milliseconds = (uint8_t)(1000.0f/get_loop_rate_hz()+0.5f);
 
     // wait 100ms for ins filter to rise
     for (uint8_t k=0; k<100/update_dt_milliseconds; k++) {

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -97,6 +97,9 @@ public:
     ///
     void init_gyro(void);
 
+    // get startup messages to output to the GCS
+    bool get_output_banner(uint8_t instance_id, char* banner, uint8_t banner_len);
+
     /// Fetch the current gyro values
     ///
     /// @returns	vector of rotational rates in radians/sec
@@ -206,8 +209,8 @@ public:
         _custom_rotation = custom_rotation;
     }
 
-    // return the selected sample rate
+    // return the selected loop rate at which samples are made avilable
-    uint16_t get_sample_rate(void) const { return _sample_rate; }
+    uint16_t get_loop_rate_hz(void) const { return _loop_rate; }
 
     // return the main loop delta_t in seconds
     float get_loop_delta_t(void) const { return _loop_delta_t; }
@@ -433,8 +436,8 @@ private:
     uint8_t _accel_count;
     uint8_t _backend_count;
 
-    // the selected sample rate
+    // the selected loop rate at which samples are made available
-    uint16_t _sample_rate;
+    uint16_t _loop_rate;
     float _loop_delta_t;
     float _loop_delta_t_max;
 
@@ -541,6 +544,9 @@ private:
     // control enable of fast sampling
     AP_Int8     _fast_sampling_mask;
 
+    // control enable of fast sampling
+    AP_Int8     _fast_sampling_rate;
+
     // control enable of detected sensors
     AP_Int8     _enable_mask;
     

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -479,11 +479,11 @@ void AP_InertialSensor_Backend::_inc_gyro_error_count(uint8_t instance)
     _imu._gyro_error_count[instance]++;
 }
 
-// return the requested sample rate in Hz
+// return the requested loop rate at which samples will be made available in Hz
-uint16_t AP_InertialSensor_Backend::get_sample_rate_hz(void) const
+uint16_t AP_InertialSensor_Backend::get_loop_rate_hz(void) const
 {
     // enum can be directly cast to Hz
-    return (uint16_t)_imu._sample_rate;
+    return (uint16_t)_imu._loop_rate;
 }
 
 /*

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -76,6 +76,9 @@ public:
     // notify of a fifo reset
     void notify_fifo_reset(void);
 
+    // get a startup banner to output to the GCS
+    virtual bool get_output_banner(char* banner, uint8_t banner_len) { return false; }
+
     /*
       device driver IDs. These are used to fill in the devtype field
       of the device ID, which shows up as INS*ID* parameters to
@@ -234,7 +237,7 @@ protected:
     uint16_t _gyro_filter_cutoff(void) const { return _imu._gyro_filter_cutoff; }
 
     // return the requested sample rate in Hz
-    uint16_t get_sample_rate_hz(void) const;
+    uint16_t get_loop_rate_hz(void) const;
 
     // return the notch filter center in Hz for the sample rate
     float _gyro_notch_center_freq_hz(void) const { return _imu._notch_filter.center_freq_hz(); }
@@ -301,6 +304,11 @@ protected:
         return (_imu._fast_sampling_mask & (1U<<instance)) != 0;
     }
 
+    // if fast sampling is enabled, the rate to use in kHz
+    uint8_t get_fast_sampling_rate() {
+        return (1 << uint8_t(_imu._fast_sampling_rate));
+    }
+
     // called by subclass when data is received from the sensor, thus
     // at the 'sensor rate'
     void _notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel);

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp

@@ -265,11 +265,6 @@ void AP_InertialSensor_Invensense::start()
     // readings to fit them into an int16_t:
     _set_raw_sample_accel_multiplier(_accel_instance, multiplier_accel);
 
-    if (_fast_sampling) {
-        hal.console->printf("MPU[%u]: enabled fast sampling rate %uHz/%uHz\n",
-                            _accel_instance, _backend_rate_hz*_fifo_downsample_rate, _backend_rate_hz);
-    }
-    
     // set sample rate to 1000Hz and apply a software filter
     // In this configuration, the gyro sample rate is 8kHz
     _register_write(MPUREG_SMPLRT_DIV, 0, true);
@@ -350,6 +345,15 @@ void AP_InertialSensor_Invensense::start()
     _dev->register_periodic_callback(1000000UL / _backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensense::_poll_data, void));
 }
 
+// get a startup banner to output to the GCS
+bool AP_InertialSensor_Invensense::get_output_banner(char* banner, uint8_t banner_len) {
+    if (_fast_sampling) {
+        snprintf(banner, banner_len, "IMU%u: fast sampling enabled %.1fkHz/%.1fkHz",
+            _accel_instance, _backend_rate_hz*_fifo_downsample_rate / 1000.0, _backend_rate_hz/ 1000.0);
+        return true;
+    }
+    return false;
+}
 
 /*
   publish any pending data
@@ -509,13 +513,13 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
 
             _accum.accel *= _fifo_accel_scale;
             _accum.gyro *= _fifo_gyro_scale;
-            
+
             _rotate_and_correct_accel(_accel_instance, _accum.accel);
             _rotate_and_correct_gyro(_gyro_instance, _accum.gyro);
-            
+
             _notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false);
             _notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro);
-            
+
             _accum.accel.zero();
             _accum.gyro.zero();
             _accum.count = 0;
@@ -684,15 +688,20 @@ void AP_InertialSensor_Invensense::_set_filter_register(void)
     if (enable_fast_sampling(_accel_instance)) {
         _fast_sampling = (_mpu_type >= Invensense_MPU9250 && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI);
         if (_fast_sampling) {
-            if (get_sample_rate_hz() <= 1000) {
+            // constrain the gyro rate to be at least the loop rate
-                _fifo_downsample_rate = 8;
+            uint8_t loop_limit = 1;
-            } else if (get_sample_rate_hz() <= 2000) {
+            if (get_loop_rate_hz() > 1000) {
-                _fifo_downsample_rate = 4;
+                loop_limit = 2;
-            } else {
-                _fifo_downsample_rate = 2;
             }
-            // calculate rate we will be giving samples to the backend
+            if (get_loop_rate_hz() > 2000) {
-            _backend_rate_hz *= (8 / _fifo_downsample_rate);
+                loop_limit = 4;
+            }
+            // constrain the gyro rate to be a 2^N multiple
+            uint8_t fast_sampling_rate = constrain_int16(get_fast_sampling_rate(), loop_limit, 4);
+
+            // calculate rate we will be giving gyro samples to the backend
+            _fifo_downsample_rate = 8 / fast_sampling_rate;
+            _backend_rate_hz *= fast_sampling_rate;
 
             // for logging purposes set the oversamping rate
             _set_accel_oversampling(_accel_instance, _fifo_downsample_rate/2);

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h

@@ -56,6 +56,9 @@ public:
 
     void start() override;
 
+    // get a startup banner to output to the GCS
+    bool get_output_banner(char* banner, uint8_t banner_len) override;
+
     enum Invensense_Type {
         Invensense_MPU6000=0,
         Invensense_MPU6500,

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp

@@ -207,11 +207,6 @@ void AP_InertialSensor_Invensensev2::start()
     // readings to fit them into an int16_t:
     _set_raw_sample_accel_multiplier(_accel_instance, multiplier_accel);
 
-    if (_fast_sampling) {
-        hal.console->printf("INV2[%u]: enabled fast sampling rate %uHz/%uHz\n",
-                            _accel_instance, _backend_rate_hz*_fifo_downsample_rate, _backend_rate_hz);
-    }
-    
     // set sample rate to 1.125KHz
     _register_write(INV2REG_GYRO_SMPLRT_DIV, 0, true);
     hal.scheduler->delay(1);
@@ -243,6 +238,15 @@ void AP_InertialSensor_Invensensev2::start()
     _dev->register_periodic_callback(1265625UL / _backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev2::_poll_data, void));
 }
 
+// get a startup banner to output to the GCS
+bool AP_InertialSensor_Invensensev2::get_output_banner(char* banner, uint8_t banner_len) {
+    if (_fast_sampling) {
+        snprintf(banner, banner_len, "IMU%u: fast sampling enabled %.1fkHz/%.1fkHz",
+            _accel_instance, _backend_rate_hz*_fifo_downsample_rate / 1000.0, _backend_rate_hz/ 1000.0);
+        return true;
+    }
+    return false;
+}
 
 /*
   publish any pending data
@@ -402,10 +406,10 @@ bool AP_InertialSensor_Invensensev2::_accumulate_sensor_rate_sampling(uint8_t *s
             _accum.gyro *= _fifo_gyro_scale;
             _rotate_and_correct_accel(_accel_instance, _accum.accel);
             _rotate_and_correct_gyro(_gyro_instance, _accum.gyro);
-            
+
             _notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false);
             _notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro);
-            
+
             _accum.accel.zero();
             _accum.gyro.zero();
             _accum.count = 0;
@@ -580,15 +584,20 @@ void AP_InertialSensor_Invensensev2::_set_filter_and_scaling(void)
     if (enable_fast_sampling(_accel_instance)) {
         _fast_sampling = _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
         if (_fast_sampling) {
-            if (get_sample_rate_hz() <= 1125) {
+            // constrain the gyro rate to be at least the loop rate
-                _fifo_downsample_rate = 8;
+            uint8_t loop_limit = 1;
-            } else if (get_sample_rate_hz() <= 2250) {
+            if (get_loop_rate_hz() > 1125) {
-                _fifo_downsample_rate = 4;
+                loop_limit = 2;
-            } else {
-                _fifo_downsample_rate = 2;
             }
-            // calculate rate we will be giving samples to the backend
+            if (get_loop_rate_hz() > 2250) {
-            _backend_rate_hz *= (8 / _fifo_downsample_rate);
+                loop_limit = 4;
+            }
+            // constrain the gyro rate to be a 2^N multiple
+            uint8_t fast_sampling_rate = constrain_int16(get_fast_sampling_rate(), loop_limit, 8);
+
+            // calculate rate we will be giving gyro samples to the backend
+            _fifo_downsample_rate = 8 / fast_sampling_rate;
+            _backend_rate_hz *= fast_sampling_rate;
 
             // for logging purposes set the oversamping rate
             _set_accel_oversampling(_accel_instance, _fifo_downsample_rate/2);

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h

@@ -52,6 +52,9 @@ public:
 
     void start() override;
 
+    // get a startup banner to output to the GCS
+    bool get_output_banner(char* banner, uint8_t banner_len) override;
+
     enum Invensensev2_Type {
         Invensensev2_ICM20948 = 0,
         Invensensev2_ICM20648,