SerialIO.cpp
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_driver
00012  * ROS package name: cob_relayboard
00013  * Description: Class for general Serial communication under Linux OS.
00014  *                                                              
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *                      
00017  * Author: Philipp Koehler
00018  * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de
00019  *
00020  * Date of creation: March 2010
00021  * ToDo:
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *     * Redistributions of source code must retain the above copyright
00029  *       notice, this list of conditions and the following disclaimer.
00030  *     * Redistributions in binary form must reproduce the above copyright
00031  *       notice, this list of conditions and the following disclaimer in the
00032  *       documentation and/or other materials provided with the distribution.
00033  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00034  *       Engineering and Automation (IPA) nor the names of its
00035  *       contributors may be used to endorse or promote products derived from
00036  *       this software without specific prior written permission.
00037  *
00038  * This program is free software: you can redistribute it and/or modify
00039  * it under the terms of the GNU Lesser General Public License LGPL as 
00040  * published by the Free Software Foundation, either version 3 of the 
00041  * License, or (at your option) any later version.
00042  * 
00043  * This program is distributed in the hope that it will be useful,
00044  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00046  * GNU Lesser General Public License LGPL for more details.
00047  * 
00048  * You should have received a copy of the GNU Lesser General Public 
00049  * License LGPL along with this program. 
00050  * If not, see <http://www.gnu.org/licenses/>.
00051  *
00052  ****************************************************************/
00053 
00054 //#include "stdafx.h"
00055 #include <cob_relayboard/SerialIO.h>
00056 #include <math.h>
00057 #include <string.h>
00058 #include <iostream>
00059 
00060 #include <errno.h>
00061 #include <sys/types.h>
00062 #include <sys/stat.h>
00063 #include <fcntl.h>
00064 #include <sys/ioctl.h>
00065 #include <linux/serial.h>
00066 #include <unistd.h>
00067 #include <termios.h>
00068 
00069 
00070 
00071 //#define _PRINT_BYTES
00072 
00073 /*
00074 #ifdef _DEBUG
00075 #define new DEBUG_NEW
00076 #undef THIS_FILE
00077 static char THIS_FILE[] = __FILE__;
00078 #endif
00079 */
00080 
00081 
00082 bool getBaudrateCode(int iBaudrate, int* iBaudrateCode)
00083 {
00084         // baudrate codes are defined in termios.h
00085         // currently upto B1000000
00086         const int baudTable[] = {
00087                 0, 50, 75, 110, 134, 150, 200, 300, 600,
00088                 1200, 1800, 2400, 4800,
00089                 9600, 19200, 38400, 57600, 115200, 230400,
00090                 460800, 500000, 576000, 921600, 1000000
00091         };
00092         const int baudCodes[] = {
00093                 B0, B50, B75, B110, B134, B150, B200, B300, B600,
00094                 B1200, B1800, B2400, B4800,
00095                 B9600, B19200, B38400, B57600, B115200, B230400,
00096                 B460800, B500000, B576000, B921600, B1000000
00097         };
00098         const int iBaudsLen = sizeof(baudTable) / sizeof(int);
00099 
00100         bool ret = false;
00101         *iBaudrateCode = B38400;
00102         int i;
00103         for( i=0; i<iBaudsLen; i++ ) {
00104                 if( baudTable[i] == iBaudrate ) {
00105                         *iBaudrateCode = baudCodes[i];
00106                         ret = true;
00107                         break;
00108                 }
00109         }
00110         /*
00111         int iStart = 0;
00112         int iEnd = iBaudsLen;
00113         int iPos = (iStart + iEnd) / 2;
00114         while (  iPos + 1 < iBaudsLen
00115                && (iBaudrate < baudTable[iPos] || iBaudrate >= baudTable[iPos + 1])
00116                && iPos != 0)
00117         {
00118                 if (iBaudrate < baudTable[iPos])
00119                 {
00120                         iEnd = iPos;
00121                 }
00122                 else
00123                 {
00124                         iStart = iPos;
00125                 }
00126                 iPos = (iStart + iEnd) / 2;
00127         }
00128 
00129         return baudCodes[iPos];
00130         */
00131         return ret;
00132 }
00133 
00134 
00135 
00136 
00138 // Konstruktion/Destruktion
00140 
00141 SerialIO::SerialIO()
00142         : m_DeviceName(""),
00143           m_Device(-1),
00144           m_BaudRate(9600),
00145           m_Multiplier(1.0),
00146           m_ByteSize(8),
00147           m_StopBits(SB_ONE),
00148           m_Parity(PA_NONE),
00149           m_Handshake(HS_NONE),
00150           m_ReadBufSize(1024),
00151           m_WriteBufSize(m_ReadBufSize),
00152           m_Timeout(0),
00153           m_ShortBytePeriod(false)
00154 {
00155         m_BytePeriod.tv_sec = 0;
00156         m_BytePeriod.tv_usec = 0;
00157 }
00158 
00159 SerialIO::~SerialIO()
00160 {
00161         closeIO();
00162 }
00163 
00164 int SerialIO::openIO()
00165 {
00166         int Res;
00167 
00168         // open device
00169         m_Device = open(m_DeviceName.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
00170 
00171         if(m_Device < 0)
00172         {       
00173                 //RF_ERR("Open " << m_DeviceName << " failed, error code " << errno);
00174                 std::cout << "Trying to open " << m_DeviceName << " failed: "
00175                         << strerror(errno) << " (Error code " << errno << ")";
00176 
00177                 return -1;
00178         }
00179 
00180         // set parameters
00181         Res = tcgetattr(m_Device, &m_tio);
00182         if (Res == -1)
00183         {
00184                 std::cerr << "tcgetattr of " << m_DeviceName << " failed: "
00185                         << strerror(errno) << " (Error code " << errno << ")";
00186 
00187                 close(m_Device);
00188                 m_Device = -1;
00189 
00190                 return -1;
00191         }
00192 
00193         // Default values
00194         m_tio.c_iflag = 0;
00195         m_tio.c_oflag = 0;
00196         m_tio.c_cflag = B9600 | CS8 | CREAD | HUPCL | CLOCAL;
00197         m_tio.c_lflag = 0;
00198         cfsetispeed(&m_tio, B9600);
00199         cfsetospeed(&m_tio, B9600);
00200 
00201         m_tio.c_cc[VINTR] = 3;  // Interrupt
00202         m_tio.c_cc[VQUIT] = 28; // Quit
00203         m_tio.c_cc[VERASE] = 127;       // Erase
00204         m_tio.c_cc[VKILL] = 21; // Kill-line
00205         m_tio.c_cc[VEOF] = 4;   // End-of-file
00206         m_tio.c_cc[VTIME] = 0;  // Time to wait for data (tenths of seconds)
00207         m_tio.c_cc[VMIN] = 1;   // Minimum number of characters to read
00208         m_tio.c_cc[VSWTC] = 0;
00209         m_tio.c_cc[VSTART] = 17;
00210         m_tio.c_cc[VSTOP] = 19;
00211         m_tio.c_cc[VSUSP] = 26;
00212         m_tio.c_cc[VEOL] = 0;   // End-of-line
00213         m_tio.c_cc[VREPRINT] = 18;
00214         m_tio.c_cc[VDISCARD] = 15;
00215         m_tio.c_cc[VWERASE] = 23;
00216         m_tio.c_cc[VLNEXT] = 22;
00217         m_tio.c_cc[VEOL2] = 0;  // Second end-of-line
00218 
00219 
00220         // set baud rate
00221         int iNewBaudrate = int(m_BaudRate * m_Multiplier + 0.5);
00222         std::cerr << "Setting Baudrate to " << iNewBaudrate;
00223 
00224         int iBaudrateCode = 0;
00225         bool bBaudrateValid = getBaudrateCode(iNewBaudrate, &iBaudrateCode);
00226         
00227         cfsetispeed(&m_tio, iBaudrateCode);
00228         cfsetospeed(&m_tio, iBaudrateCode);
00229 
00230         if( !bBaudrateValid ) {
00231                 std::cerr << "Baudrate code not available - setting baudrate directly";
00232                 struct serial_struct ss;
00233                 ioctl( m_Device, TIOCGSERIAL, &ss );
00234                 ss.flags |= ASYNC_SPD_CUST;
00235                 ss.custom_divisor = ss.baud_base / iNewBaudrate;
00236                 ioctl( m_Device, TIOCSSERIAL, &ss );
00237         }
00238 
00239 
00240         // set data format
00241         m_tio.c_cflag &= ~CSIZE;
00242         switch (m_ByteSize)
00243         {
00244                 case 5:
00245                         m_tio.c_cflag |= CS5;
00246                         break;
00247                 case 6:
00248                         m_tio.c_cflag |= CS6;
00249                         break;
00250                 case 7:
00251                         m_tio.c_cflag |= CS7;
00252                         break;
00253                 case 8:
00254                 default:
00255                         m_tio.c_cflag |= CS8;
00256         }
00257 
00258         m_tio.c_cflag &= ~ (PARENB | PARODD);
00259 
00260         switch (m_Parity)
00261         {
00262                 case PA_ODD:
00263                         m_tio.c_cflag |= PARODD;
00264                         //break;  // break must not be active here as we need the combination of PARODD and PARENB on odd parity.
00265                         
00266                 case PA_EVEN:
00267                         m_tio.c_cflag |= PARENB;
00268                         break;
00269                         
00270                 case PA_NONE:
00271                 default: {}
00272         }
00273         
00274         switch (m_StopBits)
00275         {
00276                 case SB_TWO:
00277                         m_tio.c_cflag |= CSTOPB;
00278                         break;
00279                         
00280                 case SB_ONE:
00281                 default:
00282                         m_tio.c_cflag &= ~CSTOPB;
00283         }
00284 
00285         // hardware handshake
00286         switch (m_Handshake)
00287         {
00288                 case HS_NONE:
00289                         m_tio.c_cflag &= ~CRTSCTS;
00290                         m_tio.c_iflag &= ~(IXON | IXOFF | IXANY);
00291                         break;
00292                 case HS_HARDWARE:
00293                         m_tio.c_cflag |= CRTSCTS;
00294                         m_tio.c_iflag &= ~(IXON | IXOFF | IXANY);
00295                         break;
00296                 case HS_XONXOFF:
00297                         m_tio.c_cflag &= ~CRTSCTS;
00298                         m_tio.c_iflag |= (IXON | IXOFF | IXANY);
00299                         break;
00300         }
00301 
00302         m_tio.c_oflag &= ~OPOST;
00303         m_tio.c_lflag &= ~ICANON;
00304 
00305         // write parameters
00306         Res = tcsetattr(m_Device, TCSANOW, &m_tio);
00307 
00308         if (Res == -1)
00309         {
00310                 std::cerr << "tcsetattr " << m_DeviceName << " failed: "
00311                         << strerror(errno) << " (Error code " << errno << ")";
00312 
00313                 close(m_Device);
00314                 m_Device = -1;
00315 
00316                 return -1;
00317         }
00318 
00319         // set buffer sizes
00320 //      SetupComm(m_Device, m_ReadBufSize, m_WriteBufSize);
00321 
00322         // set timeout
00323         setTimeout(m_Timeout);
00324 
00325         return 0;
00326 }
00327 
00328 void SerialIO::closeIO()
00329 {
00330         if (m_Device != -1)
00331         {
00332                 close(m_Device);
00333                 m_Device = -1;
00334         }
00335 }
00336 
00337 void SerialIO::setTimeout(double Timeout)
00338 {
00339         m_Timeout = Timeout;
00340         if (m_Device != -1)
00341         {
00342                 m_tio.c_cc[VTIME] = cc_t(ceil(m_Timeout * 10.0));
00343                 tcsetattr(m_Device, TCSANOW, &m_tio);
00344         }
00345 
00346 }
00347 
00348 void SerialIO::setBytePeriod(double Period)
00349 {
00350         m_ShortBytePeriod = false;
00351         m_BytePeriod.tv_sec = time_t(Period);
00352         m_BytePeriod.tv_usec = suseconds_t((Period - m_BytePeriod.tv_sec) * 1000);
00353 }
00354 
00355 //-----------------------------------------------
00356 void SerialIO::changeBaudRate(int iBaudRate)
00357 {
00358         /*
00359         int iRetVal;
00360 
00361         m_BaudRate = iBaudRate;
00362 
00363         int iNewBaudrate = int(m_BaudRate * m_Multiplier + 0.5);
00364         int iBaudrateCode = getBaudrateCode(iNewBaudrate);
00365         cfsetispeed(&m_tio, iBaudrateCode);
00366         cfsetospeed(&m_tio, iBaudrateCode);
00367         
00368         iRetVal = tcsetattr(m_Device, TCSANOW, &m_tio);
00369         if(iRetVal == -1)
00370         {
00371                 LOG_OUT("error in SerialCom::changeBaudRate()");
00372                 char c;
00373                 std::cin >> c;
00374                 exit(0);
00375         }*/
00376 }
00377 
00378 
00379 int SerialIO::readBlocking(char *Buffer, int Length)
00380 {
00381         ssize_t BytesRead;
00382         BytesRead = read(m_Device, Buffer, Length);
00383 #ifdef PRINT_BYTES
00384         printf("%2d Bytes read:", BytesRead);
00385         for(int i=0; i<BytesRead; i++)
00386                 printf(" %.2x", (unsigned char)Buffer[i]);
00387         printf("\n");
00388 #endif
00389         return BytesRead;
00390 }
00391 
00392 int SerialIO::readNonBlocking(char *Buffer, int Length)
00393 {
00394         int iAvaibleBytes = getSizeRXQueue();
00395         int iBytesToRead = (Length < iAvaibleBytes) ? Length : iAvaibleBytes;
00396         ssize_t BytesRead;
00397 
00398 
00399         BytesRead = read(m_Device, Buffer, iBytesToRead);
00400         
00401 
00402         // Debug
00403 //      printf("%2d Bytes read:", BytesRead);
00404 //      for(int i=0; i<BytesRead; i++)
00405 //      {
00406 //              unsigned char uc = (unsigned char)Buffer[i];
00407 //              printf(" %u", (unsigned int) uc );
00408 //      }
00409 //      printf("\n");
00410         
00411 
00412         return BytesRead;
00413 }
00414 
00415 int SerialIO::writeIO(const char *Buffer, int Length)
00416 {
00417         ssize_t BytesWritten;
00418 
00419         if (m_BytePeriod.tv_usec || m_BytePeriod.tv_sec)
00420         {
00421                 int i;
00422                 for (i = 0; i < Length; i++)
00423                 {
00424                         BytesWritten = write(m_Device, Buffer + i, 1);
00425                         if (BytesWritten != 1)
00426                                 break;
00427                         select(0, 0, 0, 0, &m_BytePeriod);
00428                 }
00429                 BytesWritten = i;
00430         }
00431         else
00432                 BytesWritten = write(m_Device, Buffer, Length);
00433 #ifdef PRINT_BYTES
00434         printf("%2d Bytes sent:", BytesWritten);
00435         for(int i=0; i<BytesWritten; i++)
00436                 printf(" %.2x", (unsigned char)Buffer[i]);
00437         printf("\n");
00438 #endif
00439 
00440         return BytesWritten;
00441 }
00442 
00443 int SerialIO::getSizeRXQueue()
00444 {
00445         int cbInQue;
00446         int Res = ioctl(m_Device, FIONREAD, &cbInQue);
00447         if (Res == -1) {
00448                 return 0;
00449         }
00450         return cbInQue;
00451 }
00452 
00453 


cob_relayboard
Author(s): Christian Connette
autogenerated on Thu Aug 27 2015 12:45:34