From 10724f57388ad93ea2e0ba2ee25060555ba3d184 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 9 Mar 2015 14:28:29 +0900 Subject: [PATCH] Copter: reject preflight calibration when armed --- ArduCopter/GCS_Mavlink.pde | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index af7b94091f..4047eef5cd 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1122,6 +1122,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_PREFLIGHT_CALIBRATION: + // exit immediately if armed + if (motors.armed()) { + result = MAV_RESULT_FAILED; + break; + } if (packet.param1 == 1) { // gyro offset calibration ins.init_gyro();