diff --git a/libraries/AP_HAL_Linux/UARTDevice.cpp b/libraries/AP_HAL_Linux/UARTDevice.cpp new file mode 100644 index 0000000000..7187f4dbf1 --- /dev/null +++ b/libraries/AP_HAL_Linux/UARTDevice.cpp @@ -0,0 +1,123 @@ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX + +#include +#include +#include +#include +#include +#include + +#include "UARTDevice.h" + +#include +#include +#include +#include +#include +#include + +UARTDevice::UARTDevice(const char *device_path): + _device_path(device_path) +{ +} + +UARTDevice::~UARTDevice() +{ + +} + +bool UARTDevice::close() +{ + if (_fd != -1) { + if (::close(_fd) < 0) { + return false; + } + } + + _fd = -1; + + return true; +} + +bool UARTDevice::open() +{ + _fd = ::open(_device_path, O_RDWR | O_CLOEXEC); + + if (_fd < 0) { + ::fprintf(stderr, "Failed to open UART device %s - %s\n", + _device_path, strerror(errno)); + return false; + } + + _disable_crlf(); + + return true; +} + +ssize_t UARTDevice::read(uint8_t *buf, uint16_t n) +{ + return ::read(_fd, buf, n); +} + +ssize_t UARTDevice::write(const uint8_t *buf, uint16_t n) +{ + struct pollfd fds; + fds.fd = _fd; + fds.events = POLLOUT; + fds.revents = 0; + + int ret = 0; + + if (poll(&fds, 1, 0) == 1) { + ret = ::write(_fd, buf, n); + } + + return ret; +} + +void UARTDevice::set_blocking(bool blocking) +{ + int flags = fcntl(_fd, F_GETFL, 0); + + if (blocking) { + flags = flags & ~O_NONBLOCK; + } else { + flags = flags | O_NONBLOCK; + } + + if (fcntl(_fd, F_SETFL, flags) < 0) { + ::fprintf(stderr, "Failed to make UART nonblocking %s - %s\n", + _device_path, strerror(errno)); + } + +} + +void UARTDevice::_disable_crlf() +{ + struct termios t; + memset(&t, 0, sizeof(t)); + + tcgetattr(_fd, &t); + + // disable LF -> CR/LF + t.c_iflag &= ~(BRKINT | ICRNL | IMAXBEL | IXON | IXOFF); + t.c_oflag &= ~(OPOST | ONLCR); + t.c_lflag &= ~(ISIG | ICANON | IEXTEN | ECHO | ECHOE | ECHOK | ECHOCTL | ECHOKE); + t.c_cc[VMIN] = 0; + + tcsetattr(_fd, TCSANOW, &t); +} + +void UARTDevice::set_speed(uint32_t baudrate) +{ + struct termios t; + memset(&t, 0, sizeof(t)); + + tcgetattr(_fd, &t); + cfsetspeed(&t, baudrate); + tcsetattr(_fd, TCSANOW, &t); +} + +#endif diff --git a/libraries/AP_HAL_Linux/UARTDevice.h b/libraries/AP_HAL_Linux/UARTDevice.h new file mode 100644 index 0000000000..bb6f9a7d66 --- /dev/null +++ b/libraries/AP_HAL_Linux/UARTDevice.h @@ -0,0 +1,26 @@ +#ifndef __AP_HAL_LINUX_UARTDEVICE_UDP_H__ +#define __AP_HAL_LINUX_UARTDEVICE_UDP_H__ + +#include "SerialDevice.h" +#include "../AP_HAL/utility/Socket.h" + +class UARTDevice: public SerialDevice { +public: + UARTDevice(const char *device_path); + virtual ~UARTDevice(); + + virtual bool open() override; + virtual bool close() override; + virtual ssize_t write(const uint8_t *buf, uint16_t n) override; + virtual ssize_t read(uint8_t *buf, uint16_t n) override; + virtual void set_blocking(bool blocking) override; + virtual void set_speed(uint32_t speed) override; + +private: + void _disable_crlf(); + + int _fd = -1; + const char *_device_path; +}; + +#endif