From 29655d149c6ebba115a1c99d2f5984b31a5c7a06 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Thu, 23 Jan 2025 11:36:33 +0000
Subject: [PATCH] AP_InertialSensor: remove changes to primary accel since it
 is always the same as the gyro

---
 libraries/AP_InertialSensor/AP_InertialSensor.cpp | 15 ++++-----------
 libraries/AP_InertialSensor/AP_InertialSensor.h   | 10 ++++------
 .../AP_InertialSensor_Backend.cpp                 |  8 ++++----
 .../AP_InertialSensor_Logging.cpp                 |  2 +-
 4 files changed, 13 insertions(+), 22 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index b1a5d6d866e9e..25ff1dcc0fcc0 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -1891,14 +1891,10 @@ void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool conv
 }
 #endif
 
-// notify IMUs of the new primaries
-void AP_InertialSensor::set_primary_gyro(uint8_t instance)
+// notify IMUs of the new primary
+void AP_InertialSensor::set_primary(uint8_t instance)
 {
-    _primary_gyro = instance;
-}
-void AP_InertialSensor::set_primary_accel(uint8_t instance)
-{
-    _primary_accel = instance;
+    _primary = instance;
 }
 
 /*
@@ -1974,7 +1970,7 @@ void AP_InertialSensor::update(void)
             if (_gyro_healthy[i] && _use(i)) {
                 _first_usable_gyro = i;
 #if !AP_AHRS_ENABLED
-                _primary_gyro = _first_usable_gyro;
+                _primary = _first_usable_gyro;
 #endif
                 break;
             }
@@ -1982,9 +1978,6 @@ void AP_InertialSensor::update(void)
         for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
             if (_accel_healthy[i] && _use(i)) {
                 _first_usable_accel = i;
-#if !AP_AHRS_ENABLED
-                _primary_accel = _first_usable_accel;
-#endif
                 break;
             }
         }
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index be4bc5ce4d6cd..056e283f9e051 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -283,9 +283,8 @@ class AP_InertialSensor : AP_AccelCal_Client
     // Returns newly calculated trim values if calculated
     bool get_new_trim(Vector3f &trim_rad);
 
-    // notify IMUs of the new primaries
-    void set_primary_gyro(uint8_t instance);
-    void set_primary_accel(uint8_t instance);
+    // notify IMUs of the new primary
+    void set_primary(uint8_t instance);
 
 #if HAL_INS_ACCELCAL_ENABLED
     // initialise and register accel calibrator
@@ -667,9 +666,8 @@ class AP_InertialSensor : AP_AccelCal_Client
     uint8_t _first_usable_gyro;
     uint8_t _first_usable_accel;
 
-    // primary accel and gyro
-    uint8_t _primary_gyro;
-    uint8_t _primary_accel;
+    // primary instance
+    uint8_t _primary;
 
     // mask of accels and gyros which we will be actively using
     // and this should wait for in wait_for_sample()
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index 01ab1528b79fc..0a202cf3edc43 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -235,7 +235,7 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
         // currently active IMU we reset the inactive notch filters so
         // that if we switch IMUs we're not left with old data
         if (!notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs) &&
-            instance != _imu._primary_gyro) {
+            instance != _imu._primary) {
             inactive = true;
         }
         if (inactive) {
@@ -471,7 +471,7 @@ void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sa
     }
 
 #if AP_AHRS_ENABLED
-    const bool log_because_primary_gyro = _imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::PRIMARY_GYRO_ONLY) && (instance == AP::ahrs().get_primary_gyro_index());
+    const bool log_because_primary_gyro = _imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::PRIMARY_GYRO_ONLY) && (instance == _imu._primary);
 #else
     const bool log_because_primary_gyro = false;
 #endif
@@ -811,9 +811,9 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
 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. Currently the _primary_gyro and _primary_accel can never
+    // 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);
+    const bool is_new_primary = (gyro_instance == _imu._primary);
     uint32_t now_us = AP_HAL::micros();
     if (is_primary != is_new_primary
         || AP_HAL::timeout_expired(last_primary_update_us, now_us, PRIMARY_UPDATE_TIMEOUT_US)) {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
index 4025ca1403e16..a86ad4ebe8d25 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
@@ -196,7 +196,7 @@ void AP_InertialSensor::write_notch_log_messages() const
 
         // ask the HarmonicNotchFilter object for primary gyro to
         // log the actual notch centers
-        notch.filter[_primary_gyro].log_notch_centers(i, now_us);
+        notch.filter[_primary].log_notch_centers(i, now_us);
     }
 }
 #endif  // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED