GCS_MAVLink: add support for alternative protocol

this allows an alternative to MAVLink to be installed on a GCS
port. This is used for BLHeli passthru support

libraries/GCS_MAVLink/GCS.cpp

@@ -39,4 +39,24 @@ void GCS::send_ekf_origin(const Location &ekf_origin) const
     FOR_EACH_ACTIVE_CHANNEL(send_ekf_origin(ekf_origin));
 }
 
+/*
+  install an alternative protocol handler. This allows another
+  protocol to take over the link if MAVLink goes idle. It is used to
+  allow for the AP_BLHeli pass-thru protocols to run on hal.uartA
+ */
+bool GCS::install_alternative_protocol(mavlink_channel_t c, GCS_MAVLINK::protocol_handler_fn_t handler)
+{
+    if (c >= num_gcs()) {
+        return false;
+    }
+    if (chan(c).alternative.handler) {
+        // already have one installed - we may need to add support for
+        // multiple alternative handlers
+        return false;
+    }
+    chan(c).alternative.handler = handler;
+    return true;
+}
+
+
 #undef FOR_EACH_ACTIVE_CHANNEL

libraries/GCS_MAVLink/GCS.h

@@ -92,6 +92,7 @@ enum ap_message {
 class GCS_MAVLINK
 {
 public:
+    friend class GCS;
     GCS_MAVLINK();
     void        update(uint32_t max_time_us=1000);
     void        init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan);
@@ -218,6 +219,9 @@ public:
     // return current packet overhead for a channel
     static uint8_t packet_overhead_chan(mavlink_channel_t chan);
 
+    // alternative protocol function handler
+    FUNCTOR_TYPEDEF(protocol_handler_fn_t, bool, uint8_t, AP_HAL::UARTDriver *);
+    
 protected:
 
     // overridable method to check for packet acceptance. Allows for
@@ -327,6 +331,7 @@ protected:
     bool try_send_gps_message(enum ap_message id);
     void send_hwstatus();
     void handle_data_packet(mavlink_message_t *msg);
+
 private:
 
     float       adjust_rate_for_stream_trigger(enum streams stream_num);
@@ -467,6 +472,15 @@ private:
     void load_signing_key(void);
     bool signing_enabled(void) const;
     static void save_signing_timestamp(bool force_save_now);
+
+    // alternative protocol handler support
+    struct {
+        GCS_MAVLINK::protocol_handler_fn_t handler;
+        uint32_t last_mavlink_ms;
+        uint32_t last_alternate_ms;
+        bool active;
+    } alternative;
+    
 };
 
 /// @class GCS
@@ -536,6 +550,10 @@ public:
     // static frsky_telem pointer to support queueing text messages
     AP_Frsky_Telem *frsky_telemetry_p;
 
+    
+    // install an alternative protocol handler
+    bool install_alternative_protocol(mavlink_channel_t chan, GCS_MAVLINK::protocol_handler_fn_t handler);
+    
 private:
 
     static GCS *_singleton;
@@ -555,7 +573,6 @@ private:
 
     // true if we are running short on time in our main loop
     bool _out_of_time;
-
 };
 
 GCS &gcs();

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -861,6 +861,7 @@ GCS_MAVLINK::update(uint32_t max_time_us)
     mavlink_message_t msg;
     mavlink_status_t status;
     uint32_t tstart_us = AP_HAL::micros();
+    uint32_t now_ms = AP_HAL::millis();
 
     hal.util->perf_begin(_perf_update);
 
@@ -871,15 +872,39 @@ GCS_MAVLINK::update(uint32_t max_time_us)
     for (uint16_t i=0; i<nbytes; i++)
     {
         uint8_t c = comm_receive_ch(chan);
+        const uint32_t protocol_timeout = 4000;
+        
+        if (alternative.handler &&
+            now_ms - alternative.last_mavlink_ms > protocol_timeout) {
+            /*
+              we have an alternative protocol handler installed and we
+              haven't parsed a MAVLink packet for 4 seconds. Try
+              parsing using alternative handler
+             */
+            if (alternative.handler(c, mavlink_comm_port[chan])) {
+                alternative.last_alternate_ms = now_ms;
+                gcs_alternative_active[chan] = true;
+            }
+            
+            /*
+              we may also try parsing as MAVLink if we haven't had a
+              successful parse on the alternative protocol for 4s
+             */
+            if (now_ms - alternative.last_alternate_ms <= protocol_timeout) {
+                continue;
+            }
+        }
 
         bool parsed_packet = false;
-        
+
         // Try to get a new message
         if (mavlink_parse_char(chan, c, &msg, &status)) {
             hal.util->perf_begin(_perf_packet);
             packetReceived(status, msg);
             hal.util->perf_end(_perf_packet);
             parsed_packet = true;
+            gcs_alternative_active[chan] = false;
+            alternative.last_mavlink_ms = now_ms;
         }
 
         if (parsed_packet || i % 100 == 0) {

libraries/GCS_MAVLink/GCS_MAVLink.cpp

@@ -37,6 +37,7 @@ This provides some support code and variables for MAVLink enabled sketches
 #endif
 
 AP_HAL::UARTDriver	*mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS];
+bool gcs_alternative_active[MAVLINK_COMM_NUM_BUFFERS];
 
 mavlink_system_t mavlink_system = {7,1};
 
@@ -141,6 +142,10 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
     if (!valid_channel(chan)) {
         return;
     }
+    if (gcs_alternative_active[chan]) {
+        // an alternative protocol is active
+        return;
+    }
     mavlink_comm_port[chan]->write(buf, len);
 }
 

libraries/GCS_MAVLink/GCS_MAVLink.h

@@ -36,6 +36,7 @@
 
 /// MAVLink stream used for uartA
 extern AP_HAL::UARTDriver	*mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS];
+extern bool gcs_alternative_active[MAVLINK_COMM_NUM_BUFFERS];
 
 /// MAVLink system definition
 extern mavlink_system_t mavlink_system;