diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index d7930ccbb99c3c687c7f524addc1ab1ec1b26da1..c7fb2c42914fe42be6aa672fb636ea5d1bd72b84 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1159,8 +1159,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // run pre_arm_checks and arm_checks and display failures pre_arm_checks(true); if(ap.pre_arm_check && arm_checks(true)) { - init_arm_motors(); + if (init_arm_motors()) { result = MAV_RESULT_ACCEPTED; + } else { + AP_Notify::flags.arming_failed = true; // init_arm_motors function will reset flag back to false + result = MAV_RESULT_UNSUPPORTED; + } }else{ AP_Notify::flags.arming_failed = true; // init_arm_motors function will reset flag back to false result = MAV_RESULT_UNSUPPORTED; diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index de1b08b69c0c1491f656c7654ee00b18ca802dab..9e736b0052d7d0531d3559cd7e56099c4f74e88b 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -59,7 +59,11 @@ static void arm_motors_check() // run pre-arm-checks and display failures pre_arm_checks(true); if(ap.pre_arm_check && arm_checks(true)) { - init_arm_motors(); + if (!init_arm_motors()) { + // reset arming counter if arming fail + arming_counter = 0; + AP_Notify::flags.arming_failed = true; + } }else{ // reset arming counter if pre-arm checks fail arming_counter = 0; @@ -120,7 +124,8 @@ static void auto_disarm_check() } // init_arm_motors - performs arming process including initialisation of barometer and gyros -static void init_arm_motors() +// returns false in the unlikely case that arming fails (because of a gyro calibration failure) +static bool init_arm_motors() { // arming marker // Flag used to track if we have armed the motors the first time. @@ -167,8 +172,14 @@ static void init_arm_motors() } if(did_ground_start == false) { - did_ground_start = true; startup_ground(true); + // final check that gyros calibrated successfully + if (((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) && !ins.gyro_calibrated_ok_all()) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Gyro cal failed")); + AP_Notify::flags.armed = false; + return false; + } + did_ground_start = true; } // fast baro calibration to reset ground pressure @@ -219,6 +230,9 @@ static void init_arm_motors() // reenable failsafe failsafe_enable(); + + // return success + return true; } // perform pre-arm checks and set ap.pre_arm_check flag