From dc3509ef5523bd214d961583239da93279d7b9a7 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 29 Oct 2014 15:19:10 +0900
Subject: [PATCH] Copter: fail to arm if gyro cal fails
---
ArduCopter/GCS_Mavlink.pde | 6 +++++-
ArduCopter/motors.pde | 20 +++++++++++++++++---
2 files changed, 22 insertions(+), 4 deletions(-)
diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index d7930ccbb..c7fb2c429 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 de1b08b69..9e736b005 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
--
GitLab