linux/net_serial.cpp
Go to the documentation of this file.
1 /*
2  * RPLIDAR SDK
3  *
4  * Copyright (c) 2009 - 2014 RoboPeak Team
5  * http://www.robopeak.com
6  * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
7  * http://www.slamtec.com
8  *
9  */
10 /*
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions are met:
13  *
14  * 1. Redistributions of source code must retain the above copyright notice,
15  * this list of conditions and the following disclaimer.
16  *
17  * 2. Redistributions in binary form must reproduce the above copyright notice,
18  * this list of conditions and the following disclaimer in the documentation
19  * and/or other materials provided with the distribution.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
23  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
24  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
25  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
26  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
27  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
28  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
29  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
30  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
31  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32  *
33  */
34 
35 #include "arch/linux/arch_linux.h"
36 #include <stdio.h>
37 #include <stdlib.h>
38 #include <string.h>
39 #include <unistd.h>
40 #include <assert.h>
41 // linux specific
42 
43 #include <errno.h>
44 #include <fcntl.h>
45 
46 #include <time.h>
47 #include "hal/types.h"
48 #include "arch/linux/net_serial.h"
49 #include <sys/select.h>
50 
51 #include <algorithm>
52 //__GNUC__
53 #if defined(__GNUC__)
54 // for Linux extension
55 #include <asm/ioctls.h>
56 #include <asm/termbits.h>
57 #include <sys/ioctl.h>
58 extern "C" int tcflush(int fildes, int queue_selector);
59 #else
60 // for other standard UNIX
61 #include <termios.h>
62 #include <sys/ioctl.h>
63 
64 #endif
65 
66 
67 namespace rp{ namespace arch{ namespace net{
68 
70  : rp::hal::serial_rxtx()
71  , _baudrate(0)
72  , _flags(0)
73  , serial_fd(-1)
74 {
75  _init();
76 }
77 
79 {
80  close();
81 
82 }
83 
85 {
86  return open(_portName, _baudrate, _flags);
87 }
88 
89 bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags)
90 {
91  strncpy(_portName, portname, sizeof(_portName));
92  _baudrate = baudrate;
93  _flags = flags;
94  return true;
95 }
96 
97 bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags)
98 {
99  if (isOpened()) close();
100 
101  serial_fd = ::open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
102 
103  if (serial_fd == -1) return false;
104 
105 
106 
107 #if !defined(__GNUC__)
108  // for standard UNIX
109  struct termios options, oldopt;
110  tcgetattr(serial_fd, &oldopt);
111  bzero(&options,sizeof(struct termios));
112 
113  // enable rx and tx
114  options.c_cflag |= (CLOCAL | CREAD);
115 
116  _u32 termbaud = getTermBaudBitmap(baudrate);
117 
118  if (termbaud == (_u32)-1) {
119  close();
120  return false;
121  }
122  cfsetispeed(&options, termbaud);
123  cfsetospeed(&options, termbaud);
124 
125  options.c_cflag &= ~PARENB; //no checkbit
126  options.c_cflag &= ~CSTOPB; //1bit stop bit
127  options.c_cflag &= ~CRTSCTS; //no flow control
128 
129  options.c_cflag &= ~CSIZE;
130  options.c_cflag |= CS8; /* Select 8 data bits */
131 
132 #ifdef CNEW_RTSCTS
133  options.c_cflag &= ~CNEW_RTSCTS; // no hw flow control
134 #endif
135 
136  options.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control
137 
138  // raw input mode
139  options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
140  // raw output mode
141  options.c_oflag &= ~OPOST;
142 
143 
144 
145  if (tcsetattr(serial_fd, TCSANOW, &options))
146  {
147  close();
148  return false;
149  }
150 
151 #else
152 
153  // using Linux extension ...
154  struct termios2 tio;
155 
156  ioctl(serial_fd, TCGETS2, &tio);
157  bzero(&tio, sizeof(struct termios2));
158 
159  tio.c_cflag = BOTHER;
160  tio.c_cflag |= (CLOCAL | CREAD | CS8); //8 bit no hardware handshake
161 
162  tio.c_cflag &= ~CSTOPB; //1 stop bit
163  tio.c_cflag &= ~CRTSCTS; //No CTS
164  tio.c_cflag &= ~PARENB; //No Parity
165 
166 #ifdef CNEW_RTSCTS
167  tio.c_cflag &= ~CNEW_RTSCTS; // no hw flow control
168 #endif
169 
170  tio.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control
171 
172 
173  tio.c_cc[VMIN] = 0; //min chars to read
174  tio.c_cc[VTIME] = 0; //time in 1/10th sec wait
175 
176  tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
177  // raw output mode
178  tio.c_oflag &= ~OPOST;
179 
180  tio.c_ispeed = baudrate;
181  tio.c_ospeed = baudrate;
182 
183 
184  ioctl(serial_fd, TCSETS2, &tio);
185 
186 #endif
187 
188 
189  tcflush(serial_fd, TCIFLUSH);
190 
191  if (fcntl(serial_fd, F_SETFL, FNDELAY))
192  {
193  close();
194  return false;
195  }
196 
197 
198  _is_serial_opened = true;
199  _operation_aborted = false;
200 
201  //Clear the DTR bit to let the motor spin
202  clearDTR();
203  do {
204  // create self pipeline for wait cancellation
205  if (pipe(_selfpipe) == -1) break;
206 
207  int flags = fcntl(_selfpipe[0], F_GETFL);
208  if (flags == -1)
209  break;
210 
211  flags |= O_NONBLOCK; /* Make read end nonblocking */
212  if (fcntl(_selfpipe[0], F_SETFL, flags) == -1)
213  break;
214 
215  flags = fcntl(_selfpipe[1], F_GETFL);
216  if (flags == -1)
217  break;
218 
219  flags |= O_NONBLOCK; /* Make write end nonblocking */
220  if (fcntl(_selfpipe[1], F_SETFL, flags) == -1)
221  break;
222 
223  } while (0);
224 
225  return true;
226 }
227 
229 {
230  if (serial_fd != -1)
232  serial_fd = -1;
233 
234  if (_selfpipe[0] != -1)
235  ::close(_selfpipe[0]);
236 
237  if (_selfpipe[1] != -1)
238  ::close(_selfpipe[1]);
239 
240  _selfpipe[0] = _selfpipe[1] = -1;
241 
242  _operation_aborted = false;
243  _is_serial_opened = false;
244 }
245 
246 int raw_serial::senddata(const unsigned char * data, size_t size)
247 {
248 // FIXME: non-block io should be used
249  if (!isOpened()) return 0;
250 
251  if (data == NULL || size ==0) return 0;
252 
253  size_t tx_len = 0;
254  required_tx_cnt = 0;
255  do {
256  int ans = ::write(serial_fd, data + tx_len, size-tx_len);
257 
258  if (ans == -1) return tx_len;
259 
260  tx_len += ans;
261  required_tx_cnt = tx_len;
262  }while (tx_len<size);
263 
264 
265  return tx_len;
266 }
267 
268 
269 int raw_serial::recvdata(unsigned char * data, size_t size)
270 {
271  if (!isOpened()) return 0;
272 
273  int ans = ::read(serial_fd, data, size);
274 
275  if (ans == -1) ans=0;
276  required_rx_cnt = ans;
277  return ans;
278 }
279 
280 
282 {
283  tcflush(serial_fd,TCIFLUSH);
284 }
285 
286 int raw_serial::waitforsent(_u32 timeout, size_t * returned_size)
287 {
288  if (returned_size) *returned_size = required_tx_cnt;
289  return 0;
290 }
291 
292 int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size)
293 {
294  if (!isOpened() ) return -1;
295 
296  if (returned_size) *returned_size = required_rx_cnt;
297  return 0;
298 }
299 
300 int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_size)
301 {
302  size_t length = 0;
303  if (returned_size==NULL) returned_size=(size_t *)&length;
304  *returned_size = 0;
305 
306  int max_fd;
307  fd_set input_set;
308  struct timeval timeout_val;
309 
310  /* Initialize the input set */
311  FD_ZERO(&input_set);
312  FD_SET(serial_fd, &input_set);
313 
314  if (_selfpipe[0] != -1)
315  FD_SET(_selfpipe[0], &input_set);
316 
317  max_fd = std::max<int>(serial_fd, _selfpipe[0]) + 1;
318 
319  /* Initialize the timeout structure */
320  timeout_val.tv_sec = timeout / 1000;
321  timeout_val.tv_usec = (timeout % 1000) * 1000;
322 
323  if ( isOpened() )
324  {
325  if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR;
326  if (*returned_size >= data_count)
327  {
328  return 0;
329  }
330  }
331 
332  while ( isOpened() )
333  {
334  /* Do the select */
335  int n = ::select(max_fd, &input_set, NULL, NULL, &timeout_val);
336 
337  if (n < 0)
338  {
339  // select error
340  *returned_size = 0;
341  return ANS_DEV_ERR;
342  }
343  else if (n == 0)
344  {
345  // time out
346  *returned_size =0;
347  return ANS_TIMEOUT;
348  }
349  else
350  {
351  if (FD_ISSET(_selfpipe[0], &input_set)) {
352  // require aborting the current operation
353  int ch;
354  for (;;) {
355  if (::read(_selfpipe[0], &ch, 1) == -1) {
356  break;
357  }
358 
359  }
360 
361  // treat as timeout
362  *returned_size = 0;
363  return ANS_TIMEOUT;
364  }
365 
366  // data avaliable
367  assert (FD_ISSET(serial_fd, &input_set));
368 
369 
370  if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR;
371  if (*returned_size >= data_count)
372  {
373  return 0;
374  }
375  else
376  {
377  int remain_timeout = timeout_val.tv_sec*1000000 + timeout_val.tv_usec;
378  int expect_remain_time = (data_count - *returned_size)*1000000*8/_baudrate;
379  if (remain_timeout > expect_remain_time)
380  usleep(expect_remain_time);
381  }
382  }
383 
384  }
385 
386  return ANS_DEV_ERR;
387 }
388 
390 {
391  if ( !isOpened() ) return 0;
392  size_t remaining;
393 
394  if (::ioctl(serial_fd, FIONREAD, &remaining) == -1) return 0;
395  return remaining;
396 }
397 
399 {
400  if ( !isOpened() ) return;
401 
402  uint32_t dtr_bit = TIOCM_DTR;
403  ioctl(serial_fd, TIOCMBIS, &dtr_bit);
404 }
405 
407 {
408  if ( !isOpened() ) return;
409 
410  uint32_t dtr_bit = TIOCM_DTR;
411  ioctl(serial_fd, TIOCMBIC, &dtr_bit);
412 }
413 
415 {
416  serial_fd = -1;
417  _portName[0] = 0;
419  _operation_aborted = false;
420  _selfpipe[0] = _selfpipe[1] = -1;
421 }
422 
424 {
425  _operation_aborted = true;
426  if (_selfpipe[1] == -1) return;
427 
428  ::write(_selfpipe[1], "x", 1);
429 }
430 
432 {
433 #define BAUD_CONV( _baud_) case _baud_: return B##_baud_
434 switch (baud) {
435  BAUD_CONV(1200);
436  BAUD_CONV(1800);
437  BAUD_CONV(2400);
438  BAUD_CONV(4800);
439  BAUD_CONV(9600);
440  BAUD_CONV(19200);
441  BAUD_CONV(38400);
442  BAUD_CONV(57600);
443  BAUD_CONV(115200);
444  BAUD_CONV(230400);
445  BAUD_CONV(460800);
446  BAUD_CONV(500000);
447  BAUD_CONV(576000);
448  BAUD_CONV(921600);
449  BAUD_CONV(1000000);
450  BAUD_CONV(1152000);
451  BAUD_CONV(1500000);
452  BAUD_CONV(2000000);
453  BAUD_CONV(2500000);
454  BAUD_CONV(3000000);
455  BAUD_CONV(3500000);
456  BAUD_CONV(4000000);
457  }
458  return -1;
459 }
460 
461 }}} //end rp::arch::net
462 
463 //begin rp::hal
464 namespace rp{ namespace hal{
465 
466 serial_rxtx * serial_rxtx::CreateRxTx()
467 {
468  return new rp::arch::net::raw_serial();
469 }
470 
471 void serial_rxtx::ReleaseRxTx(serial_rxtx *rxtx)
472 {
473  delete rxtx;
474 }
475 
476 }} //end rp::hal
virtual void flush(_u32 flags)
virtual bool isOpened()
Definition: abs_rxtx.h:76
virtual int waitfordata(size_t data_count, _u32 timeout=-1, size_t *returned_size=NULL)
virtual int waitforsent(_u32 timeout=-1, size_t *returned_size=NULL)
#define BAUD_CONV(_baud_)
_u8 size
volatile bool _is_serial_opened
Definition: abs_rxtx.h:82
virtual bool bind(const char *portname, uint32_t baudrate, uint32_t flags=0)
virtual int senddata(const unsigned char *data, size_t size)
virtual int recvdata(unsigned char *data, size_t size)
uint32_t _u32
Definition: rptypes.h:69
virtual int waitforrecv(_u32 timeout=-1, size_t *returned_size=NULL)
_u8 data[0]


rplidar_ros
Author(s):
autogenerated on Wed Jan 1 2020 04:01:40