From 2552acbf12b381a7f41cbf778661b74a6d9403f4 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 8 May 2015 20:23:21 +1000
Subject: [PATCH] AP_Arming: fixed accel cal test in arming

use a per-imu time of last cal pass, and triple accel threshold for
IMU3. Raise threshold to 0.5 for IMU1/IMU2
---
 libraries/AP_Arming/AP_Arming.cpp | 25 +++++++++++++++++--------
 libraries/AP_Arming/AP_Arming.h   |  4 ++--
 2 files changed, 19 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp
index 4aab747cb2..1365eb5446 100644
--- a/libraries/AP_Arming/AP_Arming.cpp
+++ b/libraries/AP_Arming/AP_Arming.cpp
@@ -57,10 +57,10 @@ AP_Arming::AP_Arming(const AP_AHRS &ahrs_ref, const AP_Baro &baro, Compass &comp
    , _compass(compass)
    , home_is_set(home_set)
    , gcs_send_text_P(gcs_print_func)
-   , last_accel_pass_ms(0)
-   , last_gyro_pass_ms(0)
 {
     AP_Param::setup_object_defaults(this, var_info);
+    memset(last_accel_pass_ms, 0, sizeof(last_accel_pass_ms));
+    memset(last_gyro_pass_ms, 0, sizeof(last_gyro_pass_ms));
 }
 
 bool AP_Arming::is_armed() 
@@ -169,12 +169,21 @@ bool AP_Arming::ins_checks(bool report)
                 // get next accel vector
                 const Vector3f &accel_vec = ins.get_accel(i);
                 Vector3f vec_diff = accel_vec - prime_accel_vec;
-                // allow for up to 0.3 m/s/s difference. Has to pass
+                // allow for up to 0.5 m/s/s difference. Has to pass
                 // in last 10 seconds
-                if (vec_diff.length() <= 0.3f) {
-                    last_accel_pass_ms = hal.scheduler->millis();
+                float threshold = 0.5f;
+                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;
                 }
-                if (hal.scheduler->millis() - last_accel_pass_ms > 10000) {
+                if (vec_diff.length() <= threshold) {
+                    last_accel_pass_ms[i] = hal.scheduler->millis();
+                }
+                if (hal.scheduler->millis() - last_accel_pass_ms[i] > 10000) {
                     if (report) {
                         gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent Accelerometers"));
                     }
@@ -193,9 +202,9 @@ bool AP_Arming::ins_checks(bool report)
                 // allow for up to 5 degrees/s difference. Pass if its
                 // been OK in last 10 seconds
                 if (vec_diff.length() <= radians(5)) {
-                    last_gyro_pass_ms = hal.scheduler->millis();
+                    last_gyro_pass_ms[i] = hal.scheduler->millis();
                 }
-                if (hal.scheduler->millis() - last_gyro_pass_ms > 10000) {
+                if (hal.scheduler->millis() - last_gyro_pass_ms[i] > 10000) {
                     if (report) {
                         gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent gyros"));
                     }
diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h
index f351248df1..2dc12aef09 100644
--- a/libraries/AP_Arming/AP_Arming.h
+++ b/libraries/AP_Arming/AP_Arming.h
@@ -77,8 +77,8 @@ private:
     Compass                                         &_compass;
     const enum HomeState                         &home_is_set;
     gcs_send_t_p                              gcs_send_text_P;
-    uint32_t                                  last_accel_pass_ms;
-    uint32_t                                  last_gyro_pass_ms;
+    uint32_t                                  last_accel_pass_ms[INS_MAX_INSTANCES];
+    uint32_t                                  last_gyro_pass_ms[INS_MAX_INSTANCES];
 
     void set_enabled_checks(uint16_t);