From 49e7ee9ba7876d44d1d422f1024b22fb9a805969 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 16 May 2014 11:44:33 +1000
Subject: [PATCH] GCS_MAVLink: added setup_uart() method

this provides a common way of dealing with UART setup for a GCS
instance. It includes code to cope with SiK radios stuck in bootloader
mode.
---
 libraries/GCS_MAVLink/GCS.h          |  1 +
 libraries/GCS_MAVLink/GCS_Common.cpp | 35 ++++++++++++++++++++++++++++
 2 files changed, 36 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 3c735b91f..abd541da5 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -144,6 +144,7 @@ public:
     GCS_MAVLINK();
     void        update(void);
     void        init(AP_HAL::UARTDriver *port);
+    void        setup_uart(AP_HAL::UARTDriver *port, uint32_t baudrate, uint16_t rxS, uint16_t txS);
     void        send_message(enum ap_message id);
     void        send_text(gcs_severity severity, const char *str);
     void        send_text_P(gcs_severity severity, const prog_char_t *str);
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 98034e5d3..4d3e2c356 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -54,6 +54,41 @@ GCS_MAVLINK::init(AP_HAL::UARTDriver *port)
 }
 
 
+/*
+  setup a UART, handling begin() and init()
+ */
+void
+GCS_MAVLINK::setup_uart(AP_HAL::UARTDriver *port, uint32_t baudrate, uint16_t rxS, uint16_t txS)
+{
+    if (port == NULL) {
+        return;
+    }
+
+    /*
+      Now try to cope with SiK radios that may be stuck in bootloader
+      mode because CTS was held while powering on. This tells the
+      bootloader to wait for a firmware. It affects any SiK radio with
+      CTS connected that is externally powered. To cope we send 0x30
+      0x20 at 115200 on startup, which tells the bootloader to reset
+      and boot normally
+     */
+    port->begin(115200, rxS, txS);
+    AP_HAL::UARTDriver::flow_control old_flow_control = port->get_flow_control();
+    port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
+    for (uint8_t i=0; i<3; i++) {
+        hal.scheduler->delay(1);
+        port->write(0x30);
+        port->write(0x20);
+    }
+    port->set_flow_control(old_flow_control);
+
+    // now change to desired baudrate
+    port->begin(baudrate);
+
+    // and init the gcs instance
+    init(port);
+}
+
 uint16_t
 GCS_MAVLINK::_count_parameters()
 {
-- 
GitLab