From 865937306ca0dffe100238dc9aff6d4c52fe2964 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 17 Oct 2019 18:07:23 +1100
Subject: [PATCH] GCS_MAVLink: correct encoding used for send_to_components

These have to be packed onto the channel - otherwise they may ge
tencoded as mavlink1 instead of mavlink2 (or vice-versa)
---
 libraries/GCS_MAVLink/GCS.h               |  4 +-
 libraries/GCS_MAVLink/MAVLink_routing.cpp | 55 +++++++++++++++++------
 libraries/GCS_MAVLink/MAVLink_routing.h   |  7 ++-
 3 files changed, 49 insertions(+), 17 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 1063962140..332c4acff5 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -241,8 +241,8 @@ 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
     */
diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp
index d318513813..896c326e2e 100644
--- a/libraries/GCS_MAVLink/MAVLink_routing.cpp
+++ b/libraries/GCS_MAVLink/MAVLink_routing.cpp
@@ -184,27 +184,54 @@ 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,
-                         (unsigned)routes[i].channel,
-                         (unsigned)routes[i].sysid,
-                         (unsigned)routes[i].compid);
+        ::printf("send msg %u on chan %u sysid=%u compid=%u\n",
+                 msg.msgid,
+                 (unsigned)routes[i].channel,
+                 (unsigned)routes[i].sysid,
+                 (unsigned)routes[i].compid);
 #endif
-                _mavlink_resend_uart(routes[i].channel, &msg);
-                sent_to_chan[routes[i].channel] = true;
-            }
+#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;
     }
 }
 
diff --git a/libraries/GCS_MAVLink/MAVLink_routing.h b/libraries/GCS_MAVLink/MAVLink_routing.h
index b3b2c80fd4..b985851873 100644
--- a/libraries/GCS_MAVLink/MAVLink_routing.h
+++ b/libraries/GCS_MAVLink/MAVLink_routing.h
@@ -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);
 };