AP_InertialSensor: have a single callback for primary switching

ArduPilot · Dec 19, 2024 · 100dbec · 100dbec
1 parent bf8f376
commit 100dbec
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 35 deletions.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -361,7 +361,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
 
     // 5us
     log_gyro_raw(instance, sample_us, gyro, _imu._gyro_filtered[instance]);
-    update_primary_gyro();
+    update_primary();
 }
 
 /*
@@ -449,7 +449,7 @@ void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const
     }
 
     log_gyro_raw(instance, sample_us, gyro, _imu._gyro_filtered[instance]);
-    update_primary_gyro();
+    update_primary();
 }
 
 void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &raw_gyro, const Vector3f &filtered_gyro)
@@ -607,7 +607,6 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
     // assume we're doing pre-filter logging:
     log_accel_raw(instance, sample_us, accel);
 #endif
-    update_primary_accel();
 }
 
 /*
@@ -685,7 +684,6 @@ void AP_InertialSensor_Backend::_notify_new_delta_velocity(uint8_t instance, con
     // assume we're doing pre-filter logging
     log_accel_raw(instance, sample_us, accel);
 #endif
-    update_primary_accel();
 }
 
 
@@ -801,17 +799,19 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
     update_gyro_filters(instance);
 }
 
-void AP_InertialSensor_Backend::update_primary_gyro()
+void AP_InertialSensor_Backend::update_primary()
 {
     // timing changes need to be made in the bus thread in order to take effect which is
-    // why they are actioned here
+    // why they are actioned here. Currently the _primary_gyro and _primary_accel can never
+    // be different for a particular IMU
     const bool is_new_primary = gyro_instance == _imu._primary_gyro;
     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)) {
+    if (is_primary != is_new_primary
+        || AP_HAL::timeout_expired(last_primary_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;
+        set_primary_accel(is_new_primary);
+        is_primary = is_new_primary;
+        last_primary_update_us = now_us;
     }
 }
 
@@ -858,20 +858,6 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance) /* front end */
     update_accel_filters(instance);
 }
 
-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;
-    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;
-    }
-}
-
 /*
   propagate filter changes from front end to backend
  */

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -160,11 +160,9 @@ 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;
+    bool is_primary = true;
+    uint32_t last_primary_update_us;
 
     void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel) __RAMFUNC__;
     void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) __RAMFUNC__;
@@ -297,10 +295,9 @@ class AP_InertialSensor_Backend
     void update_accel_filters(uint8_t instance) __RAMFUNC__; /* front end */
 
     // catch updates to the primary gyro and accel
-    void update_primary_gyro() __RAMFUNC__; /* backend */
-    void update_primary_accel() __RAMFUNC__; /* backend */
-    virtual void set_primary_gyro(bool is_primary) {}
-    virtual void set_primary_accel(bool is_primary) {}
+    void update_primary() __RAMFUNC__; /* backend */
+    virtual void set_primary_gyro(bool is_primary_gyro) {}
+    virtual void set_primary_accel(bool is_primary_accel) {}
 
     // support for updating filter at runtime
     uint16_t _last_accel_filter_hz;

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
@@ -406,11 +406,11 @@ bool AP_InertialSensor_Invensensev3::update()
     return true;
 }
 
-void AP_InertialSensor_Invensensev3::set_primary_gyro(bool is_primary)
+void AP_InertialSensor_Invensensev3::set_primary_gyro(bool _is_primary)
 {
 #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
     if (_imu.is_dynamic_fifo_enabled(gyro_instance)) {
-        if (is_primary) {
+        if (_is_primary) {
             dev->adjust_periodic_callback(periodic_handle, backend_period_us);
         } else {
             dev->adjust_periodic_callback(periodic_handle, backend_period_us * get_fast_sampling_rate());
@@ -575,7 +575,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
-    if (is_primary_gyro) {
+    if (is_primary) {
         dev->adjust_periodic_callback(periodic_handle, backend_period_us);
     }