00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
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
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082 bool getBaudrateCode(int iBaudrate, int* iBaudrateCode)
00083 {
00084
00085
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
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131 return ret;
00132 }
00133
00134
00135
00136
00138
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
00169 m_Device = open(m_DeviceName.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
00170
00171 if(m_Device < 0)
00172 {
00173
00174 std::cout << "Trying to open " << m_DeviceName << " failed: "
00175 << strerror(errno) << " (Error code " << errno << ")";
00176
00177 return -1;
00178 }
00179
00180
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
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;
00202 m_tio.c_cc[VQUIT] = 28;
00203 m_tio.c_cc[VERASE] = 127;
00204 m_tio.c_cc[VKILL] = 21;
00205 m_tio.c_cc[VEOF] = 4;
00206 m_tio.c_cc[VTIME] = 0;
00207 m_tio.c_cc[VMIN] = 1;
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;
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;
00218
00219
00220
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
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
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
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
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
00320
00321
00322
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
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
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
00403
00404
00405
00406
00407
00408
00409
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