diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index d01d95822d..de9306f393 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -18,6 +18,9 @@ #include #include +#define AP_ARMING_COMPASS_OFFSETS_MAX 600 +#define AP_ARMING_COMPASS_MAGFIELD_MIN 185 // 0.35 * 530 milligauss +#define AP_ARMING_COMPASS_MAGFIELD_MAX 875 // 1.65 * 530 milligauss #define AP_ARMING_BOARD_VOLTAGE_MIN 4.3f #define AP_ARMING_BOARD_VOLTAGE_MAX 5.8f @@ -256,28 +259,29 @@ bool AP_Arming::compass_checks(bool report) // check for unreasonable compass offsets Vector3f offsets = _compass.get_offsets(); - if (offsets.length() > 600) { + if (offsets.length() > AP_ARMING_COMPASS_OFFSETS_MAX) { if (report) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: Compass offsets too high")); } return false; } -#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2 -# define COMPASS_MAGFIELD_EXPECTED 330 // pre arm will fail if mag field > 544 or < 115 -#else // PX4, SITL -#define COMPASS_MAGFIELD_EXPECTED 530 // pre arm will fail if mag field > 874 or < 185 -#endif - // check for unreasonable mag field length float mag_field = _compass.get_field().length(); - if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65f || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35f) { + if (mag_field > AP_ARMING_COMPASS_MAGFIELD_MAX || mag_field < AP_ARMING_COMPASS_MAGFIELD_MIN) { if (report) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: Check mag field")); } return false; } - + + // check all compasses point in roughly same direction + if (!_compass.consistent()) { + if (report) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent compasses")); + } + return false; + } } return true;