From 264a757095bc9eda66af46ea92a048861bd6db34 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 7 Nov 2018 13:11:08 +1100
Subject: [PATCH] GCS_MAVLink: add send_to_active_channels method

---
 libraries/GCS_MAVLink/GCS.cpp | 42 ++++++++++++++++++++++++-----------
 libraries/GCS_MAVLink/GCS.h   | 20 +++++++++++++++++
 2 files changed, 49 insertions(+), 13 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp
index 6c0075d67a..8ce292fb0b 100644
--- a/libraries/GCS_MAVLink/GCS.cpp
+++ b/libraries/GCS_MAVLink/GCS.cpp
@@ -40,22 +40,38 @@ void GCS::send_text(MAV_SEVERITY severity, const char *fmt, ...)
     va_end(arg_list);
 }
 
-#define FOR_EACH_ACTIVE_CHANNEL(methodcall)          \
-    do {                                             \
-        for (uint8_t i=0; i<num_gcs(); i++) {        \
-            if (!chan(i).initialised) {              \
-                continue;                            \
-            }                                        \
-            if (!(GCS_MAVLINK::active_channel_mask() & (1 << (chan(i).get_chan()-MAVLINK_COMM_0)))) { \
-                continue;                            \
-            }                                        \
-            chan(i).methodcall;                      \
-        }                                            \
-    } while (0)
+void GCS::send_to_active_channels(uint32_t msgid, const char *pkt)
+{
+    const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid);
+    if (entry == nullptr) {
+        return;
+    }
+    for (uint8_t i=0; i<num_gcs(); i++) {
+        GCS_MAVLINK &c = chan(i);
+        if (!c.initialised) {
+            continue;
+        }
+        if (!c.is_active()) {
+            continue;
+        }
+        if (entry->max_msg_len + c.packet_overhead() > c.get_uart()->txspace()) {
+            // no room on this channel
+            continue;
+        }
+        c.send_message(pkt, entry);
+    }
+}
 
 void GCS::send_named_float(const char *name, float value) const
 {
-    FOR_EACH_ACTIVE_CHANNEL(send_named_float(name, value));
+
+    mavlink_named_value_float_t packet;
+    packet.time_boot_ms = AP_HAL::millis();
+    packet.value = value;
+    memcpy(packet.name, name, MIN(strlen(name), (uint8_t)MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN));
+
+    gcs().send_to_active_channels(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT,
+                                  (const char *)&packet);
 }
 
 /*
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index f23cf1b29e..516d8379d3 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -324,6 +324,24 @@ public:
     virtual void packetReceived(const mavlink_status_t &status,
                                 mavlink_message_t &msg);
 
+    // send a mavlink_message_t out this GCS_MAVLINK connection.
+    // Caller is responsible for ensuring space.
+    void send_message(uint32_t msgid, const char *pkt) const {
+        const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid);
+        if (entry == nullptr) {
+            return;
+        }
+        send_message(pkt, entry);
+    }
+    void send_message(const char *pkt, const mavlink_msg_entry_t *entry) const {
+        _mav_finalize_message_chan_send(chan,
+                                        entry->msgid,
+                                        pkt,
+                                        entry->min_msg_len,
+                                        entry->max_msg_len,
+                                        entry->crc_extra);
+    }
+
     // accessor for uart
     AP_HAL::UARTDriver *get_uart() { return _port; }
 
@@ -926,6 +944,8 @@ public:
     virtual MAV_TYPE frame_type() const = 0;
     virtual const char* frame_string() const { return nullptr; }
 
+    void send_to_active_channels(uint32_t msgid, const char *pkt);
+
     void send_text(MAV_SEVERITY severity, const char *fmt, ...);
     void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list);
     virtual void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text);