fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 04:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

MAVLink app: Warn sender if a command has been rejected

Browse Source
This commit is contained in:
Lorenz Meier
2017-04-23 19:23:09 +02:00
parent 1766f65f02
commit 21f409366a
1 changed files with 19 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+19 -13
View File
@@ -375,13 +375,14 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
if (!target_ok) {
return;
}
bool send_ack = true;
int ret = 0;
if (!target_ok) {
ret = PX4_ERROR;
goto out;
}
//check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 10) {
/* This is the link shutdown command, terminate mavlink */
@@ -411,8 +412,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
send_ack = false;
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
warnx("ignoring CMD with same SYS/COMP (%d/%d) ID",
mavlink_system.sysid, mavlink_system.compid);
PX4_WARN("ignoring CMD with same SYS/COMP (%d/%d) ID",
mavlink_system.sysid, mavlink_system.compid);
return;
}
@@ -467,6 +468,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
}
}
out:
if (send_ack) {
vehicle_command_ack_s command_ack;
command_ack.command = cmd_mavlink.command;
@@ -497,17 +500,18 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
if (!target_ok) {
return;
}
bool send_ack = true;
int ret = 0;
if (!target_ok) {
ret = PX4_ERROR;
goto out;
}
//check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 10) {
/* This is the link shutdown command, terminate mavlink */
warnx("terminated by remote");
PX4_WARN("terminated by remote");
fflush(stdout);
usleep(50000);
@@ -524,8 +528,8 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
} else {
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
warnx("ignoring CMD with same SYS/COMP (%d/%d) ID",
mavlink_system.sysid, mavlink_system.compid);
PX4_WARN("ignoring CMD with same SYS/COMP (%d/%d) ID",
mavlink_system.sysid, mavlink_system.compid);
return;
}
@@ -570,6 +574,8 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
}
}
out:
if (send_ack) {
vehicle_command_ack_s command_ack;
command_ack.command = cmd_mavlink.command;