From 0b24dc239feb5764fba1c98557592b1fc3288023 Mon Sep 17 00:00:00 2001
From: stephan <schiwo1@gmail.com>
Date: Tue, 23 May 2023 10:11:13 +0200
Subject: [PATCH] GCS_MAVlink: correct routing for Solo Gimbal

Check for a opro camera in a Solo gimbal added and re-enable the routing of Gopro Mavlink commands
---
 libraries/GCS_MAVLink/MAVLink_routing.cpp | 18 +++++++++++++++++-
 libraries/GCS_MAVLink/MAVLink_routing.h   |  3 +++
 2 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp
index 2c05833e61..39d7975062 100644
--- a/libraries/GCS_MAVLink/MAVLink_routing.cpp
+++ b/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;
     }
 
diff --git a/libraries/GCS_MAVLink/MAVLink_routing.h b/libraries/GCS_MAVLink/MAVLink_routing.h
index 0f9fcc4299..b680f405a2 100644
--- a/libraries/GCS_MAVLink/MAVLink_routing.h
+++ b/libraries/GCS_MAVLink/MAVLink_routing.h
@@ -73,4 +73,7 @@ private:
     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
 };