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_sick_s300
00013  * Description:
00014  *                                                              
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *                      
00017  * Author: Christian Connette, email:christian.connette@ipa.fhg.de
00018  * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de
00019  *
00020  * Date of creation: Jan 2009
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_sick_s300/SerialIO.h"
00056 #include <math.h>
00057 #include <iostream>
00058 #include <unistd.h>
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 
00067 
00068 //#define _PRINT_BYTES
00069 
00070 /*
00071 #ifdef _DEBUG
00072 #define new DEBUG_NEW
00073 #undef THIS_FILE
00074 static char THIS_FILE[] = __FILE__;
00075 #endif
00076 */
00077 
00078 
00079 bool getBaudrateCode(int iBaudrate, int* iBaudrateCode)
00080 {
00081         // baudrate codes are defined in termios.h
00082         // currently upto B1000000
00083         const int baudTable[] = {
00084                 0, 50, 75, 110, 134, 150, 200, 300, 600,
00085                 1200, 1800, 2400, 4800,
00086                 9600, 19200, 38400, 57600, 115200, 230400,
00087                 460800, 500000, 576000, 921600, 1000000
00088         };
00089         const int baudCodes[] = {
00090                 B0, B50, B75, B110, B134, B150, B200, B300, B600,
00091                 B1200, B1800, B2400, B4800,
00092                 B9600, B19200, B38400, B57600, B115200, B230400,
00093                 B460800, B500000, B576000, B921600, B1000000
00094         };
00095         const int iBaudsLen = sizeof(baudTable) / sizeof(int);
00096 
00097         bool ret = false;
00098         *iBaudrateCode = B38400;
00099         int i;
00100         for( i=0; i<iBaudsLen; i++ ) {
00101                 if( baudTable[i] == iBaudrate ) {
00102                         *iBaudrateCode = baudCodes[i];
00103                         ret = true;
00104                         break;
00105                 }
00106         }
00107         /*
00108         int iStart = 0;
00109         int iEnd = iBaudsLen;
00110         int iPos = (iStart + iEnd) / 2;
00111         while (  iPos + 1 < iBaudsLen
00112                && (iBaudrate < baudTable[iPos] || iBaudrate >= baudTable[iPos + 1])
00113                && iPos != 0)
00114         {
00115                 if (iBaudrate < baudTable[iPos])
00116                 {
00117                         iEnd = iPos;
00118                 }
00119                 else
00120                 {
00121                         iStart = iPos;
00122                 }
00123                 iPos = (iStart + iEnd) / 2;
00124         }
00125 
00126         return baudCodes[iPos];
00127         */
00128         return ret;
00129 }
00130 
00131 
00132 
00133 
00135 // Konstruktion/Destruktion
00137 
00138 SerialIO::SerialIO()
00139         : m_DeviceName(""),
00140           m_Device(-1),
00141           m_BaudRate(9600),
00142           m_Multiplier(1.0),
00143           m_ByteSize(8),
00144           m_StopBits(SB_ONE),
00145           m_Parity(PA_NONE),
00146           m_Handshake(HS_NONE),
00147           m_ReadBufSize(1024),
00148           m_WriteBufSize(m_ReadBufSize),
00149           m_Timeout(0),
00150           m_ShortBytePeriod(false)
00151 {
00152         m_BytePeriod.tv_sec = 0;
00153         m_BytePeriod.tv_usec = 0;
00154 }
00155 
00156 SerialIO::~SerialIO()
00157 {
00158         closeIO();
00159 }
00160 
00161 int SerialIO::openIO()
00162 {
00163         int Res;
00164 
00165         // open device
00166         m_Device = open(m_DeviceName.c_str(), O_RDWR | O_NOCTTY /*| O_NONBLOCK*/);
00167 
00168         if(m_Device < 0)
00169         {       
00170                 //RF_ERR("Open " << m_DeviceName << " failed, error code " << errno);
00171                 std::cout << "Trying to open " << m_DeviceName << " failed: "
00172                         << strerror(errno) << " (Error code " << errno << ")" << std::endl;
00173 
00174                 return -1;
00175         }
00176 
00177         // set parameters
00178         Res = tcgetattr(m_Device, &m_tio);
00179         if (Res == -1)
00180         {
00181                 std::cout << "tcgetattr of " << m_DeviceName << " failed: "
00182                         << strerror(errno) << " (Error code " << errno << ")" << std::endl;
00183 
00184                 close(m_Device);
00185                 m_Device = -1;
00186 
00187                 return -1;
00188         }
00189 
00190         // Default values
00191         m_tio.c_iflag = 0;
00192         m_tio.c_oflag = 0;
00193         m_tio.c_cflag = B9600 | CS8 | CREAD | HUPCL | CLOCAL;
00194         m_tio.c_lflag = 0;
00195         cfsetispeed(&m_tio, B9600);
00196         cfsetospeed(&m_tio, B9600);
00197 
00198         m_tio.c_cc[VINTR] = 3;  // Interrupt
00199         m_tio.c_cc[VQUIT] = 28; // Quit
00200         m_tio.c_cc[VERASE] = 127;       // Erase
00201         m_tio.c_cc[VKILL] = 21; // Kill-line
00202         m_tio.c_cc[VEOF] = 4;   // End-of-file
00203         m_tio.c_cc[VTIME] = 0;  // Time to wait for data (tenths of seconds)
00204         m_tio.c_cc[VMIN] = 1;   // Minimum number of characters to read
00205         m_tio.c_cc[VSWTC] = 0;
00206         m_tio.c_cc[VSTART] = 17;
00207         m_tio.c_cc[VSTOP] = 19;
00208         m_tio.c_cc[VSUSP] = 26;
00209         m_tio.c_cc[VEOL] = 0;   // End-of-line
00210         m_tio.c_cc[VREPRINT] = 18;
00211         m_tio.c_cc[VDISCARD] = 15;
00212         m_tio.c_cc[VWERASE] = 23;
00213         m_tio.c_cc[VLNEXT] = 22;
00214         m_tio.c_cc[VEOL2] = 0;  // Second end-of-line
00215 
00216 
00217         // set baud rate
00218         int iNewBaudrate = int(m_BaudRate * m_Multiplier + 0.5);
00219         std::cout << "Setting Baudrate to " << iNewBaudrate << std::endl;
00220 
00221         int iBaudrateCode = 0;
00222         bool bBaudrateValid = getBaudrateCode(iNewBaudrate, &iBaudrateCode);
00223         
00224         cfsetispeed(&m_tio, iBaudrateCode);
00225         cfsetospeed(&m_tio, iBaudrateCode);
00226 
00227         if( !bBaudrateValid ) {
00228                 std::cout << "Baudrate code not available - setting baudrate directly" << std::endl;
00229                 struct serial_struct ss;
00230                 ioctl( m_Device, TIOCGSERIAL, &ss );
00231                 ss.flags |= ASYNC_SPD_CUST;
00232                 ss.custom_divisor = ss.baud_base / iNewBaudrate;
00233                 ioctl( m_Device, TIOCSSERIAL, &ss );
00234         }
00235 
00236 
00237         // set data format
00238         m_tio.c_cflag &= ~CSIZE;
00239         switch (m_ByteSize)
00240         {
00241                 case 5:
00242                         m_tio.c_cflag |= CS5;
00243                         break;
00244                 case 6:
00245                         m_tio.c_cflag |= CS6;
00246                         break;
00247                 case 7:
00248                         m_tio.c_cflag |= CS7;
00249                         break;
00250                 case 8:
00251                 default:
00252                         m_tio.c_cflag |= CS8;
00253         }
00254 
00255         m_tio.c_cflag &= ~ (PARENB | PARODD);
00256 
00257         switch (m_Parity)
00258         {
00259                 case PA_ODD:
00260                         m_tio.c_cflag |= PARODD;
00261                         //break;  // break must not be active here as we need the combination of PARODD and PARENB on odd parity.
00262                         
00263                 case PA_EVEN:
00264                         m_tio.c_cflag |= PARENB;
00265                         break;
00266                         
00267                 case PA_NONE:
00268                 default: {}
00269         }
00270         
00271         switch (m_StopBits)
00272         {
00273                 case SB_TWO:
00274                         m_tio.c_cflag |= CSTOPB;
00275                         break;
00276                         
00277                 case SB_ONE:
00278                 default:
00279                         m_tio.c_cflag &= ~CSTOPB;
00280         }
00281 
00282         // hardware handshake
00283         switch (m_Handshake)
00284         {
00285                 case HS_NONE:
00286                         m_tio.c_cflag &= ~CRTSCTS;
00287                         m_tio.c_iflag &= ~(IXON | IXOFF | IXANY);
00288                         break;
00289                 case HS_HARDWARE:
00290                         m_tio.c_cflag |= CRTSCTS;
00291                         m_tio.c_iflag &= ~(IXON | IXOFF | IXANY);
00292                         break;
00293                 case HS_XONXOFF:
00294                         m_tio.c_cflag &= ~CRTSCTS;
00295                         m_tio.c_iflag |= (IXON | IXOFF | IXANY);
00296                         break;
00297         }
00298 
00299         m_tio.c_oflag &= ~OPOST;
00300         m_tio.c_lflag &= ~ICANON;
00301 
00302         // write parameters
00303         Res = tcsetattr(m_Device, TCSANOW, &m_tio);
00304 
00305         if (Res == -1)
00306         {
00307                 std::cout << "tcsetattr " << m_DeviceName << " failed: "
00308                         << strerror(errno) << " (Error code " << errno << ")" << std::endl;
00309 
00310                 close(m_Device);
00311                 m_Device = -1;
00312 
00313                 return -1;
00314         }
00315 
00316         // set buffer sizes
00317         // SetupComm(m_Device, m_ReadBufSize, m_WriteBufSize);
00318 
00319         // set timeout
00320         setTimeout(m_Timeout);
00321 
00322         return 0;
00323 }
00324 
00325 void SerialIO::closeIO()
00326 {
00327         if (m_Device != -1)
00328         {
00329                 close(m_Device);
00330                 m_Device = -1;
00331         }
00332 }
00333 
00334 void SerialIO::setTimeout(double Timeout)
00335 {
00336         m_Timeout = Timeout;
00337         if (m_Device != -1)
00338         {
00339                 m_tio.c_cc[VTIME] = cc_t(ceil(m_Timeout * 10.0));
00340                 tcsetattr(m_Device, TCSANOW, &m_tio);
00341         }
00342 
00343 }
00344 
00345 void SerialIO::setBytePeriod(double Period)
00346 {
00347         m_ShortBytePeriod = false;
00348         m_BytePeriod.tv_sec = time_t(Period);
00349         m_BytePeriod.tv_usec = suseconds_t((Period - m_BytePeriod.tv_sec) * 1000);
00350 }
00351 
00352 //-----------------------------------------------
00353 void SerialIO::changeBaudRate(int iBaudRate)
00354 {
00355         /*
00356         int iRetVal;
00357 
00358         m_BaudRate = iBaudRate;
00359 
00360         int iNewBaudrate = int(m_BaudRate * m_Multiplier + 0.5);
00361         int iBaudrateCode = getBaudrateCode(iNewBaudrate);
00362         cfsetispeed(&m_tio, iBaudrateCode);
00363         cfsetospeed(&m_tio, iBaudrateCode);
00364         
00365         iRetVal = tcsetattr(m_Device, TCSANOW, &m_tio);
00366         if(iRetVal == -1)
00367         {
00368                 std::cout << "error in SerialCom::changeBaudRate()" << std::endl;
00369                 char c;
00370                 std::cin >> c;
00371                 exit(0);
00372         }*/
00373 }
00374 
00375 
00376 int SerialIO::readBlocking(char *Buffer, int Length)
00377 {
00378         ssize_t BytesRead;
00379         BytesRead = read(m_Device, Buffer, Length);
00380 #ifdef PRINT_BYTES
00381         printf("%2d Bytes read:", BytesRead);
00382         for(int i=0; i<BytesRead; i++)
00383                 printf(" %.2x", (unsigned char)Buffer[i]);
00384         printf("\n");
00385 #endif
00386         return BytesRead;
00387 }
00388 
00389 int SerialIO::readNonBlocking(char *Buffer, int Length)
00390 {
00391         int iAvaibleBytes = getSizeRXQueue();
00392         int iBytesToRead = (Length < iAvaibleBytes) ? Length : iAvaibleBytes;
00393         ssize_t BytesRead;
00394 
00395 
00396         BytesRead = read(m_Device, Buffer, iBytesToRead);
00397 
00398 
00399         // Debug
00400 //      printf("%2d Bytes read:", BytesRead);
00401 //      for(int i=0; i<BytesRead; i++)
00402 //      {
00403 //              unsigned char uc = (unsigned char)Buffer[i];
00404 //              printf(" %u", (unsigned int) uc );
00405 //      }
00406 //      printf("\n");
00407         
00408 
00409         return BytesRead;
00410 }
00411 
00412 int SerialIO::writeIO(const char *Buffer, int Length)
00413 {
00414         ssize_t BytesWritten;
00415 
00416         if (m_BytePeriod.tv_usec || m_BytePeriod.tv_sec)
00417         {
00418                 int i;
00419                 for (i = 0; i < Length; i++)
00420                 {
00421                         BytesWritten = write(m_Device, Buffer + i, 1);
00422                         if (BytesWritten != 1)
00423                                 break;
00424                         select(0, 0, 0, 0, &m_BytePeriod);
00425                 }
00426                 BytesWritten = i;
00427         }
00428         else
00429                 BytesWritten = write(m_Device, Buffer, Length);
00430 #ifdef PRINT_BYTES
00431         printf("%2d Bytes sent:", BytesWritten);
00432         for(int i=0; i<BytesWritten; i++)
00433                 printf(" %.2x", (unsigned char)Buffer[i]);
00434         printf("\n");
00435 #endif
00436 
00437         return BytesWritten;
00438 }
00439 
00440 int SerialIO::getSizeRXQueue()
00441 {
00442         int cbInQue;
00443         int Res = ioctl(m_Device, FIONREAD, &cbInQue);
00444         if (Res == -1) {
00445                 return 0;
00446         }
00447         return cbInQue;
00448 }
00449 
00450 


cob_sick_s300
Author(s): Florian Weisshardt
autogenerated on Thu Aug 27 2015 12:45:42