diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 9d3edad354..5f30ff1ac9 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -313,11 +313,6 @@ #endif #endif -// arming check's maximum acceptable vector difference between internal and external compass after vectors are normalized to field length of 1.0 -#ifndef COMPASS_ACCEPTABLE_VECTOR_DIFF - #define COMPASS_ACCEPTABLE_VECTOR_DIFF 0.75f // pre arm compass check will fail if internal vs external compass direction differ by more than 45 degrees - #endif - ////////////////////////////////////////////////////////////////////////////// // OPTICAL_FLOW #ifndef OPTFLOW diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index d2e4f81b09..d4db96d18b 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -373,25 +373,13 @@ bool Copter::pre_arm_checks(bool display_failure) return false; } -#if COMPASS_MAX_INSTANCES > 1 // check all compasses point in roughly same direction - if (compass.get_count() > 1) { - Vector3f prime_mag_vec = compass.get_field_milligauss(); - prime_mag_vec.normalize(); - for(uint8_t i=0; i COMPASS_ACCEPTABLE_VECTOR_DIFF) { - if (display_failure) { - gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent compasses")); - } - return false; - } + if (!compass.consistent()) { + if (display_failure) { + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent compasses")); } + return false; } -#endif }