From f3a31b988ac793d2865908851065f0249fd494f2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 17 Aug 2016 13:14:56 +1000 Subject: [PATCH] Copter: use ins_checks from AP_Arming Functionality changes: - gyros and accels only have to be consistent in last 10 seconds to pass - ins.use_accel() is honoured when checking for consistency - ins.use_gyro() is honoured when checking for consistency - threshold is trippled rather than doubled for accel cal checks - checks are reordered --- ArduCopter/arming_checks.cpp | 83 +++--------------------------------- ArduCopter/arming_checks.h | 4 +- ArduCopter/config.h | 10 ----- 3 files changed, 8 insertions(+), 89 deletions(-) diff --git a/ArduCopter/arming_checks.cpp b/ArduCopter/arming_checks.cpp index 27cf179c51..e9a50164a3 100644 --- a/ArduCopter/arming_checks.cpp +++ b/ArduCopter/arming_checks.cpp @@ -212,93 +212,20 @@ bool AP_Arming_Copter::fence_checks(bool display_failure) bool AP_Arming_Copter::ins_checks(bool display_failure) { - // check INS - const AP_InertialSensor &ins = _ins; // avoid code churn + bool ret = AP_Arming::ins_checks(display_failure); + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) { - // check accelerometers have been calibrated - if (!ins.accel_calibrated_ok_all()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accels not calibrated"); - } - return false; - } - - // check accels are healthy - if (!ins.get_accel_health_all()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accelerometers not healthy"); - } - return false; - } - - //check if accelerometers have calibrated and require reboot - if (ins.accel_cal_requires_reboot()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accelerometers calibrated requires reboot"); - } - return false; - } - - // 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= 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; - } - - // EKF is less sensitive to Z-axis error - vec_diff.z *= 0.5f; - - if (vec_diff.length() > threshold) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Accelerometers"); - } - return false; - } - } - } - - // check gyros are healthy - if (!ins.get_gyro_health_all()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Gyros not healthy"); - } - return false; - } - - // check all gyros are consistent - if (ins.get_gyro_count() > 1) { - for(uint8_t i=0; i PREARM_MAX_GYRO_VECTOR_DIFF) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Gyros"); - } - return false; - } - } - } // get ekf attitude (if bad, it's usually the gyro biases) if (!pre_arm_ekf_attitude_check()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: gyros still settling"); } - return false; + ret = false; } } - return true; + + return ret; } bool AP_Arming_Copter::board_voltage_checks(bool display_failure) diff --git a/ArduCopter/arming_checks.h b/ArduCopter/arming_checks.h index 6591ad3d92..a35ff65961 100644 --- a/ArduCopter/arming_checks.h +++ b/ArduCopter/arming_checks.h @@ -28,11 +28,13 @@ protected: bool pre_arm_proximity_check(bool display_failure); bool arm_checks(bool display_failure, bool arming_from_gcs); + // NOTE! the following check functions *DO* call into AP_Arming: + bool ins_checks(bool display_failure) override; + // NOTE! the following check functions *DO NOT* call into AP_Arming! bool gps_checks(bool display_failure); bool fence_checks(bool display_failure); bool compass_checks(bool display_failure); - bool ins_checks(bool display_failure) override; bool board_voltage_checks(bool display_failure); bool parameter_checks(bool display_failure); bool pilot_throttle_checks(bool display_failure); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 630f0c382d..6427592fc8 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -188,16 +188,6 @@ # define PREARM_MAX_ALT_DISPARITY_CM 100 // barometer and inertial nav altitude must be within this many centimeters #endif -// arming check's maximum acceptable accelerometer vector difference (in m/s/s) between primary and backup accelerometers -#ifndef PREARM_MAX_ACCEL_VECTOR_DIFF - #define PREARM_MAX_ACCEL_VECTOR_DIFF 0.70f // pre arm accel check will fail if primary and backup accelerometer vectors differ by 0.7m/s/s -#endif - -// arming check's maximum acceptable rotation rate difference (in rad/sec) between primary and backup gyros -#ifndef PREARM_MAX_GYRO_VECTOR_DIFF - #define PREARM_MAX_GYRO_VECTOR_DIFF 0.0873f // pre arm gyro check will fail if primary and backup gyro vectors differ by 0.0873 rad/sec (=5deg/sec) -#endif - ////////////////////////////////////////////////////////////////////////////// // EKF Failsafe #ifndef FS_EKF_ACTION_DEFAULT