13 #include <sys/ioctl.h> 14 #include <sys/signal.h> 19 #include <sys/param.h> 22 #if defined(__linux__) 23 # include <linux/serial.h> 26 #include <sys/select.h> 30 #include <AvailabilityMacros.h> 31 #include <mach/clock.h> 32 #include <mach/mach.h> 39 #define TIOCINQ FIONREAD 41 #define TIOCINQ 0x541B 45 #if defined(MAC_OS_X_VERSION_10_3) && (MAC_OS_X_VERSION_MIN_REQUIRED >= MAC_OS_X_VERSION_10_3) 46 #include <IOKit/serial/ioss.h> 50 using std::stringstream;
51 using std::invalid_argument;
59 MillisecondTimer::MillisecondTimer (
const uint32_t millis)
60 : expiry(timespec_now())
62 int64_t tv_nsec =
expiry.tv_nsec + (millis * 1e6);
64 int64_t sec_diff = tv_nsec /
static_cast<int> (1e9);
65 expiry.tv_nsec = tv_nsec %
static_cast<int>(1e9);
76 int64_t millis = (
expiry.tv_sec - now.tv_sec) * 1e3;
77 millis += (
expiry.tv_nsec - now.tv_nsec) / 1e6;
85 # ifdef __MACH__ // OS X does not have clock_gettime, use clock_get_time 88 host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock);
89 clock_get_time(cclock, &mts);
90 mach_port_deallocate(mach_task_self(), cclock);
91 time.tv_sec = mts.tv_sec;
92 time.tv_nsec = mts.tv_nsec;
94 clock_gettime(CLOCK_MONOTONIC, &time);
103 time.tv_sec = millis / 1e3;
104 time.tv_nsec = (millis - (time.tv_sec * 1e3)) * 1e6;
108 Serial::SerialImpl::SerialImpl (
const string &port,
unsigned long baudrate,
112 : port_ (port), fd_ (-1), is_open_ (false), xonxoff_ (false), rtscts_ (false),
113 baudrate_ (baudrate), parity_ (parity),
114 bytesize_ (bytesize), stopbits_ (stopbits), flowcontrol_ (flowcontrol)
116 pthread_mutex_init(&this->read_mutex, NULL);
117 pthread_mutex_init(&this->write_mutex, NULL);
118 if (port_.empty () ==
false)
122 Serial::SerialImpl::~SerialImpl ()
125 pthread_mutex_destroy(&this->read_mutex);
126 pthread_mutex_destroy(&this->write_mutex);
130 Serial::SerialImpl::open ()
132 if (port_.empty ()) {
133 throw invalid_argument (
"Empty port is invalid.");
135 if (is_open_ ==
true) {
139 fd_ =
::open (port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
160 Serial::SerialImpl::reconfigurePort ()
167 struct termios options;
169 if (tcgetattr(fd_, &options) == -1) {
174 options.c_cflag |= (tcflag_t) (CLOCAL | CREAD);
175 options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL |
178 options.c_oflag &= (tcflag_t) ~(OPOST);
179 options.c_iflag &= (tcflag_t) ~(INLCR | IGNCR | ICRNL | IGNBRK);
181 options.c_iflag &= (tcflag_t) ~IUCLC;
184 options.c_iflag &= (tcflag_t) ~PARMRK;
188 bool custom_baud =
false;
192 case 0: baud = B0;
break;
195 case 50: baud = B50;
break;
198 case 75: baud = B75;
break;
201 case 110: baud = B110;
break;
204 case 134: baud = B134;
break;
207 case 150: baud = B150;
break;
210 case 200: baud = B200;
break;
213 case 300: baud = B300;
break;
216 case 600: baud = B600;
break;
219 case 1200: baud = B1200;
break;
222 case 1800: baud = B1800;
break;
225 case 2400: baud = B2400;
break;
228 case 4800: baud = B4800;
break;
231 case 7200: baud = B7200;
break;
234 case 9600: baud = B9600;
break;
237 case 14400: baud = B14400;
break;
240 case 19200: baud = B19200;
break;
243 case 28800: baud = B28800;
break;
246 case 57600: baud = B57600;
break;
249 case 76800: baud = B76800;
break;
252 case 38400: baud = B38400;
break;
255 case 115200: baud = B115200;
break;
258 case 128000: baud = B128000;
break;
261 case 153600: baud = B153600;
break;
264 case 230400: baud = B230400;
break;
267 case 256000: baud = B256000;
break;
270 case 460800: baud = B460800;
break;
273 case 500000: baud = B500000;
break;
276 case 576000: baud = B576000;
break;
279 case 921600: baud = B921600;
break;
282 case 1000000: baud = B1000000;
break;
285 case 1152000: baud = B1152000;
break;
288 case 1500000: baud = B1500000;
break;
291 case 2000000: baud = B2000000;
break;
294 case 2500000: baud = B2500000;
break;
297 case 3000000: baud = B3000000;
break;
300 case 3500000: baud = B3500000;
break;
303 case 4000000: baud = B4000000;
break;
308 #if defined(MAC_OS_X_VERSION_10_4) && (MAC_OS_X_VERSION_MIN_REQUIRED >= MAC_OS_X_VERSION_10_4) 313 speed_t new_baud =
static_cast<speed_t
> (baudrate_);
314 if (-1 == ioctl (fd_, IOSSIOSPEED, &new_baud, 1)) {
318 #elif defined(__linux__) && defined (TIOCSSERIAL) 319 struct serial_struct ser;
321 if (-1 == ioctl (fd_, TIOCGSERIAL, &ser)) {
326 ser.custom_divisor = ser.baud_base /
static_cast<int> (baudrate_);
328 ser.flags &= ~ASYNC_SPD_MASK;
329 ser.flags |= ASYNC_SPD_CUST;
331 if (-1 == ioctl (fd_, TIOCSSERIAL, &ser)) {
335 throw invalid_argument (
"OS does not currently support custom bauds");
338 if (custom_baud ==
false) {
340 ::cfsetspeed(&options, baud);
342 ::cfsetispeed(&options, baud);
343 ::cfsetospeed(&options, baud);
348 options.c_cflag &= (tcflag_t) ~CSIZE;
350 options.c_cflag |= CS8;
352 options.c_cflag |= CS7;
354 options.c_cflag |= CS6;
356 options.c_cflag |= CS5;
358 throw invalid_argument (
"invalid char len");
361 options.c_cflag &= (tcflag_t) ~(CSTOPB);
364 options.c_cflag |= (CSTOPB);
366 options.c_cflag |= (CSTOPB);
368 throw invalid_argument (
"invalid stop bit");
370 options.c_iflag &= (tcflag_t) ~(INPCK | ISTRIP);
372 options.c_cflag &= (tcflag_t) ~(PARENB | PARODD);
374 options.c_cflag &= (tcflag_t) ~(PARODD);
375 options.c_cflag |= (PARENB);
377 options.c_cflag |= (PARENB | PARODD);
381 options.c_cflag |= (PARENB | CMSPAR | PARODD);
384 options.c_cflag |= (PARENB | CMSPAR);
385 options.c_cflag &= (tcflag_t) ~(PARODD);
390 throw invalid_argument (
"OS does not support mark or space parity");
392 #endif // ifdef CMSPAR 394 throw invalid_argument (
"invalid parity");
412 options.c_iflag |= (IXON | IXOFF);
414 options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | IXANY);
417 options.c_iflag |= (IXON | IXOFF);
419 options.c_iflag &= (tcflag_t) ~(IXON | IXOFF);
424 options.c_cflag |= (CRTSCTS);
426 options.c_cflag &= (
unsigned long) ~(CRTSCTS);
427 #elif defined CNEW_RTSCTS 429 options.c_cflag |= (CNEW_RTSCTS);
431 options.c_cflag &= (
unsigned long) ~(CNEW_RTSCTS);
433 #error "OS Support seems wrong." 440 options.c_cc[VMIN] = 0;
441 options.c_cc[VTIME] = 0;
444 ::tcsetattr (fd_, TCSANOW, &options);
447 uint32_t bit_time_ns = 1e9 / baudrate_;
448 byte_time_ns_ = bit_time_ns * (1 + bytesize_ + parity_ + stopbits_);
458 Serial::SerialImpl::close ()
460 if (is_open_ ==
true) {
475 Serial::SerialImpl::isOpen ()
const 481 Serial::SerialImpl::available ()
487 if (-1 == ioctl (fd_,
TIOCINQ, &count)) {
490 return static_cast<size_t> (count);
495 Serial::SerialImpl::waitReadable (uint32_t timeout)
500 FD_SET (fd_, &readfds);
502 int r = pselect (fd_ + 1, &readfds, NULL, NULL, &timeout_ts, NULL);
506 if (errno == EINTR) {
517 if (!FD_ISSET (fd_, &readfds)) {
519 " in the list, this shouldn't happen!");
526 Serial::SerialImpl::waitByteTimes (
size_t count)
528 timespec wait_time = { 0,
static_cast<long>(byte_time_ns_ * count)};
529 pselect (0, NULL, NULL, NULL, &wait_time, NULL);
533 Serial::SerialImpl::read (uint8_t *buf,
size_t size)
539 size_t bytes_read = 0;
542 long total_timeout_ms = timeout_.read_timeout_constant;
543 total_timeout_ms += timeout_.read_timeout_multiplier *
static_cast<long> (size);
548 ssize_t bytes_read_now =
::read (fd_, buf, size);
549 if (bytes_read_now > 0) {
550 bytes_read = bytes_read_now;
554 while (bytes_read < size) {
555 int64_t timeout_remaining_ms = total_timeout.
remaining();
556 if (timeout_remaining_ms <= 0) {
562 uint32_t timeout = std::min(static_cast<uint32_t> (timeout_remaining_ms),
563 timeout_.inter_byte_timeout);
569 if (size > 1 && timeout_.inter_byte_timeout ==
Timeout::max()) {
571 if (bytes_available + bytes_read < size) {
577 ssize_t bytes_read_now =
578 ::read (fd_, buf + bytes_read, size - bytes_read);
581 if (bytes_read_now < 1) {
586 "returned no data (device disconnected?)");
589 bytes_read +=
static_cast<size_t> (bytes_read_now);
591 if (bytes_read == size) {
595 if (bytes_read < size) {
599 if (bytes_read > size) {
601 "read, this shouldn't happen, might be " 610 Serial::SerialImpl::write (
const uint8_t *data,
size_t length)
612 if (is_open_ ==
false) {
616 size_t bytes_written = 0;
619 long total_timeout_ms = timeout_.write_timeout_constant;
620 total_timeout_ms += timeout_.write_timeout_multiplier *
static_cast<long> (length);
623 bool first_iteration =
true;
624 while (bytes_written < length) {
625 int64_t timeout_remaining_ms = total_timeout.
remaining();
628 if (!first_iteration && (timeout_remaining_ms <= 0)) {
632 first_iteration =
false;
637 FD_SET (fd_, &writefds);
640 int r = pselect (fd_ + 1, NULL, &writefds, NULL, &timeout, NULL);
646 if (errno == EINTR) {
659 if (FD_ISSET (fd_, &writefds)) {
661 ssize_t bytes_written_now =
662 ::write (fd_, data + bytes_written, length - bytes_written);
665 if (bytes_written_now < 1) {
670 "returned no data (device disconnected?)");
673 bytes_written +=
static_cast<size_t> (bytes_written_now);
675 if (bytes_written == length) {
679 if (bytes_written < length) {
683 if (bytes_written > length) {
685 "written, this shouldn't happen, might be " 691 " in the list, this shouldn't happen!");
694 return bytes_written;
698 Serial::SerialImpl::setPort (
const string &port)
704 Serial::SerialImpl::getPort ()
const 716 Serial::SerialImpl::getTimeout ()
const 722 Serial::SerialImpl::setBaudrate (
unsigned long baudrate)
724 baudrate_ = baudrate;
730 Serial::SerialImpl::getBaudrate ()
const 738 bytesize_ = bytesize;
744 Serial::SerialImpl::getBytesize ()
const 758 Serial::SerialImpl::getParity ()
const 766 stopbits_ = stopbits;
772 Serial::SerialImpl::getStopbits ()
const 780 flowcontrol_ = flowcontrol;
786 Serial::SerialImpl::getFlowcontrol ()
const 792 Serial::SerialImpl::flush ()
794 if (is_open_ ==
false) {
801 Serial::SerialImpl::flushInput ()
803 if (is_open_ ==
false) {
806 tcflush (fd_, TCIFLUSH);
810 Serial::SerialImpl::flushOutput ()
812 if (is_open_ ==
false) {
815 tcflush (fd_, TCOFLUSH);
819 Serial::SerialImpl::sendBreak (
int duration)
821 if (is_open_ ==
false) {
824 tcsendbreak (fd_, static_cast<int> (duration / 4));
828 Serial::SerialImpl::setBreak (
bool level)
830 if (is_open_ ==
false) {
835 if (-1 == ioctl (fd_, TIOCSBRK))
838 ss <<
"setBreak failed on a call to ioctl(TIOCSBRK): " << errno <<
" " << strerror(errno);
842 if (-1 == ioctl (fd_, TIOCCBRK))
845 ss <<
"setBreak failed on a call to ioctl(TIOCCBRK): " << errno <<
" " << strerror(errno);
852 Serial::SerialImpl::setRTS (
bool level)
854 if (is_open_ ==
false) {
861 if (-1 == ioctl (fd_, TIOCMBIS, &command))
864 ss <<
"setRTS failed on a call to ioctl(TIOCMBIS): " << errno <<
" " << strerror(errno);
868 if (-1 == ioctl (fd_, TIOCMBIC, &command))
871 ss <<
"setRTS failed on a call to ioctl(TIOCMBIC): " << errno <<
" " << strerror(errno);
878 Serial::SerialImpl::setDTR (
bool level)
880 if (is_open_ ==
false) {
887 if (-1 == ioctl (fd_, TIOCMBIS, &command))
890 ss <<
"setDTR failed on a call to ioctl(TIOCMBIS): " << errno <<
" " << strerror(errno);
894 if (-1 == ioctl (fd_, TIOCMBIC, &command))
897 ss <<
"setDTR failed on a call to ioctl(TIOCMBIC): " << errno <<
" " << strerror(errno);
904 Serial::SerialImpl::waitForChange ()
908 while (is_open_ ==
true) {
912 if (-1 == ioctl (fd_, TIOCMGET, &status))
915 ss <<
"waitForChange failed on a call to ioctl(TIOCMGET): " << errno <<
" " << strerror(errno);
920 if (0 != (status & TIOCM_CTS)
921 || 0 != (status & TIOCM_DSR)
922 || 0 != (status & TIOCM_RI)
923 || 0 != (status & TIOCM_CD))
934 int command = (TIOCM_CD|TIOCM_DSR|TIOCM_RI|TIOCM_CTS);
936 if (-1 == ioctl (fd_, TIOCMIWAIT, &command)) {
938 ss <<
"waitForDSR failed on a call to ioctl(TIOCMIWAIT): " 939 << errno <<
" " << strerror(errno);
947 Serial::SerialImpl::getCTS ()
949 if (is_open_ ==
false) {
955 if (-1 == ioctl (fd_, TIOCMGET, &status))
958 ss <<
"getCTS failed on a call to ioctl(TIOCMGET): " << errno <<
" " << strerror(errno);
963 return 0 != (status & TIOCM_CTS);
968 Serial::SerialImpl::getDSR ()
970 if (is_open_ ==
false) {
976 if (-1 == ioctl (fd_, TIOCMGET, &status))
979 ss <<
"getDSR failed on a call to ioctl(TIOCMGET): " << errno <<
" " << strerror(errno);
984 return 0 != (status & TIOCM_DSR);
989 Serial::SerialImpl::getRI ()
991 if (is_open_ ==
false) {
997 if (-1 == ioctl (fd_, TIOCMGET, &status))
1000 ss <<
"getRI failed on a call to ioctl(TIOCMGET): " << errno <<
" " << strerror(errno);
1005 return 0 != (status & TIOCM_RI);
1010 Serial::SerialImpl::getCD ()
1012 if (is_open_ ==
false) {
1018 if (-1 == ioctl (fd_, TIOCMGET, &status))
1021 ss <<
"getCD failed on a call to ioctl(TIOCMGET): " << errno <<
" " << strerror(errno);
1026 return 0 != (status & TIOCM_CD);
1031 Serial::SerialImpl::readLock ()
1033 int result = pthread_mutex_lock(&this->read_mutex);
1040 Serial::SerialImpl::readUnlock ()
1042 int result = pthread_mutex_unlock(&this->read_mutex);
1049 Serial::SerialImpl::writeLock ()
1051 int result = pthread_mutex_lock(&this->write_mutex);
1058 Serial::SerialImpl::writeUnlock ()
1060 int result = pthread_mutex_unlock(&this->write_mutex);
1066 #endif // !defined(_WIN32)
void waitByteTimes(size_t count)
static timespec timespec_now()
ROSLIB_DECL std::string command(const std::string &cmd)
#define THROW(exceptionClass, message)
size_t read(uint8_t *buffer, size_t size)
timespec timespec_from_ms(const uint32_t millis)
size_t write(const uint8_t *data, size_t size)