/********************************************************* * * This source code is part of the Carnegie Mellon Robot * Navigation Toolkit (CARMEN) * * CARMEN Copyright (c) 2002 Michael Montemerlo, Nicholas * Roy, Sebastian Thrun, Dirk Haehnel, Cyrill Stachniss, * and Jared Glover * * CARMEN 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. * * CARMEN is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR * PURPOSE.  See the GNU General Public License for more  * details. * * You should have received a copy of the GNU General  * Public License along with CARMEN; if not, write to the * Free Software Foundation, Inc., 59 Temple Place,  * Suite 330, Boston, MA  02111-1307 USA * ********************************************************/#include <stdio.h>#include <stdlib.h>#include <string.h>#include <signal.h>#include <math.h>#include <unistd.h>#include <termios.h>#include <fcntl.h>#include <sys/signal.h>#include <sys/types.h>#include <sys/time.h>#include <sys/ioctl.h>#ifdef CYGWIN#include <sys/socket.h>#endif#include "rflex.h"#include "rflex-io.h"intiParity( enum PARITY_TYPE par ){  if (par==N)    return(IGNPAR);  else    return(INPCK);}intiSoftControl( int flowcontrol ){  if (flowcontrol)    return(IXON);  else    return(IXOFF);}intcDataSize( int numbits ){  switch(numbits) {  case 5:    return(CS5);    break;  case 6:    return(CS6);    break;  case 7:    return(CS7);    break;  case 8:    return(CS8);    break;  default:    return(CS8);    break;  }}intcStopSize( int numbits ){  if (numbits==2) {    return(CSTOPB);  } else {    return(0);  }}intcFlowControl( int flowcontrol ){  if (flowcontrol) {    return(CRTSCTS);  } else {    return(CLOCAL);  }}intcParity( enum PARITY_TYPE par ){  if (par!=N) {    if (par==O) {      return(PARENB | PARODD);    } else {      return(PARENB);    }  } else {    return(0);  }}intcBaudrate( int baudrate ){  switch(baudrate) {  case 0:    return(B0);    break;  case 300:    return(B300);    break;  case 600:    return(B600);    break;  case 1200:    return(B1200);    break;  case 2400:    return(B2400);    break;  case 4800:    return(B4800);    break;  case 9600:    return(B9600);    break;  case 19200:    return(B19200);    break;  case 38400:    return(B38400);    break;  case 57600:    return(B57600);    break;  case 115200:    return(B115200);    break;#if !defined(CYGWIN) && !defined(__APPLE__)  case 500000:    /* to use 500k you have to change the entry of B460800 in you kernel:       /usr/src/linux/drivers/usb/serial/ftdi_sio.h:       ftdi_8U232AM_48MHz_b460800 = 0x0006    */    return(B460800);    break;#endif  default:    return(B9600);    break;  }}long bytesWaiting( int sd ){  long available=0;  if ( ioctl( sd, FIONREAD, &available ) == 0 )    return available;  else    return -1;}    void DEVICE_set_params( Device dev ){  struct termios  ctio;  tcgetattr(dev.fd,&ctio); /* save current port settings */  ctio.c_iflag =    iSoftControl(dev.swf) |    iParity(dev.parity);  ctio.c_oflag = 0;  ctio.c_cflag =    CREAD                            |    cFlowControl(dev.hwf || dev.swf) |    cParity(dev.parity)              |     cDataSize(dev.databits)          |    cStopSize(dev.stopbits);  ctio.c_lflag = 0;  ctio.c_cc[VTIME]    = 0;   /* inter-character timer unused */  ctio.c_cc[VMIN]     = 0;   /* blocking read until 0 chars received */  cfsetispeed ( &ctio, (speed_t) cBaudrate(dev.baud) );  cfsetospeed ( &ctio, (speed_t) cBaudrate(dev.baud) );  tcflush(dev.fd, TCIFLUSH);  tcsetattr(dev.fd,TCSANOW,&ctio);}void DEVICE_set_baudrate( Device dev, int brate ){  struct termios  ctio;  tcgetattr(dev.fd,&ctio); /* save current port settings */  cfsetispeed ( &ctio, (speed_t) cBaudrate(brate) );  cfsetospeed ( &ctio, (speed_t) cBaudrate(brate) );  tcflush(dev.fd, TCIFLUSH);  tcsetattr(dev.fd,TCSANOW,&ctio);}intDEVICE_connect_port( Device *dev ){  if ( ( dev->fd =	 open( (dev->ttyport), (O_RDWR | O_NOCTTY),0) ) < 0 ) {    return( -1 );  }  DEVICE_set_params( *dev );  return( dev->fd );}intwriteData( int fd, unsigned char *buf, int nChars ){  int written = 0;  while (nChars > 0) {    written = write( fd, buf, nChars );    if (written < 0) {      return FALSE;    } else {      nChars -= written;      buf    += written;    }    usleep(1000);  }  return TRUE;}intwaitForETX( int fd, unsigned char *buf, int  *len ){  static int pos, loop, val, dlen;#ifdef IO_DEBUG  int i;#endif  pos = 2; loop = 0; dlen = -1;  while( loop<MAX_NUM_LOOPS ) {    val = bytesWaiting( fd );    if (val>0) {      read( fd, &(buf[pos]), 1 );      pos++;      if (pos>5) {	dlen = buf[5] + 9; /* std length (9 char) + data length - end chars (2)*/      }      if (dlen>0 && pos>=dlen) {	  if (buf[dlen-2]==B_ESC && buf[dlen-1]==B_ETX) {	      *len = dlen;#ifdef IO_DEBUG	      fprintf( stderr, "- answer ->" );	      for (i=0;i<pos;i++)		  fprintf( stderr, "[0x%s%x]", buf[i]<16?"0":"", buf[i] );	      fprintf( stderr, "\n" );#else#ifdef DEBUG	      fprintf( stderr, "(%d)", *len );#endif#endif	      return(TRUE);	  } else {#ifdef IO_DEBUG	      fprintf( stderr, "- wrong answer ->" );	      for (i=0;i<pos;i++)		  fprintf( stderr, "[0x%s%x]", buf[i]<16?"0":"", buf[i] );	      fprintf( stderr, "\n" );#else#ifdef DEBUG	      fprintf( stderr, "-" );#endif#endif	      return(FALSE);	  }      }    } else {      usleep(100);      loop++;    }  }#ifdef IO_DEBUG  fprintf( stderr, "\n" );#endif  return(FALSE);}intwaitForAnswer( int fd, unsigned char *buf, int *len ){  int loop = 0;  *len = 0;  while( loop<MAX_NUM_LOOPS ) {    if (bytesWaiting( fd )) {      read( fd, &(buf[*len]), 1 );      *len = *len+1;      if (*len>=2 && buf[*len-2]==B_ESC && buf[*len-1]==B_STX ) {	buf[0] = B_ESC;	buf[1] = B_STX;	*len = 2;	return(waitForETX(fd,buf,len));      }    } else {      usleep(100);      loop++;    }  }  return(FALSE);}