ardupilot/libraries/AP_HAL_Linux/UARTDevice.cpp

153 lines
2.8 KiB
C++
Raw Normal View History

#include "UARTDevice.h"
2015-06-15 18:20:29 -03:00
#include <errno.h>
#include <fcntl.h>
#include <poll.h>
#include <stdio.h>
#include <termios.h>
2015-06-15 18:20:29 -03:00
#include <unistd.h>
#include <AP_HAL/AP_HAL.h>
UARTDevice::UARTDevice(const char *device_path):
2015-06-15 18:20:29 -03:00
_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 | O_NOCTTY);
2015-06-15 18:20:29 -03:00
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);
2015-06-15 18:20:29 -03:00
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);
}
void UARTDevice::set_flow_control(AP_HAL::UARTDriver::flow_control flow_control_setting)
{
struct termios t;
if (_flow_control == flow_control_setting) {
return;
}
tcgetattr(_fd, &t);
if (flow_control_setting != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE) {
t.c_cflag |= CRTSCTS;
} else {
t.c_cflag &= ~CRTSCTS;
}
tcsetattr(_fd, TCSANOW, &t);
_flow_control = flow_control_setting;
}
void UARTDevice::set_parity(int v)
{
struct termios t;
tcgetattr(_fd, &t);
if (v != 0) {
// enable parity
t.c_cflag |= PARENB;
if (v == 1) {
t.c_cflag |= PARODD;
} else {
t.c_cflag &= ~PARODD;
}
}
else {
// disable parity
t.c_cflag &= ~PARENB;
}
tcsetattr(_fd, TCSANOW, &t);
}