diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp index cc6cc87479..410824dcd2 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.cpp +++ b/libraries/AP_HAL_Linux/UARTDriver.cpp @@ -131,16 +131,6 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) } } - // we have enough memory to have a larger transmit buffer for - // all ports. This means we don't get delays while waiting to - // write GPS config packets - if (rxS < 1024) { - rxS = 8192; - } - if (txS < 8192) { - txS = 8192; - } - _initialised = false; while (_in_timer) hal.scheduler->delay(1); @@ -158,6 +148,23 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) tcsetattr(_rd_fd, TCSANOW, &t); } + _allocate_buffers(rxS, txS); +} + +void LinuxUARTDriver::_allocate_buffers(uint16_t rxS, uint16_t txS) +{ + /* we have enough memory to have a larger transmit buffer for + * all ports. This means we don't get delays while waiting to + * write GPS config packets + */ + + if (rxS < 8192) { + rxS = 8192; + } + if (txS < 8192) { + txS = 8192; + } + /* allocate the read buffer */ diff --git a/libraries/AP_HAL_Linux/UARTDriver.h b/libraries/AP_HAL_Linux/UARTDriver.h index 10acca7aa5..a9b6e21136 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.h +++ b/libraries/AP_HAL_Linux/UARTDriver.h @@ -44,6 +44,7 @@ private: bool _packetise; // true if writes should try to be on mavlink boundaries enum flow_control _flow_control; + void _allocate_buffers(uint16_t rxS, uint16_t txS); void _tcp_start_connection(bool wait_for_connection); void _udp_start_connection(void);