00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <math.h>
00022 #include <stdio.h>
00023 #include <string.h>
00024 #include <strings.h>
00025 #include <unistd.h>
00026
00027 #include <fcntl.h>
00028 #include <sys/stat.h>
00029 #include <sys/time.h>
00030 #include <sys/types.h>
00031
00032 #ifdef HAVE_CONFIG_H
00033 #include <config.h>
00034 #endif // HAVE_CONFIG_H
00035
00036
00037 #include <shvel-param.h>
00038
00039
00040 #include <serial.h>
00041 #include <communication.h>
00042 #include <utility.h>
00043 #include <param.h>
00044 #include <ypspur-coordinator.h>
00045 #include <yprintf.h>
00046
00047 #include <errno.h>
00048 double SER_BAUDRATE;
00049
00050
00051 #if !defined(__MINGW32__)
00052
00053
00054 #include <sys/select.h>
00055 #include <sys/termios.h>
00056 int g_device_port = 0;
00057 struct termios g_oldtio;
00058
00059 #else
00060
00061
00062 #include <windows.h>
00063 HANDLE g_hdevices = NULL;
00064 DCB g_olddcb;
00065 COMMTIMEOUTS g_oldcto;
00066
00067 #endif // !defined(__MINGW32__)
00068
00069 #if !defined(__MINGW32__)
00070 speed_t i2baud(int baud)
00071 {
00072 switch (baud)
00073 {
00074 case 0:
00075 return B0;
00076 case 9600:
00077 return B9600;
00078 case 19200:
00079 return B19200;
00080 case 38400:
00081 return B38400;
00082 #ifdef B57600
00083 case 57600:
00084 return B57600;
00085 #endif // B57600
00086 #ifdef B128000
00087 case 128000:
00088 return B128000;
00089 #endif // B128000
00090 #ifdef B115200
00091 case 115200:
00092 return B115200;
00093 #endif // B115200
00094 #ifdef B230400
00095 case 230400:
00096 return B230400;
00097 #endif // B230400
00098 #ifdef B460800
00099 case 460800:
00100 return B460800;
00101 #endif // B460800
00102 #ifdef B576000
00103 case 576000:
00104 return B576000;
00105 #endif // B576000
00106 #ifdef B921600
00107 case 921600:
00108 return B921600;
00109 #endif // B921600
00110 }
00111 return -1;
00112 }
00113
00114 int baud2i(speed_t baud)
00115 {
00116 switch (baud)
00117 {
00118 case B0:
00119 return 0;
00120 case B9600:
00121 return 9600;
00122 case B19200:
00123 return 19200;
00124 case B38400:
00125 return 38400;
00126 #ifdef B57600
00127 case B57600:
00128 return 57600;
00129 #endif // B57600
00130 #ifdef B128000
00131 case B128000:
00132 return 128000;
00133 #endif // B128000
00134 #ifdef B115200
00135 case B115200:
00136 return 115200;
00137 #endif // B115200
00138 #ifdef B230400
00139 case B230400:
00140 return 230400;
00141 #endif // B230400
00142 #ifdef B460800
00143 case B460800:
00144 return 460800;
00145 #endif // B460800
00146 #ifdef B576000
00147 case B576000:
00148 return 576000;
00149 #endif // B576000
00150 #ifdef B921600
00151 case B921600:
00152 return 921600;
00153 #endif // B921600
00154 }
00155 return -1;
00156 }
00157 #else
00158 DWORD i2baud(int baud)
00159 {
00160 switch (baud)
00161 {
00162 case 9600:
00163 return CBR_9600;
00164 case 19200:
00165 return CBR_19200;
00166 case 38400:
00167 return CBR_38400;
00168 case 115200:
00169 return CBR_115200;
00170 case 128000:
00171 return CBR_128000;
00172 }
00173 return CBR_110;
00174 }
00175 #endif // !defined(__MINGW32__)
00176
00177
00178 int serial_tryconnect(char *device_name)
00179 {
00180 #if !defined(__MINGW32__)
00181
00182 g_device_port = open(device_name, O_RDWR);
00183 if (g_device_port < 0)
00184 {
00185 return 0;
00186 }
00187 close(g_device_port);
00188 #else
00189
00190
00191 g_hdevices = CreateFile(device_name, GENERIC_WRITE, 0, 0, OPEN_EXISTING, 0, 0);
00192 if (g_hdevices == INVALID_HANDLE_VALUE)
00193 {
00194 return 0;
00195 }
00196 CloseHandle(g_hdevices);
00197 #endif // !defined(__MINGW32__)
00198 return 1;
00199 }
00200
00201 int recieve_throw(char *buf, int len, double t, void *data)
00202 {
00203 buf[len] = 0;
00204 strcat((char *)data, buf);
00205 if (strstr((char *)data, "\n\n"))
00206 {
00207 return -2;
00208 }
00209 return 0;
00210 }
00211
00216 int serial_change_baudrate(int baud)
00217 {
00218 #if !defined(__MINGW32__)
00219 struct termios newtio;
00220 int ret, errnum;
00221 char buf[2048];
00222
00223 memset(&newtio, 0, sizeof(newtio));
00224
00225
00226
00227
00228
00229
00230
00231
00232 newtio.c_cflag = CS8 | CLOCAL | CREAD | CRTSCTS;
00233 errno = 0;
00234 ret = cfsetispeed(&newtio, i2baud(baud));
00235 if (ret < 0)
00236 {
00237 errnum = errno;
00238 yprintf(OUTPUT_LV_ERROR, "Error: Failed to set input baud rate to %d\n", baud);
00239 yprintf(OUTPUT_LV_DEBUG, "Error: cfsetispeed: %s(%d)\n", strerror(errnum), errnum);
00240 return 0;
00241 }
00242 errno = 0;
00243 ret = cfsetospeed(&newtio, i2baud(baud));
00244 if (ret < 0)
00245 {
00246 errnum = errno;
00247 yprintf(OUTPUT_LV_ERROR, "Error: Failed to set output baud rate to %d\n", baud);
00248 yprintf(OUTPUT_LV_DEBUG, "Error: cfsetospeed: %s(%d)\n", strerror(errnum), errnum);
00249 return 0;
00250 }
00251 SER_BAUDRATE = (double)baud;
00252 newtio.c_iflag = IGNPAR;
00253 newtio.c_oflag = 0;
00254
00255 newtio.c_cc[VTIME] = 0;
00256 newtio.c_cc[VMIN] = 1;
00257
00258 if (tcsetattr(g_device_port, TCSAFLUSH, &newtio))
00259 {
00260 errnum = errno;
00261 yprintf(OUTPUT_LV_ERROR, "Error: Failed to set attribute of serial port\n");
00262 yprintf(OUTPUT_LV_DEBUG, "Error: tcsetattr: %s(%d)\n", strerror(errnum), errnum);
00263 return 0;
00264 }
00265
00266 {
00267 struct termios term;
00268 speed_t isp, osp;
00269 errno = 0;
00270
00271 ret = tcgetattr(g_device_port, &term);
00272 if (ret < 0)
00273 {
00274 errnum = errno;
00275 yprintf(OUTPUT_LV_ERROR, "Error: Failed to get attribute of serial port\n");
00276 yprintf(OUTPUT_LV_DEBUG, "Error: tcgetattr: %s(%d)\n", strerror(errnum), errnum);
00277 return 0;
00278 }
00279 isp = cfgetispeed(&term);
00280 osp = cfgetospeed(&term);
00281
00282 if (baud2i(isp) != baud || baud2i(osp) != baud)
00283 {
00284
00285 yprintf(OUTPUT_LV_ERROR, "Error: Requested baudrate is %d/%d, \n but sellected baudrate is %d/%d.\n",
00286 baud, baud, baud2i(isp), baud2i(osp));
00287 return 0;
00288 }
00289 }
00290
00291 ret = write(g_device_port, "\n\nVV\n\n", 6);
00292 yp_usleep(10000);
00293 serial_recieve(recieve_throw, buf);
00294 #else
00295
00296 DCB dcb;
00297
00298
00299 GetCommState(g_hdevices, &dcb);
00300 dcb.fBinary = 1;
00301 dcb.fParity = 0;
00302 dcb.fOutxCtsFlow = 0;
00303 dcb.fOutxDsrFlow = 0;
00304 dcb.fDtrControl = DTR_CONTROL_DISABLE;
00305 dcb.fDsrSensitivity = FALSE;
00306 dcb.Parity = NOPARITY;
00307 dcb.StopBits = ONESTOPBIT;
00308 dcb.ByteSize = 8;
00309 dcb.BaudRate = i2baud(baud);
00310 SER_BAUDRATE = (double)baud;
00311 SetCommState(g_hdevices, &dcb);
00312 #endif // !defined(__MINGW32__)
00313 return 1;
00314 }
00315
00316
00317 int serial_connect(char *device_name)
00318 {
00319 #if !defined(__MINGW32__)
00320 g_device_port = open(device_name, O_RDWR);
00321
00322 if (g_device_port < 0)
00323 {
00324 yprintf(OUTPUT_LV_ERROR, "Error: Can't open serial port.\n");
00325 return 0;
00326 }
00327
00328 tcgetattr(g_device_port, &g_oldtio);
00329
00330 serial_change_baudrate(DEFAULT_BAUDRATE);
00331 #else
00332
00333
00334 COMMTIMEOUTS cto;
00335
00336
00337 g_hdevices = CreateFile(device_name, GENERIC_WRITE | GENERIC_READ, 0, 0, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0);
00338 if (g_hdevices == INVALID_HANDLE_VALUE)
00339 {
00340 return 0;
00341 }
00342
00343 GetCommState(g_hdevices, &g_olddcb);
00344 serial_change_baudrate(DEFAULT_BAUDRATE);
00345
00346
00347 GetCommTimeouts(g_hdevices, &cto);
00348 GetCommTimeouts(g_hdevices, &g_oldcto);
00349 cto.ReadIntervalTimeout = 0;
00350 cto.ReadTotalTimeoutMultiplier = 0;
00351 cto.ReadTotalTimeoutConstant = 1000;
00352 cto.WriteTotalTimeoutMultiplier = 0;
00353 cto.WriteTotalTimeoutConstant = 1000;
00354 SetCommTimeouts(g_hdevices, &cto);
00355 #endif // !defined(__MINGW32__)
00356 return 1;
00357 }
00358
00359
00360 int serial_close(void)
00361 {
00362 #if !defined(__MINGW32__)
00363
00364
00365 if (g_device_port != 0)
00366 {
00367 tcsetattr(g_device_port, TCSANOW, &g_oldtio);
00368 close(g_device_port);
00369 g_device_port = 0;
00370 }
00371 #else
00372
00373 if (g_hdevices != NULL)
00374 {
00375 SetCommState(g_hdevices, &g_olddcb);
00376 SetCommTimeouts(g_hdevices, &g_oldcto);
00377 CloseHandle(g_hdevices);
00378 g_hdevices = NULL;
00379 }
00380 #endif // !defined(__MINGW32__)
00381
00382 return 1;
00383 }
00384
00385 void serial_flush_in(void)
00386 {
00387 #if !defined(__MINGW32__)
00388
00389 tcflush(g_device_port, TCIFLUSH);
00390 #else
00391
00392 char buf[4096];
00393
00394 DWORD len;
00395 DWORD ret;
00396 COMSTAT state;
00397
00398 while (1)
00399 {
00400 ClearCommError(g_hdevices, &ret, &state);
00401 len = state.cbInQue;
00402
00403 if (len > 0)
00404 {
00405 if (len > 4000)
00406 len = 4000;
00407 if (!ReadFile(g_hdevices, buf, len, &len, NULL))
00408 {
00409 return;
00410 }
00411 break;
00412 }
00413 else if (len == 0)
00414 break;
00415 yp_usleep(5000);
00416 }
00417 #endif // !defined(__MINGW32__)
00418 }
00419
00420 void serial_flush_out(void)
00421 {
00422 #if !defined(__MINGW32__)
00423
00424 tcflush(g_device_port, TCOFLUSH);
00425 #else
00426
00427 #endif // !defined(__MINGW32__)
00428 }
00429
00430
00431 int serial_recieve(int (*serial_event)(char *, int, double, void *), void *data)
00432 {
00433 char buf[4096];
00434 double receive_time;
00435 int retval;
00436
00437 while (1)
00438 {
00439 #if !defined(__MINGW32__)
00440
00441 fd_set rfds;
00442 struct timeval tv;
00443 size_t len;
00444
00445 tv.tv_sec = 0;
00446 tv.tv_usec = 200000;
00447 FD_ZERO(&rfds);
00448 FD_SET(g_device_port, &rfds);
00449
00450 retval = select(g_device_port + 1, &rfds, NULL, NULL, &tv);
00451 if (retval < 0)
00452 {
00453 int errnum;
00454 errnum = errno;
00455 yprintf(OUTPUT_LV_DEBUG, "Error: Select in serial_recieve failed. (%s)\n", strerror(errnum));
00456 return -1;
00457 }
00458 else if (retval == 0)
00459 {
00460 yprintf(OUTPUT_LV_DEBUG, "Error: Select timed out\n");
00461 return -1;
00462 }
00463 receive_time = get_time();
00464
00465 len = read(g_device_port, buf, 4000);
00466 if (len < 0)
00467 {
00468 int errnum;
00469 errnum = errno;
00470 yprintf(OUTPUT_LV_DEBUG, "Error: Read in serial_recieve failed. (%s)\n", strerror(errnum));
00471 return -1;
00472 }
00473 else if (len == 0)
00474 {
00475 yprintf(OUTPUT_LV_DEBUG, "Error: Read timed out\n");
00476 return -1;
00477 }
00478 #else
00479
00480 DWORD len;
00481 DWORD ret;
00482 COMSTAT state;
00483 int timeout_count;
00484
00485 timeout_count = 0;
00486 while (1)
00487 {
00488 if (!ClearCommError(g_hdevices, &ret, &state))
00489 return -1;
00490 if (ret)
00491 return -1;
00492 len = state.cbInQue;
00493 if (len > 4000)
00494 len = 4000;
00495 if (len > 0)
00496 break;
00497 yp_usleep(5000);
00498 timeout_count++;
00499 if (timeout_count > 500 / 5)
00500 return -1;
00501 }
00502 receive_time = get_time();
00503 if (!ReadFile(g_hdevices, buf, len, &len, NULL))
00504 {
00505 return -1;
00506 }
00507 buf[len] = 0;
00508 #endif // !defined(__MINGW32__)
00509 buf[len] = 0;
00510
00511 if (len > 0)
00512 {
00513 retval = serial_event(buf, len, receive_time, data);
00514 if (retval < 0)
00515 return retval;
00516 }
00517 }
00518 }
00519
00520 int encode_write(char *data, int len)
00521 {
00522 unsigned char buf[128];
00523 int encode_len, ret;
00524
00525 buf[0] = COMMUNICATION_START_BYTE;
00526 encode_len = encode((unsigned char *)data, len, buf + 1, 126);
00527 buf[encode_len + 1] = COMMUNICATION_END_BYTE;
00528
00529 ret = serial_write((char *)buf, encode_len + 2);
00530 if (ret <= 0)
00531 {
00532 return -1;
00533 }
00534 serial_flush_out();
00535
00536 return 0;
00537 }
00538
00539 int serial_write(char *buf, int len)
00540 {
00541 #if !defined(__MINGW32__)
00542
00543 int ret;
00544 #else
00545
00546 DWORD ret;
00547 #endif // !defined(__MINGW32__)
00548
00549 do
00550 {
00551 #if !defined(__MINGW32__)
00552
00553 ret = write(g_device_port, buf, len);
00554 #else
00555
00556 buf[len] = 0;
00557 WriteFile(g_hdevices, buf, len, (LPDWORD)&ret, 0);
00558 #endif // !defined(__MINGW32__)
00559
00560 if (ret <= 0 && len > 0)
00561 {
00562 return -1;
00563 }
00564 len -= ret;
00565 buf += ret;
00566 } while (len > 0);
00567
00568 #ifdef __APPLE__
00569 yp_usleep(100);
00570 #endif // __APPLE__
00571 return 1;
00572 }