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

mavlink_command_sender: fix logic after first test

Browse Source 
- 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)
This commit is contained in:
Julian Oes
2017-07-08 11:52:23 +02:00
committed by Lorenz Meier
parent af0107ae0a
commit e6d31f951c
3 changed files with 12 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_command_sender.cpp
+1 -1
View File
@@ -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();
src/modules/mavlink/mavlink_messages.cpp
+9 -4
View File
@@ -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());
src/modules/mavlink/mavlink_receiver.cpp
+2 -2
View File
@@ -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);
}