AP_InertialSensor: adjust read rate of Invensense v2 and v3 depending…

… on primary
andyp1per · Sep 5, 2024 · c477a2b · c477a2b
1 parent 035f420
commit c477a2b
Show file tree

Hide file tree

Showing 4 changed files with 31 additions and 4 deletions.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
@@ -61,7 +61,8 @@ static const float GYRO_SCALE = (0.0174532f / 16.4f);
 #include "AP_InertialSensor_Invensensev2_registers.h"
 
 #define INV2_SAMPLE_SIZE 14
-#define INV2_FIFO_BUFFER_LEN 8
+#define INV2_FIFO_BUFFERS 4
+#define INV2_FIFO_BUFFER_LEN (8 * INV2_FIFO_BUFFERS)
 
 #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
 #define uint16_val(v, idx)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])
@@ -252,7 +253,7 @@ void AP_InertialSensor_Invensensev2::start()
     }
 
     // start the timer process to read samples
-    _dev->register_periodic_callback(1265625UL / _gyro_backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev2::_poll_data, void));
+    periodic_handle = _dev->register_periodic_callback(1265625UL / _gyro_backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev2::_poll_data, void));
 }
 
 // get a startup banner to output to the GCS
@@ -278,6 +279,15 @@ bool AP_InertialSensor_Invensensev2::update()
     return true;
 }
 
+void AP_InertialSensor_Invensensev2::set_primary_gyro(bool is_primary)
+{
+    if (is_primary) {
+        _dev->adjust_periodic_callback(periodic_handle, 1265625UL / _gyro_backend_rate_hz);
+    } else {
+        _dev->adjust_periodic_callback(periodic_handle, (1265625UL / _gyro_backend_rate_hz) * INV2_FIFO_BUFFERS);
+    }
+}
+
 /*
   accumulate new samples
  */

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h
@@ -66,6 +66,9 @@ class AP_InertialSensor_Invensensev2 : public AP_InertialSensor_Backend
     // 16G
     const uint16_t multiplier_accel = INT16_MAX/(26*GRAVITY_MSS);
 
+protected:
+    void set_primary_gyro(bool is_primary) override;
+
 private:
     AP_InertialSensor_Invensensev2(AP_InertialSensor &imu,
                               AP_HAL::OwnPtr<AP_HAL::Device> dev,
@@ -116,6 +119,7 @@ class AP_InertialSensor_Invensensev2 : public AP_InertialSensor_Backend
 
     AP_HAL::DigitalSource *_drdy_pin;
     AP_HAL::OwnPtr<AP_HAL::Device> _dev;
+    AP_HAL::Device::PeriodicHandle periodic_handle;
     AP_Invensensev2_AuxiliaryBus *_auxiliary_bus;
 
     // which sensor type this is

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
@@ -169,7 +169,8 @@ struct PACKED FIFODataHighRes {
 static_assert(sizeof(FIFOData) == 16, "FIFOData must be 16 bytes");
 static_assert(sizeof(FIFODataHighRes) == 20, "FIFODataHighRes must be 20 bytes");
 
-#define INV3_FIFO_BUFFER_LEN 8
+#define INV3_FIFO_BUFFERS 4
+#define INV3_FIFO_BUFFER_LEN (8 * INV3_FIFO_BUFFERS)
 
 AP_InertialSensor_Invensensev3::AP_InertialSensor_Invensensev3(AP_InertialSensor &imu,
                                                                AP_HAL::OwnPtr<AP_HAL::Device> _dev,
@@ -397,6 +398,15 @@ bool AP_InertialSensor_Invensensev3::update()
     return true;
 }
 
+void AP_InertialSensor_Invensensev3::set_primary_gyro(bool is_primary)
+{
+    if (is_primary) {
+        dev->adjust_periodic_callback(periodic_handle, backend_period_us);
+    } else {
+        dev->adjust_periodic_callback(periodic_handle, backend_period_us * INV3_FIFO_BUFFERS);
+    }
+}
+
 /*
   accumulate new samples
  */
@@ -551,7 +561,7 @@ void AP_InertialSensor_Invensensev3::read_fifo()
 
     // 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);
+    //dev->adjust_periodic_callback(periodic_handle, backend_period_us);
 
     while (n_samples > 0) {
         uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN);

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
@@ -44,6 +44,9 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend
     // acclerometers on Invensense sensors will return values up to 32G
     const uint16_t multiplier_accel = INT16_MAX/(32*GRAVITY_MSS);
 
+protected:
+    void set_primary_gyro(bool is_primary) override;
+
 private:
     AP_InertialSensor_Invensensev3(AP_InertialSensor &imu,
                                    AP_HAL::OwnPtr<AP_HAL::Device> dev,