diff --git a/APMrover2/AP_Arming.cpp b/APMrover2/AP_Arming.cpp index 9a2f1addc5..a0ee05fe46 100644 --- a/APMrover2/AP_Arming.cpp +++ b/APMrover2/AP_Arming.cpp @@ -42,6 +42,18 @@ bool AP_Arming_Rover::pre_arm_rc_checks(const bool display_failure) // performs pre_arm gps related checks and returns true if passed bool AP_Arming_Rover::gps_checks(bool display_failure) { + const AP_AHRS &ahrs = AP::ahrs(); + + // always check if inertial nav has started and is ready + if (!ahrs.prearm_healthy()) { + const char *reason = ahrs.prearm_failure_reason(); + if (reason == nullptr) { + reason = "AHRS not healthy"; + } + check_failed(ARMING_CHECK_NONE, display_failure, "%s", reason); + return false; + } + if (!rover.control_mode->requires_position() && !rover.control_mode->requires_velocity()) { // we don't care! return true; @@ -55,7 +67,7 @@ bool AP_Arming_Rover::gps_checks(bool display_failure) // ensure position esetimate is ok if (!rover.ekf_position_ok()) { - const char *reason = AP::ahrs().prearm_failure_reason(); + const char *reason = ahrs.prearm_failure_reason(); if (reason == nullptr) { reason = "Need Position Estimate"; }