dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: Safely avoid send the same vehicle_command and vehicle_command_ack back 

The previous approach was checking system id and component id but it
will not work in 100% of cases as external devices can send MAVLink
message with the right system id but with broadcast component id.
This commit is contained in:
José Roberto de Souza 2017-07-13 15:11:51 -07:00 committed by Lorenz Meier
parent 89a428fbfe
commit 7c268f4fa1
5 changed files with 16 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/vehicle_command.msg
View File

@ -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

1
msg/vehicle_command_ack.msg
View File

@ -8,3 +8,4 @@ uint32 ORB_QUEUE_LENGTH = 3
uint16 command
uint8 result
uint8 from_external

12
src/modules/mavlink/mavlink_main.cpp
View File

@ -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;

5
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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);
}

6
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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);