From 3e637ac5d9362a7809294177aec2a45e499564b5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 10 Feb 2014 12:54:50 +1100
Subject: [PATCH] AP_HAL: added enable_flow_control() option in AP_HAL

---
 libraries/AP_HAL/UARTDriver.h       |  1 +
 libraries/AP_HAL_PX4/UARTDriver.cpp | 19 +++++++++++++++++++
 libraries/AP_HAL_PX4/UARTDriver.h   |  2 ++
 3 files changed, 22 insertions(+)

diff --git a/libraries/AP_HAL/UARTDriver.h b/libraries/AP_HAL/UARTDriver.h
index 429f8900c8..42ff319ecf 100644
--- a/libraries/AP_HAL/UARTDriver.h
+++ b/libraries/AP_HAL/UARTDriver.h
@@ -38,6 +38,7 @@ public:
     virtual bool is_initialized() = 0;
     virtual void set_blocking_writes(bool blocking) = 0;
     virtual bool tx_pending() = 0;
+    virtual void enable_flow_control(bool enable) {};
 
     /* Implementations of BetterStream virtual methods. These are
      * provided by AP_HAL to ensure consistency between ports to
diff --git a/libraries/AP_HAL_PX4/UARTDriver.cpp b/libraries/AP_HAL_PX4/UARTDriver.cpp
index 4ef5be0f7f..60c88bfd44 100644
--- a/libraries/AP_HAL_PX4/UARTDriver.cpp
+++ b/libraries/AP_HAL_PX4/UARTDriver.cpp
@@ -117,6 +117,9 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
 		cfsetspeed(&t, _baudrate);
 		// disable LF -> CR/LF
 		t.c_oflag &= ~ONLCR;
+        // always enable input flow control (notifying devices that we
+        // have space). This is harmless even if the pin is not connected
+        t.c_cflag |= CRTS_IFLOW;
 		tcsetattr(_fd, TCSANOW, &t);
 	}
 
@@ -129,6 +132,22 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
     }
 }
 
+void PX4UARTDriver::enable_flow_control(bool enable)
+{
+	if (_fd == -1) {
+        return;
+    }
+    struct termios t;
+    tcgetattr(_fd, &t);
+    // we already enabled CRTS_IFLOW above, just enable output flow control
+    if (enable) {
+        t.c_cflag |= CRTSCTS;
+    } else {
+        t.c_cflag &= ~CRTSCTS;
+    }
+    tcsetattr(_fd, TCSANOW, &t);
+}
+
 void PX4UARTDriver::begin(uint32_t b) 
 {
 	begin(b, 0, 0);
diff --git a/libraries/AP_HAL_PX4/UARTDriver.h b/libraries/AP_HAL_PX4/UARTDriver.h
index 817b045e05..9542832bb1 100644
--- a/libraries/AP_HAL_PX4/UARTDriver.h
+++ b/libraries/AP_HAL_PX4/UARTDriver.h
@@ -36,6 +36,8 @@ public:
 	    return _fd;
     }
 
+    void enable_flow_control(bool enable);
+
 private:
     const char *_devpath;
     int _fd;