GCS_MAVLink: allocate mavlink status and send buffers dynamically

This makes ArduPilot responsible for supplying buffers to the mavlink layer, rather than that layer allocating them based on MAVLINK_COMM_NUM_BUFFERS.

We will want to rename MAVLINK_COMM_NUM_BUFFERS to AP_GCS_MAX_BACKENDS at some stage.

libraries/GCS_MAVLink/GCS.h

@@ -195,6 +195,10 @@ public:
     GCS_MAVLINK(GCS_MAVLINK_Parameters &parameters, AP_HAL::UARTDriver &uart);
     virtual ~GCS_MAVLINK() {}
 
+    // accessors used to retrieve objects used for parsing incoming messages:
+    mavlink_message_t *channel_buffer() { return &_channel_buffer; }
+    mavlink_status_t *channel_status() { return &_channel_status; }
+
     void        update_receive(uint32_t max_time_us=1000);
     void        update_send();
     bool        init(uint8_t instance);
@@ -727,6 +731,10 @@ protected:
 
 private:
 
+    // define the two objects used for parsing incoming messages:
+    mavlink_message_t _channel_buffer;
+    mavlink_status_t _channel_status;
+
     const AP_SerialManager::UARTState *uartstate;
 
     // last time we got a non-zero RSSI from RADIO_STATUS

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -164,18 +164,14 @@ bool GCS_MAVLINK::init(uint8_t instance)
     mavlink_comm_port[chan] = _port;
 
     const auto mavlink_protocol = uartstate->get_protocol();
-    mavlink_status_t *status = mavlink_get_channel_status(chan);
+
-    if (status == nullptr) {
-        return false;
-    }
-    
     if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2 ||
         mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLinkHL) {
         // load signing key
         load_signing_key();
     } else {
         // user has asked to only send MAVLink1
-        status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+        _channel_status.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
     }
 
 #if HAL_HIGH_LATENCY2_ENABLED
@@ -1512,11 +1508,8 @@ void GCS_MAVLINK::update_send()
     // update the number of packets transmitted base on seqno, making
     // the assumption that we don't send more than 256 messages
     // between the last pass through here
-    mavlink_status_t *status = mavlink_get_channel_status(chan);
+    send_packet_count += uint8_t(_channel_status.current_tx_seq - last_tx_seq);
-    if (status != nullptr) {
+    last_tx_seq = _channel_status.current_tx_seq;
-        send_packet_count += uint8_t(status->current_tx_seq - last_tx_seq);
-        last_tx_seq = status->current_tx_seq;
-    }
 }
 
 void GCS_MAVLINK::remove_message_from_bucket(int8_t bucket, ap_message id)
@@ -1686,10 +1679,7 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,
         // if we receive any MAVLink2 packets on a connection
         // currently sending MAVLink1 then switch to sending
         // MAVLink2
-        mavlink_status_t *cstatus = mavlink_get_channel_status(chan);
+        _channel_status.flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
-        if (cstatus != nullptr) {
-            cstatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
-        }
     }
     if (!routing.check_and_forward(*this, msg)) {
         // the routing code has indicated we should not handle this packet locally
@@ -1878,11 +1868,6 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us)
 */
 void GCS_MAVLINK::log_mavlink_stats()
 {
-    mavlink_status_t *status = mavlink_get_channel_status(chan);
-    if (status == nullptr) {
-        return;
-    }
-
     uint8_t flags = 0;
     if (signing_enabled()) {
         flags |= (uint8_t)Flags::USING_SIGNING;
@@ -1905,8 +1890,8 @@ void GCS_MAVLINK::log_mavlink_stats()
     time_us                : AP_HAL::micros64(),
     chan                   : (uint8_t)chan,
     packet_tx_count        : send_packet_count,
-    packet_rx_success_count: status->packet_rx_success_count,
+    packet_rx_success_count: _channel_status.packet_rx_success_count,
-    packet_rx_drop_count   : status->packet_rx_drop_count,
+    packet_rx_drop_count   : _channel_status.packet_rx_drop_count,
     flags                  : flags,
     stream_slowdown_ms     : stream_slowdown_ms,
     times_full             : out_of_space_to_send_count,
@@ -1965,12 +1950,7 @@ void GCS_MAVLINK::send_rc_channels() const
 
 bool GCS_MAVLINK::sending_mavlink1() const
 {
-    const mavlink_status_t *status = mavlink_get_channel_status(chan);
+    return ((_channel_status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0);
-    if (status == nullptr) {
-        // should not happen
-        return true;
-    }
-    return ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0);
 }
 
 void GCS_MAVLINK::send_rc_channels_raw() const

libraries/GCS_MAVLink/GCS_MAVLink.cpp

@@ -56,6 +56,22 @@ GCS_MAVLINK *GCS_MAVLINK::find_by_mavtype_and_compid(uint8_t mav_type, uint8_t c
     return gcs().chan(channel);
 }
 
+mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) {
+    GCS_MAVLINK *link = gcs().chan(chan);
+    if (link == nullptr) {
+        return nullptr;
+    }
+    return link->channel_buffer();
+}
+
+mavlink_status_t* mavlink_get_channel_status(uint8_t chan) {
+    GCS_MAVLINK *link = gcs().chan(chan);
+    if (link == nullptr) {
+        return nullptr;
+    }
+    return link->channel_status();
+}
+
 // set a channel as private. Private channels get sent heartbeats, but
 // don't get broadcast packets or forwarded packets
 void GCS_MAVLINK::set_channel_private(mavlink_channel_t _chan)

libraries/GCS_MAVLink/GCS_MAVLink.h

@@ -17,6 +17,9 @@
 // allow five telemetry ports
 #define MAVLINK_COMM_NUM_BUFFERS 5
 
+#define MAVLINK_GET_CHANNEL_BUFFER 1
+#define MAVLINK_GET_CHANNEL_STATUS 1
+
 /*
   The MAVLink protocol code generator does its own alignment, so
   alignment cast warnings can be ignored
@@ -52,6 +55,9 @@ static inline bool valid_channel(mavlink_channel_t chan)
 #pragma clang diagnostic pop
 }
 
+mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);
+mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
+
 void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len);
 
 /// Check for available transmit space on the nominated MAVLink channel

libraries/GCS_MAVLink/GCS_Signing.cpp

@@ -132,11 +132,6 @@ void GCS_MAVLINK::load_signing_key(void)
     if (!signing_key_load(key)) {
         return;
     }
-    mavlink_status_t *status = mavlink_get_channel_status(chan);
-    if (status == nullptr) {
-        DEV_PRINTF("Failed to load signing key - no status");
-        return;        
-    }
     memcpy(signing.secret_key, key.secret_key, 32);
     signing.link_id = (uint8_t)chan;
     // use a timestamp 1 minute past the last recorded
@@ -156,11 +151,11 @@ void GCS_MAVLINK::load_signing_key(void)
     }
     if (all_zero) {
         // disable signing
-        status->signing = nullptr;
+        _channel_status.signing = nullptr;
-        status->signing_streams = nullptr;
+        _channel_status.signing_streams = nullptr;
     } else {
-        status->signing = &signing;
+        _channel_status.signing = &signing;
-        status->signing_streams = &signing_streams;
+        _channel_status.signing_streams = &signing_streams;
     }
 }