GCS_MAVLink: create queue scheme for static statustext msgs

libraries/GCS_MAVLink/GCS.h

@@ -17,12 +17,19 @@
 #include "MAVLink_routing.h"
 #include <AP_SerialManager/AP_SerialManager.h>
 #include <AP_Mount/AP_Mount.h>
+#include <AP_HAL/utility/RingBuffer.h>
 
 // check if a message will fit in the payload space available
 #define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN)
 #define CHECK_PAYLOAD_SIZE(id) if (comm_get_txspace(chan) < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) return false
 #define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false
 
+#if HAL_CPU_CLASS <= HAL_CPU_CLASS_150 || CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    #define GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY          5
+#else
+    #define GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY          30
+#endif
+
 //  GCS Message ID's
 /// NOTE: to ensure we never block on sending MAVLink messages
 /// please keep each MSG_ to a single MAVLink message. If need be
@@ -94,6 +101,14 @@ public:
         msg_snoop = _msg_snoop;
     }
 
+    struct statustext_t {
+        uint32_t                timestamp_ms;
+        uint8_t                 bitmask;
+        mavlink_statustext_t    msg;
+    };
+    static ObjectBuffer<statustext_t> _statustext_queue;
+
+
     // accessor for uart
     AP_HAL::UARTDriver *get_uart() { return _port; }
 
@@ -119,10 +134,6 @@ public:
     // see if we should send a stream now. Called at 50Hz
     bool        stream_trigger(enum streams stream_num);
 
-	// this costs us 51 bytes per instance, but means that low priority
-	// messages don't block the CPU
-    mavlink_statustext_t pending_status;
-
     // call to reset the timeout window for entering the cli
     void reset_cli_timeout();
 
@@ -160,11 +171,13 @@ public:
     static uint8_t active_channel_mask(void) { return mavlink_active; }
 
     /*
-      send a statustext message to all active MAVLink
+    send a statustext message to active MAVLink connections, or a specific
-      connections. This function is static so it can be called from
+    one. This function is static so it can be called from any library.
-      any library
     */
     static void send_statustext_all(MAV_SEVERITY severity, const char *fmt, ...);
+    static void send_statustext_chan(MAV_SEVERITY severity, uint8_t dest_chan, const char *fmt, ...);
+    static void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text);
+    static void service_statustext(void);
 
     // send a PARAM_VALUE message to all active MAVLink connections.
     static void send_parameter_value_all(const char *param_name, ap_var_type param_type, float param_value);

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -27,6 +27,7 @@ extern const AP_HAL::HAL& hal;
 
 uint32_t GCS_MAVLINK::last_radio_status_remrssi_ms;
 uint8_t GCS_MAVLINK::mavlink_active = 0;
+ObjectBuffer<GCS_MAVLINK::statustext_t> GCS_MAVLINK::_statustext_queue(GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY);
 
 GCS_MAVLINK::GCS_MAVLINK()
 {
@@ -584,20 +585,7 @@ void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *Data
 void
 GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *str)
 {
-    if (severity < MAV_SEVERITY_WARNING && 
+    GCS_MAVLINK::send_statustext_chan(severity, chan, str);
-        comm_get_txspace(chan) >= 
-        MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) {
-        // send immediately
-        char msg[50] {};
-        strncpy(msg, str, sizeof(msg));
-        mavlink_msg_statustext_send(chan, severity, msg);
-    } else {
-        // send via the deferred queuing system
-        mavlink_statustext_t *s = &pending_status;
-        s->severity = (uint8_t)severity;
-        strncpy((char *)s->text, str, sizeof(s->text));
-        send_message(MSG_STATUSTEXT);
-    }
 }
 
 void GCS_MAVLINK::handle_radio_status(mavlink_message_t *msg, DataFlash_Class &dataflash, bool log_radio)
@@ -1137,21 +1125,126 @@ void GCS_MAVLINK::send_ahrs(AP_AHRS &ahrs)
  */
 void GCS_MAVLINK::send_statustext_all(MAV_SEVERITY severity, const char *fmt, ...)
 {
-    for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
+    char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] {};
-        if ((1U<<i) & mavlink_active) {
-            mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
-            if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
-                char msg2[50] {};
     va_list arg_list;
     va_start(arg_list, fmt);
-                hal.util->vsnprintf((char *)msg2, sizeof(msg2), fmt, arg_list);
+    hal.util->vsnprintf((char *)text, sizeof(text), fmt, arg_list);
     va_end(arg_list);
-                mavlink_msg_statustext_send(chan,
+    send_statustext(severity, mavlink_active, text);
-                                            severity,
-                                            msg2);
 }
+
+/*
+    send a statustext message to specific MAVLink channel, zero indexed
+*/
+void GCS_MAVLINK::send_statustext_chan(MAV_SEVERITY severity, uint8_t dest_chan, const char *fmt, ...)
+{
+    char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] {};
+    va_list arg_list;
+    va_start(arg_list, fmt);
+    hal.util->vsnprintf((char *)text, sizeof(text), fmt, arg_list);
+    va_end(arg_list);
+    send_statustext(severity, (1<<dest_chan), text);
 }
+
+
+/*
+    send a statustext text string to specific MAVLink bitmask
+*/
+void GCS_MAVLINK::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text)
+{
+    if (dataflash_p != NULL) {
+        dataflash_p->Log_Write_Message(text);
     }
+
+    // filter destination ports to only allow active ports.
+    statustext_t statustext{};
+    statustext.bitmask = mavlink_active & dest_bitmask;
+    if (!statustext.bitmask) {
+        // nowhere to send
+        return;
+    }
+
+    statustext.timestamp_ms = AP_HAL::millis();
+    statustext.msg.severity = severity;
+    strncpy(statustext.msg.text, text, sizeof(statustext.msg.text));
+
+    // The force push will ensure comm links do not block other comm links forever if they fail.
+    // If we push to a full buffer then we overwrite the oldest entry, effectively removing the
+    // block but not until the buffer fills up.
+    _statustext_queue.push_force(statustext);
+}
+/*
+    send a statustext message to specific MAVLink connections in a bitmask
+ */
+void GCS_MAVLINK::service_statustext(void)
+{
+    // create bitmask of what mavlink ports we should send this text to.
+    // note, if sending to all ports, we only need to store the bitmask for each and the string only once.
+    // once we send over a link, clear the port but other busy ports bit may stay allowing for faster links
+    // to clear the bit and send quickly but slower links to still store the string. Regardless of mixed
+    // bitrates of ports, a maximum of GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY strings can be buffered. Downside
+    // is if you have a super slow link mixed with a faster port, if there are GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY
+    // strings in the slow queue then the next item can not be queued for the faster link
+
+    if (_statustext_queue.empty()) {
+    // nothing to do
+    return;
+    }
+
+    // keep sending until all channels have unable to send (full) or have nothing to send
+    bool still_sending = true;
+
+    // using 'while' to ensure we either run out of text to send or all buffers on all ports are full
+    while (still_sending) {
+        still_sending = false; // if we send something, keep going
+
+        // populate statustext
+        statustext_t statustext; // no need to {} init it because the pop will memcpy all bytes and when it was loaded it was properly zero initialized
+        if (!_statustext_queue.peek(statustext)) {
+            // all done, nothing else to send
+            return;
+        }
+
+        // try and send to all active mavlink ports listed in the statustext.bitmask
+        for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
+
+            uint8_t chan_bit = (1<<i);
+            // logical AND (&) to mask them together
+            if (statustext.bitmask & chan_bit) { // something is queued on a port and that's the port index we're looped at
+
+                mavlink_channel_t chan_index = (mavlink_channel_t)(MAVLINK_COMM_0+i);
+                if (comm_get_txspace(chan_index) > MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
+                    // we have space so send then clear that channel bit on the mask
+                    mavlink_msg_statustext_send(chan_index, statustext.msg.severity, statustext.msg.text);
+                    statustext.bitmask &= ~chan_bit;
+                    still_sending = true;
+
+                } else if (AP_HAL::millis() - statustext.timestamp_ms > 2000) {
+                    // port has been blocking for 2 seconds. This is blocking all further messages on all
+                    // ports so its best to toss the packet by clearing bitmask.
+                    statustext.bitmask &= ~chan_bit;
+
+                    // Also, forget that mavlink port. The port will be re-remembered if we get any data from it
+                    mavlink_active &= ~chan_bit;
+
+                    // restart loop so the other blocked ports can send immediately.
+                    still_sending = true;
+
+                    send_statustext_all(MAV_SEVERITY_CRITICAL, "Can not send statustext on Mavlink port %i", i);
+                } // if comm
+            } // if bitmask
+        } // for i
+
+        if (!statustext.bitmask) {
+            // throw away what we just peeked at, all done with it
+            _statustext_queue.pop();
+
+        } else if (still_sending) {
+            // something was sent so the bitmask changed but there is still something pending.
+            // Perform an in-place update to the peeked entry index. Only the bitmask should be changing
+            _statustext_queue.update(statustext);
+        }
+    } // while
 }
 
 /*