AP_InertialSensor: estimate and log sensor rates for all IMUs

this adds IMU.GHz and IMU.AHz log fields so we can see the actual
observed sensor rates of each IMU

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -512,6 +512,7 @@ uint8_t AP_InertialSensor::register_gyro(uint16_t raw_sample_rate_hz,
     }
 
     _gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz;
+    _gyro_over_sampling[_gyro_count] = 1;
 
     bool saved = _gyro_id[_gyro_count].load();
 
@@ -544,6 +545,8 @@ uint8_t AP_InertialSensor::register_accel(uint16_t raw_sample_rate_hz,
     }
 
     _accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz;
+    _accel_over_sampling[_accel_count] = 1;
+
     bool saved = _accel_id[_accel_count].load();
 
     if (!saved) {

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -133,6 +133,10 @@ public:
     bool accel_calibrated_ok_all() const;
     bool use_accel(uint8_t instance) const;
 
+    // get observed sensor rates, including any internal sampling multiplier
+    uint16_t get_gyro_rate_hz(uint8_t instance) const { return uint16_t(_gyro_raw_sample_rates[instance] * _gyro_over_sampling[instance]); }
+    uint16_t get_accel_rate_hz(uint8_t instance) const { return uint16_t(_accel_raw_sample_rates[instance] * _accel_over_sampling[instance]); }
+    
     // get accel offsets in m/s/s
     const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; }
     const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_primary_accel); }
@@ -345,6 +349,10 @@ private:
     float  _accel_raw_sample_rates[INS_MAX_INSTANCES];
     float  _gyro_raw_sample_rates[INS_MAX_INSTANCES];
 
+    // how many sensors samples per notify to the backend
+    uint8_t _accel_over_sampling[INS_MAX_INSTANCES];
+    uint8_t _gyro_over_sampling[INS_MAX_INSTANCES];
+
     // last sample time in microseconds. Use for deltaT calculations
     // on non-FIFO sensors
     uint64_t _accel_last_sample_us[INS_MAX_INSTANCES];

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -33,6 +33,18 @@ void AP_InertialSensor_Backend::notify_gyro_fifo_reset(uint8_t instance)
     _imu._sample_gyro_start_us[instance] = 0;
 }
 
+// set the amount of oversamping a accel is doing
+void AP_InertialSensor_Backend::_set_accel_oversampling(uint8_t instance, uint8_t n)
+{
+    _imu._accel_over_sampling[instance] = n;
+}
+
+// set the amount of oversamping a gyro is doing
+void AP_InertialSensor_Backend::_set_gyro_oversampling(uint8_t instance, uint8_t n)
+{
+    _imu._gyro_over_sampling[instance] = n;
+}
+
 /*
   update the sensor rate for FIFO sensors
 
@@ -116,6 +128,9 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
 {
     float dt;
 
+    _update_sensor_rate(_imu._sample_gyro_count[instance], _imu._sample_gyro_start_us[instance],
+                        _imu._gyro_raw_sample_rates[instance]);
+
     /*
       we have two classes of sensors. FIFO based sensors produce data
       at a very predictable overall rate, but the data comes in
@@ -127,9 +142,6 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
     if (sample_us != 0 && _imu._gyro_last_sample_us[instance] != 0) {
         dt = (sample_us - _imu._gyro_last_sample_us[instance]) * 1.0e-6;
     } else {
-        _update_sensor_rate(_imu._sample_gyro_count[instance], _imu._sample_gyro_start_us[instance],
-                            _imu._gyro_raw_sample_rates[instance]);
-
         // don't accept below 100Hz
         if (_imu._gyro_raw_sample_rates[instance] < 100) {
             return;
@@ -234,6 +246,9 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
 {
     float dt;
 
+    _update_sensor_rate(_imu._sample_accel_count[instance], _imu._sample_accel_start_us[instance],
+                        _imu._accel_raw_sample_rates[instance]);
+
     /*
       we have two classes of sensors. FIFO based sensors produce data
       at a very predictable overall rate, but the data comes in
@@ -245,9 +260,6 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
     if (sample_us != 0 && _imu._accel_last_sample_us[instance] != 0) {
         dt = (sample_us - _imu._accel_last_sample_us[instance]) * 1.0e-6;
     } else {
-        _update_sensor_rate(_imu._sample_accel_count[instance], _imu._sample_accel_start_us[instance],
-                            _imu._accel_raw_sample_rates[instance]);
-
         // don't accept below 100Hz
         if (_imu._accel_raw_sample_rates[instance] < 100) {
             return;

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -125,6 +125,12 @@ protected:
     // sensors, and should be set to zero for FIFO based sensors
     void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0, bool fsync_set=false);
 
+    // set the amount of oversamping a accel is doing
+    void _set_accel_oversampling(uint8_t instance, uint8_t n);
+
+    // set the amount of oversamping a gyro is doing
+    void _set_gyro_oversampling(uint8_t instance, uint8_t n);
+    
     // update the sensor rate for FIFO sensors
     void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz);
     

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp

@@ -810,6 +810,10 @@ void AP_InertialSensor_Invensense::_set_filter_register(void)
         _fast_sampling = (_mpu_type != Invensense_MPU6000 && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI);
         if (_fast_sampling) {
             hal.console->printf("MPU[%u]: enabled fast sampling\n", _accel_instance);
+
+            // for logging purposes set the oversamping rate
+            _set_accel_oversampling(_accel_instance, MPU_FIFO_DOWNSAMPLE_COUNT/2);
+            _set_gyro_oversampling(_accel_instance, MPU_FIFO_DOWNSAMPLE_COUNT);
         }
     }