AP_InertialSensor: periodically notify of primary gyro and accels

ArduPilot · Dec 11, 2024 · bf8f376 · bf8f376
1 parent 5896d47
commit bf8f376
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 2 deletions.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -18,6 +18,8 @@
 #define AP_HEATER_IMU_INSTANCE 0
 #endif
 
+#define PRIMARY_UPDATE_TIMEOUT_US 200000UL    // continue to notify the primary at 5Hz
+
 const extern AP_HAL::HAL& hal;
 
 AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
@@ -804,9 +806,12 @@ void AP_InertialSensor_Backend::update_primary_gyro()
     // timing changes need to be made in the bus thread in order to take effect which is
     // why they are actioned here
     const bool is_new_primary = gyro_instance == _imu._primary_gyro;
-    if (is_primary_gyro != is_new_primary) {
+    uint32_t now_us = AP_HAL::micros();
+    if (is_primary_gyro != is_new_primary
+        || AP_HAL::timeout_expired(last_primary_gyro_update_us, now_us, PRIMARY_UPDATE_TIMEOUT_US)) {
         set_primary_gyro(is_new_primary);
         is_primary_gyro = is_new_primary;
+        last_primary_gyro_update_us = now_us;
     }
 }
 
@@ -858,9 +863,12 @@ void AP_InertialSensor_Backend::update_primary_accel()
     // timing changes need to be made in the bus thread in order to take effect which is
     // why they are actioned here
     const bool is_new_primary = accel_instance == _imu._primary_accel;
-    if (is_primary_accel != is_new_primary) {
+    uint32_t now_us = AP_HAL::micros();
+    if (is_primary_accel != is_new_primary
+        || AP_HAL::timeout_expired(last_primary_accel_update_us, now_us, PRIMARY_UPDATE_TIMEOUT_US)) {
         set_primary_accel(is_new_primary);
         is_primary_accel = is_new_primary;
+        last_primary_accel_update_us = now_us;
     }
 }
 

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -161,8 +161,10 @@ class AP_InertialSensor_Backend
     // instance numbers of accel and gyro data
     uint8_t gyro_instance;
     bool is_primary_gyro = true;
+    uint32_t last_primary_gyro_update_us;
     uint8_t accel_instance;
     bool is_primary_accel = true;
+    uint32_t last_primary_accel_update_us;
 
     void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel) __RAMFUNC__;
     void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) __RAMFUNC__;