aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: fixed forwarding of non-targetted messages

Browse Source
This commit is contained in:
Andrew Tridgell 2014-12-10 18:25:47 +11:00 committed by Randy Mackay
parent fcf17829cc
commit ade7f9e1a9
1 changed files with 17 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/GCS_MAVLink/MAVLink_routing.cpp
View File

@ -51,23 +51,29 @@ bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavl
} }
// extract the targets for this packet // extract the targets for this packet
int16_t target_system =-1; int16_t target_system = -1;
int16_t target_component =-1; int16_t target_component = -1;
get_targets(msg, target_system, target_component); get_targets(msg, target_system, target_component);
// see if it is for us bool targetted = true;
if ((target_system == -1 || target_system == mavlink_system.sysid) &&
(target_component == -1 || target_component == mavlink_system.compid)) { if (target_system == -1 && target_component == -1) {
// it is for us // it is not a targetted message. We need to both forward it
// and process it locally
targetted = false;
} else if ((target_system == -1 || target_system == mavlink_system.sysid) &&
(target_component == -1 || target_component == mavlink_system.compid)) {
// it is for us only
return false; return false;
} }
// forward on any channels matching the targets // forward on any channels matching the targets
for (uint8_t i=0; i<num_routes; i++) { for (uint8_t i=0; i<num_routes; i++) {
if (target_system == routes[i].sysid && if (targetted == false ||
target_component == routes[i].compid && (target_system == routes[i].sysid &&
in_channel != routes[i].channel) { target_component == routes[i].compid)) {
if (comm_get_txspace(routes[i].channel) >= ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) { if (in_channel != routes[i].channel &&
comm_get_txspace(routes[i].channel) >= ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
#if ROUTING_DEBUG #if ROUTING_DEBUG
::printf("fwd msg %u from chan %u on chan %u sysid=%u compid=%u\n", ::printf("fwd msg %u from chan %u on chan %u sysid=%u compid=%u\n",
msg->msgid, msg->msgid,
@ -81,7 +87,7 @@ bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavl
} }
} }
return true; return targetted;
} }
/* /*