diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp index 410824dcd2..3bcc69fbcb 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.cpp +++ b/libraries/AP_HAL_Linux/UARTDriver.cpp @@ -101,20 +101,9 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) case DEVICE_SERIAL: { - _rd_fd = open(device_path, O_RDWR); - _wr_fd = _rd_fd; - if (_rd_fd == -1) { - ::fprintf(stdout, "Failed to open UART device %s - %s\n", - device_path, strerror(errno)); - return; + if (!_serial_start_connection()) { + return; /* Whatever it might mean */ } - - // always run the file descriptor non-blocking, and deal with - // blocking IO in the higher level calls - fcntl(_rd_fd, F_SETFL, fcntl(_rd_fd, F_GETFL, 0) | O_NONBLOCK); - - // TODO: add proper flow control support - _flow_control = FLOW_CONTROL_DISABLE; break; } default: @@ -248,6 +237,28 @@ LinuxUARTDriver::device_type LinuxUARTDriver::_parseDevicePath(const char *arg) return DEVICE_UNKNOWN; } +bool LinuxUARTDriver::_serial_start_connection() +{ + _rd_fd = open(device_path, O_RDWR); + _wr_fd = _rd_fd; + + if (_rd_fd == -1) { + ::fprintf(stdout, "Failed to open UART device %s - %s\n", + device_path, strerror(errno)); + return false; + } + + // always run the file descriptor non-blocking, and deal with + // blocking IO in the higher level calls + fcntl(_rd_fd, F_SETFL, fcntl(_rd_fd, F_GETFL, 0) | O_NONBLOCK); + + // TODO: add proper flow control support + _flow_control = FLOW_CONTROL_DISABLE; + + return true; +} + + /* start a TCP connection for the serial port. If wait_for_connection is true then block until a client connects diff --git a/libraries/AP_HAL_Linux/UARTDriver.h b/libraries/AP_HAL_Linux/UARTDriver.h index a9b6e21136..3a4e7cd986 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.h +++ b/libraries/AP_HAL_Linux/UARTDriver.h @@ -47,6 +47,7 @@ private: void _allocate_buffers(uint16_t rxS, uint16_t txS); void _tcp_start_connection(bool wait_for_connection); void _udp_start_connection(void); + bool _serial_start_connection(void); enum device_type { DEVICE_TCP,