From 866c7a7d54b84f989519bef249b67c440340649e Mon Sep 17 00:00:00 2001
From: James O'Shannessy <12959316+joshanne@users.noreply.github.com>
Date: Wed, 17 Jul 2024 17:33:14 +1000
Subject: [PATCH] AP_InertialSensor: Check the gyro/accel id has not been
 previously registered

If the Gyro/Accel ID is already in the registered list, do not try to add it again.

This stops an issue seen on a CubeOrangePlus BG3 where, during the very first boot after a parameter wipe, software incorrectly registers a fourth IMU.
The Fourth IMU is registered because the AUX IMU is the same DevID as the third ICM45686.
---
 .../AP_InertialSensor/AP_InertialSensor.cpp      | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index afb4df645c..b0bc11aaca 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -734,6 +734,14 @@ bool AP_InertialSensor::register_gyro(uint8_t &instance, uint16_t raw_sample_rat
         return false;
     }
 
+    // Loop over the existing instances and check if the instance already exists
+    for (uint8_t instance_to_check = 0; instance_to_check < _gyro_count; instance_to_check++) {
+        if ((uint32_t)_gyro_id(instance_to_check) == id) {
+            // if it does, then bail
+            return false;
+        }
+    }
+
     _gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz;
     _gyro_over_sampling[_gyro_count] = 1;
     _gyro_raw_sampling_multiplier[_gyro_count] = INT16_MAX/radians(2000);
@@ -794,6 +802,14 @@ bool AP_InertialSensor::register_accel(uint8_t &instance, uint16_t raw_sample_ra
         return false;
     }
 
+    // Loop over the existing instances and check if the instance already exists
+    for (uint8_t instance_to_check = 0; instance_to_check < _accel_count; instance_to_check++) {
+        if ((uint32_t)_accel_id(instance_to_check) == id) {
+            // if it does, then bail
+            return false;
+        }
+    }
+
     _accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz;
     _accel_over_sampling[_accel_count] = 1;
     _accel_raw_sampling_multiplier[_accel_count] = INT16_MAX/(16*GRAVITY_MSS);