Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: correct encoding used for send_to_components

Browse Source 
These have to be packed onto the channel - otherwise they may ge
tencoded as mavlink1 instead of mavlink2 (or vice-versa)
This commit is contained in:
Peter Barker 2019-10-17 18:07:23 +11:00 committed by Randy Mackay
parent cf99227a8c
commit 865937306c
3 changed files with 51 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.h
View File

@ -241,7 +241,7 @@ public:
send a MAVLink message to all components with this vehicle's system id
This is a no-op if no routes to components have been learned
*/
static void send_to_components(const mavlink_message_t &msg) { routing.send_to_components(msg); }
static void send_to_components(uint32_t msgid, const char *pkt, uint8_t pkt_len) { routing.send_to_components(msgid, pkt, pkt_len); }
/*
allow forwarding of packets / heartbeats to be blocked as required by some components to reduce traffic

45
libraries/GCS_MAVLink/MAVLink_routing.cpp
View File

@ -184,16 +184,34 @@ bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavl
This is a no-op if no routes to components have been learned
*/
void MAVLink_routing::send_to_components(const mavlink_message_t &msg)
void MAVLink_routing::send_to_components(uint32_t msgid, const char *pkt, uint8_t pkt_len)
{
bool sent_to_chan[MAVLINK_COMM_NUM_BUFFERS];
memset(sent_to_chan, 0, sizeof(sent_to_chan));
const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid);
if (entry == nullptr) {
return;
}
send_to_components(pkt, entry, pkt_len);
}
void MAVLink_routing::send_to_components(const char *pkt, const mavlink_msg_entry_t *entry, const uint8_t pkt_len)
{
bool sent_to_chan[MAVLINK_COMM_NUM_BUFFERS] {};
// check learned routes
for (uint8_t i=0; i<num_routes; i++) {
if ((routes[i].sysid == mavlink_system.sysid) && !sent_to_chan[routes[i].channel]) {
if (comm_get_txspace(routes[i].channel) >= ((uint16_t)msg.len) +
GCS_MAVLINK::packet_overhead_chan(routes[i].channel)) {
if (routes[i].sysid != mavlink_system.sysid) {
// our system ID hasn't been seen on this link
continue;
}
if (sent_to_chan[routes[i].channel]) {
// we've already send it on this link
continue;
}
if (comm_get_txspace(routes[i].channel) <
((uint16_t)entry->max_msg_len) + GCS_MAVLINK::packet_overhead_chan(routes[i].channel)) {
// it doesn't fit on this channel
continue;
}
#if ROUTING_DEBUG
::printf("send msg %u on chan %u sysid=%u compid=%u\n",
msg.msgid,
@ -201,11 +219,20 @@ void MAVLink_routing::send_to_components(const mavlink_message_t &msg)
(unsigned)routes[i].sysid,
(unsigned)routes[i].compid);
#endif
_mavlink_resend_uart(routes[i].channel, &msg);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (entry->max_msg_len > pkt_len) {
AP_HAL::panic("Passed packet message length (%u > %u)",
entry->max_msg_len, pkt_len);
}
#endif
_mav_finalize_message_chan_send(routes[i].channel,
entry->msgid,
pkt,
entry->min_msg_len,
MIN(entry->max_msg_len, pkt_len),
entry->crc_extra);
sent_to_chan[routes[i].channel] = true;
}
}
}
}
/*

7
libraries/GCS_MAVLink/MAVLink_routing.h
View File

@ -32,8 +32,11 @@ public:
/*
send a MAVLink message to all components with this vehicle's system id
This is a no-op if no routes to components have been learned
msgid here is the mavlink message ID, pkt is a pointer to a
mavlink message structure (e.g. a mavlink_command_long_t)
*/
void send_to_components(const mavlink_message_t &msg);
void send_to_components(uint32_t msgid, const char *pkt, uint8_t pkt_len);
/*
search for the first vehicle or component in the routing table with given mav_type and retrieve it's sysid, compid and channel
@ -63,4 +66,6 @@ private:
// special handling for heartbeat messages
void handle_heartbeat(mavlink_channel_t in_channel, const mavlink_message_t &msg);
void send_to_components(const char *pkt, const mavlink_msg_entry_t *entry, uint8_t pkt_len);
};