From f3bc75c53820c061d1c84f51deab791b8a824d59 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Sat, 10 Jul 2021 23:44:23 +0530
Subject: [PATCH] GCS_MAVLink: add support for passthrough with baud changes

---
 libraries/GCS_MAVLink/GCS_Common.cpp | 39 ++++++++++++++++++++++++++--
 1 file changed, 37 insertions(+), 2 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 5af105f6d9..a119cf0e56 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -5265,14 +5265,16 @@ void GCS::update_passthru(void)
 {
     WITH_SEMAPHORE(_passthru.sem);
     uint32_t now = AP_HAL::millis();
-
+    uint32_t baud1, baud2;
     bool enabled = AP::serialmanager().get_passthru(_passthru.port1, _passthru.port2, _passthru.timeout_s,
-                                                    _passthru.baud1, _passthru.baud2);
+                                                    baud1, baud2);
     if (enabled && !_passthru.enabled) {
         _passthru.start_ms = now;
         _passthru.last_ms = 0;
         _passthru.enabled = true;
         _passthru.last_port1_data_ms = now;
+        _passthru.baud1 = baud1;
+        _passthru.baud2 = baud2;
         gcs().send_text(MAV_SEVERITY_INFO, "Passthru enabled");
         if (!_passthru.timer_installed) {
             _passthru.timer_installed = true;
@@ -5282,6 +5284,15 @@ void GCS::update_passthru(void)
         _passthru.enabled = false;
         _passthru.port1->lock_port(0, 0);
         _passthru.port2->lock_port(0, 0);
+        // Restore original baudrates
+        if (_passthru.baud1 != baud1) {
+            _passthru.port1->end();
+            _passthru.port1->begin(baud1);
+        }
+        if (_passthru.baud2 != baud2) {
+            _passthru.port2->end();
+            _passthru.port2->begin(baud2);
+        }
         gcs().send_text(MAV_SEVERITY_INFO, "Passthru disabled");
     } else if (enabled &&
                _passthru.timeout_s &&
@@ -5291,6 +5302,15 @@ void GCS::update_passthru(void)
         _passthru.port1->lock_port(0, 0);
         _passthru.port2->lock_port(0, 0);
         AP::serialmanager().disable_passthru();
+        // Restore original baudrates
+        if (_passthru.baud1 != baud1) {
+            _passthru.port1->end();
+            _passthru.port1->begin(baud1);
+        }
+        if (_passthru.baud2 != baud2) {
+            _passthru.port2->end();
+            _passthru.port2->begin(baud2);
+        }
         gcs().send_text(MAV_SEVERITY_INFO, "Passthru timed out");
     }
 }
@@ -5323,6 +5343,21 @@ void GCS::passthru_timer(void)
     _passthru.port1->lock_port(lock_key, lock_key);
     _passthru.port2->lock_port(lock_key, lock_key);
 
+    // Check for requested Baud rates over USB
+    uint32_t baud = _passthru.port1->get_usb_baud();
+    if (_passthru.baud2 != baud && baud != 0) {
+        _passthru.baud2 = baud;
+        _passthru.port2->end();
+        _passthru.port2->begin_locked(baud, lock_key);
+    }
+
+    baud = _passthru.port2->get_usb_baud();
+    if (_passthru.baud1 != baud && baud != 0) {
+        _passthru.baud1 = baud;
+        _passthru.port1->end();
+        _passthru.port1->begin_locked(baud, lock_key);
+    }
+
     int16_t b;
     uint8_t buf[64];
     uint8_t nbytes = 0;