GCS_MAVLink: Fix for the Gopro/Solo Gimbal issue

libraries/GCS_MAVLink/MAVLink_routing.cpp

@@ -94,6 +94,15 @@ routing table.
 */
 bool MAVLink_routing::check_and_forward(GCS_MAVLINK &in_link, const mavlink_message_t &msg)
 {
+#if HAL_SOLO_GIMBAL_ENABLED
+    // check if a Gopro is connected. If yes, we allow the routing
+    // of mavlink messages to a private channel (Solo Gimbal case)
+    if (!gopro_status_check && (msg.msgid == MAVLINK_MSG_ID_GOPRO_HEARTBEAT)) {
+       gopro_status_check = true;
+       gcs().send_text(MAV_SEVERITY_NOTICE, "GoPro in Solo gimbal detected");
+    }
+#endif // HAL_SOLO_GIMBAL_ENABLED
+
     // handle the case of loopback of our own messages, due to
     // incorrect serial configuration.
     if (msg.sysid == mavlink_system.sysid &&
@@ -139,7 +148,14 @@ bool MAVLink_routing::check_and_forward(GCS_MAVLINK &in_link, const mavlink_mess
     bool process_locally = match_system && match_component;
 
     // don't ever forward data from a private channel
-    if (from_private_channel) {
+    // unless a Gopro camera is connected to a Solo gimbal
+    bool should_process_locally = from_private_channel;
+#if HAL_SOLO_GIMBAL_ENABLED
+    if (gopro_status_check) {
+        should_process_locally = false;
+    }
+#endif
+    if (should_process_locally) {
         return process_locally;
     }
 

libraries/GCS_MAVLink/MAVLink_routing.h

@@ -73,4 +73,7 @@ class MAVLink_routing
     void handle_heartbeat(GCS_MAVLINK &link, const mavlink_message_t &msg);
 
     void send_to_components(const char *pkt, const mavlink_msg_entry_t *entry, uint8_t pkt_len);
+
+    // check for Gopro in Solo gimbal status
+    bool gopro_status_check; // default is none
 };