From fcbaa0969e4fde997801c3ec744658459672729f Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 24 Aug 2022 21:24:58 +0200
Subject: [PATCH] AP_InertialSensor: synchronize BMI70 on fifo reads

---
 .../AP_InertialSensor_BMI270.cpp                 | 16 ++++++++--------
 .../AP_InertialSensor/AP_InertialSensor_BMI270.h |  7 ++-----
 2 files changed, 10 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp
index a2489affab..79655c278b 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp
@@ -143,6 +143,8 @@ const uint8_t AP_InertialSensor_BMI270::maximum_fifo_config_file[] = { BMI270_RE
 
 #define BMI270_HARDWARE_INIT_MAX_TRIES 5
 
+const uint32_t BACKEND_PERIOD_US = 1000000UL / BMI270_BACKEND_SAMPLE_RATE;
+
 extern const AP_HAL::HAL& hal;
 
 AP_InertialSensor_BMI270::AP_InertialSensor_BMI270(AP_InertialSensor &imu,
@@ -219,9 +221,8 @@ void AP_InertialSensor_BMI270::start()
     set_gyro_orientation(_gyro_instance, _rotation);
     set_accel_orientation(_accel_instance, _rotation);
 
-    /* Call _poll_data() at 1600Hz */
-    _dev->register_periodic_callback(1000000UL / BMI270_BACKEND_SAMPLE_RATE,
-        FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI270::poll_data, void));
+    /* Call read_fifo() at 1600Hz */
+    periodic_handle = _dev->register_periodic_callback(BACKEND_PERIOD_US, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI270::read_fifo, void));
 }
 
 bool AP_InertialSensor_BMI270::update()
@@ -407,6 +408,10 @@ void AP_InertialSensor_BMI270::read_fifo(void)
         return;
     }
 
+    // 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);
+
     const uint8_t *p = &data[0];
     while (fifo_length >= 12) {
         /*
@@ -501,11 +506,6 @@ void AP_InertialSensor_BMI270::parse_gyro_frame(const uint8_t* d)
     _notify_new_gyro_raw_sample(_gyro_instance, gyro);
 }
 
-void AP_InertialSensor_BMI270::poll_data()
-{
-    read_fifo();
-}
-
 bool AP_InertialSensor_BMI270::hardware_init()
 {
     bool init = false;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h
index bc7eb88fb1..fb21ca0f57 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h
@@ -103,11 +103,6 @@ private:
      */
     void configure_fifo();
 
-    /**
-     * Device periodic callback to read data from the sensors.
-     */
-    void poll_data();
-
     /**
      * Read samples from fifo.
      */
@@ -116,6 +111,8 @@ private:
     void parse_gyro_frame(const uint8_t* d);
 
     AP_HAL::OwnPtr<AP_HAL::Device> _dev;
+    AP_HAL::Device::PeriodicHandle periodic_handle;
+
     enum Rotation _rotation;
 
     uint8_t _accel_instance;