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 <../include/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 <unistd.h>
00065 #include <sys/ioctl.h>
00066 #include <linux/serial.h>
00067 #include <unistd.h>
00068 #include <termios.h>
00069
00070
00071 bool getBaudrateCode(int iBaudrate, int* iBaudrateCode)
00072 {
00073
00074
00075 const int baudTable[] = {
00076 0, 50, 75, 110, 134, 150, 200, 300, 600,
00077 1200, 1800, 2400, 4800,
00078 9600, 19200, 38400, 57600, 115200, 230400,
00079 460800, 500000, 576000, 921600, 1000000
00080 };
00081 const int baudCodes[] = {
00082 B0, B50, B75, B110, B134, B150, B200, B300, B600,
00083 B1200, B1800, B2400, B4800,
00084 B9600, B19200, B38400, B57600, B115200, B230400,
00085 B460800, B500000, B576000, B921600, B1000000
00086 };
00087 const int iBaudsLen = sizeof(baudTable) / sizeof(int);
00088
00089 bool ret = false;
00090 *iBaudrateCode = B38400;
00091 int i;
00092 for( i=0; i<iBaudsLen; i++ ) {
00093 if( baudTable[i] == iBaudrate ) {
00094 *iBaudrateCode = baudCodes[i];
00095 ret = true;
00096 break;
00097 }
00098 }
00099
00100 return ret;
00101 }
00102
00103
00104
00105
00107
00109
00110 SerialIO::SerialIO()
00111 : m_DeviceName("/dev/ttyUSB0"),
00112 m_Device(-1),
00113 m_BaudRate(9600),
00114 m_Multiplier(1.0),
00115 m_ByteSize(8),
00116 m_StopBits(SB_ONE),
00117 m_Parity(PA_NONE),
00118 m_Handshake(HS_NONE),
00119 m_ReadBufSize(1024),
00120 m_WriteBufSize(m_ReadBufSize),
00121 m_Timeout(0),
00122 m_ShortBytePeriod(false)
00123 {
00124 m_BytePeriod.tv_sec = 0;
00125 m_BytePeriod.tv_usec = 0;
00126 }
00127
00128 SerialIO::~SerialIO()
00129 {
00130 closeIO();
00131 }
00132
00133 int SerialIO::openIO()
00134 {
00135 int Res;
00136
00137 m_Device = open(m_DeviceName.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
00138
00139 if(m_Device < 0)
00140 {
00141
00142 std::cout << "Trying to open " << m_DeviceName << " failed: "
00143 << strerror(errno) << " (Error code " << errno << ")";
00144
00145 return -1;
00146 }
00147
00148
00149 Res = tcgetattr(m_Device, &m_tio);
00150 if (Res == -1)
00151 {
00152 std::cerr << "tcgetattr of " << m_DeviceName << " failed: "
00153 << strerror(errno) << " (Error code " << errno << ")\n";
00154
00155 close(m_Device);
00156 m_Device = -1;
00157
00158 return -1;
00159 }
00160
00161
00162 m_tio.c_iflag = 0;
00163 m_tio.c_oflag = 0;
00164 m_tio.c_cflag = B9600 | CS8 | CREAD | HUPCL | CLOCAL;
00165 m_tio.c_lflag = 0;
00166 cfsetispeed(&m_tio, B9600);
00167 cfsetospeed(&m_tio, B9600);
00168
00169 m_tio.c_cc[VINTR] = 3;
00170 m_tio.c_cc[VQUIT] = 28;
00171 m_tio.c_cc[VERASE] = 127;
00172 m_tio.c_cc[VKILL] = 21;
00173 m_tio.c_cc[VEOF] = 4;
00174 m_tio.c_cc[VTIME] = 0;
00175 m_tio.c_cc[VMIN] = 1;
00176 m_tio.c_cc[VSWTC] = 0;
00177 m_tio.c_cc[VSTART] = 17;
00178 m_tio.c_cc[VSTOP] = 19;
00179 m_tio.c_cc[VSUSP] = 26;
00180 m_tio.c_cc[VEOL] = 0;
00181 m_tio.c_cc[VREPRINT] = 18;
00182 m_tio.c_cc[VDISCARD] = 15;
00183 m_tio.c_cc[VWERASE] = 23;
00184 m_tio.c_cc[VLNEXT] = 22;
00185 m_tio.c_cc[VEOL2] = 0;
00186
00187
00188
00189 int iNewBaudrate = int(m_BaudRate * m_Multiplier + 0.5);
00190 std::cerr << "Setting Baudrate to " << iNewBaudrate << "\n";
00191
00192 int iBaudrateCode = 0;
00193 bool bBaudrateValid = getBaudrateCode(iNewBaudrate, &iBaudrateCode);
00194
00195 cfsetispeed(&m_tio, iBaudrateCode);
00196 cfsetospeed(&m_tio, iBaudrateCode);
00197
00198 if( !bBaudrateValid ) {
00199 std::cerr << "Baudrate code not available - setting baudrate directly\n";
00200 struct serial_struct ss;
00201 ioctl( m_Device, TIOCGSERIAL, &ss );
00202 ss.flags |= ASYNC_SPD_CUST;
00203 ss.custom_divisor = ss.baud_base / iNewBaudrate;
00204 ioctl( m_Device, TIOCSSERIAL, &ss );
00205 }
00206
00207
00208
00209 m_tio.c_cflag &= ~CSIZE;
00210 switch (m_ByteSize)
00211 {
00212 case 5:
00213 m_tio.c_cflag |= CS5;
00214 break;
00215 case 6:
00216 m_tio.c_cflag |= CS6;
00217 break;
00218 case 7:
00219 m_tio.c_cflag |= CS7;
00220 break;
00221 case 8:
00222 default:
00223 m_tio.c_cflag |= CS8;
00224 }
00225
00226 m_tio.c_cflag &= ~ (PARENB | PARODD);
00227
00228 switch (m_Parity)
00229 {
00230 case PA_ODD:
00231 m_tio.c_cflag |= PARODD;
00232
00233
00234 case PA_EVEN:
00235 m_tio.c_cflag |= PARENB;
00236 break;
00237
00238 case PA_NONE:
00239 default: {}
00240 }
00241
00242 switch (m_StopBits)
00243 {
00244 case SB_TWO:
00245 m_tio.c_cflag |= CSTOPB;
00246 break;
00247
00248 case SB_ONE:
00249 default:
00250 m_tio.c_cflag &= ~CSTOPB;
00251 }
00252
00253
00254 switch (m_Handshake)
00255 {
00256 case HS_NONE:
00257 m_tio.c_cflag &= ~CRTSCTS;
00258 m_tio.c_iflag &= ~(IXON | IXOFF | IXANY);
00259 break;
00260 case HS_HARDWARE:
00261 m_tio.c_cflag |= CRTSCTS;
00262 m_tio.c_iflag &= ~(IXON | IXOFF | IXANY);
00263 break;
00264 case HS_XONXOFF:
00265 m_tio.c_cflag &= ~CRTSCTS;
00266 m_tio.c_iflag |= (IXON | IXOFF | IXANY);
00267 break;
00268 }
00269
00270 m_tio.c_oflag &= ~OPOST;
00271 m_tio.c_lflag &= ~ICANON;
00272
00273
00274 Res = tcsetattr(m_Device, TCSANOW, &m_tio);
00275
00276 if (Res == -1)
00277 {
00278 std::cerr << "tcsetattr " << m_DeviceName << " failed: "
00279 << strerror(errno) << " (Error code " << errno << ")\n";
00280
00281 close(m_Device);
00282 m_Device = -1;
00283
00284 return -1;
00285 }
00286
00287
00288
00289
00290
00291 setTimeout(m_Timeout);
00292
00293 return 0;
00294 }
00295
00296 void SerialIO::closeIO()
00297 {
00298 if (m_Device != -1)
00299 {
00300 close(m_Device);
00301 m_Device = -1;
00302 }
00303 }
00304
00305 void SerialIO::setTimeout(double Timeout)
00306 {
00307 m_Timeout = Timeout;
00308 if (m_Device != -1)
00309 {
00310 m_tio.c_cc[VTIME] = cc_t(ceil(m_Timeout * 10.0));
00311 tcsetattr(m_Device, TCSANOW, &m_tio);
00312 }
00313
00314 }
00315
00316 void SerialIO::setBytePeriod(double Period)
00317 {
00318 m_ShortBytePeriod = false;
00319 m_BytePeriod.tv_sec = time_t(Period);
00320 m_BytePeriod.tv_usec = suseconds_t((Period - m_BytePeriod.tv_sec) * 1000);
00321 }
00322
00323
00324 void SerialIO::changeBaudRate(int iBaudRate)
00325 {
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344 }
00345
00346
00347 int SerialIO::readBlocking(char *Buffer, int Length)
00348 {
00349 ssize_t BytesRead;
00350 BytesRead = read(m_Device, Buffer, Length);
00351
00352 return BytesRead;
00353 }
00354
00355 int SerialIO::readNonBlocking(char *Buffer, int Length)
00356 {
00357 int iAvaibleBytes = getSizeRXQueue();
00358 int iBytesToRead = (Length < iAvaibleBytes) ? Length : iAvaibleBytes;
00359 ssize_t BytesRead;
00360
00361
00362 BytesRead = read(m_Device, Buffer, iBytesToRead);
00363
00364 return BytesRead;
00365 }
00366
00367 int SerialIO::writeIO(const char *Buffer, int Length)
00368 {
00369 ssize_t BytesWritten;
00370
00371 if (m_BytePeriod.tv_usec || m_BytePeriod.tv_sec)
00372 {
00373 int i;
00374 for (i = 0; i < Length; i++)
00375 {
00376 BytesWritten = write(m_Device, Buffer + i, 1);
00377 if (BytesWritten != 1)
00378 break;
00379 select(0, 0, 0, 0, &m_BytePeriod);
00380 }
00381 BytesWritten = i;
00382 }
00383 else
00384 BytesWritten = write(m_Device, Buffer, Length);
00385
00386 }
00387
00388 int SerialIO::getSizeRXQueue()
00389 {
00390 int cbInQue;
00391 int Res = ioctl(m_Device, FIONREAD, &cbInQue);
00392 if (Res == -1) {
00393 return 0;
00394 }
00395 return cbInQue;
00396 }
00397
00398