From d61848817b02bd008f8bb1fb7caf667cd727dbe9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 8 May 2015 20:06:21 +1000 Subject: [PATCH] Copter: double the threshold for accel inconsistency on IMU3 IMU3 varies in temperature a lot compared to IMU1/IMU2, so needs a higher threshold to prevent false positives --- ArduCopter/motors.pde | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index f32c3a0feb..c7eff6f8ee 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -416,7 +416,16 @@ static bool pre_arm_checks(bool display_failure) // get next accel vector const Vector3f &accel_vec = ins.get_accel(i); Vector3f vec_diff = accel_vec - prime_accel_vec; - if (vec_diff.length() > PREARM_MAX_ACCEL_VECTOR_DIFF) { + float threshold = PREARM_MAX_ACCEL_VECTOR_DIFF; + if (i >= 2) { + /* + for boards with 3 IMUs we only use the first two + in the EKF. Allow for larger accel discrepancy + for IMU3 as it may be running at a different temperature + */ + threshold *= 2; + } + if (vec_diff.length() > threshold) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent Accelerometers")); }