From e4b2fc711d0b75a84fcbca3c058f82862552afd0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Sep 2015 16:50:56 +1000 Subject: [PATCH] AP_Compass: added message for compass cal when armed --- libraries/AP_Compass/AP_Compass_Calibration.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index a83bfe6d07..44ee4a6c93 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -254,7 +254,12 @@ uint8_t Compass::handle_mag_cal_command(const mavlink_command_long_t &packet) switch (packet.command) { case MAV_CMD_DO_START_MAG_CAL: { result = MAV_RESULT_ACCEPTED; - if (hal.util->get_soft_armed() || packet.param1 < 0 || packet.param1 > 255) { + if (hal.util->get_soft_armed()) { + hal.console->println("Disarm for compass calibration"); + result = MAV_RESULT_FAILED; + break; + } + if (packet.param1 < 0 || packet.param1 > 255) { result = MAV_RESULT_FAILED; break; }