From 2a55e6a030f648925b5f705a9c236a7d471bb717 Mon Sep 17 00:00:00 2001
From: Hayden Donald <haydendonald@gmail.com>
Date: Thu, 9 Nov 2023 13:32:12 +1100
Subject: [PATCH] AP_Arming: Move accel/gyro_consistent to AP_IntertialSensor

Move the accel_consistent and gyro_consistent methods from AP_Arming to AP_IntertialSensor
---
 libraries/AP_Arming/AP_Arming.cpp | 66 ++++++-------------------------
 1 file changed, 11 insertions(+), 55 deletions(-)

diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp
index 0b6ae786a2..2a0a3e1fa0 100644
--- a/libraries/AP_Arming/AP_Arming.cpp
+++ b/libraries/AP_Arming/AP_Arming.cpp
@@ -352,44 +352,15 @@ bool AP_Arming::logging_checks(bool report)
 #if AP_INERTIALSENSOR_ENABLED
 bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
 {
-    const uint8_t accel_count = ins.get_accel_count();
-    if (accel_count <= 1) {
-        return true;
-    }
-
-    const Vector3f &prime_accel_vec = ins.get_accel();
     const uint32_t now = AP_HAL::millis();
-    for(uint8_t i=0; i<accel_count; i++) {
-        if (!ins.use_accel(i)) {
-            continue;
-        }
-        // get next accel vector
-        const Vector3f &accel_vec = ins.get_accel(i);
-        Vector3f vec_diff = accel_vec - prime_accel_vec;
-        // allow for user-defined difference, typically 0.75 m/s/s. Has to pass in last 10 seconds
-        float threshold = accel_error_threshold;
-        if (i >= 2) {
-            /*
-              we allow for a higher threshold for IMU3 as it
-              runs at a different temperature to IMU1/IMU2,
-              and is not used for accel data in the EKF
-            */
-            threshold *= 3;
-        }
-
-        // EKF is less sensitive to Z-axis error
-        vec_diff.z *= 0.5f;
-
-        if (vec_diff.length() > threshold) {
-            // this sensor disagrees with the primary sensor, so
-            // accels are inconsistent:
-            last_accel_pass_ms = 0;
-            return false;
-        }
+    if (!ins.accels_consistent(accel_error_threshold)) {
+        // accels are inconsistent:
+        last_accel_pass_ms = 0;
+        return false;
     }
 
     if (last_accel_pass_ms == 0) {
-        // we didn't return false in the loop above, so sensors are
+        // we didn't return false above, so sensors are
         // consistent right now:
         last_accel_pass_ms = now;
     }
@@ -404,30 +375,15 @@ bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
 
 bool AP_Arming::ins_gyros_consistent(const AP_InertialSensor &ins)
 {
-    const uint8_t gyro_count = ins.get_gyro_count();
-    if (gyro_count <= 1) {
-        return true;
-    }
-
-    const Vector3f &prime_gyro_vec = ins.get_gyro();
     const uint32_t now = AP_HAL::millis();
-    for(uint8_t i=0; i<gyro_count; i++) {
-        if (!ins.use_gyro(i)) {
-            continue;
-        }
-        // get next gyro vector
-        const Vector3f &gyro_vec = ins.get_gyro(i);
-        const Vector3f vec_diff = gyro_vec - prime_gyro_vec;
-        // allow for up to 5 degrees/s difference
-        if (vec_diff.length() > radians(5)) {
-            // this sensor disagrees with the primary sensor, so
-            // gyros are inconsistent:
-            last_gyro_pass_ms = 0;
-            return false;
-        }
+    // allow for up to 5 degrees/s difference
+    if (!ins.gyros_consistent(5)) {
+        // gyros are inconsistent:
+        last_gyro_pass_ms = 0;
+        return false;
     }
 
-    // we didn't return false in the loop above, so sensors are
+    // we didn't return false above, so sensors are
     // consistent right now:
     if (last_gyro_pass_ms == 0) {
         last_gyro_pass_ms = now;