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

@@ -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

libraries/GCS_MAVLink/MAVLink_routing.cpp

@@ -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];
+    const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid);
-    memset(sent_to_chan, 0, sizeof(sent_to_chan));
+    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 (routes[i].sysid != mavlink_system.sysid) {
-            if (comm_get_txspace(routes[i].channel) >= ((uint16_t)msg.len) +
+            // our system ID hasn't been seen on this link
-                GCS_MAVLINK::packet_overhead_chan(routes[i].channel)) {
+            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,10 +219,19 @@ 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
-                sent_to_chan[routes[i].channel] = true;
+        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;
     }
 }
 

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);
 };