From 2dd34d639d6fc57d42926f2ba650cc96c341d4c8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 2 Oct 2017 13:39:56 +0200 Subject: [PATCH] commander: ignore internal commands during calibration if vmount was enabled in mavlink mode, it was continuously sending mavlink commmands, which lead to 'command denied during calibration: 205' messages during calibration. --- .../commander/calibration_routines.cpp | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index abf9fde727..b3164491d6 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -839,19 +839,21 @@ bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, int cancel_sub) orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd); - if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION && - (int)cmd.param1 == 0 && - (int)cmd.param2 == 0 && - (int)cmd.param3 == 0 && - (int)cmd.param4 == 0 && - (int)cmd.param5 == 0 && - (int)cmd.param6 == 0) { - calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); - mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG); - return true; + if (cmd.from_external) { // ignore internal commands, such as VEHICLE_CMD_DO_MOUNT_CONTROL from vmount + if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION && + (int)cmd.param1 == 0 && + (int)cmd.param2 == 0 && + (int)cmd.param3 == 0 && + (int)cmd.param4 == 0 && + (int)cmd.param5 == 0 && + (int)cmd.param6 == 0) { + calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG); + return true; - } else { - calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + } else { + calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + } } }