rflex-io.c

卡耐基.梅隆大学的机器人仿真软件（Redhat linux 9下安装）

 /*********************************************************
 *
 * 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"

int
iParity( enum PARITY_TYPE par )
{
  if (par==N)
    return(IGNPAR);
  else
    return(INPCK);
}

int
iSoftControl( int flowcontrol )
{
  if (flowcontrol)
    return(IXON);
  else
    return(IXOFF);
}

int
cDataSize( 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;
  }
}

int
cStopSize( int numbits )
{
  if (numbits==2) {
    return(CSTOPB);
  } else {
    return(0);
  }
}

int
cFlowControl( int flowcontrol )
{
  if (flowcontrol) {
    return(CRTSCTS);
  } else {
    return(CLOCAL);
  }
}

int
cParity( enum PARITY_TYPE par )
{
  if (par!=N) {
    if (par==O) {
      return(PARENB | PARODD);
    } else {
      return(PARENB);
    }
  } else {
    return(0);
  }
}

int
cBaudrate( 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);
}

int
DEVICE_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 );
}

int
writeData( 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;
}

int
waitForETX( 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);
}

int
waitForAnswer( 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);
}