Line data Source code
1 : #include "UARTDevice.h"
2 :
3 : #include <errno.h>
4 : #include <fcntl.h>
5 : #include <poll.h>
6 : #include <stdio.h>
7 : #include <sys/ioctl.h>
8 : #include <asm/ioctls.h>
9 : #include <asm/termbits.h>
10 : #include <unistd.h>
11 :
12 : #include <AP_HAL/AP_HAL.h>
13 :
14 0 : UARTDevice::UARTDevice(const char *device_path):
15 0 : _device_path(device_path)
16 : {
17 0 : }
18 :
19 0 : UARTDevice::~UARTDevice()
20 : {
21 0 : }
22 :
23 0 : bool UARTDevice::close()
24 : {
25 0 : if (_fd != -1) {
26 0 : if (::close(_fd) < 0) {
27 0 : return false;
28 : }
29 : }
30 :
31 0 : _fd = -1;
32 :
33 0 : return true;
34 : }
35 :
36 0 : bool UARTDevice::open()
37 : {
38 0 : _fd = ::open(_device_path, O_RDWR | O_CLOEXEC | O_NOCTTY);
39 :
40 0 : if (_fd < 0) {
41 0 : ::fprintf(stderr, "Failed to open UART device %s - %s\n",
42 0 : _device_path, strerror(errno));
43 0 : return false;
44 : }
45 :
46 0 : _disable_crlf();
47 :
48 0 : return true;
49 : }
50 :
51 0 : ssize_t UARTDevice::read(uint8_t *buf, uint16_t n)
52 : {
53 0 : return ::read(_fd, buf, n);
54 : }
55 :
56 0 : ssize_t UARTDevice::write(const uint8_t *buf, uint16_t n)
57 : {
58 : struct pollfd fds;
59 0 : fds.fd = _fd;
60 0 : fds.events = POLLOUT;
61 0 : fds.revents = 0;
62 :
63 0 : int ret = 0;
64 :
65 0 : if (poll(&fds, 1, 0) == 1) {
66 0 : ret = ::write(_fd, buf, n);
67 : }
68 :
69 0 : return ret;
70 : }
71 :
72 0 : void UARTDevice::set_blocking(bool blocking)
73 : {
74 0 : int flags = fcntl(_fd, F_GETFL, 0);
75 :
76 0 : if (blocking) {
77 0 : flags = flags & ~O_NONBLOCK;
78 : } else {
79 0 : flags = flags | O_NONBLOCK;
80 : }
81 :
82 0 : if (fcntl(_fd, F_SETFL, flags) < 0) {
83 0 : ::fprintf(stderr, "Failed to make UART nonblocking %s - %s\n",
84 0 : _device_path, strerror(errno));
85 : }
86 :
87 0 : }
88 :
89 0 : void UARTDevice::_disable_crlf()
90 : {
91 0 : struct termios2 t = { 0 };
92 :
93 0 : if (ioctl(_fd, TCGETS2, &t) != 0) {
94 0 : ::fprintf(stderr, "Failed to read serial options for %s - %s\n",
95 0 : _device_path, strerror(errno));
96 0 : return;
97 : }
98 :
99 : // disable LF -> CR/LF
100 0 : t.c_iflag &= ~(BRKINT | ICRNL | IMAXBEL | IXON | IXOFF);
101 0 : t.c_oflag &= ~(OPOST | ONLCR);
102 0 : t.c_lflag &= ~(ISIG | ICANON | IEXTEN | ECHO | ECHOE | ECHOK | ECHOCTL | ECHOKE);
103 0 : t.c_cc[VMIN] = 0;
104 :
105 0 : if (ioctl(_fd, TCSETS2, &t) != 0) {
106 0 : ::fprintf(stderr, "Failed to disable crlf on %s - %s\n",
107 0 : _device_path, strerror(errno));
108 0 : return;
109 : }
110 : }
111 :
112 0 : void UARTDevice::set_speed(uint32_t baudrate)
113 : {
114 0 : struct termios2 tio = { 0 };
115 :
116 0 : if (ioctl(_fd, TCGETS2, &tio) != 0) {
117 0 : ::fprintf(stderr, "Failed to read serial options for %s - %s\n",
118 0 : _device_path, strerror(errno));
119 0 : return;
120 : }
121 :
122 : // use CBAUDEX and B(aud)OTHER to gain access to "non-standard" rates that are common for eg. RC receivers
123 0 : tio.c_cflag &= ~CBAUD;
124 0 : tio.c_cflag |= CBAUDEX;
125 : #if defined BOTHER
126 0 : tio.c_cflag |= BOTHER;
127 : #endif
128 0 : tio.c_ispeed = baudrate;
129 0 : tio.c_ospeed = baudrate;
130 :
131 0 : if (ioctl(_fd, TCSETS2, &tio) != 0) {
132 0 : ::fprintf(stderr, "Failed to set serial baud to %d for %s - %s\n",
133 0 : baudrate, _device_path, strerror(errno));
134 0 : return;
135 : }
136 : }
137 :
138 0 : void UARTDevice::set_flow_control(AP_HAL::UARTDriver::flow_control flow_control_setting)
139 : {
140 0 : if (_flow_control == flow_control_setting) {
141 0 : return;
142 : }
143 :
144 0 : struct termios2 t = { 0 };
145 :
146 0 : if (ioctl(_fd, TCGETS2, &t) != 0) {
147 0 : ::fprintf(stderr, "Failed to read serial options for %s - %s\n",
148 0 : _device_path, strerror(errno));
149 0 : return;
150 : }
151 :
152 0 : if (flow_control_setting != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE) {
153 0 : t.c_cflag |= CRTSCTS;
154 : } else {
155 0 : t.c_cflag &= ~CRTSCTS;
156 : }
157 :
158 0 : if (ioctl(_fd, TCSETS2, &t) != 0) {
159 0 : ::fprintf(stderr, "Failed to set flow control for %s - %s\n",
160 0 : _device_path, strerror(errno));
161 0 : return;
162 : }
163 :
164 0 : _flow_control = flow_control_setting;
165 : }
166 :
167 0 : void UARTDevice::set_parity(int v)
168 : {
169 0 : struct termios2 t = { 0 };
170 :
171 0 : if (ioctl(_fd, TCGETS2, &t) != 0) {
172 0 : ::fprintf(stderr, "Failed to read serial options for %s - %s\n",
173 0 : _device_path, strerror(errno));
174 0 : return;
175 : }
176 :
177 0 : if (v != 0) {
178 : // enable parity
179 0 : t.c_cflag |= PARENB;
180 0 : if (v == 1) {
181 0 : t.c_cflag |= PARODD;
182 : } else {
183 0 : t.c_cflag &= ~PARODD;
184 : }
185 : }
186 : else {
187 : // disable parity
188 0 : t.c_cflag &= ~PARENB;
189 : }
190 :
191 0 : if (ioctl(_fd, TCSETS2, &t) != 0) {
192 0 : ::fprintf(stderr, "Failed to set parity for %s - %s\n",
193 0 : _device_path, strerror(errno));
194 0 : return;
195 : }
196 : }
|
