vpSerial.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Serial communication.
 *
*****************************************************************************/
 
#include <visp3/core/vpConfig.h>
 
#if !defined(_WIN32)
 
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <termios.h>
#include <unistd.h>
 
#include <visp3/core/vpException.h>
#include <visp3/core/vpSerial.h>
 
#ifndef TIOCINQ
#ifdef FIONREAD
#define TIOCINQ FIONREAD
#else
#define TIOCINQ 0x541B
#endif
#endif
 
vpSerial::vpSerial(const std::string &port, unsigned long baudrate, bytesize_t bytesize, parity_t parity,
                   stopbits_t stopbits, flowcontrol_t flowcontrol)
  : m_port(port), m_fd(-1), m_is_open(false), m_xonxoff(false), m_rtscts(false), m_baudrate(baudrate), m_parity(parity),
    m_bytesize(bytesize), m_stopbits(stopbits), m_flowcontrol(flowcontrol)
{
  if (m_port.empty() == false)
    open();
}
 
vpSerial::~vpSerial() { close(); }
 
void vpSerial::setBaudrate(const unsigned long baudrate) { m_baudrate = baudrate; }
 
void vpSerial::setBytesize(const bytesize_t &bytesize) { m_bytesize = bytesize; }
 
void vpSerial::setFlowcontrol(const flowcontrol_t &flowcontrol) { m_flowcontrol = flowcontrol; }
 
void vpSerial::setParity(const parity_t &parity) { m_parity = parity; }
 
void vpSerial::setStopbits(const stopbits_t &stopbits) { m_stopbits = stopbits; }
 
void vpSerial::setPort(const std::string &port) { m_port = port; }
 
int vpSerial::available()
{
  if (!m_is_open) {
    return 0;
  }
  int count = 0;
  if (-1 == ioctl(m_fd, TIOCINQ, &count)) {
    throw(vpException(vpException::fatalError, "Cannot check is serial port data available"));
  } else {
    return count;
  }
}
 
void vpSerial::close()
{
  if (m_is_open == true) {
    if (m_fd != -1) {
      int ret;
      ret = ::close(m_fd);
      if (ret == 0) {
        m_fd = -1;
      } else {
        throw(vpException(vpException::fatalError, "Cannot close serial port"));
      }
    }
    m_is_open = false;
  }
}
 
void vpSerial::open()
{
  if (m_port.empty()) {
    throw(vpException(vpException::fatalError, "Serial port empty"));
  }
  if (m_is_open == true) {
    throw(vpException(vpException::fatalError, "Serial port already open"));
  }
 
  m_fd = ::open(m_port.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
 
  if (m_fd == -1) {
    switch (errno) {
    case EINTR:
      // Try again because this is a recoverable error.
      open();
      return;
    case ENFILE:
    case EMFILE:
      throw(vpException(vpException::fatalError, "Serial port has to many handles open"));
    default:
      throw(vpException(vpException::fatalError, "Serial port opening error"));
    }
  }
 
  configure();
  m_is_open = true;
}
 
bool vpSerial::read(char *c, long timeout_s)
{
  if (m_is_open == false) {
    throw(vpException(vpException::fatalError, "Serial port not opened"));
  }
 
  fd_set readfds;                          /* list of fds for select to listen to */
  struct timeval timeout = {timeout_s, 0}; // seconde, micro-sec
 
  FD_ZERO(&readfds);
  FD_SET(static_cast<unsigned int>(m_fd), &readfds);
 
  int ret = select(FD_SETSIZE, &readfds, (fd_set *)nullptr, (fd_set *)nullptr, &timeout);
 
  if (ret < 0) {
    throw(vpException(vpException::fatalError, "Serial i/o exception"));
  } else if (ret == 0) {
    // Timeout occurred
    return false;
  } else {
    ssize_t n = ::read(m_fd, c, 1); // read one character at a time
    if (n != 1)
      return false;
  }
  return true;
}
 
std::string vpSerial::readline(const std::string &eol)
{
  char c;
  size_t eol_len = eol.length();
  std::string line;
 
  while (true) {
    size_t bytes_read = this->read(&c, 1);
    if (bytes_read == 0) {
      break; // Timeout occurred on reading 1 byte
    }
    line.append(&c, 1);
    if (std::string(line, line.size() - eol_len, eol_len) == eol) {
      break; // EOL found
    }
  }
  return line;
}
 
void vpSerial::write(const std::string &s)
{
  if (m_is_open == false) {
    throw(vpException(vpException::fatalError, "Serial port not opened"));
  }
 
  ssize_t r = ::write(m_fd, s.c_str(), s.size());
  if (r != (ssize_t)(s.size())) {
    throw(vpException(vpException::fatalError, "Serial port write error"));
  }
}
 
void vpSerial::configure()
{
  if (m_fd == -1) {
    throw(vpException(vpException::fatalError, "Serial port not opened"));
  }
 
  struct termios options;
 
  if (tcgetattr(m_fd, &options) == -1) {
    ::close(m_fd);
    throw vpException(vpException::fatalError, "Cannot get serial configuration");
  }
 
  // set up raw mode / no echo / binary
  options.c_cflag |= (tcflag_t)(CLOCAL | CREAD);
  options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG | IEXTEN); //|ECHOPRT
 
  options.c_oflag &= (tcflag_t) ~(OPOST);
  options.c_iflag &= (tcflag_t) ~(INLCR | IGNCR | ICRNL | IGNBRK);
 
#ifdef IUCLC
  options.c_iflag &= (tcflag_t)~IUCLC;
#endif
#ifdef PARMRK
  options.c_iflag &= (tcflag_t)~PARMRK;
#endif
 
  speed_t baudrate;
  switch (m_baudrate) {
#ifdef B0
  case 0:
    baudrate = B0;
    break;
#endif
#ifdef B50
  case 50:
    baudrate = B50;
    break;
#endif
#ifdef B75
  case 75:
    baudrate = B75;
    break;
#endif
#ifdef B110
  case 110:
    baudrate = B110;
    break;
#endif
#ifdef B134
  case 134:
    baudrate = B134;
    break;
#endif
#ifdef B150
  case 150:
    baudrate = B150;
    break;
#endif
#ifdef B200
  case 200:
    baudrate = B200;
    break;
#endif
#ifdef B300
  case 300:
    baudrate = B300;
    break;
#endif
#ifdef B600
  case 600:
    baudrate = B600;
    break;
#endif
#ifdef B1200
  case 1200:
    baudrate = B1200;
    break;
#endif
#ifdef B1800
  case 1800:
    baudrate = B1800;
    break;
#endif
#ifdef B2400
  case 2400:
    baudrate = B2400;
    break;
#endif
#ifdef B4800
  case 4800:
    baudrate = B4800;
    break;
#endif
#ifdef B9600
  case 9600:
    baudrate = B9600;
    break;
#endif
#ifdef B14400
  case 14400:
    baudrate = B14400;
    break;
#endif
#ifdef B19200
  case 19200:
    baudrate = B19200;
    break;
#endif
#ifdef B38400
  case 38400:
    baudrate = B38400;
    break;
#endif
#ifdef B57600
  case 57600:
    baudrate = B57600;
    break;
#endif
#ifdef B115200
  case 115200:
    baudrate = B115200;
    break;
#endif
#ifdef B230400
  case 230400:
    baudrate = B230400;
    break;
#endif
#ifdef B460800
  case 460800:
    baudrate = B460800;
    break;
#endif
#ifdef B500000
  case 500000:
    baudrate = B500000;
    break;
#endif
#ifdef B576000
  case 576000:
    baudrate = B576000;
    break;
#endif
#ifdef B921600
  case 921600:
    baudrate = B921600;
    break;
#endif
#ifdef B1000000
  case 1000000:
    baudrate = B1000000;
    break;
#endif
#ifdef B1152000
  case 1152000:
    baudrate = B1152000;
    break;
#endif
#ifdef B1500000
  case 1500000:
    baudrate = B1500000;
    break;
#endif
#ifdef B2000000
  case 2000000:
    baudrate = B2000000;
    break;
#endif
#ifdef B2500000
  case 2500000:
    baudrate = B2500000;
    break;
#endif
#ifdef B3000000
  case 3000000:
    baudrate = B3000000;
    break;
#endif
#ifdef B3500000
  case 3500000:
    baudrate = B3500000;
    break;
#endif
#ifdef B4000000
  case 4000000:
    baudrate = B4000000;
    break;
#endif
  default:
    throw vpException(vpException::fatalError, "Cannot set serial baudrate to %ld", m_baudrate);
  }
 
#ifdef _BSD_SOURCE
  ::cfsetspeed(&options, baudrate);
#else
  ::cfsetispeed(&options, baudrate);
  ::cfsetospeed(&options, baudrate);
#endif
 
  // setup char len
  options.c_cflag &= (tcflag_t)~CSIZE;
  switch (m_bytesize) {
  case eightbits:
    options.c_cflag |= CS8;
    break;
  case sevenbits:
    options.c_cflag |= CS7;
    break;
  case sixbits:
    options.c_cflag |= CS6;
    break;
  case fivebits:
    options.c_cflag |= CS5;
    break;
  }
 
  switch (m_stopbits) {
  case stopbits_one:
    options.c_cflag &= (tcflag_t) ~(CSTOPB);
    break;
  case stopbits_two:
    options.c_cflag |= (CSTOPB);
    break;
  }
 
  // setup parity
  options.c_iflag &= (tcflag_t) ~(INPCK | ISTRIP);
  switch (m_parity) {
  case parity_none:
    options.c_cflag &= (tcflag_t) ~(PARENB | PARODD);
    break;
  case parity_even:
    options.c_cflag &= (tcflag_t) ~(PARODD);
    options.c_cflag |= (PARENB);
    break;
  case parity_odd:
    options.c_cflag |= (PARENB | PARODD);
    break;
  }
 
  // setup flow control
  switch (m_flowcontrol) {
  case flowcontrol_none:
    m_xonxoff = false;
    m_rtscts = false;
    break;
  case flowcontrol_software:
    m_xonxoff = true;
    m_rtscts = false;
    break;
  case flowcontrol_hardware:
    m_xonxoff = false;
    m_rtscts = true;
    break;
  }
 
  // xonxoff
  if (m_xonxoff)
    options.c_iflag |= (IXON | IXOFF);
  else
#ifdef IXANY
    options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | IXANY);
#else
    options.c_iflag &= (tcflag_t) ~(IXON | IXOFF);
#endif
 
    // rtscts
#ifdef CRTSCTS
  if (m_rtscts)
    options.c_cflag |= (CRTSCTS);
  else
    options.c_cflag &= (unsigned long)~(CRTSCTS);
#elif defined CNEW_RTSCTS
  if (m_rtscts)
    options.c_cflag |= (CNEW_RTSCTS);
  else
    options.c_cflag &= (unsigned long)~(CNEW_RTSCTS);
#else
#error "OS doesn't support serial rtscts"
#endif
 
  options.c_cc[VMIN] = 0;
  options.c_cc[VTIME] = 0;
 
  // activate settings
  ::tcsetattr(m_fd, TCSANOW, &options);
}
 
#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work around to avoid warning: libvisp_ar.a(vpAROgre.cpp.o) has no symbols
void dummy_vpSerial(){};
#endif