From b99740cb9378367b546026df4489f2532cb34b53 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 29 Jul 2015 14:03:30 +1000
Subject: [PATCH] HAL_Linux: sped up UDP transfers by about 25x

allow more than 1 packetised transfer per tick
---
 libraries/AP_HAL_Linux/UARTDriver.cpp | 40 +++++++++++++++++++--------
 libraries/AP_HAL_Linux/UARTDriver.h   |  1 +
 libraries/AP_HAL_Linux/UDPDevice.cpp  |  5 ++++
 3 files changed, 35 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp
index ba05dbf41c..7ac69c479b 100644
--- a/libraries/AP_HAL_Linux/UARTDriver.cpp
+++ b/libraries/AP_HAL_Linux/UARTDriver.cpp
@@ -76,6 +76,7 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
         case DEVICE_TCP:
         {
             _tcp_start_connection();
+            _flow_control = FLOW_CONTROL_ENABLE;
             break;
         }
 
@@ -125,8 +126,8 @@ void LinuxUARTDriver::_allocate_buffers(uint16_t rxS, uint16_t txS)
     if (rxS < 8192) {
         rxS = 8192;
     }
-    if (txS < 8192) {
-        txS = 8192;
+    if (txS < 32000) {
+        txS = 32000;
     }
 
     /*
@@ -483,21 +484,17 @@ int LinuxUARTDriver::_read_fd(uint8_t *buf, uint16_t n)
 
 
 /*
-  push any pending bytes to/from the serial port. This is called at
-  1kHz in the timer thread. Doing it this way reduces the system call
-  overhead in the main task enormously. 
+  try to push out one lump of pending bytes
+  return true if progress is made
  */
-void LinuxUARTDriver::_timer_tick(void)
+bool LinuxUARTDriver::_write_pending_bytes(void)
 {
     uint16_t n;
 
-    if (!_initialised) return;
-
-    _in_timer = true;
-
     // write any pending bytes
     uint16_t _tail;
-    n = BUF_AVAILABLE(_writebuf);
+    uint16_t available_bytes = BUF_AVAILABLE(_writebuf);
+    n = available_bytes;
     if (_packetise && n > 0 && _writebuf[_writebuf_head] == 254) {
         // this looks like a MAVLink packet - try to write on
         // packet boundaries when possible
@@ -544,6 +541,27 @@ void LinuxUARTDriver::_timer_tick(void)
         }
     }
 
+    return BUF_AVAILABLE(_writebuf) != available_bytes;
+}
+
+/*
+  push any pending bytes to/from the serial port. This is called at
+  1kHz in the timer thread. Doing it this way reduces the system call
+  overhead in the main task enormously. 
+ */
+void LinuxUARTDriver::_timer_tick(void)
+{
+    uint16_t n;
+
+    if (!_initialised) return;
+
+    _in_timer = true;
+
+    uint8_t num_send = 10;
+    while (num_send != 0 && _write_pending_bytes()) {
+        num_send--;
+    }
+
     // try to fill the read buffer
     uint16_t _head;
     n = BUF_SPACE(_readbuf);
diff --git a/libraries/AP_HAL_Linux/UARTDriver.h b/libraries/AP_HAL_Linux/UARTDriver.h
index 79d169918d..cb8d9fe410 100644
--- a/libraries/AP_HAL_Linux/UARTDriver.h
+++ b/libraries/AP_HAL_Linux/UARTDriver.h
@@ -29,6 +29,7 @@ public:
 
     void set_device_path(const char *path);
 
+    bool _write_pending_bytes(void);
     virtual void _timer_tick(void);
 
     enum flow_control get_flow_control(void) { return _flow_control; }
diff --git a/libraries/AP_HAL_Linux/UDPDevice.cpp b/libraries/AP_HAL_Linux/UDPDevice.cpp
index 0b5718ecbc..792808f93e 100644
--- a/libraries/AP_HAL_Linux/UDPDevice.cpp
+++ b/libraries/AP_HAL_Linux/UDPDevice.cpp
@@ -3,6 +3,8 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 
 #include <stdio.h>
+#include <sys/ioctl.h>
+#include <fcntl.h>
 
 #include "UDPDevice.h"
 
@@ -19,6 +21,9 @@ UDPDevice::~UDPDevice()
 
 ssize_t UDPDevice::write(const uint8_t *buf, uint16_t n)
 {
+    if (!socket.pollout(0)) {
+        return -1;
+    }
     return socket.send(buf, n);
 }