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

@@ -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"));
                     }

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);