urg_ctrl.c

00001 /*
00002   北陽電機製 測域センサ URG-X002S の基本操作クラス
00003 
00004   Satofumi KAMIMURA
00005   $Id$
00006 */
00007 
00008 #include "urg_ctrl.h"
00009 #include "connect_device.h"
00010 #include "serial_device.h"
00011 #include "tcpip_device.h"
00012 #include <math.h>
00013 #include "math_util.h"
00014 #include <stdio.h>
00015 #include <string.h>
00016 #include <ctype.h>
00017 #include "get_keyword.h"
00018 
00019 
00020 enum {
00021   FALSE = 0,
00022   TRUE = 1,
00023 };
00024 
00030 #ifndef URG_CONFIG_FILE
00031 #define URG_CONFIG_FILE "defaultargs"
00032 #endif
00033 
00034 
00038 #ifndef URG_AUTO_PORT
00039 # ifdef LINUX
00040 #  define URG_AUTO_PORT "/dev/ttyS"
00041 # else
00042 #  define URG_AUTO_PORT "COM"
00043 # endif
00044 #endif
00045 
00046 
00047 static char *Error_device = "";
00048 static long Error_baudrate = 0;
00049 static char *Error_message = "connection device is not specified";
00050 static int fd = -1;
00051 
00052 
00053 static int isLF(char ch) {
00054   if ((ch == '\r') || (ch == '\n')) {
00055     return TRUE;
00056   }
00057   return FALSE;
00058 }
00059 
00060 
00061 static int configBaudrate(int id, long baudrate) {
00062   long expected_baudrate[] = { 19200, 115200, 57600 };
00063   char message[15];
00064   char pre_ch;
00065   int i;
00066 
00067   sprintf(message, "S%06ldXXXXXXX\r", baudrate);
00068   for (i = 0; i < 3; ++i) {
00069     char recv_buffer[18];
00070     int product_check;
00071     int lf_count = 0;
00072     char ch;
00073     int ret;
00074     int was_timeout;
00075 
00076     device_set_baudrate(id, expected_baudrate[i]);
00077     device_flush(id);
00078 
00079     // 'V' コマンドによるバージョンの確認
00080     device_send(id, "V\r", 2);
00081     device_send(id, message, 15);
00082 
00083     // タイムアウトを監視しつつ1文字ずつ受信し、改行を 3 つ読み飛ばす
00084     while ((ret = device_recv(id, &ch, 1, SCI_TIMEOUT)) > 0) {
00085       if (isLF(ch)) {
00086         if (++lf_count >= 3) {
00087           break;
00088         }
00089       }
00090     }
00091     was_timeout = (ret == 0) ? TRUE : FALSE;
00092     product_check = FALSE;
00093     if (lf_count >= 3) {
00094       // PROD行のメッセージを読み込む ("PROD:SOKUIKI Sensor URG")
00095       char expected_message[] = "PROD:SOKUIKI Sensor URG";
00096       char message_buffer[sizeof(expected_message)-1];
00097       if (device_recv(id, message_buffer, sizeof(expected_message)-1,
00098                       SCI_TIMEOUT) >= (int)sizeof(expected_message)-1) {
00099         int j;
00100         for (j = 0; j < (int)sizeof(expected_message)-1; ++j) {
00101           if (expected_message[j] != message_buffer[j]) {
00102             break;
00103           }
00104         }
00105         if (j >= (int)sizeof(expected_message)-1) {
00106           product_check = TRUE;
00107         }
00108       }
00109     }
00110 
00111     pre_ch = '\0';
00112     while ((!was_timeout) && (device_recv(id, &ch, 1, SCI_TIMEOUT)) > 0) {
00113       if (isLF(ch) && isLF(pre_ch)) {
00114         break;
00115       }
00116       pre_ch = ch;
00117     }
00118 
00119     if (product_check == FALSE) {
00120       continue;
00121     }
00122 
00123     if (device_recv(id, recv_buffer, 18, SCI_TIMEOUT) >= 15) {
00124       if (recv_buffer[15] == '0') {
00125         device_set_baudrate(id, baudrate);
00126         device_flush(id);
00127         if (!product_check) {
00128           return URG_PRODUCT_MISMATCH_ERROR;
00129         }
00130         return 0;
00131       }
00132     }
00133   }
00134 
00135   closeDevice(id);
00136   return URG_BAUDRATE_ADJUST_ERROR;
00137 }
00138 
00139 
00140 static int searchConfigFile(const char *path) {
00141   enum {
00142     CONFIG_PATH_LENGTH = 256,
00143   };
00144   char file_path[CONFIG_PATH_LENGTH +1];
00145   int path_length = strlen(path);
00146   int file_length = strlen(URG_CONFIG_FILE);
00147   long baudrate = URG_BAUDRATE;
00148   char *baudrate_str;
00149   char *deviceName;
00150 
00151   if ((path_length + 1 + file_length + 1) < CONFIG_PATH_LENGTH) {
00152     strcpy(file_path, path);
00153     strcpy(&file_path[path_length], "/");
00154     strcpy(&file_path[path_length +1], URG_CONFIG_FILE);
00155     file_path[path_length + 1 + file_length] = '\0';
00156 
00157     if ((baudrate_str = get_keyword(file_path, "urg_baudrate"))) {
00158       baudrate = atoi(baudrate_str);
00159       free(baudrate_str);
00160     }
00161     deviceName = get_keyword(file_path, "urg_port");
00162   }
00163   if (deviceName) {
00164     int ret_value = initConnectDevice(deviceName, baudrate, SERIAL);
00165     free(deviceName);
00166     if (ret_value >= 0) {
00167       fd = ret_value;
00168       ret_value = configBaudrate(fd, baudrate);
00169     }
00170     return ret_value;
00171   }
00172   return NO_FILE_ERROR;
00173 }
00174 
00175 
00176 static int connectDefault(void) {
00177   int ret_value = searchConfigFile(".");
00178   char *home;
00179   long baudrate = URG_BAUDRATE;
00180   char *deviceName = NULL;
00181   char *baudrate_str = NULL;
00182 
00183   if ((ret_value >= 0) || (ret_value != NO_FILE_ERROR)) {
00184     return ret_value;
00185   }
00186 
00187   home = getenv("HOME");
00188   if (home) {
00189     ret_value = searchConfigFile(home);
00190     // !!! free する必要はあるのか?
00191     if ((ret_value >= 0) || (ret_value != NO_FILE_ERROR)) {
00192       return ret_value;
00193     }
00194   }
00195 
00196   if ((baudrate_str = getenv("URG_BAUDRATE"))) {
00197     baudrate = atoi(baudrate_str);
00198   }
00199 
00200   deviceName = getenv("URG_PORT");
00201   if (deviceName) {
00202     int ret_value;
00203     ret_value = initConnectDevice(deviceName, baudrate, SERIAL);
00204     if (ret_value >= 0) {
00205       fd = ret_value;
00206       ret_value = configBaudrate(fd, baudrate);
00207     }
00208     return ret_value;
00209   }
00210   return -1;
00211 }
00212 
00213 
00243 int initURGCtrl(int argc, char *argv[]) {
00244   long baudrate = URG_BAUDRATE;
00245   char *deviceName = NULL;
00246   int ret_value;
00247   int i;
00248 
00249   for (i = 1; i < argc; ++i) {
00250     if (!strncmp("--urg_port=", argv[i], 11) && (strlen(argv[i]) > 11)) {
00251       deviceName = &argv[i][11];
00252 
00253     } else if (!strncmp("--urg_baudrate=", argv[i], 15) &&
00254                (strlen(argv[i]) > 15)) {
00255       baudrate = atoi(&argv[i][15]);
00256 
00257     } else if (!strcmp("-s", argv[i]) || !strcmp("--simulator", argv[i])) {
00258       // シミュレータ(TCP/IP) への接続
00259       return initConnectDevice("localhost", 49763, TCP_IP);
00260     }
00261   }
00262   if (deviceName &&
00263       (!strcmp("auto", deviceName) || !strcmp("AUTO", deviceName))) {
00264     int id;
00265     for (id = 20; id >= 0; --id) {
00266       char checkDevice[16] = { '\0','\0','\0','\0','\0','\0','\0','\0',
00267                                '\0','\0','\0','\0','\0','\0','\0','\0' };
00268       sprintf(checkDevice, URG_AUTO_PORT "%d", id);
00269       ret_value = initConnectDevice(checkDevice, baudrate, SERIAL);
00270       if (ret_value >= 0) {
00271         fd = ret_value;
00272         ret_value = configBaudrate(fd, baudrate);
00273         break;
00274       }
00275     }
00276     if (ret_value < 0) {
00277       Error_device = URG_AUTO_PORT;
00278       Error_baudrate = baudrate;
00279       Error_message = "auto dvice detection is fail";
00280     } else {
00281       Error_message = "success";
00282     }
00283     return ret_value;
00284   }
00285 
00286   if (!deviceName) {
00287     return connectDefault();
00288   }
00289   ret_value = initConnectDevice(deviceName, baudrate, SERIAL);
00290   if (ret_value >= 0) {
00291     fd = ret_value;
00292     ret_value = configBaudrate(fd, baudrate);
00293   }
00294   return ret_value;
00295 }
00296 
00297 
00307 char* urg_getError(void) {
00308 
00309   // 保持しているエラーメッセージバッファへのポインタを返す
00310 
00311   return Error_message;
00312 }
00313 
00314 
00315 static void swap(int *a, int *b) {
00316   int tmp = *a;
00317   *a = *b;
00318   *b = tmp;
00319 }
00320 
00321 
00322 static unsigned char encode6bit(char ch) {
00323   return 0x3f & (ch - 0x30);
00324 }
00325 
00326 
00362 int urg_capture(long *data, int from ,int to, int group) {
00363   char message[10];
00364   char reply[10];
00365   char ret;
00366   char recv_buffer[3072];
00367   char lf[2];
00368   int range[2] = { from, to };
00369   int numdata;
00370   int index;
00371   int total;
00372   int i;
00373   int n;
00374 
00375   for (i = 0; i < 2; ++i) {
00376     if (range[i] < 0) {
00377       range[i] = 0;
00378     } else if (range[i] >= URG_DATA_SIZE) {
00379       range[i] = URG_DATA_SIZE -1;
00380     }
00381   }
00382   if (range[0] > range[1]) {
00383     swap(&range[0], &range[1]);
00384   }
00385   if (group <= 0) {
00386     group = 1;
00387   } else if (group > 99) {
00388     group = 99;
00389   }
00390 
00391   // データ要求の送信
00392   sprintf(message, "G%03d%03d%02d\r", range[0], range[1], group);
00393   device_flush(fd);
00394   device_send(fd, message, 10);
00395 
00396   // データの受信
00397   device_recv(fd, reply, 9, SCI_TIMEOUT);
00398   if (strncmp(message, reply, 9)) {
00399     return -5;
00400   }
00401 
00402   device_recv(fd, lf, 1, SCI_TIMEOUT);
00403   if (!isLF(lf[0])) {
00404     return -6;
00405   }
00406   device_recv(fd, &ret, 1, SCI_TIMEOUT);
00407   if (ret != '0') {
00408     return -7;
00409   }
00410   device_recv(fd, lf, 1, SCI_TIMEOUT);
00411   if (!isLF(lf[0])) {
00412     return -8;
00413   }
00414 
00415   numdata = range[1] - range[0] +1;
00416   numdata = (numdata / group) + ((numdata % group == 0) ? 0 : 1);
00417   total = (numdata << 1) + (numdata >> 5);
00418   n = device_recv(fd, recv_buffer, total, SCI_TIMEOUT);
00419   if (n != total) {
00420     //printf("total: %d, n: %d\n", total, n);
00421     return -9;
00422   }
00423   if ((2 != device_recv(fd, lf, 2, SCI_TIMEOUT))
00424       || !isLF(lf[0]) || !isLF(lf[1])) {
00425     return -10;
00426   }
00427 
00428   // 距離データを配列に格納
00429   for (i = 0; i < range[0]; ++i) {
00430     data[i] = -1;
00431   }
00432   index = range[0];
00433   for (i = 0; (index <= range[1]) && (i < numdata); ++i) {
00434     int j;
00435     int actual_index = (i << 1) + (i >> 5);
00436     if ((i > 0) && ((i & 0x3f) == 0) && !isLF(recv_buffer[actual_index -1])) {
00437       printf("i: %d\n", i);
00438 #if 0
00439       for (j = 0; j < 70; ++j) {
00440         printf("%d: %c\n", j, recv_buffer[j]);
00441       }
00442 #endif
00443       return -11;
00444     }
00445     for (j = 0; (index <= range[1]) && (j < group); ++j) {
00446       data[index++] =
00447         (encode6bit(recv_buffer[actual_index]) << 6) |
00448         encode6bit(recv_buffer[actual_index +1]);
00449     }
00450   }
00451   for (i = range[1] +1; i < URG_DATA_SIZE; ++i) {
00452     data[i] = -1;
00453   }
00454 
00455   return URG_DATA_SIZE;
00456 }
00457 
00458 
00467 int deg2index(const int degree) {
00468   int index;
00469   int sense_deg = ((degree % 360) + 360) % 360;
00470   if (sense_deg > 180) {
00471     sense_deg = sense_deg - 360;
00472   }
00473   index = (int)((135 - sense_deg) / (360.0/1024.0));
00474   if (index < 0) {
00475     index = 0;
00476   } else if (index > URG_DATA_SIZE-1) {
00477     index = URG_DATA_SIZE-1;
00478   }
00479   
00480   return index;
00481 }
00482 
00483 
00492 int index2deg(const int index) {
00493   return (int)(360 * index2rad(index) / (2.0 * M_PI));
00494 }
00495 
00496 
00505 double index2rad(const int index) {
00506   static const double rad135 = 2.0 * M_PI * 135.0/360.0;
00507 
00508   double rad = rad135 - (index * 2.0 * M_PI / 1024.0);
00509   if (rad < -rad135) {
00510     rad = -rad135;
00511   } else if (rad > rad135) {
00512     rad = rad135;
00513   }
00514 
00515   return rad;
00516 }
00517 
00518 
00525 double deg2rad(const int degree) {
00526   return (2.0 * M_PI * degree / 360.0);
00527 }
00528 

Generated on Mon Apr 13 22:52:04 2009 by  doxygen 1.5.7.1