serial.cpp

Go to the documentation of this file.

#include "toposens_driver/serial.h"

#include <fcntl.h>
#include <ros/console.h>
#include <string.h>

namespace toposens_driver
{
Serial::Serial(std::string port)
{
  _fd = -1;
  _port = port;

  // Open serial port to access sensor stream
  _fd = open(_port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);

  int retry_counter = 5;

  while (_fd == -1)
  {
    std::string msg = "Error opening connection at " + _port + ": " + strerror(errno);

    if (retry_counter--)
    {
      ROS_WARN_STREAM(msg);
      ros::Duration(0.5).sleep();

      ROS_INFO_STREAM("Retrying to establish connection at " << _port << " ...");
      _fd = open(_port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
    }
    else
    {
      throw std::runtime_error(msg);
    }
  }
  ROS_DEBUG("Toposens serial established with fd %d\n", _fd);

  // Set options of serial data transfer
  struct termios tty;
  memset(&tty, 0, sizeof(tty));

  // Get current attributes of termios structure
  if (tcgetattr(_fd, &tty) != 0)
  {
    ROS_WARN("Error retrieving attributes at %s: %s", _port.c_str(), strerror(errno));
    return;
  }

  // set I/O baud rate
  cfsetispeed(&tty, kBaud);
  cfsetospeed(&tty, kBaud);

  // enable reading, ignore ctrl lines, 8 bits per byte
  tty.c_cflag |= CREAD | CLOCAL | CS8;
  // turn off parity, use one stop bit, disable HW flow control
  tty.c_cflag &= ~(PARENB | CSTOPB | CRTSCTS);

  // disable canonical mode and echoes
  tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHONL);

  // disable software flow control
  tty.c_iflag &= ~(IXON | IXOFF | IXANY);
  // disable break condition handling
  tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK);
  // disable special data pre-processing
  tty.c_iflag &= ~(ISTRIP | INLCR | IGNCR | ICRNL);

  // disable special data post-processing
  tty.c_oflag &= ~(OPOST | ONLCR);

  // wait for at least 1 byte to be received
  // length of a valid empty data frame
  tty.c_cc[VMIN] = 1;
  // wait for 0.1s till data received
  tty.c_cc[VTIME] = 1;

  // Set attributes of termios structure
  if (tcsetattr(_fd, TCSANOW, &tty) != 0)
  {
    ROS_WARN("Error configuring device at %s: %s", _port.c_str(), strerror(errno));
    return;
  }

  ROS_DEBUG(
      "Serial settings updated:\n  BaudRate = %d \n  DataBits = 8 \n  "
      "Parity = disabled",
      kBaud);
  tcflush(_fd, TCIFLUSH);  // Discard old data in RX buffer
  ROS_INFO("Device at %s ready for communication", _port.c_str());
}

Serial::~Serial(void)
{
  ROS_INFO("Closing serial connection...");
  tcflush(_fd, TCIOFLUSH);

  if (close(_fd) == -1)
  {
    ROS_ERROR("Error closing serial connection: %s", strerror(errno));
  }
  else
  {
    ROS_INFO("Serial connection killed");
  }
}

void Serial::getFrame(std::stringstream &data)
{
  char buffer[1];
  int nBytes = 0;
  ros::Time latest = ros::Time::now();

  do
  {
    memset(&buffer, '\0', sizeof(buffer));
    nBytes = read(_fd, &buffer, sizeof(buffer));

    if (nBytes < 1)
    {
      ros::Duration(0.01).sleep();
      continue;
    }

    // add only first byte in buffer to data stream to work on armhf
    // architectures
    data << buffer[0];
    latest = ros::Time::now();

    if (buffer[nBytes - 1] == 'E') break;

  } while (ros::Time::now() - latest < ros::Duration(1));
}

void Serial::sendCmd(Command cmd, std::stringstream &data)
{
  char *bytes = cmd.getBytes();
  data.str("");

  try
  {
    if (_fd == -1)
    {
      std::string msg = "Connection at " + _port + " unavailable: " + strerror(errno);
      throw std::runtime_error(msg);
    }

    int tx_length = write(_fd, bytes, strlen(bytes));

    if (tx_length != -1)
    {
      ROS_DEBUG("Bytes transmitted: %s", bytes);
      if (!waitForAcknowledgement(data)) ROS_WARN_STREAM("Settings update timed out! - Aborting.");
    }
    else
    {
      ROS_ERROR("Failed to transmit %s: %s", bytes, strerror(errno));
    }
  }
  catch (const std::exception &e)
  {
    ROS_ERROR("%s: %s", e.what(), cmd.getBytes());
  }
}

bool Serial::isAcknowledgementFrame(std::string frame)
{
  size_t frame_start = frame.find('S');

  if (frame_start < 0)
    return false;
  else
    return (frame[frame_start + 7] == 'C');
}

bool Serial::waitForAcknowledgement(std::stringstream &buffer)
{
  std::string data;
  size_t frame_start = -1;

  ros::Time latest = ros::Time::now();

  while (ros::Time::now() - latest <= ros::Duration(2))
  {
    buffer.str(std::string());
    buffer.clear();

    this->getFrame(buffer);

    if (isAcknowledgementFrame(buffer.str().c_str())) return true;
  }

  // Watchdog timer expired
  buffer.str("");
  return false;
}

}  // namespace toposens_driver