AP_InertialSensor: synchronise fifo reads on the ICM42xxx

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -316,6 +316,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
         _imu._new_gyro_data[instance] = true;
     }
 
+    // 5us
     if (!_imu.batchsampler.doing_post_filter_logging()) {
         log_gyro_raw(instance, sample_us, gyro);
     }
@@ -543,6 +544,7 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
         _imu._new_accel_data[instance] = true;
     }
 
+    // 5us
     if (!_imu.batchsampler.doing_post_filter_logging()) {
         log_accel_raw(instance, sample_us, accel);
     } else {

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

@@ -119,7 +119,7 @@ struct PACKED FIFOData {
 };
 
 #define INV3_SAMPLE_SIZE sizeof(FIFOData)
-#define INV3_FIFO_BUFFER_LEN 8
+#define INV3_FIFO_BUFFER_LEN 2
 
 AP_InertialSensor_Invensensev3::AP_InertialSensor_Invensensev3(AP_InertialSensor &imu,
                                                                AP_HAL::OwnPtr<AP_HAL::Device> _dev,
@@ -230,6 +230,9 @@ void AP_InertialSensor_Invensensev3::start()
         set_filter_and_scaling();
     }
 
+    // pre-calculate backend period
+    backend_period_us = 1000000UL / backend_rate_hz;
+
     if (!_imu.register_gyro(gyro_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype)) ||
         !_imu.register_accel(accel_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype))) {
         return;
@@ -257,7 +260,7 @@ void AP_InertialSensor_Invensensev3::start()
     }
 
     // start the timer process to read samples, using the fastest rate avilable
-    dev->register_periodic_callback(1000000UL / backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev3::read_fifo, void));
+    periodic_handle = dev->register_periodic_callback(backend_period_us, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev3::read_fifo, void));
 }
 
 // get a startup banner to output to the GCS
@@ -315,9 +318,9 @@ bool AP_InertialSensor_Invensensev3::accumulate_samples(const FIFOData *data, ui
 
         accel *= accel_scale;
         gyro *= GYRO_SCALE;
-
         const float temp = d.temperature * temp_sensitivity + temp_zero;
 
+        // these four calls are about 40us
         _rotate_and_correct_accel(accel_instance, accel);
         _rotate_and_correct_gyro(gyro_instance, gyro);
 
@@ -336,9 +339,9 @@ void AP_InertialSensor_Invensensev3::read_fifo()
 {
     bool need_reset = false;
     uint16_t n_samples;
+
     const uint8_t reg_counth = (inv3_type == Invensensev3_Type::ICM42670)?INV3REG_70_FIFO_COUNTH:INV3REG_FIFO_COUNTH;
     const uint8_t reg_data   = (inv3_type == Invensensev3_Type::ICM42670)?INV3REG_70_FIFO_DATA:INV3REG_FIFO_DATA;
-
     if (!block_read(reg_counth, (uint8_t*)&n_samples, 2)) {
         goto check_registers;
     }
@@ -348,6 +351,10 @@ void AP_InertialSensor_Invensensev3::read_fifo()
         goto check_registers;
     }
 
+    // adjust the periodic callback to be synchronous with the incoming data
+    // this means that we rarely run read_fifo() without updating the sensor data
+    dev->adjust_periodic_callback(periodic_handle, backend_period_us);
+
     while (n_samples > 0) {
         uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN);
         if (!block_read(reg_data, (uint8_t*)fifo_buffer, n * INV3_SAMPLE_SIZE)) {

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h

@@ -88,8 +88,11 @@ private:
 
     // what rate are we generating samples into the backend for gyros and accels?
     uint16_t backend_rate_hz;
+    // pre-calculated backend period
+    uint32_t backend_period_us;
 
     AP_HAL::OwnPtr<AP_HAL::Device> dev;
+    AP_HAL::Device::PeriodicHandle periodic_handle;
 
     // which sensor type this is
     enum Invensensev3_Type inv3_type;