GCS_MAVLink: allow private channels to also be active

This will allow for logging of private channels.

libraries/GCS_MAVLink/GCS.cpp

@@ -38,12 +38,24 @@ void GCS::init()
     mavlink_system.sysid = sysid_this_mav();
 }
 
+/*
+ * returns a mask of channels that statustexts should be sent to
+ */
+uint8_t GCS::statustext_send_channel_mask() const
+{
+    uint8_t ret = 0;
+    ret |= GCS_MAVLINK::active_channel_mask();
+    ret |= GCS_MAVLINK::streaming_channel_mask();
+    ret &= ~GCS_MAVLINK::private_channel_mask();
+    return ret;
+}
+
 /*
   send a text message to all GCS
  */
 void GCS::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list)
 {
-    uint8_t mask = GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask();
+    uint8_t mask = statustext_send_channel_mask();
     if (!update_send_has_been_called) {
         // we have not yet initialised the streaming-channel-mask,
         // which is done as part of the update() call.  So just send
@@ -69,6 +81,9 @@ void GCS::send_to_active_channels(uint32_t msgid, const char *pkt)
     }
     for (uint8_t i=0; i<num_gcs(); i++) {
         GCS_MAVLINK &c = *chan(i);
+        if (c.is_private()) {
+            continue;
+        }
         if (!c.is_active()) {
             continue;
         }

libraries/GCS_MAVLink/GCS.h

@@ -281,6 +281,9 @@ public:
     // return a bitmap of streaming channels
     static uint8_t streaming_channel_mask(void) { return chan_is_streaming; }
 
+    // return a bitmap of private channels
+    static uint8_t private_channel_mask(void) { return mavlink_private; }
+
     // set a channel as private. Private channels get sent heartbeats, but
     // don't get broadcast packets or forwarded packets
     static void set_channel_private(mavlink_channel_t chan);
@@ -877,6 +880,7 @@ public:
     void send_text(MAV_SEVERITY severity, const char *fmt, ...) FMT_PRINTF(3, 4);
     void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list);
     virtual void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list, uint8_t mask);
+    uint8_t statustext_send_channel_mask() const;
 
     virtual GCS_MAVLINK *chan(const uint8_t ofs) = 0;
     virtual const GCS_MAVLINK *chan(const uint8_t ofs) const = 0;

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1280,10 +1280,7 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,
     // we exclude radio packets because we historically used this to
     // make it possible to use the CLI over the radio
     if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
-        const uint8_t mask = (1U<<(chan-MAVLINK_COMM_0));
+        mavlink_active |= (1U<<(chan-MAVLINK_COMM_0));
-        if (!(mask & mavlink_private)) {
-            mavlink_active |= mask;
-        }
     }
     if (!(status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) &&
         (status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) &&
@@ -1805,7 +1802,7 @@ void GCS::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list, u
         // filter destination ports to only allow active ports.
         statustext_t statustext{};
         if (update_send_has_been_called) {
-            statustext.bitmask = (GCS_MAVLINK::active_channel_mask()  | GCS_MAVLINK::streaming_channel_mask() );
+            statustext.bitmask = statustext_send_channel_mask();
         } else {
             // we have not yet initialised the streaming-channel-mask,
             // which is done as part of the update() call.  So just send

libraries/GCS_MAVLink/GCS_MAVLink.cpp

@@ -53,7 +53,6 @@ void GCS_MAVLINK::set_channel_private(mavlink_channel_t _chan)
 {
     const uint8_t mask = (1U<<(unsigned)_chan);
     mavlink_private |= mask;
-    mavlink_active &= ~mask;
 }
 
 // return a MAVLink parameter type given a AP_Param type

libraries/GCS_MAVLink/MAVLink_routing.cpp

@@ -301,7 +301,7 @@ void MAVLink_routing::learn_route(mavlink_channel_t in_channel, const mavlink_me
 */
 void MAVLink_routing::handle_heartbeat(mavlink_channel_t in_channel, const mavlink_message_t &msg)
 {
-    uint16_t mask = GCS_MAVLINK::active_channel_mask();
+    uint16_t mask = GCS_MAVLINK::active_channel_mask() & ~GCS_MAVLINK::private_channel_mask();
 
     // don't send on the incoming channel. This should only matter if
     // the routing table is full