From 492c733750df08ed997298732b0855fe288908b5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 10 Jan 2016 17:17:32 +1100 Subject: [PATCH] HAL_SITL: added RingBuffers to UART driver makes for faster log download --- libraries/AP_HAL_SITL/Scheduler.cpp | 7 ++ libraries/AP_HAL_SITL/UARTDriver.cpp | 146 +++++++++++++-------------- libraries/AP_HAL_SITL/UARTDriver.h | 13 ++- 3 files changed, 86 insertions(+), 80 deletions(-) diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index f95c192071..78018f2723 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -5,6 +5,7 @@ #include "AP_HAL_SITL.h" #include "Scheduler.h" +#include "UARTDriver.h" #include #include #include @@ -213,6 +214,12 @@ void SITLScheduler::_run_io_procs(bool called_from_isr) } _in_io_proc = false; + + SITLUARTDriver::from(hal.uartA)->_timer_tick(); + SITLUARTDriver::from(hal.uartB)->_timer_tick(); + SITLUARTDriver::from(hal.uartC)->_timer_tick(); + SITLUARTDriver::from(hal.uartD)->_timer_tick(); + SITLUARTDriver::from(hal.uartE)->_timer_tick(); } /* diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 39e9160487..03125a3da8 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -50,13 +50,6 @@ bool SITLUARTDriver::_console; void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) { - if (txSpace != 0) { - _txSpace = txSpace; - } - if (rxSpace != 0) { - _rxSpace = rxSpace; - } - const char *path = _sitlState->_uart_path[_portNumber]; if (strcmp(path, "GPS1") == 0) { @@ -99,6 +92,8 @@ void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) } free(s); } + + _set_nonblocking(_fd); } void SITLUARTDriver::end() @@ -113,73 +108,28 @@ int16_t SITLUARTDriver::available(void) return 0; } - if (_select_check(_fd)) { -#ifdef FIONREAD - // use FIONREAD to get exact value if possible - int num_ready; - if (ioctl(_fd, FIONREAD, &num_ready) == 0) { - if (num_ready > _rxSpace) { - return _rxSpace; - } - if (num_ready == 0) { - // EOF is reached - fprintf(stdout, "Closed connection on serial port %u\n", _portNumber); - close(_fd); - _connected = false; - return 0; - } - return num_ready; - } -#endif - return 1; // best we can do is say 1 byte available - } - return 0; + return _readbuffer.available(); } int16_t SITLUARTDriver::txspace(void) { - // always claim there is space available - return _txSpace; + _check_connection(); + if (!_connected) { + return 0; + } + return _writebuffer.space(); } int16_t SITLUARTDriver::read(void) { - char c; - if (available() <= 0) { return -1; } - - if (_portNumber == 1 || _portNumber == 4) { - if (_sitlState->gps_read(_fd, &c, 1) == 1) { - return (uint8_t)c; - } - return -1; - } - - if (!_use_send_recv) { - int fd = _console?0:_fd; - if (::read(fd, &c, 1) != 1) { - return -1; - } - return (uint8_t)c; - } - - int n = recv(_fd, &c, 1, MSG_DONTWAIT); - if (n <= 0) { - // the socket has reached EOF - close(_fd); - _connected = false; - fprintf(stdout, "Closed connection on serial port %u\n", _portNumber); - fflush(stdout); - return -1; - } - if (n == 1) { - return (uint8_t)c; - } - return -1; + uint8_t c; + _readbuffer.read(&c, 1); + return c; } void SITLUARTDriver::flush(void) @@ -188,29 +138,26 @@ void SITLUARTDriver::flush(void) size_t SITLUARTDriver::write(uint8_t c) { - int flags = 0; - _check_connection(); - if (!_connected) { + if (txspace() <= 0) { return 0; } - if (_nonblocking_writes) { - flags |= MSG_DONTWAIT; - } - if (!_use_send_recv) { - return ::write(_fd, &c, 1); - } - return send(_fd, &c, 1, flags); + _writebuffer.write(&c, 1); + return 1; } size_t SITLUARTDriver::write(const uint8_t *buffer, size_t size) { - size_t n = 0; - while (size--) { - n += write(*buffer++); + if (txspace() <= (ssize_t)size) { + size = txspace(); } - return n; + if (size <= 0) { + return 0; + } + _writebuffer.write(buffer, size); + return size; } + /* start a TCP connection for the serial port. If wait_for_connection is true then block until a client connects @@ -436,4 +383,51 @@ void SITLUARTDriver::_set_nonblocking(int fd) fcntl(fd, F_SETFL, v | O_NONBLOCK); } -#endif +void SITLUARTDriver::_timer_tick(void) +{ + uint32_t navail; + ssize_t nwritten; + + const uint8_t *readptr = _writebuffer.readptr(navail); + if (readptr && navail > 0) { + if (!_use_send_recv) { + nwritten = ::write(_fd, readptr, navail); + } else { + nwritten = send(_fd, readptr, navail, MSG_DONTWAIT); + } + if (nwritten > 0) { + _writebuffer.advance(nwritten); + } + } + + if (_readbuffer.space() == 0) { + return; + } + + ssize_t nread; + char c; + if (!_use_send_recv) { + int fd = _console?0:_fd; + nread = ::read(fd, &c, 1); + } else { + if (_select_check(_fd)) { + nread = recv(_fd, &c, 1, MSG_DONTWAIT); + if (nread <= 0) { + // the socket has reached EOF + close(_fd); + _connected = false; + fprintf(stdout, "Closed connection on serial port %u\n", _portNumber); + fflush(stdout); + return; + } + } else { + nread = 0; + } + } + if (nread == 1) { + _readbuffer.write((uint8_t *)&c, 1); + } +} + +#endif // CONFIG_HAL_BOARD + diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h index ebe9e9d785..cd61d9a17e 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.h +++ b/libraries/AP_HAL_SITL/UARTDriver.h @@ -9,6 +9,7 @@ #include #include "AP_HAL_SITL_Namespace.h" #include +#include class HALSITL::SITLUARTDriver : public AP_HAL::UARTDriver { public: @@ -16,14 +17,16 @@ public: SITLUARTDriver(const uint8_t portNumber, SITL_State *sitlState) { _portNumber = portNumber; - _rxSpace = _default_rx_buffer_size; - _txSpace = _default_tx_buffer_size; _sitlState = sitlState; _fd = -1; _listen_fd = -1; } + static SITLUARTDriver *from(AP_HAL::UARTDriver *uart) { + return static_cast(uart); + } + /* Implementations of UARTDriver virtual methods */ void begin(uint32_t b) { begin(b, 0, 0); @@ -57,6 +60,8 @@ public: int _fd; enum flow_control get_flow_control(void) { return FLOW_CONTROL_ENABLE; } + + void _timer_tick(void); private: uint8_t _portNumber; @@ -66,8 +71,8 @@ private: int _serial_port; static bool _console; bool _nonblocking_writes; - uint16_t _rxSpace; - uint16_t _txSpace; + ByteBuffer _readbuffer{16384}; + ByteBuffer _writebuffer{16384}; // IPv4 address of target for uartC const char *_tcp_client_addr;