From e6d31f951c0154f509d9f7e07df8acea5dc1c6a7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 6 Jul 2017 09:05:22 +0200 Subject: [PATCH] mavlink_command_sender: fix logic after first test - change/add a few printfs so they make more sense - don't accidentally ignore command_acks - don't forward commands to the same component id, and don't forward commands that are broadcast to all components (target component 0) --- src/modules/mavlink/mavlink_command_sender.cpp | 2 +- src/modules/mavlink/mavlink_messages.cpp | 13 +++++++++---- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_command_sender.cpp b/src/modules/mavlink/mavlink_command_sender.cpp index 2a0107f986..5f33354457 100644 --- a/src/modules/mavlink/mavlink_command_sender.cpp +++ b/src/modules/mavlink/mavlink_command_sender.cpp @@ -141,7 +141,7 @@ void MavlinkCommandSender::handle_mavlink_command_ack(const mavlink_command_ack_ if (item->command.command == ack.command && from_sysid == item->command.target_system && from_compid == item->command.target_component) { - PX4_INFO("handling result %d for command %d: %d", ack.result, ack.command); + PX4_INFO("dropping command %d", ack.command); // Drop it anyway because the command seems to have arrived at the destination, even if we // receive IN_PROGRESS because we trust that it will be handled after that. _commands.drop_current(); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 09677faf51..33f0a8c06c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -455,10 +455,15 @@ protected: if (_cmd_sub->update(&_cmd_time, &cmd)) { - /* only send commands for other systems/components */ - //if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { - MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel()); - //} + /* only send commands for other systems/components, don't forward broadcast commands */ + if ((cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) && + (cmd.target_component != MAV_COMP_ID_ALL)) { + PX4_INFO("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); + MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel()); + + } else { + PX4_INFO("not forwarding command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); + } } MavlinkCommandSender::instance().check_timeout(_mavlink->get_channel()); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index cc353305f3..154b23ba13 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -628,9 +628,9 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) mavlink_command_ack_t ack; mavlink_msg_command_ack_decode(msg, &ack); - if (ack.result != MAV_RESULT_ACCEPTED && ack.result != MAV_RESULT_IN_PROGRESS) { - MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid); + MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid); + if (ack.result != MAV_RESULT_ACCEPTED && ack.result != MAV_RESULT_IN_PROGRESS) { if (msg->compid == MAV_COMP_ID_CAMERA) { PX4_WARN("Got unsuccessful result %d from camera", ack.result); }