HAL_PX4: Rework support for FLOW_CONTROL_AUTO.

Now instead of requiring the buffer to fill completely before we can
detect it is not draining, we use a time based mechanism to detect
when none of the first few bytes are transmitted after sitting in our
buffer a half second or more after flow control is enabled.  This
huristic is reliable only for the first several chracters because we
believe that the radio must still have plenty of room in it's own
buffers at that time even if it is not able to transmit them to the
other radio yet.  Note that the original algorithm made the same
assumption.

The new algorithm is especially helpful for cases where only keepalive
messages are transmitted before other packets can be requested by the
GCS.  In this situation, the original code required almost 2 minutes
to disable flow control and allow communication with the GCS.

libraries/AP_HAL_PX4/UARTDriver.cpp

@@ -30,6 +30,7 @@ PX4UARTDriver::PX4UARTDriver(const char *devpath, const char *perf_name) :
     _initialised(false),
     _in_timer(false),
     _perf_uart(perf_alloc(PC_ELAPSED, perf_name)),
+    _os_start_auto_space(-1),
     _flow_control(FLOW_CONTROL_DISABLE)
 {
 }
@@ -111,17 +112,6 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
 		if (_fd == -1) {
 			return;
 		}
-
-        // work out the OS write buffer size by looking at how many
-        // bytes could be written when we first open the port
-        int nwrite = 0;
-        if (ioctl(_fd, FIONWRITE, (unsigned long)&nwrite) == 0) {
-            _os_write_buffer_size = nwrite;
-            if (_os_write_buffer_size & 1) {
-                // it is reporting one short
-                _os_write_buffer_size += 1;
-            }
-        }
 	}
 
 	if (_baudrate != 0) {
@@ -140,9 +130,6 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
 		tcgetattr(_fd, &t);
 		t.c_cflag |= CRTS_IFLOW;
 		tcsetattr(_fd, TCSANOW, &t);
-
-		// reset _total_written to reset flow control auto check
-		_total_written = 0;
 	}
 
     if (_writebuf_size != 0 && _readbuf_size != 0 && _fd != -1) {
@@ -173,6 +160,11 @@ void PX4UARTDriver::set_flow_control(enum flow_control fcontrol)
         t.c_cflag &= ~CRTSCTS;
     }
     tcsetattr(_fd, TCSANOW, &t);
+    if (fcontrol == FLOW_CONTROL_AUTO) {
+        // reset flow control auto state machine
+        _total_written = 0;
+        _first_write_time = 0;
+    }
     _flow_control = fcontrol;
 }
 
@@ -382,19 +374,33 @@ int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
 
     // the FIONWRITE check is to cope with broken O_NONBLOCK behaviour
     // in NuttX on ttyACM0
+
+    // FIONWRITE is also used for auto flow control detection
+    // Assume output flow control is not working if:
+    //     port is configured for auto flow control
+    // and this is not the first write since flow control turned on
+    // and no data has been removed from the buffer since flow control turned on
+    // and more than .5 seconds elapsed after writing a total of > 5 characters
+    //
+    
     int nwrite = 0;
 
     if (ioctl(_fd, FIONWRITE, (unsigned long)&nwrite) == 0) {
-        if (nwrite == 0 &&
+        if (_flow_control == FLOW_CONTROL_AUTO) {
-            _flow_control == FLOW_CONTROL_AUTO &&
+            if (_first_write_time == 0) {
-            _last_write_time != 0 &&
+                if (_total_written == 0) {
-            _total_written != 0 &&
+                    // save the remaining buffer bytes for comparison next write
-            _os_write_buffer_size == _total_written &&
+                    _os_start_auto_space = nwrite;
-            (hal.scheduler->micros64() - _last_write_time) > 500*1000UL) {
+                }
-            // it doesn't look like hw flow control is working
+            } else {
-            ::printf("disabling flow control on %s _total_written=%u\n", 
+                if (_os_start_auto_space - nwrite + 1 >= _total_written &&
-                     _devpath, (unsigned)_total_written);
+                    (hal.scheduler->micros64() - _first_write_time) > 500*1000UL) {
-            set_flow_control(FLOW_CONTROL_DISABLE);
+                    // it doesn't look like hw flow control is working
+                    ::printf("disabling flow control on %s _total_written=%u\n", 
+                             _devpath, (unsigned)_total_written);
+                    set_flow_control(FLOW_CONTROL_DISABLE);
+                }
+            }
         }
         if (nwrite > n) {
             nwrite = n;
@@ -408,6 +414,9 @@ int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
         BUF_ADVANCEHEAD(_writebuf, ret);
         _last_write_time = hal.scheduler->micros64();
         _total_written += ret;
+        if (! _first_write_time && _total_written > 5) {
+            _first_write_time = _last_write_time;
+        }
         return ret;
     }
 

libraries/AP_HAL_PX4/UARTDriver.h

@@ -66,12 +66,13 @@ private:
 
     int _write_fd(const uint8_t *buf, uint16_t n);
     int _read_fd(uint8_t *buf, uint16_t n);
+    uint64_t _first_write_time;
     uint64_t _last_write_time;
 
     void try_initialise(void);
     uint32_t _last_initialise_attempt_ms;
 
-    uint32_t _os_write_buffer_size;
+    uint32_t _os_start_auto_space;
     uint32_t _total_read;
     uint32_t _total_written;
     enum flow_control _flow_control;