Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: use GCS_MAVLINK& when routing

This commit is contained in:
Peter Barker 2023-03-06 10:44:55 +11:00 committed by Andrew Tridgell
parent d67e67965f
commit b55664034f
4 changed files with 30 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1644,7 +1644,7 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,
cstatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; cstatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
} }
} }
if (!routing.check_and_forward(chan, msg)) { if (!routing.check_and_forward(*this, msg)) {
// the routing code has indicated we should not handle this packet locally // the routing code has indicated we should not handle this packet locally
return; return;
} }

30
libraries/GCS_MAVLink/MAVLink_routing.cpp
View File

@ -88,7 +88,7 @@ detect a reset of the flight controller, which implies a reset of its
routing table. routing table.
*/ */
bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavlink_message_t &msg) bool MAVLink_routing::check_and_forward(GCS_MAVLINK &in_link, const mavlink_message_t &msg)
{ {
// handle the case of loopback of our own messages, due to // handle the case of loopback of our own messages, due to
// incorrect serial configuration. // incorrect serial configuration.
@ -99,7 +99,7 @@ bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavl
// learn new routes including private channels // learn new routes including private channels
// so that find_mav_type works for all channels // so that find_mav_type works for all channels
learn_route(in_channel, msg); learn_route(in_link, msg);
if (msg.msgid == MAVLINK_MSG_ID_RADIO || if (msg.msgid == MAVLINK_MSG_ID_RADIO ||
msg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) { msg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
@ -107,12 +107,12 @@ bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavl
return true; return true;
} }
const bool from_private_channel = GCS_MAVLINK::is_private(in_channel); const bool from_private_channel = in_link.is_private();
if (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) { if (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
// heartbeat needs special handling // heartbeat needs special handling
if (!from_private_channel) { if (!from_private_channel) {
handle_heartbeat(in_channel, msg); handle_heartbeat(in_link, msg);
} }
return true; return true;
} }
@ -151,7 +151,12 @@ bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavl
for (uint8_t i=0; i<num_routes; i++) { for (uint8_t i=0; i<num_routes; i++) {
// Skip if channel is private and the target system or component IDs do not match // Skip if channel is private and the target system or component IDs do not match
if ((GCS_MAVLINK::is_private(routes[i].channel)) && GCS_MAVLINK *out_link = gcs().chan(routes[i].channel);
if (out_link == nullptr) {
// this is bad
continue;
}
if (out_link->is_private() &&
(target_system != routes[i].sysid || (target_system != routes[i].sysid ||
target_component != routes[i].compid)) { target_component != routes[i].compid)) {
continue; continue;
@ -162,14 +167,12 @@ bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavl
target_component == routes[i].compid || target_component == routes[i].compid ||
!match_system))) { !match_system))) {
if (in_channel != routes[i].channel && !sent_to_chan[routes[i].channel]) { if (&in_link != out_link && !sent_to_chan[routes[i].channel]) {
if (out_link->check_payload_size(msg.len)) {
if (comm_get_txspace(routes[i].channel) >= ((uint16_t)msg.len) +
GCS_MAVLINK::packet_overhead_chan(routes[i].channel)) {
#if ROUTING_DEBUG #if ROUTING_DEBUG
::printf("fwd msg %u from chan %u on chan %u sysid=%d compid=%d\n", ::printf("fwd msg %u from chan %u on chan %u sysid=%d compid=%d\n",
msg.msgid, msg.msgid,
(unsigned)in_channel, (unsigned)in_link->get_chan(),
(unsigned)routes[i].channel, (unsigned)routes[i].channel,
(int)target_system, (int)target_system,
(int)target_component); (int)target_component);
@ -285,7 +288,7 @@ bool MAVLink_routing::find_by_mavtype_and_compid(uint8_t mavtype, uint8_t compid
/* /*
see if the message is for a new route and learn it see if the message is for a new route and learn it
*/ */
void MAVLink_routing::learn_route(mavlink_channel_t in_channel, const mavlink_message_t &msg) void MAVLink_routing::learn_route(GCS_MAVLINK &in_link, const mavlink_message_t &msg)
{ {
uint8_t i; uint8_t i;
if (msg.sysid == 0) { if (msg.sysid == 0) {
@ -304,6 +307,7 @@ void MAVLink_routing::learn_route(mavlink_channel_t in_channel, const mavlink_me
// should also process them locally. // should also process them locally.
return; return;
} }
const mavlink_channel_t in_channel = in_link.get_chan();
for (i=0; i<num_routes; i++) { for (i=0; i<num_routes; i++) {
if (routes[i].sysid == msg.sysid && if (routes[i].sysid == msg.sysid &&
routes[i].compid == msg.compid && routes[i].compid == msg.compid &&
@ -337,10 +341,12 @@ void MAVLink_routing::learn_route(mavlink_channel_t in_channel, const mavlink_me
propagation heartbeat messages need to be forwarded on all channels propagation heartbeat messages need to be forwarded on all channels
except channels where the sysid/compid of the heartbeat could come from except channels where the sysid/compid of the heartbeat could come from
*/ */
void MAVLink_routing::handle_heartbeat(mavlink_channel_t in_channel, const mavlink_message_t &msg) void MAVLink_routing::handle_heartbeat(GCS_MAVLINK &link, const mavlink_message_t &msg)
{ {
uint16_t mask = GCS_MAVLINK::active_channel_mask() & ~GCS_MAVLINK::private_channel_mask(); uint16_t mask = GCS_MAVLINK::active_channel_mask() & ~GCS_MAVLINK::private_channel_mask();
const mavlink_channel_t in_channel = link.get_chan();
// don't send on the incoming channel. This should only matter if // don't send on the incoming channel. This should only matter if
// the routing table is full // the routing table is full
mask &= ~(1U<<(in_channel-MAVLINK_COMM_0)); mask &= ~(1U<<(in_channel-MAVLINK_COMM_0));

6
libraries/GCS_MAVLink/MAVLink_routing.h
View File

@ -26,7 +26,7 @@ public:
This returns true if the message should be processed locally This returns true if the message should be processed locally
*/ */
bool check_and_forward(mavlink_channel_t in_channel, const mavlink_message_t &msg); bool check_and_forward(class GCS_MAVLINK &link, const mavlink_message_t &msg);
/* /*
send a MAVLink message to all components with this vehicle's system id send a MAVLink message to all components with this vehicle's system id
@ -64,13 +64,13 @@ private:
uint8_t no_route_mask; uint8_t no_route_mask;
// learn new routes // learn new routes
void learn_route(mavlink_channel_t in_channel, const mavlink_message_t &msg); void learn_route(GCS_MAVLINK &link, const mavlink_message_t &msg);
// extract target sysid and compid from a message // extract target sysid and compid from a message
void get_targets(const mavlink_message_t &msg, int16_t &sysid, int16_t &compid); void get_targets(const mavlink_message_t &msg, int16_t &sysid, int16_t &compid);
// special handling for heartbeat messages // special handling for heartbeat messages
void handle_heartbeat(mavlink_channel_t in_channel, const mavlink_message_t &msg); void handle_heartbeat(GCS_MAVLINK &link, const mavlink_message_t &msg);
void send_to_components(const char *pkt, const mavlink_msg_entry_t *entry, uint8_t pkt_len); void send_to_components(const char *pkt, const mavlink_msg_entry_t *entry, uint8_t pkt_len);
}; };

14
libraries/GCS_MAVLink/examples/routing/routing.cpp
View File

@ -42,7 +42,9 @@ void loop(void)
mavlink_msg_heartbeat_encode(3, 1, &msg, &heartbeat); mavlink_msg_heartbeat_encode(3, 1, &msg, &heartbeat);
if (!routing.check_and_forward(MAVLINK_COMM_0, msg)) { GCS_MAVLINK *dummy_link = gcs().chan(0);
if (!routing.check_and_forward(*dummy_link, msg)) {
hal.console->printf("heartbeat should be processed locally\n"); hal.console->printf("heartbeat should be processed locally\n");
err_count++; err_count++;
} }
@ -50,7 +52,7 @@ void loop(void)
// incoming non-targetted message // incoming non-targetted message
mavlink_attitude_t attitude = {0}; mavlink_attitude_t attitude = {0};
mavlink_msg_attitude_encode(3, 1, &msg, &attitude); mavlink_msg_attitude_encode(3, 1, &msg, &attitude);
if (!routing.check_and_forward(MAVLINK_COMM_0, msg)) { if (!routing.check_and_forward(*dummy_link, msg)) {
hal.console->printf("attitude should be processed locally\n"); hal.console->printf("attitude should be processed locally\n");
err_count++; err_count++;
} }
@ -60,7 +62,7 @@ void loop(void)
param_set.target_system = mavlink_system.sysid+1; param_set.target_system = mavlink_system.sysid+1;
param_set.target_component = mavlink_system.compid; param_set.target_component = mavlink_system.compid;
mavlink_msg_param_set_encode(3, 1, &msg, &param_set); mavlink_msg_param_set_encode(3, 1, &msg, &param_set);
if (routing.check_and_forward(MAVLINK_COMM_0, msg)) { if (routing.check_and_forward(*dummy_link, msg)) {
hal.console->printf("param set 1 should not be processed locally\n"); hal.console->printf("param set 1 should not be processed locally\n");
err_count++; err_count++;
} }
@ -69,7 +71,7 @@ void loop(void)
param_set.target_system = mavlink_system.sysid; param_set.target_system = mavlink_system.sysid;
param_set.target_component = mavlink_system.compid; param_set.target_component = mavlink_system.compid;
mavlink_msg_param_set_encode(3, 1, &msg, &param_set); mavlink_msg_param_set_encode(3, 1, &msg, &param_set);
if (!routing.check_and_forward(MAVLINK_COMM_0, msg)) { if (!routing.check_and_forward(*dummy_link, msg)) {
hal.console->printf("param set 2 should be processed locally\n"); hal.console->printf("param set 2 should be processed locally\n");
err_count++; err_count++;
} }
@ -79,7 +81,7 @@ void loop(void)
param_set.target_system = mavlink_system.sysid; param_set.target_system = mavlink_system.sysid;
param_set.target_component = mavlink_system.compid+1; param_set.target_component = mavlink_system.compid+1;
mavlink_msg_param_set_encode(3, 1, &msg, &param_set); mavlink_msg_param_set_encode(3, 1, &msg, &param_set);
if (!routing.check_and_forward(MAVLINK_COMM_0, msg)) { if (!routing.check_and_forward(*dummy_link, msg)) {
hal.console->printf("param set 3 should be processed locally\n"); hal.console->printf("param set 3 should be processed locally\n");
err_count++; err_count++;
} }
@ -88,7 +90,7 @@ void loop(void)
param_set.target_system = 0; param_set.target_system = 0;
param_set.target_component = mavlink_system.compid+1; param_set.target_component = mavlink_system.compid+1;
mavlink_msg_param_set_encode(3, 1, &msg, &param_set); mavlink_msg_param_set_encode(3, 1, &msg, &param_set);
if (!routing.check_and_forward(MAVLINK_COMM_0, msg)) { if (!routing.check_and_forward(*dummy_link, msg)) {
hal.console->printf("param set 4 should be processed locally\n"); hal.console->printf("param set 4 should be processed locally\n");
err_count++; err_count++;
} }