GCS_MAVLink: handle difference between SERIALn_PROTOCOL=1 and 2

libraries/GCS_MAVLink/GCS.h

@@ -314,6 +314,8 @@ private:
     // pointer to static dataflash for logging of text messages
     static DataFlash_Class *dataflash_p;
 
+    static const AP_SerialManager *serialmanager_p;
+
     // a vehicle can optionally snoop on messages for other systems
     static void (*msg_snoop)(const mavlink_message_t* msg);
 

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -58,6 +58,8 @@ GCS_MAVLINK::init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan)
 void
 GCS_MAVLINK::setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager::SerialProtocol protocol, uint8_t instance)
 {
+    serialmanager_p = &serial_manager;
+
     // search for serial port
     
     AP_HAL::UARTDriver *uart;
@@ -101,13 +103,26 @@ GCS_MAVLINK::setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager
     // and init the gcs instance
     init(uart, mav_chan);
 
+    AP_SerialManager::SerialProtocol mavlink_protocol = serialmanager_p->get_mavlink_protocol(mav_chan);
+    mavlink_status_t *status = mavlink_get_channel_status(chan);
+    if (status == nullptr) {
+        return;
+    }
+    
+    if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2) {
         // load signing key
         load_signing_key();
 
-    mavlink_status_t *status = mavlink_get_channel_status(chan);
+        if (status->signing == NULL) {
-    if ((status && status->signing == NULL) ||
-        chan == MAVLINK_COMM_0) {
             // if signing is off start by sending MAVLink1.
+            status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+        }
+    } else if (status) {
+        // user has asked to only send MAVLink1
+        status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+    }
+
+    if (chan == MAVLINK_COMM_0) {
         // Always start with MAVLink1 on first port for now, to allow for recovery
         // after experiments with MAVLink2
         status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
@@ -939,7 +954,9 @@ GCS_MAVLINK::update(run_cli_fn run_cli)
                 mavlink_active |= (1U<<(chan-MAVLINK_COMM_0));
             }
             if (!(status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) &&
-                (status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
+                (status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) &&
+                serialmanager_p &&
+                serialmanager_p->get_mavlink_protocol(chan) == AP_SerialManager::SerialProtocol_MAVLink2) {
                 // if we receive any MAVLink2 packets on a connection
                 // currently sending MAVLink1 then switch to sending
                 // MAVLink2

libraries/GCS_MAVLink/GCS_MAVLink.cpp

@@ -45,6 +45,9 @@ MAVLink_routing GCS_MAVLINK::routing;
 // static dataflash pointer to support logging text messages
 DataFlash_Class *GCS_MAVLINK::dataflash_p;
 
+// static AP_SerialManager pointer
+const AP_SerialManager *GCS_MAVLINK::serialmanager_p;
+
 // snoop function for vehicle types that want to see messages for
 // other targets
 void (*GCS_MAVLINK::msg_snoop)(const mavlink_message_t* msg) = NULL;