SerialDevice.cc | searchcode

/Lib/Firmware/DynamixelGateway/Host/SerialDevice.cc

http://github.com/UPenn-RoboCup/UPennalizers
C++ | 931 lines | 693 code | 130 blank | 108 comment | 85 complexity | 341444a3a49d0f4adce7c512ac0cb37c MD5 | raw file
Possible License(s): GPL-3.0

/* Driver for reading data from a serial port

    Aleksandr Kushleyev <akushley(at)seas(dot)upenn(dot)edu>
    University of Pennsylvania, 2008

    BSD license.
    --------------------------------------------------------------------
    Copyright (c) 2008 Aleksandr Kushleyev
    All rights reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions
    are met:
    1. Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    2. Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    3. The name of the author may not be used to endorse or promote products
      derived from this software without specific prior written permission.

      THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
      IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
      OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
      IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
      INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
      NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
      DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
      THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
      (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
      THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include "SerialDevice.hh"
#include <stdlib.h>
#include <stdio.h>

//constructor
SerialDevice::SerialDevice()
{
  _fd=-1;
  _connected=false;
  _block=-1;
  _baud=B2400;
  
  _ioMode=IO_BLOCK_W_TIMEOUT;
  _delay_us=0;
  _numTermChars=0;
  _retTermSequence=false;
}

//destructor
SerialDevice::~SerialDevice()
{
  Disconnect();
}

//wrapper for ConnectSerial function
int SerialDevice::Connect(const char * device, const int speed)
{
  return ConnectSerial(device,speed);
}

int SerialDevice::Connect(const char * device, const char * speedStr)
{
  int speed = strtol(speedStr,NULL,10);
  return ConnectSerial(device,speed);
}

//connect to the serial device
int SerialDevice::ConnectSerial(const char * device, const int speed)
{

  if (_connected)
  {
    std::cout << "SerialDevice::ConnectSerial: Warning: already connected" << std::endl;
    return 0;
  }

  //store the device name
  strncpy(_device,device,MAX_DEVICE_NAME_LENGTH);

  // Open the device
  if((_fd = open(_device, O_RDWR | O_NOCTTY | O_NONBLOCK)) < 0)
  //if((_fd = open(_device, O_RDWR | O_NOCTTY)) < 0)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::ConnectSerial: Error: Unable to open serial port" << std::endl;
#endif
    return -1;
  }

  //update the connected flag
  _connected=true;

   //set the default IO mode
  if (Set_IO_BLOCK_W_TIMEOUT())
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::ConnectSerial: Error: Unable to set the io mode" << std::endl;
#endif
    close(_fd);
    _connected=false;
    return -1;
  }
  
  //set the device type
  _device_type = DEVICE_TYPE_SERIAL;

  if (speed==0)
    return 0;
  
  //save current attributes so they can be restored after use
  if( tcgetattr( _fd, &_oldterm ) < 0 )
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout<<"SerialDevice::ConnectSerial: Error: Unable to get old serial port attributes" << std::endl;
#endif
    close(_fd);
    _connected=false;
    return -1;
  }
  
  //set up the terminal and set the baud rate
  if (SetBaudRate(speed))
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::ConnectSerial: Error: Unable to set baud rate" << std::endl;
#endif
    close(_fd);
    _connected=false;
    return -1;
  }

  return 0;
}

int SerialDevice::ConnectTCP(const char * device, const int port, const int buff_size)
{
  //int buffSize, nonblock=1;
	struct sockaddr_in serv_addr;
	struct hostent *hostptr;
	
  
  //check the port
  if ( (port < 0) || (port > 65536) )
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::ConnectTCP: Error: bad port number" << std::endl;
#endif
    return -1;
  }
  
  //get the hostname
	if ((hostptr = gethostbyname(device)) == NULL)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::ConnectTCP: Error: could not get hostname" << std::endl;
#endif
		return -1;
	}
  
	//Get host info
	bzero((char *) &serv_addr, sizeof(serv_addr));
	serv_addr.sin_family = AF_INET;
	bcopy(hostptr->h_addr, (char *) &serv_addr.sin_addr, hostptr->h_length);
	serv_addr.sin_port = htons(port);
	
	if ((_fd = socket(AF_INET, SOCK_STREAM, 0)) < 0)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::ConnectTCP: Error: could not open a socket" << std::endl;
#endif
		 return -1;
	}
	
	// Set read buffer size
	if (setsockopt(_fd, SOL_SOCKET, SO_RCVBUF, &buff_size, sizeof(int)) < 0)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::ConnectTCP: Error: could not set receive buffer size" << std::endl;
#endif
		 close(_fd);
		 return -1;
	}

  //update the connected flag
  _connected=true;
	
	// Set nonblocking I/O so that we can attempt to connect
  if (Set_IO_NONBLOCK_WO_TIMEOUT())
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::ConnectTCP: Error: could not set non-blocking io mode" << std::endl;
#endif
    close(_fd);
    _connected=false;
    return -1;
  }
	
	struct timeval selTimeout = {0,DEFAULT_TCP_CONNECT_TIMEOUT_US};
	
	fd_set wrfds;
	FD_ZERO(&wrfds);
	FD_SET(_fd, &wrfds);
	
	connect(_fd, (struct sockaddr *) &serv_addr, sizeof(serv_addr));
	int selret = 0;
	
	selret = select(_fd+1,NULL,&wrfds,NULL,&selTimeout);


  //make sure that we actually connected by getting the status from getsocketopt
  int option_value;
  int option_length=4;  //need to set this value to 4 to let getsockopt know that we are expecting an int (4 bytes)

  if (getsockopt(_fd,SOL_SOCKET,SO_ERROR,(char*)&option_value,(socklen_t*)&option_length) != 0)

  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::ConnectTCP: Error: could not get socket option" << std::endl;
#endif
    close(_fd);
    _connected=false;
    return -1;
  }

  //std::cout <<"option length= "<<option_length<<" option value= " << *(int*)option_value<<std::endl;

  if (option_value != 0)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::ConnectTCP: Error: could not connect to the server:"<< std::endl;
    std::cout<<strerror(option_value)<<std::endl;
#endif
    close(_fd);
    _connected=false;
    return -1;
  }

  if (selret <= 0)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::ConnectTCP: Error: could not connect to the server" << std::endl;
#endif
    close(_fd);
    _connected=false;
    return -1;
  }

  
  //set the default IO mode
  if (Set_IO_BLOCK_W_TIMEOUT())
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::ConnectSerial: Error: Unable to set the io mode" << std::endl;
#endif
    close(_fd);
    _connected=false;
    return -1;
  }

  
  _device_type = DEVICE_TYPE_TCP;
  return 0;
}

//disconnect from the device
int SerialDevice::Disconnect()
{
  //check whether we are connected to the device  
  if (!_connected)
  {
    return 0;
  }  

  if (_device_type == DEVICE_TYPE_SERIAL)
  {
    // Restore old terminal settings
    if(tcsetattr(_fd,TCSANOW,&_oldterm) < 0) 
    {
#ifdef SERIAL_DEVICE_DEBUG
      std::cout << "SerialDevice::Disconnect: Failed to restore attributes!" << std::endl;
#endif
    }
  }
  
  // Actually close the device
  if(close(_fd) != 0)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::Disconnect: Failed to close device!" << std::endl;
#endif
    _connected=false;
    return -1;
  }
  
  _connected=false;
  return 0;
}

bool SerialDevice::IsConnected()
{
  return _connected;
}


//convert speed (integer) to baud rate (speed_t)
int SerialDevice::_SpeedToBaud(int speed, speed_t & baud)
{
  switch (speed) 
  {
    case 2400:
      baud=B2400;
      return 0;
    case 4800:
      baud=B4800;
      return 0;
    case 9600:
      baud=B9600;
      return 0;
    case 19200:
      baud=B19200;
      return 0;
    case 38400:
      baud=B38400;
      return 0;
    case 57600:
      baud=B57600;
      return 0;
    case 115200:
      baud=B115200;
      return 0;
    case 230400:
      baud=B230400;
      return 0;
    case 460800:
#ifndef __APPLE__
      baud=B460800;
      return 0;
#endif
    case 921600:
#ifndef __APPLE__
      baud=B921600;
      return 0;
#endif
    case 1000000:
#ifndef __APPLE__
      baud=B1000000;
      return 0;
#endif
    case 2000000:
#ifndef __APPLE__
      baud=B2000000;
      return 0;
#endif

    default:
#ifdef SERIAL_DEVICE_DEBUG
      std::cout << "SerialDevice::speedToBaud: ERROR: unknown baud rate" <<std::endl;
#endif
      return -1;
  }
}

//set the terminal baud rate. Argument can be either an integer (ex. 115200) or speed_t (ex. B115200)
int SerialDevice::SetBaudRate(const int speed)
{
  speed_t tempBaud;

  //check whether we are connected to the device
  if (!_connected)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::FlushInputBuffer: Error: not connected to the device" << std::endl;
#endif
    return -1;
  }

  //convert the integer speed value to speed_t if needed
  if (_SpeedToBaud(speed,tempBaud))
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout<<"SerialDevice::SetBaudRate: Error: bad baud rate" << std::endl;
#endif
    return -1;
  }
  
  //get current port settings
  if( tcgetattr( _fd, &_newterm ) < 0 )
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout<<"SerialDevice::SetBaudRate: Error: Unable to get serial port attributes" << std::endl;
#endif
    return -1;
  }

  //cfmakeraw initializes the port to standard configuration. Use this!
  cfmakeraw( &_newterm );
  
  //set input baud rate
  if (cfsetispeed( &_newterm, tempBaud ) < 0 )
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout<<"SerialDevice::SetBaudRate: Error: Unable to set baud rate" <<std::endl;
#endif
    return -1;
  }
  
  //set output baud rate
  if (cfsetospeed( &_newterm, tempBaud ) < 0 )
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout<<"SerialDevice::SetBaudRate: Error: Unable to set baud rate" <<std::endl;
#endif
    return -1;
  }
  
  //set new attributes 
  if( tcsetattr( _fd, TCSAFLUSH, &_newterm ) < 0 )
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout<<"SerialDevice::SetBaudRate: Error: Unable to set serial port attributes" <<std::endl;
#endif
    return -1;
  }
  
  //make sure queue is empty
  tcflush(_fd, TCIOFLUSH);

  //save the baud rate value
  _baud=tempBaud;

  return 0;
}


int SerialDevice::Set_IO_BLOCK_W_TIMEOUT()
{
  //check whether we are connected to the device  
  if (!_connected)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::Set_IO_BLOCK_WO_TIMEOUT: Error: not connected to the device" << std::endl;
#endif
    return -1;
  }
  
  if (_SetBlockingIO())
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::Set_IO_BLOCK_WO_TIMEOUT: Error: could not set blocking io" << std::endl;
#endif
    return -1;
  }
  _ioMode=IO_BLOCK_W_TIMEOUT;
  _delay_us=0;
  _numTermChars=0;
  _retTermSequence=false;
  
  return 0;
}

int SerialDevice::Set_IO_BLOCK_WO_TIMEOUT()
{
  //check whether we are connected to the device  
  if (!_connected)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::Set_IO_BLOCK_WO_TIMEOUT: Error: not connected to the device" << std::endl;
#endif
    return -1;
  }
  
  if (_SetBlockingIO())
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::Set_IO_BLOCK_WO_TIMEOUT: Error: could not set blocking io" << std::endl;
#endif
    return -1;
  }
  _ioMode=IO_BLOCK_WO_TIMEOUT;
  _delay_us=0;
  _numTermChars=0;
  _retTermSequence=false;
  
  return 0;
}

int SerialDevice::Set_IO_BLOCK_W_TIMEOUT_W_TERM_SEQUENCE(const char * termSequence, int numTermChars, bool retTermSequence)
{
  //check whether we are connected to the device  
  if (!_connected)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::Set_IO_BLOCK_W_TIMEOUT_W_TERM_SEQUENCE: Error: not connected to the device" << std::endl;
#endif
    return -1;
  }
  
  if (_SetBlockingIO())
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::Set_IO_BLOCK_W_TIMEOUT_W_TERM_SEQUENCE: Error: could not set blocking io" << std::endl;
#endif
    return -1;
  }
  
  if (numTermChars < 1 || numTermChars > MAX_NUM_TERM_CHARS)
  {
    std::cout<<" IOMode::Set_IO_BLOCK_W_TIMEOUT_W_TERM_SEQUENCE: ERROR: bad number of chars: " <<numTermChars<<std::endl;
    return -1;
  }
  _ioMode=IO_BLOCK_W_TIMEOUT_W_TERM_SEQUENCE;
  _numTermChars=numTermChars;
  _retTermSequence=retTermSequence;
  memcpy(_termSequence,termSequence, numTermChars*sizeof(char));
  _delay_us=0;
  
  return 0;
}

int SerialDevice::Set_IO_NONBLOCK_WO_TIMEOUT()
{
  //check whether we are connected to the device  
  if (!_connected)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::Set_IO_NONBLOCK_WO_TIMEOUT: Error: not connected to the device" << std::endl;
#endif
    return -1;
  }
  
  if (_SetNonBlockingIO())
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::Set_IO_NONBLOCK_WO_TIMEOUT: Error: could not set non-blocking io" << std::endl;
#endif
    return -1;
  }

  _ioMode=IO_NONBLOCK_WO_TIMEOUT;
  _delay_us=0;
  _numTermChars=0;
  _retTermSequence=false;
  
  return 0;
}

int SerialDevice::Set_IO_NONBLOCK_POLL_W_DELAY_W_TIMEOUT(int delay_us)
{
  //check whether we are connected to the device  
  if (!_connected)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::Set_IO_NONBLOCK_POLL_W_DELAY_W_TIMEOUT: Error: not connected to the device" << std::endl;
#endif
    return -1;
  }
  
  if (_SetNonBlockingIO())
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::Set_IO_NONBLOCK_POLL_W_DELAY_W_TIMEOUT: Error: could not set non-blocking io" << std::endl;
#endif
    return -1;
  }

  _ioMode=IO_NONBLOCK_POLL_W_DELAY_W_TIMEOUT; 
  _delay_us=delay_us;
  _numTermChars=0;
  _retTermSequence=false;
  
  return 0;
}


//set blocking terminal mode
int SerialDevice::_SetBlockingIO()
{  
  //check whether we are connected to the device  
  if (!_connected)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::SetBlockingIO: Error: not connected to the device" << std::endl;
#endif
    return -1;
  }  

  //only set blocking if not already set  
  if (_block != 1)
  {
    // Read the flags
    int flags;
    if((flags = fcntl(_fd,F_GETFL)) < 0) 
    {
#ifdef SERIAL_DEVICE_DEBUG
      std::cout << "SerialDevice::_setBlockingIO: unable to get device flags" << std::endl;
#endif
      return -1;
    } 
  
    // Set the new flags
    if(fcntl(_fd,F_SETFL,flags & (~O_NONBLOCK)) < 0)
    {
#ifdef SERIAL_DEVICE_DEBUG
      std::cout << "SerialDevice::_setBlockingIO: unable to set device flags" << std::endl;
#endif
      return -1;
    }
  
    _block=1;
  }
  return 0;
}

//set non-blocking terminal mode
int SerialDevice::_SetNonBlockingIO()
{
  //check whether we are connected to the device  
  if (!_connected)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::SetNonBlockingIO: Error: not connected to the device" << std::endl;
#endif
    return -1;
  }  

  //only set non-blocking if not already set
  if (_block != 0)
  {
    // Read the flags
    int flags;
    if((flags = fcntl(_fd,F_GETFL)) < 0)
    {
#ifdef SERIAL_DEVICE_DEBUG
      std::cout << "SerialDevice::_setNonBlockingIO failed! - unable to retrieve device flags" << std::endl;
#endif
      return -1;
    }
  
    // Set the new flags
    if(fcntl(_fd,F_SETFL,flags | O_NONBLOCK) < 0)
    {
#ifdef SERIAL_DEVICE_DEBUG
      std::cout << "SerialDevice::_setNonBlockingIO failed! - unable to set device flags" << std::endl;
#endif
      return -1;
    }
  
    _block=0;
  }
  return 0;
}

//delete all the data in the input buffer
int SerialDevice::FlushInputBuffer()
{
  char c[1000];
  //check whether we are connected to the device
  if (!_connected)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::FlushInputBuffer: Error: not connected to the device" << std::endl;
#endif
    return -1;
  }
  
  //TODO tcflush for some reason does not work.. 
  //tcflush(_fd, TCIFLUSH);
  
  int block=_block;
  _SetNonBlockingIO();
  
  //read off all the chars
  while (read(_fd,c,1000) > 0){}
  
  if (block==1)
  {
    _SetBlockingIO();
  }
  
  return 0;
}

//read characters from device
int SerialDevice::ReadChars(char * data, int byte_count, int timeout_us)
{
  //check whether we are connected to the device
  if (!_connected)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::ReadChars: Error: not connected to the device" << std::endl;
#endif
    return -1;
  }

//TODO: tcp behaves strangely when connection is closed. The check below does not work
/*
  struct stat fd_info;
  if (fstat(_fd,&fd_info) != 0 )
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::ReadChars: Error: file descriptor is not valid" << std::endl;
#endif   
    return -1;
  }
*/
  fd_set watched_fds;
  struct timeval timeout, start, end;
  int bytes_read_total = 0;
  int bytes_left = byte_count;
  int retval;
  int bytes_read;
  int charsMatched=0;

  switch (_ioMode)
  {

    case IO_BLOCK_W_TIMEOUT:
      //set up for the "select" call
      FD_ZERO(&watched_fds);
      FD_SET(_fd, &watched_fds);
      timeout.tv_sec = timeout_us / 1000000;
      timeout.tv_usec = timeout_us % 1000000;


      while (bytes_left) 
      {
        if ((retval = select(_fd + 1, &watched_fds, NULL, NULL, &timeout)) < 1)   //block until at least 1 char is available or timeout
        {                                                                         //error reading chars
          if (retval < 0)
          {
#ifdef SERIAL_DEVICE_DEBUG
            perror("SerialDevice::ReadChars");
#endif
          }
          else                                                                    //timeout
          {
#ifdef SERIAL_DEVICE_DEBUG
            std::cout << "SerialDevice::ReadChars: Error: timeout. #chars read= "<<bytes_read_total <<", requested= "<< byte_count << std::endl;
#endif
          }
          return bytes_read_total;
        }
        bytes_read        = read(_fd, &(data[bytes_read_total]), bytes_left);
        
        if (bytes_read > 0)
        {
          bytes_read_total += bytes_read;
          bytes_left       -= bytes_read;
        }
      }
      return bytes_read_total;

    

    case IO_NONBLOCK_POLL_W_DELAY_W_TIMEOUT:
      gettimeofday(&start,NULL);
      
      while (bytes_left) 
      {
        bytes_read = read(_fd,&(data[bytes_read_total]),bytes_left);
        if ( bytes_read < 1)
        {
          // If a time out then return false
          gettimeofday(&end,NULL);
          if((end.tv_sec*1000000 + end.tv_usec) - (start.tv_sec*1000000 + start.tv_usec) > timeout_us) 
          {
#ifdef SERIAL_DEVICE_DEBUG
            std::cout << "SerialDevice::ReadChars: Error: timeout. #chars read= "<<bytes_read_total <<", requested= "<< byte_count << std::endl;
#endif
            return bytes_read_total;
          }
          usleep(_delay_us);
          continue;
        }

        bytes_read_total += bytes_read;
        bytes_left       -= bytes_read;
      }
      return bytes_read_total;


    case IO_BLOCK_WO_TIMEOUT:
      while (bytes_left) 
      {
        bytes_read = read(_fd,&(data[bytes_read_total]),bytes_left);
        if (bytes_read < 1)
        {
          return -1;
        }
        
        bytes_read_total += bytes_read;
        bytes_left       -= bytes_read;
      }
      return bytes_read_total;

    case IO_NONBLOCK_WO_TIMEOUT:
      bytes_read = read(_fd,&(data[0]),bytes_left);
      if (bytes_read < 0) bytes_read=0;
      return bytes_read;
      
      
    case IO_BLOCK_W_TIMEOUT_W_TERM_SEQUENCE:
    
      //set up for the "select" call
      FD_ZERO(&watched_fds);
      FD_SET(_fd, &watched_fds);
      timeout.tv_sec = timeout_us / 1000000;
      timeout.tv_usec = timeout_us % 1000000;

      while (bytes_left) {
        if ((retval = select(_fd + 1, &watched_fds, NULL, NULL, &timeout)) < 1)   //block until at least 1 char is available or timeout
        {                                                                         //error reading chars
          if (retval < 0) 
          {
#ifdef SERIAL_DEVICE_DEBUG
            perror("SerialDevice::ReadChars");
#endif
          }
          else                                                                    //timeout
          {
#ifdef SERIAL_DEVICE_DEBUG
            std::cout << "SerialDevice::ReadChars: Error: timeout. The terminating sequence has not been read"<< std::endl;
#endif
          }
          return -1;
        }
        bytes_read = read(_fd, &(data[bytes_read_total]), 1);
        
        if (bytes_read==1)
        {
          if (data[bytes_read_total]==_termSequence[charsMatched])
          {
            charsMatched++;
          }
          else 
          {
            charsMatched=0;
          }
          
          //std::cout<<data[bytes_read_total];
          bytes_read_total += bytes_read;
          bytes_left       -= bytes_read;
          
          if (charsMatched==_numTermChars)
          {
            if (_retTermSequence)
            {
              return bytes_read_total;
            }
            
            else 
            {
              return bytes_read_total-_numTermChars;
            }
          }
        }
      }
#ifdef SERIAL_DEVICE_DEBUG
      std::cout << "SerialDevice::ReadChars: Error: Read too much data. The terminating sequence has not been read"<< std::endl;
#endif
      return -1;

    default:
#ifdef SERIAL_DEVICE_DEBUG
      std::cout << "SerialDevice::ReadChars: Error: Bad io mode " << std::endl;
#endif
      return -1;

  }
}

int SerialDevice::WriteChars(const char * data, int byte_count, int delay_us)
{
  int bytes_written_total=0;
  int bytes_written;
  int bytes_left=byte_count;
    
  
  //check whether we are connected to the device  
  if (!_connected)
  {
#ifdef SERIAL_DEVICE_DEBUG
    std::cout << "SerialDevice::ReadChars: Error: not connected to the device" << std::endl;
#endif
    return -1;
  }
  
  if (delay_us==0)
  {
    bytes_written_total=write(_fd,data,byte_count);
  }
  
  else
  {
    while (bytes_left)
    {
      bytes_written=write(_fd,&(data[bytes_written_total]),1);
      if (bytes_written < 0)
      {
#ifdef SERIAL_DEVICE_DEBUG
        perror("SerialDevice::ReadChars");
#endif
      }
      if (bytes_written < 1)
      {
#ifdef SERIAL_DEVICE_DEBUG
        std::cout << "SerialDevice::WriteChars: Error: Could not write a char. #chars written= "<<bytes_written_total <<", requested= "<< byte_count << std::endl;
#endif
        return bytes_written_total;
      }

      bytes_written_total += bytes_written;
      bytes_left       -= bytes_written;
      usleep(delay_us);
    }
  }  
  
  tcdrain(_fd);   //wait till all the data written to the file descriptor is transmitted

  return bytes_written_total;
}


double SerialDevice::GetLastInputPacketTime()
{
  return 0;
}