AP_InertialSensor: separate handling of FIFO and non-FIFO sensors

FIFO sensors produce data at a well known rate, but samples come in
bunches, so we can't use the system clock to calculate deltaT.

non-FIFO sensors produce data when we sample them, but that rate is
less regular due to timing jitter.

For FIFO sensors this changes makes us use a learned sample rate,
which allows for different clock speeds on sensor and system board.

For non-FIFO sensors we use the system clock to measure deltaT

the overall effect is a fix for sensors that produce samples at other
than the claimed datasheet rate.

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -342,9 +342,20 @@ private:
     float _accel_max_abs_offsets[INS_MAX_INSTANCES];
 
     // accelerometer and gyro raw sample rate in units of Hz
-    uint16_t _accel_raw_sample_rates[INS_MAX_INSTANCES];
+    float  _accel_raw_sample_rates[INS_MAX_INSTANCES];
-    uint16_t _gyro_raw_sample_rates[INS_MAX_INSTANCES];
+    float  _gyro_raw_sample_rates[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];
+    uint64_t _gyro_last_sample_us[INS_MAX_INSTANCES];
+
+    // sample times for checking real sensor rate for FIFO sensors
+    uint16_t _sample_accel_count[INS_MAX_INSTANCES];
+    uint32_t _sample_accel_start_us[INS_MAX_INSTANCES];
+    uint16_t _sample_gyro_count[INS_MAX_INSTANCES];
+    uint32_t _sample_gyro_start_us[INS_MAX_INSTANCES];
+    
     // temperatures for an instance if available
     float _temperature[INS_MAX_INSTANCES];
 

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -3,6 +3,9 @@
 #include "AP_InertialSensor_Backend.h"
 #include <DataFlash/DataFlash.h>
 #include <AP_Module/AP_Module.h>
+#include <stdio.h>
+
+#define SENSOR_RATE_DEBUG 0
 
 const extern AP_HAL::HAL& hal;
 
@@ -12,6 +15,52 @@ AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
     _sem = hal.util->new_semaphore();
 }
 
+/*
+  notify of a FIFO reset so we don't use bad data to update observed sensor rate
+ */
+void AP_InertialSensor_Backend::notify_accel_fifo_reset(uint8_t instance)
+{
+    _imu._sample_accel_count[instance] = 0;
+    _imu._sample_accel_start_us[instance] = 0;    
+}
+
+/*
+  notify of a FIFO reset so we don't use bad data to update observed sensor rate
+ */
+void AP_InertialSensor_Backend::notify_gyro_fifo_reset(uint8_t instance)
+{
+    _imu._sample_gyro_count[instance] = 0;
+    _imu._sample_gyro_start_us[instance] = 0;
+}
+
+/*
+  update the sensor rate for FIFO sensors
+
+  FIFO sensors produce samples at a fixed rate, but the clock in the
+  sensor may vary slightly from the system clock. This slowly adjusts
+  the rate to the observed rate
+*/
+void AP_InertialSensor_Backend::_update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz)
+{
+    uint32_t now = AP_HAL::micros();
+    if (start_us == 0) {
+        count = 0;
+        start_us = now;
+    } else {
+        count++;
+        if (now - start_us > 1000000UL) {
+            float observed_rate_hz = count * 1.0e6 / (now - start_us);
+#if SENSOR_RATE_DEBUG
+            printf("RATE: %.1f should be %.1f\n", observed_rate_hz, rate_hz);
+#endif
+            observed_rate_hz = constrain_float(observed_rate_hz, rate_hz*0.95, rate_hz*1.05);
+            rate_hz = 0.90 * rate_hz + 0.1 * observed_rate_hz;
+            count = 0;
+            start_us = now;
+        }
+    }
+}
+
 void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vector3f &accel) 
 {
     /*
@@ -55,10 +104,6 @@ void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &
     _imu._gyro[instance] = gyro;
     _imu._gyro_healthy[instance] = true;
 
-    if (_imu._gyro_raw_sample_rates[instance] <= 0) {
-        return;
-    }
-
     // publish delta angle
     _imu._delta_angle[instance] = _imu._delta_angle_acc[instance];
     _imu._delta_angle_dt[instance] = _imu._delta_angle_acc_dt[instance];
@@ -71,11 +116,28 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
 {
     float dt;
 
-    if (_imu._gyro_raw_sample_rates[instance] <= 0) {
+    /*
-        return;
+      we have two classes of sensors. FIFO based sensors produce data
-    }
+      at a very predictable overall rate, but the data comes in
+      bunches, so we use the provided sample rate for deltaT. Non-FIFO
+      sensors don't bunch up samples, but also tend to vary in actual
+      rate, so we use the provided sample_us to get the deltaT. The
+      difference between the two is whether sample_us is provided.
+     */
+    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]);
 
-    dt = 1.0f / _imu._gyro_raw_sample_rates[instance];
+        // don't accept below 100Hz
+        if (_imu._gyro_raw_sample_rates[instance] < 100) {
+            return;
+        }
+
+        dt = 1.0f / _imu._gyro_raw_sample_rates[instance];
+    }
+    _imu._gyro_last_sample_us[instance] = sample_us;
 
     // call gyro_sample hook if any
     AP_Module::call_hook_gyro_sample(instance, dt, gyro);
@@ -140,10 +202,6 @@ void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f
     _imu._accel[instance] = accel;
     _imu._accel_healthy[instance] = true;
 
-    if (_imu._accel_raw_sample_rates[instance] <= 0) {
-        return;
-    }
-
     // publish delta velocity
     _imu._delta_velocity[instance] = _imu._delta_velocity_acc[instance];
     _imu._delta_velocity_dt[instance] = _imu._delta_velocity_acc_dt[instance];
@@ -176,13 +234,30 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
 {
     float dt;
 
-    if (_imu._accel_raw_sample_rates[instance] <= 0) {
+    /*
-        return;
+      we have two classes of sensors. FIFO based sensors produce data
+      at a very predictable overall rate, but the data comes in
+      bunches, so we use the provided sample rate for deltaT. Non-FIFO
+      sensors don't bunch up samples, but also tend to vary in actual
+      rate, so we use the provided sample_us to get the deltaT. The
+      difference between the two is whether sample_us is provided.
+     */
+    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;
+        }
+
+        dt = 1.0f / _imu._accel_raw_sample_rates[instance];
     }
+    _imu._accel_last_sample_us[instance] = sample_us;
 
-    dt = 1.0f / _imu._accel_raw_sample_rates[instance];
+    // call accel_sample hook if any
-
-    // call gyro_sample hook if any
     AP_Module::call_hook_accel_sample(instance, dt, accel, fsync_set);
     
     _imu.calc_vibration_and_clipping(instance, accel, dt);

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -70,6 +70,9 @@ public:
      */
     int16_t get_id() const { return _id; }
 
+    // notify of a fifo reset
+    void notify_fifo_reset(void);
+    
     /*
       device driver IDs. These are used to fill in the devtype field
       of the device ID, which shows up as INS*ID* parameters to
@@ -103,10 +106,12 @@ protected:
     // rotate gyro vector, offset and publish
     void _publish_gyro(uint8_t instance, const Vector3f &gyro);
 
-    // this should be called every time a new gyro raw sample is available -
+    // this should be called every time a new gyro raw sample is
-    // be it published or not
+    // available - be it published or not the sample is raw in the
-    // the sample is raw in the sense that it's not filtered yet, but it must
+    // sense that it's not filtered yet, but it must be rotated and
-    // be rotated and corrected (_rotate_and_correct_gyro)
+    // corrected (_rotate_and_correct_gyro)
+    // The sample_us value must be provided for non-FIFO based
+    // sensors, and should be set to zero for FIFO based sensors
     void _notify_new_gyro_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0);
 
     // rotate accel vector, scale, offset and publish
@@ -116,8 +121,13 @@ protected:
     // be it published or not
     // the sample is raw in the sense that it's not filtered yet, but it must
     // be rotated and corrected (_rotate_and_correct_accel)
+    // The sample_us value must be provided for non-FIFO based
+    // 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);
 
+    // update the sensor rate for FIFO sensors
+    void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz);
+    
     // set accelerometer max absolute offset for calibration
     void _set_accel_max_abs_offset(uint8_t instance, float offset);
 
@@ -191,6 +201,12 @@ protected:
     bool enable_fast_sampling(uint8_t instance) {
         return (_imu._fast_sampling_mask & (1U<<instance)) != 0;
     }
+
+    /*
+      notify of a FIFO reset so we don't use bad data to update observed sensor rate
+    */
+    void notify_accel_fifo_reset(uint8_t instance);
+    void notify_gyro_fifo_reset(uint8_t instance);
     
     // 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_Invensense.cpp

@@ -367,6 +367,9 @@ void AP_InertialSensor_Invensense::_fifo_reset()
     hal.scheduler->delay_microseconds(1);
     _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
     _last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN;
+
+    notify_accel_fifo_reset(_accel_instance);
+    notify_gyro_fifo_reset(_gyro_instance);
 }
 
 bool AP_InertialSensor_Invensense::_has_auxiliary_bus()
@@ -587,7 +590,7 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl
         _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(), fsync_set);
+        _notify_new_accel_raw_sample(_accel_instance, accel, 0, fsync_set);
         _notify_new_gyro_raw_sample(_gyro_instance, gyro);
 
         _temp_filtered = _temp_filter.apply(temp);
@@ -653,7 +656,7 @@ bool AP_InertialSensor_Invensense::_accumulate_fast_sampling(uint8_t *samples, u
             _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, AP_HAL::micros64(), false);
+            _notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false);
             _notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro);
             
             _accum.accel.zero();

libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp

@@ -744,7 +744,7 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a()
     accel_data *= _accel_scale;
 
     _rotate_and_correct_accel(_accel_instance, accel_data);
-    _notify_new_accel_raw_sample(_accel_instance, accel_data);
+    _notify_new_accel_raw_sample(_accel_instance, accel_data, AP_HAL::micros64());
 }
 
 /*
@@ -765,7 +765,7 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_g()
     gyro_data *= _gyro_scale;
 
     _rotate_and_correct_gyro(_gyro_instance, gyro_data);
-    _notify_new_gyro_raw_sample(_gyro_instance, gyro_data);
+    _notify_new_gyro_raw_sample(_gyro_instance, gyro_data, AP_HAL::micros64());
 }
 
 bool AP_InertialSensor_LSM9DS0::update()