AP_Frsky_Telem: use ringbuffer for Frsky message queueing

libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp

@@ -23,6 +23,8 @@
 #include "AP_Frsky_Telem.h"
 extern const AP_HAL::HAL& hal;
 
+ObjectArray<mavlink_statustext_t> AP_Frsky_Telem::_statustext_queue(FRSKY_TELEM_PAYLOAD_STATUS_CAPACITY);
+
 //constructor
 AP_Frsky_Telem::AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery, const RangeFinder &rng) :
     _ahrs(ahrs),
@@ -95,7 +97,7 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
             // build mavlink message queue for sensor_status_flags
             check_sensor_status_flags();
             // if there's any message in the queue, start sending them chunk by chunk; three times each chunk
-            if (_msg.sent_idx != _msg.queued_idx) {
+            if (!_statustext_queue.empty()) {
                 send_uint32(DIY_FIRST_ID, get_next_msg_chunk());
                 return;
             }
@@ -417,16 +419,16 @@ uint32_t AP_Frsky_Telem::get_next_msg_chunk(void)
 {
     if (_msg_chunk.repeats == 0) {
         _msg_chunk.chunk = 0;
-        uint8_t character = _msg.data[_msg.sent_idx].text[_msg_chunk.char_index++];
+        uint8_t character = _statustext_queue[0]->text[_msg_chunk.char_index++];
         if (character) {
             _msg_chunk.chunk |= character<<24;
-            character = _msg.data[_msg.sent_idx].text[_msg_chunk.char_index++];
+            character = _statustext_queue[0]->text[_msg_chunk.char_index++];
             if (character) {
                 _msg_chunk.chunk |= character<<16;
-                character = _msg.data[_msg.sent_idx].text[_msg_chunk.char_index++];
+                character = _statustext_queue[0]->text[_msg_chunk.char_index++];
                 if (character) {
                     _msg_chunk.chunk |= character<<8;
-                    character = _msg.data[_msg.sent_idx].text[_msg_chunk.char_index++];
+                    character = _statustext_queue[0]->text[_msg_chunk.char_index++];
                     if (character) {
                         _msg_chunk.chunk |= character;
                     }
@@ -436,16 +438,16 @@ uint32_t AP_Frsky_Telem::get_next_msg_chunk(void)
         if (!character) { // we've reached the end of the message (string terminated by '\0')
             _msg_chunk.char_index = 0;
             // add severity which is sent as the MSB of the last three bytes of the last chunk (bits 24, 16, and 8) since a character is on 7 bits
-            _msg_chunk.chunk |= (_msg.data[_msg.sent_idx].severity & 0x4)<<21;
-            _msg_chunk.chunk |= (_msg.data[_msg.sent_idx].severity & 0x2)<<14;
-            _msg_chunk.chunk |= (_msg.data[_msg.sent_idx].severity & 0x1)<<7;
+            _msg_chunk.chunk |= (_statustext_queue[0]->severity & 0x4)<<21;
+            _msg_chunk.chunk |= (_statustext_queue[0]->severity & 0x2)<<14;
+            _msg_chunk.chunk |= (_statustext_queue[0]->severity & 0x1)<<7;
         }
     }
     _msg_chunk.repeats++;
     if (_msg_chunk.repeats > 2) { // repeat each message chunk 3 times to ensure transmission
         _msg_chunk.repeats = 0;
         if (_msg_chunk.char_index == 0) { // if we're ready for the next message
-            _msg.sent_idx = (_msg.sent_idx + 1) % MSG_BUFFER_LENGTH; // flag the current message as sent
+            _statustext_queue.remove(0);
         }
     }
     return _msg_chunk.chunk;
@@ -457,9 +459,15 @@ uint32_t AP_Frsky_Telem::get_next_msg_chunk(void)
  */
 void AP_Frsky_Telem::queue_message(MAV_SEVERITY severity, const char *text)
 {
-    _msg.data[_msg.queued_idx].severity = severity;
-    _msg.data[_msg.queued_idx].text = text;
-    _msg.queued_idx = (_msg.queued_idx + 1) % MSG_BUFFER_LENGTH;
+    mavlink_statustext_t statustext{};
+
+    statustext.severity = severity;
+    strncpy(statustext.text, text, sizeof(statustext.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);
 }
 
 /*

libraries/AP_Frsky_Telem/AP_Frsky_Telem.h

@@ -21,8 +21,9 @@
 #include <AP_Notify/AP_Notify.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>
 #include <AP_SerialManager/AP_SerialManager.h>
+#include <AP_HAL/utility/RingBuffer.h>
 
-#define MSG_BUFFER_LENGTH           5 // size of the message buffer queue (number of messages waiting to be sent)
+#define FRSKY_TELEM_PAYLOAD_STATUS_CAPACITY          5 // size of the message buffer queue (max number of messages waiting to be sent)
 
 /* 
 for FrSky D protocol (D-receivers)
@@ -119,7 +120,7 @@ public:
     // init - perform required initialisation
     void init(const AP_SerialManager &serial_manager, const char *firmware_str, const uint8_t mav_type, AP_Float *fs_batt_voltage = nullptr, AP_Float *fs_batt_mah = nullptr, uint32_t *ap_value = nullptr);
 
-    // add statustext message to FrSky lib queue.
+    // add statustext message to FrSky lib message queue
     void queue_message(MAV_SEVERITY severity, const char *text);
 
     // update flight control mode. The control mode is vehicle type specific
@@ -132,17 +133,10 @@ public:
     // MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
     // indicates that the sensor or subsystem is present but not
     // functioning correctly
+
     void update_sensor_status_flags(uint32_t error_mask) { _ap.sensor_status_flags = error_mask; }
-    
-    struct msg_t
-    {
-        struct {
-            const char *text;
-            uint8_t severity;
-        } data[MSG_BUFFER_LENGTH];
-        uint8_t queued_idx;
-        uint8_t sent_idx;
-    };
+        
+    static ObjectArray<mavlink_statustext_t> _statustext_queue;
     
 private:
     AP_AHRS &_ahrs;
@@ -224,8 +218,6 @@ private:
         uint8_t repeats; // send each message "chunk" 3 times to make sure the entire messsage gets through without getting cut
         uint8_t char_index; // index of which character to get in the message
     } _msg_chunk;
-
-    msg_t _msg;
     
     // main transmission function when protocol is FrSky SPort Passthrough (OpenTX)
     void send_SPort_Passthrough(void);