From a66cb0cbaf919aa1bed312f18a61919089493df4 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 27 Dec 2020 18:44:16 -0500 Subject: [PATCH] mavlink: check TX buffer before processing command ack --- src/modules/mavlink/mavlink_main.cpp | 35 +++++++++++++++------------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a2a268c7ec..d763ed5e66 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2336,24 +2336,27 @@ Mavlink::task_main(int argc, char *argv[]) /* send command ACK */ uint16_t current_command_ack = 0; - vehicle_command_ack_s command_ack; - if (ack_sub.update(&command_ack)) { - if (!command_ack.from_external) { - mavlink_command_ack_t msg; - msg.result = command_ack.result; - msg.command = command_ack.command; - msg.progress = command_ack.result_param1; - msg.result_param2 = command_ack.result_param2; - msg.target_system = command_ack.target_system; - msg.target_component = command_ack.target_component; - current_command_ack = command_ack.command; + if (ack_sub.updated() && (get_free_tx_buf() >= MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + vehicle_command_ack_s command_ack; - // TODO: always transmit the acknowledge once it is only sent over the instance the command is received - //bool _transmitting_enabled_temp = _transmitting_enabled; - //_transmitting_enabled = true; - mavlink_msg_command_ack_send_struct(get_channel(), &msg); - //_transmitting_enabled = _transmitting_enabled_temp; + if (ack_sub.update(&command_ack)) { + if (!command_ack.from_external) { + mavlink_command_ack_t msg; + msg.result = command_ack.result; + msg.command = command_ack.command; + msg.progress = command_ack.result_param1; + msg.result_param2 = command_ack.result_param2; + msg.target_system = command_ack.target_system; + msg.target_component = command_ack.target_component; + current_command_ack = command_ack.command; + + // TODO: always transmit the acknowledge once it is only sent over the instance the command is received + //bool _transmitting_enabled_temp = _transmitting_enabled; + //_transmitting_enabled = true; + mavlink_msg_command_ack_send_struct(get_channel(), &msg); + //_transmitting_enabled = _transmitting_enabled_temp; + } } }