serial.c
Go to the documentation of this file.
00001 // Copyright (c) 2010-2016 The YP-Spur Authors, except where otherwise indicated.
00002 //
00003 // Permission is hereby granted, free of charge, to any person obtaining a copy
00004 // of this software and associated documentation files (the "Software"), to
00005 // deal in the Software without restriction, including without limitation the
00006 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
00007 // sell copies of the Software, and to permit persons to whom the Software is
00008 // furnished to do so, subject to the following conditions:
00009 //
00010 // The above copyright notice and this permission notice shall be included in
00011 // all copies or substantial portions of the Software.
00012 //
00013 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00014 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00015 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
00016 // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00017 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00018 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
00019 // SOFTWARE.
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 /* yp-spur用 */
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 /* serial */
00051 #if !defined(__MINGW32__)
00052 
00053 // Unix用コード
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 // Windows用コード
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   // Unix用
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   // Windows用
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   // 8bit 通信
00227   // スタートビット1
00228   // エンドビット1
00229   // パリティー無し
00230   // 合計 10bit
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;       // Disable implementation dependent processing
00254 
00255   newtio.c_cc[VTIME] = 0; /* キャラクタ間タイマを使わない */
00256   newtio.c_cc[VMIN] = 1;  /* 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   {  // ---> check bit rate
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       // fail to set bit rate
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   }  // <--- check bit rate
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   // Windows用
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   // Windows用
00333   // DCB dcb;
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   // Unix用
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   // Windows用
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   // Unix用
00389   tcflush(g_device_port, TCIFLUSH);
00390 #else
00391   // Windows用
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   // Unix用
00424   tcflush(g_device_port, TCOFLUSH);
00425 #else
00426 // Windows用
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     // Unix用
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     // Windows用
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   // Unix用
00543   int ret;
00544 #else
00545   // Windows用
00546   DWORD ret;
00547 #endif  // !defined(__MINGW32__)
00548 
00549   do
00550   {
00551 #if !defined(__MINGW32__)
00552     // Unix用
00553     ret = write(g_device_port, buf, len);
00554 #else
00555     // Windows用
00556     buf[len] = 0;
00557     WriteFile(g_hdevices, buf, len, (LPDWORD)&ret, 0);
00558 #endif  // !defined(__MINGW32__)
00559     // printf("SEND: %s - %d(%d)\n",buf,len,ret);fflush(stdout);
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 }


yp-spur
Author(s):
autogenerated on Fri May 10 2019 02:52:19