From fd9ce90b8e7f7eb468c4acb0189ae272447ef85b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 26 Sep 2016 23:03:18 +1000 Subject: [PATCH] Copter: option to force arm, bypassing checks --- ArduCopter/Copter.h | 2 +- ArduCopter/GCS_Mavlink.cpp | 3 ++- ArduCopter/motors.cpp | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index f8ea13a98d..8866c04f1e 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -834,7 +834,7 @@ private: // motors.cpp void arm_motors_check(); void auto_disarm_check(); - bool init_arm_motors(bool arming_from_gcs); + bool init_arm_motors(bool arming_from_gcs, bool do_arming_checks=true); void init_disarm_motors(); void motors_output(); void lost_vehicle_check(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 43a4ca9d9b..2f9422f71c 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -961,7 +961,8 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) case MAV_CMD_COMPONENT_ARM_DISARM: if (is_equal(packet.param1,1.0f)) { // attempt to arm and return success or failure - if (copter.init_arm_motors(true)) { + const bool do_arming_checks = !is_equal(packet.param2,2989.0f); + if (copter.init_arm_motors(true, do_arming_checks)) { result = MAV_RESULT_ACCEPTED; } } else if (is_zero(packet.param1)) { diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index e340282fb0..4cd16f59c9 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -127,7 +127,7 @@ void Copter::auto_disarm_check() // init_arm_motors - performs arming process including initialisation of barometer and gyros // returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure -bool Copter::init_arm_motors(bool arming_from_gcs) +bool Copter::init_arm_motors(const bool arming_from_gcs, const bool do_arming_checks) { static bool in_arm_motors = false; @@ -144,7 +144,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs) } // run pre-arm-checks and display failures - if (!arming.all_checks_passing(arming_from_gcs)) { + if (do_arming_checks && !arming.all_checks_passing(arming_from_gcs)) { AP_Notify::events.arming_failed = true; in_arm_motors = false; return false;