serial.cc
Go to the documentation of this file.
00001 
00019 #include <stdio.h>
00020 #include <fcntl.h>
00021 #include <sys/time.h>
00022 #include <unistd.h>
00023 
00024 #include "serial.h"
00025 #include <string.h>
00026 #include <errno.h>
00027 
00028 SerialDevice::~SerialDevice(){
00029   if (tty_fd_slot != -1) {
00030     empty_serial (tty_fd_slot);
00031     close_serial (tty_fd_slot);
00032   }
00033 }
00034 SerialDevice::SerialDevice(){
00035   tty_fd_slot = -1;
00036   strcpy(device_name,"");
00037 }
00038 
00039 
00040 /******************************************************************************
00041  ***
00042  *** Name:       open_serial
00043  ***
00044  *** Purpose:    oeffnet ein serielles Device
00045  ***
00046  *** Parameters: devicename (z.B. "/dev/ttyS4") und flags (z.B. CS8|B9600 ) 
00047  ***
00048  *** Returns:    filedescriptor, bzw. -1 im Fehlerfall
00049  ***
00050  ******************************************************************************
00051  */
00052 
00053 int  SerialDevice::open_serial (const char *p_tty_name)
00054 {
00055    struct termio tty_set;
00056    int tty_fd = -1;
00057     
00058    if ( (tty_fd = open(p_tty_name, O_RDWR | O_NOCTTY | O_NDELAY)) < 0 )//NONBLOCK
00059      {
00060        printf("ERROR: Couldn't open port %s!\n", p_tty_name);
00061        perror(NULL);
00062        return (-1);
00063      }
00064    
00065    if (ioctl(tty_fd, TCGETA, &tty_set ) == -1 )
00066      {
00067        printf("ERROR: ioctl() failed to read settings!\n");
00068        perror(NULL);
00069        return (-1);
00070      }
00071    
00072    tty_set.c_cflag = (B115200 | CS8 | CLOCAL | CREAD);
00073    tty_set.c_iflag = IXOFF;
00074    tty_set.c_lflag = 0;
00075    tty_set.c_oflag = 0;
00076    tty_set.c_cc[VTIME]    = 20;
00077    tty_set.c_cc[VMIN]     = 0;
00078 
00079    if (ioctl(tty_fd, TCSETAW, &tty_set ) == -1 ) 
00080      {
00081        printf("ERROR: ioctl() failed to set settings!\n");
00082        perror(NULL);
00083        return (-1);
00084      }
00085 
00086    tty_fd_slot = tty_fd;
00087    strcpy(device_name,p_tty_name);
00088    return (tty_fd);
00089 }
00090 
00091 /******************************************************************************
00092  ***
00093  *** Name:       change_baud_serial
00094  ***
00095  *** Purpose:    stellt die Geschwindigkeit der seriellen Schnittstelle um
00096  ***
00097  *** Parameters: filedescriptor und neue Geschwindigkeit (z.B. B38400)
00098  ***
00099  *** Returns:    STATUS_OK, bzw. STATUS_ERROR im Fehlerfall
00100  ***
00101  ******************************************************************************
00102  */
00103 
00104 long SerialDevice::change_baud_serial (int tty_fd, speed_t speed)
00105  {
00106    struct termio tty_set;
00107 
00108    if (ioctl(tty_fd, TCGETA, &tty_set ) == -1 ) 
00109      {
00110        printf("ERROR: ioctl() failed to read settings!\n");
00111        perror(NULL);
00112        return (STATUS_ERROR);
00113      }
00114    
00115    tty_set.c_cflag &= ~CBAUD;
00116    tty_set.c_cflag |= speed;
00117    
00118    if (ioctl(tty_fd, TCSETAW, &tty_set ) == -1 ) 
00119      {
00120        printf("ERROR: ioctl() failed to set settings!\n");
00121        perror(NULL);
00122        return (STATUS_ERROR);
00123      }
00124 
00125    return (STATUS_OK);
00126  }
00127 
00128 /******************************************************************************
00129  ***
00130  *** Name:       write_serial
00131  ***
00132  *** Purpose:    Schickt Daten an die serielle Schnittstelle
00133  ***
00134  *** Parameters: filedescriptor, Pointer auf Speicher, Zahl der zu versendenden bytes
00135  ***
00136  *** Returns:    STATUS_OK, bzw. STATUS_ERROR im Fehlerfall
00137  ***
00138  ******************************************************************************
00139  */
00140 
00141 long SerialDevice::write_serial(int tty_fd, unsigned char *p_buffer, long nb_byte)
00142  {
00143    int bytes_out;
00144 
00145    //printf("write serial tty:%d msg:%s nb:%d\n", tty_fd, p_buffer, nb_byte);
00146 
00147    if (( bytes_out = write(tty_fd, p_buffer, nb_byte)) < nb_byte)
00148      {
00149        printf("ERROR: write() failed!\n");
00150        perror(NULL);
00151        return (STATUS_ERROR);
00152      };
00153 
00154    return (STATUS_OK);
00155  }
00156 
00157 /******************************************************************************
00158  ***
00159  *** Name:       read_serial
00160  ***
00161  *** Purpose:    Liest Daten von der seriellen Schnittstelle. Liegen gerade
00162  ***             keine Daten an, wird eine 20tel Sekunde darauf gewartet!
00163  ***
00164  *** Parameters: filedescriptor, Pointer auf Speicherbereich, maximale 
00165  ***             Zahl der zu lesenden bytes
00166  ***
00167  *** Returns:    Zahl der gelesenen bytes
00168  ***
00169  ******************************************************************************
00170  */
00171 
00172 long SerialDevice::read_serial(int tty_fd, unsigned char *p_buffer, long nb_bytes_max)
00173  {
00174    struct timeval tz;
00175    fd_set fds;
00176    int tries       = 0;
00177    int qu_new      = 0;
00178    int bytes_read  = 0;
00179    int ready       = STATUS_ERROR;
00180 
00181    
00182    /* read max nb_bytes_max bytes */
00183 
00184    while (tries < 2 && !ready)
00185      {
00186        tz.tv_sec  = 1;
00187        tz.tv_usec = 0;
00188        
00189        FD_ZERO(&fds);
00190        FD_SET(tty_fd, &fds);
00191        if (select(FD_SETSIZE, &fds, 0, 0, &tz) > 0)
00192          {
00193            qu_new = read(tty_fd, &p_buffer[bytes_read], (nb_bytes_max - bytes_read));
00194            if (qu_new > 0)
00195              bytes_read += qu_new;
00196            else
00197              tries++;
00198            ready = (bytes_read == nb_bytes_max);
00199          }
00200        else
00201          tries++;
00202      }
00203 
00204    //printf("read serial tty:%d msg:%s nb:%d\n", tty_fd, p_buffer, bytes_read);
00205   
00206    return(bytes_read);
00207  }
00208 
00209 /******************************************************************************
00210  ***
00211  *** Name:      close_serial
00212  ***
00213  *** Purpose:   schliesst das serielle device
00214  ***
00215  *** Parameter: filedescriptor
00216  ***
00217  ******************************************************************************
00218  */
00219 
00220 void SerialDevice::close_serial (int tty_fd)
00221  {
00222    close(tty_fd);
00223  }
00224 
00225 /******************************************************************************
00226  ***
00227  *** Name:       wait_for_serial
00228  ***
00229  *** Purpose:    Wartet maximal eine vorgegebene Zeit auf das Eintreffen von 
00230  ***             Daten an der seriellen Schnittstelle
00231  ***
00232  *** Parameters: filedescriptor, maximale Wartezeit
00233  ***
00234  *** Returns:    STATUS_OK wenn Daten anliegen, bzw. STATUS_ERROR wenn nicht
00235  ***
00236  ******************************************************************************
00237  */
00238 
00239 long SerialDevice::wait_for_serial(int tty_fd, long max_time_secs)
00240  {
00241    struct timeval tz;
00242    fd_set fds;
00243 
00244    while (max_time_secs--)
00245      {
00246        tz.tv_sec  = 1;
00247        tz.tv_usec = 0;
00248        
00249        FD_ZERO (&fds);
00250        FD_SET(tty_fd, &fds);
00251        
00252        if (select(FD_SETSIZE, &fds, 0, 0, &tz) > 0)
00253          return (STATUS_OK);
00254      }  
00255 
00256    return (STATUS_ERROR);
00257  }
00258 
00259 
00260 /******************************************************************************
00261  ***
00262  *** Name:       empty_serial
00263  ***
00264  *** Purpose:    Loescht den Eingangspuffer der seriellen Schnittstelle
00265  ***
00266  *** Caveats:    Noch nicht getestet... (see manpage zu termios)
00267  ***
00268  *** Parameters: filedescriptor
00269  ***
00270  *** Returns:    STATUS_OK, bzw. STATUS_ERROR im Fehlerfall
00271  ***
00272  ******************************************************************************
00273  */
00274 
00275 long SerialDevice::empty_serial(int tty_fd)
00276  {
00277    if (-1 == tcflush (tty_fd, TCIFLUSH))
00278      return (STATUS_ERROR);
00279    else
00280      return (STATUS_OK);
00281  }
00282 
00283 
00284 
00285 
00286 
00287 
00288 
00289 
00290 
00291 
00292 
00293 
00294 
00295 
00296 
00297 
00298 


asr_cyberglove_lib
Author(s): Heller Florian, Meißner Pascal, Nguyen Trung, Yi Xie
autogenerated on Thu Jun 6 2019 22:02:33