diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index a02b5205dd..85f48b4fe9 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -104,3 +104,4 @@ uint8 target_component # Component which should execute the command, 0 for all uint8 source_system # System sending the command uint8 source_component # Component sending the command uint8 confirmation # 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) +uint8 from_external diff --git a/msg/vehicle_command_ack.msg b/msg/vehicle_command_ack.msg index ef7799ee06..a04407494a 100644 --- a/msg/vehicle_command_ack.msg +++ b/msg/vehicle_command_ack.msg @@ -8,3 +8,4 @@ uint32 ORB_QUEUE_LENGTH = 3 uint16 command uint8 result +uint8 from_external diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ba47151d1c..2eba718e93 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2203,12 +2203,14 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_command_ack_s command_ack = {}; if (ack_sub->update(&ack_time, &command_ack)) { - mavlink_command_ack_t msg; - msg.result = command_ack.result; - msg.command = command_ack.command; - current_command_ack = command_ack.command; + if (!command_ack.from_external) { + mavlink_command_ack_t msg; + msg.result = command_ack.result; + msg.command = command_ack.command; + current_command_ack = command_ack.command; - mavlink_msg_command_ack_send_struct(get_channel(), &msg); + mavlink_msg_command_ack_send_struct(get_channel(), &msg); + } } struct mavlink_log_s mavlink_log; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index feb80b300a..d382803c2e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -462,10 +462,7 @@ protected: if (_cmd_sub->update(&_cmd_time, &cmd)) { - /* 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_system != 0)) { - + if (!cmd.from_external) { if (_mavlink->verbose()) { PX4_INFO("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f9eea363e8..fff5d55d7b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -473,6 +473,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) vcmd.confirmation = cmd_mavlink.confirmation; + vcmd.from_external = 1; + if (_cmd_pub == nullptr) { _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); @@ -581,6 +583,8 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) vcmd.source_component = msg->compid; + vcmd.from_external = 1; + if (_cmd_pub == nullptr) { _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); @@ -624,6 +628,7 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) command_ack.command = ack.command; command_ack.result = ack.result; command_ack.timestamp = hrt_absolute_time(); + command_ack.from_external = 1; if (_command_ack_pub == nullptr) { _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, @@ -779,6 +784,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = 1; vcmd.timestamp = hrt_absolute_time(); + vcmd.from_external = 1; if (_cmd_pub == nullptr) { _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);