From d2b8ea4bb0ed5ca830cd3730ef3a515e90fd692f Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 27 Jul 2017 14:16:38 +1000
Subject: [PATCH] AP_Arming: break out an accels-consistent method

---
 libraries/AP_Arming/AP_Arming.cpp | 75 ++++++++++++++++++-------------
 libraries/AP_Arming/AP_Arming.h   |  4 ++
 2 files changed, 47 insertions(+), 32 deletions(-)

diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp
index e775f99b44..951cac1ace 100644
--- a/libraries/AP_Arming/AP_Arming.cpp
+++ b/libraries/AP_Arming/AP_Arming.cpp
@@ -169,6 +169,45 @@ bool AP_Arming::logging_checks(bool report)
     return true;
 }
 
+bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
+{
+    if (ins.get_accel_count() <= 1) {
+        return true;
+    }
+
+    const Vector3f &prime_accel_vec = ins.get_accel();
+    for(uint8_t i=0; i<ins.get_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) {
+            last_accel_pass_ms[i] = AP_HAL::millis();
+        }
+        if (AP_HAL::millis() - last_accel_pass_ms[i] > 10000) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
 bool AP_Arming::ins_checks(bool report)
 {
     if ((checks_to_perform & ARMING_CHECK_ALL) ||
@@ -208,39 +247,11 @@ bool AP_Arming::ins_checks(bool report)
         }
 
         // check all accelerometers point in roughly same direction
-        if (ins.get_accel_count() > 1) {
-            const Vector3f &prime_accel_vec = ins.get_accel();
-            for(uint8_t i=0; i<ins.get_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) {
-                    last_accel_pass_ms[i] = AP_HAL::millis();
-                }
-                if (AP_HAL::millis() - last_accel_pass_ms[i] > 10000) {
-                    if (report) {
-                        gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accels inconsistent");
-                    }
-                    return false;
-                }
+        if (!ins_accels_consistent(ins)) {
+            if (report) {
+                gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accels inconsistent");
             }
+            return false;
         }
 
         // check all gyros are giving consistent readings
diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h
index bb5daa1a62..7b15ef2d49 100644
--- a/libraries/AP_Arming/AP_Arming.h
+++ b/libraries/AP_Arming/AP_Arming.h
@@ -107,4 +107,8 @@ protected:
     bool servo_checks(bool report) const;
     bool rc_checks_copter_sub(bool display_failure, const RC_Channel *channels[4], const bool check_min_max = true) const;
 
+private:
+
+    bool ins_accels_consistent(const AP_InertialSensor &ins);
+
 };