From 0df36a8d8136270910b066d85a6eabe68167fc4e Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Tue, 2 Jan 2024 18:20:46 +0000
Subject: [PATCH] AP_RCProtocol: bootstrap Ghost to correct baudrate

---
 libraries/AP_RCProtocol/AP_RCProtocol.cpp     |  6 +-----
 .../AP_RCProtocol/AP_RCProtocol_GHST.cpp      | 21 ++++++++++++++++++-
 libraries/AP_RCProtocol/AP_RCProtocol_GHST.h  |  1 +
 3 files changed, 22 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp
index 9d9174e89f..d2d11cfb30 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp
+++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp
@@ -301,16 +301,12 @@ static const AP_RCProtocol::SerialConfig serial_configs[] {
     // FastSBUS:
     { 200000,  2,   2, true },
 #endif
-#if AP_RCPROTOCOL_CRSF_ENABLED
+#if AP_RCPROTOCOL_CRSF_ENABLED || AP_RCPROTOCOL_GHST_ENABLED
     // CrossFire:
     { 416666,  0,   1, false },
     // CRSFv3 can negotiate higher rates which are sticky on soft reboot
     { 2000000, 0,   1, false },
 #endif
-#if AP_RCPROTOCOL_GHST_ENABLED
-    // Ghost:
-    { 420000,  0,   1, false },
-#endif
 };
 
 static_assert(ARRAY_SIZE(serial_configs) > 1, "must have at least one serial config");
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp
index 1f2fa06950..195e56bfb9 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp
@@ -21,6 +21,8 @@
 
 #if AP_RCPROTOCOL_GHST_ENABLED
 
+#define CRSF_BAUDRATE      416666U
+
 #include "AP_RCProtocol.h"
 #include "AP_RCProtocol_GHST.h"
 #include <AP_HAL/AP_HAL.h>
@@ -413,12 +415,29 @@ bool AP_RCProtocol_GHST::is_telemetry_supported() const
 void AP_RCProtocol_GHST::process_byte(uint8_t byte, uint32_t baudrate)
 {
     // reject RC data if we have been configured for standalone mode
-    if (baudrate != GHST_BAUDRATE) {
+    if (baudrate != CRSF_BAUDRATE && baudrate != GHST_BAUDRATE) {
         return;
     }
     _process_byte(AP_HAL::micros(), byte);
 }
 
+// change the bootstrap baud rate to ELRS standard if configured
+void AP_RCProtocol_GHST::process_handshake(uint32_t baudrate)
+{
+    AP_HAL::UARTDriver *uart = get_current_UART();
+
+    // only change the baudrate if we are bootstrapping CRSF
+    if (uart == nullptr
+        || baudrate != CRSF_BAUDRATE
+        || baudrate == GHST_BAUDRATE
+        || uart->get_baud_rate() == GHST_BAUDRATE
+        || (get_rc_protocols_mask() & ((1U<<(uint8_t(AP_RCProtocol::GHST)+1))+1)) == 0) {
+        return;
+    }
+
+    uart->begin(GHST_BAUDRATE);
+}
+
 //returns uplink link quality on 0-255 scale
 int16_t AP_RCProtocol_GHST::derive_scaled_lq_value(uint8_t uplink_lq)
 {
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_GHST.h b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.h
index e710a70fa9..248fb3f179 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_GHST.h
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.h
@@ -36,6 +36,7 @@ public:
     AP_RCProtocol_GHST(AP_RCProtocol &_frontend);
     virtual ~AP_RCProtocol_GHST();
     void process_byte(uint8_t byte, uint32_t baudrate) override;
+    void process_handshake(uint32_t baudrate) override;
     void update(void) override;
 
     // is the receiver active, used to detect power loss and baudrate changes