SerialDevice.cc
Go to the documentation of this file.
00001 /*
00002 *  Copyright (c) 2012, Robotnik Automation, SLL
00003 * 
00004 *   This file is part of sick-s3000-ros-pkg.
00005 *
00006 *   sick-s3000-ros-pkg is free software: you can redistribute it and/or modify
00007 *   it under the terms of the GNU General Public License as published by
00008 *   the Free Software Foundation, either version 3 of the License, or
00009 *   (at your option) any later version.
00010 *
00011 *   This program is distributed in the hope that it will be useful,
00012 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 *   GNU General Public License for more details.
00015 *
00016 *   You should have received a copy of the GNU General Public License
00017 *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
00018 *
00019 */
00030 #include <s3000_laser/SerialDevice.h>
00031 #include <time.h>
00032 #include <string.h> 
00033 #include <fcntl.h>
00034 #include <stdio.h>
00035 #include <unistd.h>  /* UNIX standard function definitions */
00036 #include <sys/stat.h>
00037 #include <termios.h>
00038 #include <sys/ioctl.h>
00039 #include <iostream>
00040 #include <errno.h>   /* Error number definitions */
00041 #include <stropts.h>
00042 #include <ros/ros.h>
00043 
00047 SerialDevice::SerialDevice(void){       
00048         
00049         strcpy(cDevice,DEFAULT_PORT);
00050         strcpy(cParity,DEFAULT_PARITY);
00051         iBaudRate=DEFAULT_TRANSFERRATE;
00052         iBitDataSize = DEFAULT_DATA_SIZE;
00053         bReady = false; 
00054         bCanon = false;
00055 }
00056 
00057 
00061 SerialDevice::SerialDevice(const char *device, int baudrate, const char *parity, int datasize){ 
00062         
00063         strcpy(cDevice,device);
00064         std::cout << "SerialDevice::SerialDevice: " << cDevice << " Parity= " << parity 
00065         << " DataSize=" << datasize <<" BaudRate=" << baudrate << std::endl;
00066         strcpy(cParity,parity);
00067         iBaudRate=baudrate;
00068         iBitDataSize = datasize;
00069         bReady = false; 
00070         bCanon = false;
00071 }
00072 
00076 SerialDevice::~SerialDevice(void){
00077         //std::cout << "SerialDevice::~SerialDevice" << std::endl;
00078 }
00079 
00084 int SerialDevice::InitPort() {
00085         struct termios options;
00086 
00087         // set up comm flags
00088         //memset(&options, 0,sizeof(options));
00089         // Get the current options for the port...
00090         if ( tcgetattr(fd, &options) < 0) {
00091                 //ROS_ERROR("unable to get device attributes");
00092                 return -1;
00093                 }
00094 
00095         // Set the baud rates to X
00096         switch(iBaudRate){
00097                 case 9600:
00098                         cfsetispeed(&options, B9600);
00099                         cfsetospeed(&options, B9600);
00100                 break;
00101                 case 19200:
00102                         cfsetispeed(&options, B19200);
00103                         cfsetospeed(&options, B19200);
00104                 break;
00105                 case 38400:
00106                         cfsetispeed(&options, B38400);
00107                         cfsetospeed(&options, B38400);
00108                 break;
00109                 case 115200:
00110                         cfsetispeed(&options, B115200);
00111                         cfsetospeed(&options, B115200);
00112                 break;
00113                 case 500000:
00114                         ROS_INFO("SerialDevice::InitPort - Setting speed to 500000");
00115                         if(cfsetispeed( &options, iBaudRate ) < 0 || cfsetospeed( &options, iBaudRate ) < 0)
00116                           {
00117                                 // ROS_ERROR("failed to set serial baud rate");
00118                                 return -1; 
00119                           }
00120                 break;
00121                 default:
00122                         cfsetispeed(&options, B19200);
00123                         cfsetospeed(&options, B19200);
00124                 break;
00125         }       
00126         // Enable the receiver and set local mode...
00127         options.c_cflag |= CLOCAL | CREAD; 
00128         options.c_cflag &= ~HUPCL;
00129         // 
00130         // PARITY
00131         if(!strcmp(cParity, "none")){
00132                 //parity = NONE 
00133                 options.c_cflag &= ~PARENB;
00134                 options.c_cflag &= ~PARODD;  
00135                 printf("SerialDevice::InitPort: Parity = none\n");
00136         } else if(!strcmp(cParity, "even")){
00137                 //parity = EVEN 
00138                 options.c_cflag |= PARENB;  
00139                 printf("SerialDevice::InitPort: Parity = even\n");
00140         } else if(!strcmp(cParity, "odd")){
00141                 options.c_cflag |= PARENB;
00142                 options.c_cflag |= PARODD; 
00143                 printf("SerialDevice::InitPort: Parity = odd\n");
00144         } else {
00145                 //parity = NONE 
00146                 options.c_cflag &= ~PARENB;
00147                 options.c_cflag &= ~PARODD; // NONE 
00148                 printf("SerialDevice::InitPort: Parity = none\n");
00149         }
00150         
00151         options.c_cflag &= ~CSTOPB;// 1 Stop bit        
00152         options.c_cflag &= ~CSIZE;
00153         //
00154         // Character size
00155         switch(iBitDataSize){
00156                 case 5:
00157                         options.c_cflag |= CS5; 
00158                         printf("SerialDevice::InitPort: Data size = CS5\n");
00159                 break;
00160                 case 6:
00161                         options.c_cflag |= CS6; 
00162                         printf("SerialDevice::InitPort: Data size = CS6\n");
00163                 break;
00164                 case 7:
00165                         options.c_cflag |= CS7; 
00166                         printf("SerialDevice::InitPort: Data size = CS7\n");
00167                 break;
00168                 case 8:
00169                         options.c_cflag |= CS8; 
00170                         printf("SerialDevice::InitPort: Data size = CS8\n");
00171                 break;
00172                 default:
00173                         options.c_cflag |= CS7; 
00174                         printf("SerialDevice::InitPort: Data size = CS7\n");
00175                 break;
00176         }
00177         /*
00178         -parenb -parodd cs8 -hupcl -cstopb cread clocal -crtscts
00179 ignbrk -brkint -ignpar -parmrk -inpck -istrip -inlcr -igncr -icrnl -ixon -ixoff -iuclc -ixany -imaxbel -iutf8
00180 -opost -olcuc -ocrnl -onlcr -onocr -onlret -ofill -ofdel nl0 cr0 tab0 bs0 vt0 ff0
00181 -isig -icanon -iexten -echo -echoe -echok -echonl -noflsh -xcase -tostop -echoprt -echoctl -echoke
00182          */
00183         options.c_cflag &= ~CRTSCTS;    //Disables hardware flow control        
00184 
00185         options.c_iflag |= IGNBRK;
00186         options.c_iflag &= ~ICRNL;
00187         options.c_iflag &= ~IXON;
00188         options.c_oflag &= ~OPOST;
00189         options.c_oflag &= ~ONLCR;
00190         options.c_lflag &= ~ISIG;
00191         options.c_lflag &= ~IEXTEN;
00192         options.c_lflag &= ~ECHOK;
00193         options.c_lflag &= ~ECHOCTL;
00194         options.c_lflag &= ~ECHOKE;
00195         options.c_lflag &= ~ECHO;
00196         options.c_lflag &= ~ECHOE;
00197         // 
00198         // Entrada canónica-> La entrada canónica es orientada a línea. Los caracteres se meten en un buffer hasta recibir un CR o LF.
00199         if(bCanon)
00200                 options.c_lflag |= ICANON;
00201         else
00202                 options.c_lflag &= ~ICANON;
00203 
00204         // carácteres de control
00205         //options.c_cc[VMIN] = (cc_t)1;
00206         options.c_cc[VMIN] = (cc_t)1;
00207         options.c_cc[VTIME] = (cc_t)5;
00208 
00209         tcflush(fd, TCIFLUSH);
00210         // Set the new options for the port...
00211         tcsetattr(fd, TCSANOW, &options);
00212 
00213         printf("SerialDevice::InitPort: Port %s initialized\n",cDevice);
00214         return SERIAL_OK;
00215 }
00216 
00220 int SerialDevice::OpenPort1(void){
00221 
00222         fd = open(cDevice,  O_RDWR | O_NOCTTY);// | O_NDELAY);// | O_NONBLOCK)
00223         
00224         if(fd==-1){
00225                 std::cout << "SerialDevice::OpenPort: Error opening " << cDevice << " port" << std::endl;
00226                 return SERIAL_ERROR;  // invalid device file    
00227         }else{
00228                 std::cout << "SerialDevice::OpenPort: " << cDevice << " opened properly" << std::endl;
00229                 //return SERIAL_OK;
00230         }
00231         
00232         InitPort();             //TEST
00233         
00234         fcntl(fd,F_SETFL, FNDELAY);     //TEST
00235         
00236         bReady = true;
00237         
00238         return SERIAL_OK;
00239 }
00240 
00241 
00245 int SerialDevice::OpenPort2(void){
00246         struct termios newtio;
00247                 
00248         fd = open(cDevice,  O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
00249         if(fd==-1){
00250                 std::cout << "SerialDevice::OpenPort2: Error opening " << cDevice << " port" << std::endl;
00251                 return SERIAL_ERROR;  // invalid device file    
00252         }//else{
00253                 //std::cout << "SerialDevice::OpenPort: " << cDevice << " opened properly" << std::endl;
00254                 //return SERIAL_OK;
00255         //}
00256         
00257         // set up comm flags
00258         memset(&newtio, 0,sizeof(newtio));
00259         
00260         switch(iBitDataSize)
00261         {
00262                 case 5:
00263                         newtio.c_cflag = CS5 | CREAD;
00264                 break;
00265                 case 6:
00266                         newtio.c_cflag = CS6 | CREAD;
00267                 break;
00268                 case 7:
00269                         newtio.c_cflag = CS7 | CREAD;
00270                 break;
00271                 case 8:
00272                         newtio.c_cflag = CS8 | CREAD;
00273                 break;
00274                 default:
00275                         //std::cout << "SerialDevice::OpenPort: unknown datasize (" << iBitDataSize << " bits) : setting default datasize ("<< DEFAULT_DATA_SIZE <<" bits)" << std::endl; 
00276                         iBitDataSize = 8;
00277                         newtio.c_cflag = CS8 | CREAD;
00278                 break;
00279         }
00280         
00281         newtio.c_iflag = INPCK;
00282         newtio.c_oflag = 0;
00283         newtio.c_lflag = 0;
00284         
00285         if(!strcmp(cParity,"even"))
00286                 newtio.c_cflag |= PARENB;//Even parity
00287         else if(!strcmp(cParity,"odd"))
00288                 newtio.c_cflag |= PARODD;//Odd parity
00289         else if(!strcmp(cParity,"none")){       //No parity
00290                 newtio.c_cflag &= ~PARODD;
00291                 newtio.c_cflag &= ~PARENB;
00292         }
00293         
00294         newtio.c_lflag &= ~ICANON;      // TEST
00295         
00296         tcsetattr(fd, TCSANOW, &newtio);
00297         tcflush(fd, TCIOFLUSH);
00298                 
00299         if (SetTermSpeed(iBaudRate) == SERIAL_ERROR)
00300             return SERIAL_ERROR;
00301         
00302         // Make sure queue is empty
00303         tcflush(fd, TCIOFLUSH);
00304         usleep(1000);
00305         tcflush(fd, TCIFLUSH);
00306         
00307         bReady = true;
00308         
00309         return SERIAL_OK;
00310         
00311 }
00312 
00316 int SerialDevice::ClosePort(void){
00317         bReady = false;
00318         
00319         printf("SerialDevice::ClosePort: closing port %s\n", cDevice);
00320         return close(fd);
00321         //return SERIAL_OK;
00322 }
00323 
00324 
00330 int SerialDevice::ReceiveMessage(char *msg)
00331 {       
00332         int n=0;
00333         if(bReady) {            
00334                 // n= read(fd, msg, BUFFER);
00335                 n= ReadPort(msg);
00336                 //if(n>0)
00337                 //      printf("SerialDevice::ReceiveMessage: read %d bytes -> %s \n", n, msg);
00338                 return n;
00339         } else {
00340                 std::cout << "SerialDevice::ReceiveMessage: Device %s "<< cDevice << " is not ready" << std::endl;
00341                 return SERIAL_ERROR;
00342         }
00343 }
00344 
00345 
00352 int SerialDevice::WritePort(char *chars, int length) {
00353 
00354         //int len = strlen(chars);      
00355         //chars[len] = 0x0d; // stick a <CR> after the command  
00356         //chars[len+1] = 0x00; // terminate the string properly
00357 
00358         int n = write(fd, chars, length);//strlen(chars));
00359         if (n < 0) {
00360                 //printf("SerialDevice::WritePort: write failed!: %s\n", stderr(errno));
00361                 perror("SerialDevice::WritePort: Error\n");
00362                 return SERIAL_ERROR;
00363         }
00364         return n;
00365 }
00366 
00374 int SerialDevice::WritePort(char *chars, int *written_bytes, int length) {
00375 
00376         //if(bInitialized){
00377         *written_bytes = write(fd, chars, length);//strlen(chars));
00378 
00379         if (written_bytes < 0) {
00380             perror("SerialDevice::WritePort: Error writting on the port");
00381             return SERIAL_ERROR;
00382         }
00383         /*
00384         }else{
00385         sprintf(cAux, "%s::WritePort: Device %s not ready", sComponentName.c_str(), cDevice);
00386         rlcLog->AddEvent((char*)cAux);
00387         return NOT_INITIALIZED;
00388         }
00389         */
00390         return SERIAL_OK;
00391 }
00392 
00398 int SerialDevice::ReadPort(char *result) {
00399         int iIn = read(fd, result, 254);
00400         //result[iIn-1] = 0x00;
00401         
00402         if (iIn < 0) {
00403                 if (errno == EAGAIN) {
00404                         //printf("SerialDevice::ReadPort: Read= %d, SERIAL EAGAIN ERROR, errno= %d\n",iIn, errno);
00405                         return -1;
00406                 } else {
00407                         printf("SerialDevice::ReadPort: SERIAL read error %d %s\n", errno, strerror(errno));
00408                         return -1;
00409                 }
00410         }
00411         //printf("SerialDevice::ReadPort: read %d bytes -> %s \n", iIn, result);        
00412         return iIn;
00413 }
00414 
00420 int SerialDevice::ReadPort(char *result, int num_bytes) {
00421         int iIn = read(fd, result, num_bytes);
00422         //result[iIn-1] = 0x00;
00423         
00424         if (iIn < 0) {
00425                 if (errno == EAGAIN) {
00426                         //printf("SerialDevice::ReadPort: Read= %d, SERIAL EAGAIN ERROR, errno= %d\n",iIn, errno);
00427                         return -1;
00428                 } else {
00429                         printf("SerialDevice::ReadPort: SERIAL read error %d %s\n", errno, strerror(errno));
00430                         return -1;
00431                 }
00432         }
00433         //printf("SerialDevice::ReadPort: read %d bytes -> %s \n", iIn, result);        
00434         return iIn;
00435 }
00436 
00446 int SerialDevice::ReadPort(char *result, int *read_bytes, int num_bytes) {
00447         int n = 0;
00448 
00449         n =  read(fd, result, num_bytes);
00450         *read_bytes = n;
00451 
00452         if(n < 0){
00453             if (errno == EAGAIN) {
00454                 return SERIAL_OK;
00455             }else{
00456                 printf("SerialDevice::ReadPort: SERIAL read error %d %s\n", errno, strerror(errno));
00457                 return SERIAL_ERROR;
00458             }
00459         }
00460         return SERIAL_OK;
00461 }
00462 
00466 int SerialDevice::GetBaud(void) {
00467         
00468         struct termios termAttr;
00469         int inputSpeed = -1;
00470         speed_t baudRate;
00471         tcgetattr(fd, &termAttr);
00472         /* Get the input speed. */
00473         baudRate = cfgetispeed(&termAttr);
00474         switch (baudRate) {
00475                 case B0:      inputSpeed = 0; break;
00476                 case B50:     inputSpeed = 50; break;
00477                 case B110:    inputSpeed = 110; break;
00478                 case B134:    inputSpeed = 134; break;
00479                 case B150:    inputSpeed = 150; break;
00480                 case B200:    inputSpeed = 200; break;
00481                 case B300:    inputSpeed = 300; break;
00482                 case B600:    inputSpeed = 600; break;
00483                 case B1200:   inputSpeed = 1200; break;
00484                 case B1800:   inputSpeed = 1800; break;
00485                 case B2400:   inputSpeed = 2400; break;
00486                 case B4800:   inputSpeed = 4800; break;
00487                 case B9600:   inputSpeed = 9600; break;
00488                 case B19200:  inputSpeed = 19200; break;
00489                 case B38400:  inputSpeed = 38400; break;
00490         }
00491         return inputSpeed;
00492 }
00493 
00497 int SerialDevice::SendMessage(char *msg, int length){
00498         int n=0;
00499         
00500         //n=write(fd, SendBuf, length);
00501         if(bReady){
00502         //printf("SerialDevice::SendMessage: strlen=%d\n", strlen(msg));
00503                 n = WritePort(msg, length);             
00504                 return n;
00505                 //if(n==0){
00506                 //      printf("SerialDevice::SendMessage: Could not send command ");
00507                 //      return SERIAL_ERROR;
00508                 //}else
00509                 //      printf("SerialDevice::SendMessage: Transmited %d", n);
00510                 
00511                 //return SERIAL_OK;
00512         }else   {
00513                 std::cout << "SerialDevice::SendMessage: Device is not ready" << std::endl;
00514                 return SERIAL_ERROR;
00515         }
00516 }
00517 
00524 int SerialDevice::SetTermSpeed(int speed){
00525         struct termios term;
00526         int term_speed;
00527         
00528         switch(speed){
00529                 case 9600:
00530                         term_speed = B9600;
00531                         break;
00532                 case 19200:
00533                         term_speed = B19200;
00534                         break;
00535                 case 38400:
00536                         term_speed = B38400;
00537                         break;
00538                 case 115200:
00539                         term_speed = B115200;
00540                         break;
00541                 case 500000:
00542                         term_speed = B500000;
00543                         break;
00544                 default:
00545                         term_speed = B19200;
00546                         //std::cout << "SerialDevice::SetTermSpeed: Unknown speed ("<< speed <<") speedL Setting default value ("<< term_speed <<")"<< std::endl;
00547                         break;
00548         }
00549         
00550         switch(term_speed)
00551         {
00552                 case B9600:
00553                 case B19200:
00554                 case B38400:
00555                 case B115200:
00556                 case B500000:
00557                         if( tcgetattr( fd, &term ) < 0 )
00558                         {
00559                                 //std::cout << "SerialDevice::SetTermSpeed: Unable to get device attributes" << std::endl;
00560                                 return SERIAL_ERROR;
00561                         }
00562                         
00563                         //cfmakeraw( &term );
00564                         if(cfsetispeed( &term, term_speed ) < 0 || cfsetospeed( &term, term_speed ) < 0)
00565                         {
00567                                 return SERIAL_ERROR;
00568                         }
00569                         
00570                         if( tcsetattr( fd, TCSAFLUSH, &term ) < 0 )
00571                         {
00572                                 //std::cout << "SerialDevice::SetTermSpeed: Unable to set device attributes" << std::endl;                      
00573                                 return SERIAL_ERROR;
00574                         }
00575                         //std::cout << "SerialDevice::SetTermSpeed: Communication rate changed to " << speed << std::endl;
00576                         return SERIAL_OK;
00577                 break;
00578         
00579         default:
00580                 //std::cout << "SerialDevice::SetTermSpeed: Unknown speed "<< speed << std::endl;
00581                 return SERIAL_ERROR;
00582                 
00583         }
00584         return SERIAL_OK;
00585 }
00586 
00592 int SerialDevice::Flush(){
00593         if (tcflush( fd, TCIOFLUSH ) < 0 ) return SERIAL_OK;
00594         else return SERIAL_ERROR;
00595 }
00596 
00597 
00598 char *SerialDevice::GetDevice(){
00599         return cDevice;
00600 }
00602 void SerialDevice::SetCanonicalInput(bool value){
00603         bCanon = value;
00604 }
00605 
00606 /*      
00607 agvservicios:/robotrans# stty -F /dev/ttyS0 -a 
00608 
00609 speed 19200 baud; rows 0; columns 0; line = 0;
00610 intr = ^C; quit = ^\; erase = ^?; kill = ^U; eof = ^D; eol = <undef>;
00611 eol2 = <undef>; swtch = <undef>; start = ^Q; stop = ^S; susp = ^Z; rprnt = ^R;
00612 werase = ^W; lnext = ^V; flush = ^O; min = 1; time = 5;
00613 parenb parodd cs7 -hupcl -cstopb cread clocal -crtscts
00614 
00615 ignbrk -brkint -ignpar -parmrk -inpck -istrip -inlcr -igncr -icrnl -ixon -ixoff
00616 -iuclc -ixany -imaxbel -iutf8
00617 -opost -olcuc -ocrnl -onlcr -onocr -onlret -ofill -ofdel nl0 cr0 tab0 bs0 vt0 ff0
00618 -isig -icanon -iexten -echo -echoe -echok -echonl -noflsh -xcase -tostop -echoprt
00619 -echoctl -echoke
00620 
00621 
00622 agvservicios:/robotrans/src# stty -F /dev/ttyS1 -a
00623 speed 19200 baud; rows 0; columns 0; line = 0;
00624 intr = ^C; quit = ^\; erase = ^?; kill = ^U; eof = ^D; eol = <undef>; 
00625 eol2 = <undef>; swtch = <undef>; start = ^Q; stop = ^S; susp = ^Z; rprnt = ^R; 
00626 
00627 werase = ^W; lnext = ^V; flush = ^O; min = 1; time = 5;
00628 parenb parodd cs7 -hupcl -cstopb cread clocal -crtscts
00629 ignbrk -brkint -ignpar -parmrk -inpck -istrip -inlcr -igncr -icrnl -ixon -ixoff 
00630 -iuclc -ixany -imaxbel -iutf8 
00631 -opost -olcuc -ocrnl -onlcr -onocr -onlret -ofill -ofdel nl0 cr0 tab0 bs0 vt0 ff0
00632 
00633 -isig icanon -iexten -echo -echoe -echok -echonl -noflsh -xcase -tostop -echoprt 
00634 -echoctl -echoke
00635 
00636 */
00637 


s3000_laser
Author(s): Román Navarro
autogenerated on Mon Oct 6 2014 07:26:44