urg_sensor.c
Go to the documentation of this file.
00001 
00011 #include "urg_c/urg_sensor.h"
00012 #include "urg_c/urg_errno.h"
00013 #include <stddef.h>
00014 #include <string.h>
00015 #include <stdio.h>
00016 #include <stdlib.h>
00017 
00018 #if defined(URG_MSC)
00019 #define snprintf _snprintf
00020 #endif
00021 
00022 
00023 enum {
00024     URG_FALSE = 0,
00025     URG_TRUE = 1,
00026 
00027     BUFFER_SIZE = 64 + 2 + 6,
00028 
00029     EXPECTED_END = -1,
00030 
00031     RECEIVE_DATA_TIMEOUT,
00032     RECEIVE_DATA_COMPLETE,      
00034     PP_RESPONSE_LINES = 10,
00035     VV_RESPONSE_LINES = 7,
00036     II_RESPONSE_LINES = 9,
00037 
00038     MAX_TIMEOUT = 140,
00039 };
00040 
00041 
00042 static const char NOT_CONNECTED_MESSAGE[] = "not connected.";
00043 static const char RECEIVE_ERROR_MESSAGE[] = "receive error.";
00044 
00045 
00047 static char scip_checksum(const char buffer[], int size)
00048 {
00049     unsigned char sum = 0x00;
00050     int i;
00051 
00052     for (i = 0; i < size; ++i) {
00053         sum += buffer[i];
00054     }
00055 
00056     // 計算の意味は SCIP 仕様書を参照のこと
00057     return (sum & 0x3f) + 0x30;
00058 }
00059 
00060 
00061 static int set_errno_and_return(urg_t *urg, int urg_errno)
00062 {
00063     urg->last_errno = urg_errno;
00064     return urg_errno;
00065 }
00066 
00067 
00068 // 受信した応答の行数を返す
00069 static int scip_response(urg_t *urg, const char* command,
00070                          const int expected_ret[], int timeout,
00071                          char *receive_buffer, int receive_buffer_max_size)
00072 {
00073     char *p = receive_buffer;
00074     char buffer[BUFFER_SIZE];
00075     int filled_size = 0;
00076     int line_number = 0;
00077     int ret = URG_UNKNOWN_ERROR;
00078 
00079     int write_size = (int)strlen(command);
00080     int n = connection_write(&urg->connection, command, write_size);
00081 
00082     if (n != write_size) {
00083         return set_errno_and_return(urg, URG_SEND_ERROR);
00084     }
00085 
00086     if (p) {
00087         *p = '\0';
00088     }
00089 
00090     do {
00091         n = connection_readline(&urg->connection, buffer, BUFFER_SIZE, timeout);
00092         if (n < 0) {
00093             return set_errno_and_return(urg, URG_NO_RESPONSE);
00094 
00095         } else if (p && (line_number > 0)
00096                    && (n < (receive_buffer_max_size - filled_size))) {
00097             // エコーバックは完全一致のチェックを行うため、格納しない
00098             memcpy(p, buffer, n);
00099             p += n;
00100             *p++ = '\0';
00101             filled_size += n;
00102         }
00103 
00104         if (line_number == 0) {
00105             // エコーバック文字列が、一致するかを確認する
00106             if (strncmp(buffer, command, write_size - 1)) {
00107                 return set_errno_and_return(urg, URG_INVALID_RESPONSE);
00108             }
00109         } else if (n > 0) {
00110             // エコーバック以外の行のチェックサムを評価する
00111             char checksum = buffer[n - 1];
00112             if ((checksum != scip_checksum(buffer, n - 1)) &&
00113                 (checksum != scip_checksum(buffer, n - 2))) {
00114                 return set_errno_and_return(urg, URG_CHECKSUM_ERROR);
00115             }
00116         }
00117 
00118         // ステータス応答を評価して、戻り値を決定する
00119         if (line_number == 1) {
00120             if (n == 1) {
00121                 // SCIP 1.1 応答の場合は、正常応答とみなす
00122                 ret = 0;
00123 
00124             } else if (n != 3) {
00125                 return set_errno_and_return(urg, URG_INVALID_RESPONSE);
00126 
00127             } else {
00128                 int i;
00129                 int actual_ret = strtol(buffer, NULL, 10);
00130                 for (i = 0; expected_ret[i] != EXPECTED_END; ++i) {
00131                     if (expected_ret[i] == actual_ret) {
00132                         ret = 0;
00133                         break;
00134                     }
00135                 }
00136             }
00137         }
00138 
00139         ++line_number;
00140     } while (n > 0);
00141 
00142     return (ret < 0) ? ret : (line_number - 1);
00143 }
00144 
00145 
00146 static void ignore_receive_data(urg_t *urg, int timeout)
00147 {
00148     char buffer[BUFFER_SIZE];
00149     int n;
00150 
00151     if (urg->is_sending == URG_FALSE) {
00152         return;
00153     }
00154 
00155     do {
00156         n = connection_readline(&urg->connection,
00157                                 buffer, BUFFER_SIZE, timeout);
00158     } while (n >= 0);
00159 
00160     urg->is_sending = URG_FALSE;
00161 }
00162 
00163 
00164 static void ignore_receive_data_with_qt(urg_t *urg, int timeout)
00165 {
00166     if ((urg->is_sending == URG_FALSE) && (urg->is_laser_on == URG_FALSE)) {
00167         return;
00168     }
00169 
00170     connection_write(&urg->connection, "QT\n", 3);
00171     urg->is_sending = URG_TRUE;
00172     urg->is_laser_on = URG_FALSE;
00173     ignore_receive_data(urg, timeout);
00174 }
00175 
00176 
00177 static int change_sensor_baudrate(urg_t *urg,
00178                                   long current_baudrate, long next_baudrate)
00179 {
00180     enum { SS_COMMAND_SIZE = 10 };
00181     char buffer[SS_COMMAND_SIZE];
00182     int ss_expected[] = { 0, 3, 4, EXPECTED_END };
00183     int ret;
00184 
00185     if (current_baudrate == next_baudrate) {
00186         // 現在のボーレートと設定するボーレートが一緒ならば、戻る
00187         return set_errno_and_return(urg, URG_NO_ERROR);
00188     }
00189 
00190     // "SS" コマンドでボーレートを変更する
00191     snprintf(buffer, SS_COMMAND_SIZE, "SS%06ld\n", next_baudrate);
00192     ret = scip_response(urg, buffer, ss_expected, urg->timeout, NULL, 0);
00193 
00194     // 0F 応答のときは Ethernet 用のセンサとみなし、正常応答を返す
00195     if (ret == -15) {
00196         return set_errno_and_return(urg, URG_NO_ERROR);
00197     }
00198     if (ret <= 0) {
00199         return set_errno_and_return(urg, URG_INVALID_PARAMETER);
00200     }
00201 
00202     // 正常応答ならば、ホスト側のボーレートを変更する
00203     ret = connection_set_baudrate(&urg->connection, next_baudrate);
00204 
00205     // センサ側の設定反映を待つために少しだけ待機する
00206     ignore_receive_data(urg, MAX_TIMEOUT);
00207 
00208     return set_errno_and_return(urg, ret);
00209 }
00210 
00211 
00212 // ボーレートを変更しながら接続する
00213 static int connect_urg_device(urg_t *urg, long baudrate)
00214 {
00215     long try_baudrate[] = { 19200, 38400, 115200 };
00216     int try_times = sizeof(try_baudrate) / sizeof(try_baudrate[0]);
00217     int i;
00218 
00219     // 指示されたボーレートから接続する
00220     for (i = 0; i < try_times; ++i) {
00221         if (try_baudrate[i] == baudrate) {
00222             try_baudrate[i] = try_baudrate[0];
00223             try_baudrate[0] = baudrate;
00224             break;
00225         }
00226     }
00227 
00228     for (i = 0; i < try_times; ++i) {
00229         enum { RECEIVE_BUFFER_SIZE = 4 };
00230         int qt_expected[] = { 0, EXPECTED_END };
00231         char receive_buffer[RECEIVE_BUFFER_SIZE + 1];
00232         int ret;
00233 
00234         connection_set_baudrate(&urg->connection, try_baudrate[i]);
00235 
00236         // QT を送信し、応答が返されるかでボーレートが一致しているかを確認する
00237         ret = scip_response(urg, "QT\n", qt_expected, MAX_TIMEOUT,
00238                             receive_buffer, RECEIVE_BUFFER_SIZE);
00239         if (ret > 0) {
00240             if (!strcmp(receive_buffer, "E")) {
00241                 int scip20_expected[] = { 0, EXPECTED_END };
00242 
00243                 // QT 応答の最後の改行を読み飛ばす
00244                 ignore_receive_data(urg, MAX_TIMEOUT);
00245 
00246                 // "E" が返された場合は、SCIP 1.1 とみなし "SCIP2.0" を送信する
00247                 ret = scip_response(urg, "SCIP2.0\n", scip20_expected,
00248                                     MAX_TIMEOUT, NULL, 0);
00249 
00250                 // SCIP2.0 応答の最後の改行を読み飛ばす
00251                 ignore_receive_data(urg, MAX_TIMEOUT);
00252 
00253                 // ボーレートを変更して戻る
00254                 return change_sensor_baudrate(urg, try_baudrate[i], baudrate);
00255 
00256             } else if (!strcmp(receive_buffer, "0Ee")) {
00257                 int tm2_expected[] = { 0, EXPECTED_END };
00258 
00259                 // "0Ee" が返された場合は、TM モードとみなし "TM2" を送信する
00260                 scip_response(urg, "TM2\n", tm2_expected,
00261                               MAX_TIMEOUT, NULL, 0);
00262 
00263                 // ボーレートを変更して戻る
00264                 return change_sensor_baudrate(urg, try_baudrate[i], baudrate);
00265             }
00266         }
00267 
00268         if (ret <= 0) {
00269             if (ret == URG_INVALID_RESPONSE) {
00270                 // 異常なエコーバックのときは、距離データ受信中とみなして
00271                 // データを読み飛ばす
00272                 ignore_receive_data_with_qt(urg, MAX_TIMEOUT);
00273 
00274                 // ボーレートを変更して戻る
00275                 return change_sensor_baudrate(urg, try_baudrate[i], baudrate);
00276 
00277             } else {
00278                 // 応答がないときは、ボーレートを変更して、再度接続を行う
00279                 ignore_receive_data_with_qt(urg, MAX_TIMEOUT);
00280                 continue;
00281             }
00282         } else if (!strcmp("00P", receive_buffer)) {
00283 
00284             // センサとホストのボーレートを変更して戻る
00285             return change_sensor_baudrate(urg, try_baudrate[i], baudrate);
00286         }
00287     }
00288 
00289     return set_errno_and_return(urg, URG_NOT_DETECT_BAUDRATE_ERROR);
00290 }
00291 
00292 
00293 // PP コマンドの応答を urg_t に格納する
00294 static int receive_parameter(urg_t *urg)
00295 {
00296     enum { RECEIVE_BUFFER_SIZE = BUFFER_SIZE * 9, };
00297     char receive_buffer[RECEIVE_BUFFER_SIZE];
00298     int pp_expected[] = { 0, EXPECTED_END };
00299     unsigned short received_bits = 0x0000;
00300     char *p;
00301     int i;
00302 
00303     int ret = scip_response(urg, "PP\n", pp_expected, MAX_TIMEOUT,
00304                             receive_buffer, RECEIVE_BUFFER_SIZE);
00305     if (ret < 0) {
00306         return ret;
00307     } else if (ret < PP_RESPONSE_LINES) {
00308         ignore_receive_data_with_qt(urg, MAX_TIMEOUT);
00309         return set_errno_and_return(urg, URG_INVALID_RESPONSE);
00310     }
00311 
00312     p = receive_buffer;
00313     for (i = 0; i < (ret - 1); ++i) {
00314 
00315         if (!strncmp(p, "DMIN:", 5)) {
00316             urg->min_distance = strtol(p + 5, NULL, 10);
00317             received_bits |= 0x0001;
00318 
00319         } else if (!strncmp(p, "DMAX:", 5)) {
00320             urg->max_distance = strtol(p + 5, NULL, 10);
00321             received_bits |= 0x0002;
00322 
00323         } else if (!strncmp(p, "ARES:", 5)) {
00324             urg->area_resolution = strtol(p + 5, NULL, 10);
00325             received_bits |= 0x0004;
00326 
00327         } else if (!strncmp(p, "AMIN:", 5)) {
00328             urg->first_data_index = strtol(p + 5, NULL, 10);
00329             received_bits |= 0x0008;
00330 
00331         } else if (!strncmp(p, "AMAX:", 5)) {
00332             urg->last_data_index = strtol(p + 5, NULL, 10);
00333             received_bits |= 0x0010;
00334 
00335         } else if (!strncmp(p, "AFRT:", 5)) {
00336             urg->front_data_index = strtol(p + 5, NULL, 10);
00337             received_bits |= 0x0020;
00338 
00339         } else if (!strncmp(p, "SCAN:", 5)) {
00340             int rpm = strtol(p + 5, NULL, 10);
00341             // タイムアウト時間は、計測周期の 16 倍程度の値にする
00342             urg->scan_usec = 1000 * 1000 * 60 / rpm;
00343             urg->timeout = urg->scan_usec >> (10 - 4);
00344             received_bits |= 0x0040;
00345         }
00346         p += strlen(p) + 1;
00347     }
00348 
00349     // 全てのパラメータを受信したか確認
00350     if (received_bits != 0x007f) {
00351         return set_errno_and_return(urg, URG_RECEIVE_ERROR);
00352     }
00353 
00354     urg_set_scanning_parameter(urg,
00355                                urg->first_data_index - urg->front_data_index,
00356                                urg->last_data_index - urg->front_data_index,
00357                                1);
00358 
00359     return set_errno_and_return(urg, URG_NO_ERROR);
00360 }
00361 
00362 
00364 long urg_scip_decode(const char data[], int size)
00365 {
00366     const char* p = data;
00367     const char* last_p = p + size;
00368     int value = 0;
00369 
00370     while (p < last_p) {
00371         value <<= 6;
00372         value &= ~0x3f;
00373         value |= *p++ - 0x30;
00374     }
00375     return value;
00376 }
00377 
00378 
00379 static int parse_parameter(const char *parameter, int size)
00380 {
00381     char buffer[5];
00382 
00383     memcpy(buffer, parameter, size);
00384     buffer[size] = '\0';
00385 
00386     return strtol(buffer, NULL, 10);
00387 }
00388 
00389 
00390 static urg_measurement_type_t parse_distance_parameter(urg_t *urg,
00391                                                        const char echoback[])
00392 {
00393     urg_measurement_type_t ret_type = URG_UNKNOWN;
00394 
00395     urg->received_range_data_byte = URG_COMMUNICATION_3_BYTE;
00396     if (echoback[1] == 'S') {
00397         urg->received_range_data_byte = URG_COMMUNICATION_2_BYTE;
00398         ret_type = URG_DISTANCE;
00399 
00400     } else if (echoback[1] == 'D') {
00401         if ((echoback[0] == 'G') || (echoback[0] == 'M')) {
00402             ret_type = URG_DISTANCE;
00403         } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) {
00404             ret_type = URG_MULTIECHO;
00405         }
00406     } else if (echoback[1] == 'E') {
00407         if ((echoback[0] == 'G') || (echoback[0] == 'M')) {
00408             ret_type = URG_DISTANCE_INTENSITY;
00409         } else if ((echoback[0] == 'H') || (echoback[0] == 'N')) {
00410             ret_type = URG_MULTIECHO_INTENSITY;
00411         }
00412     } else {
00413         return URG_UNKNOWN;
00414     }
00415 
00416     // パラメータの格納
00417     urg->received_first_index = parse_parameter(&echoback[2], 4);
00418     urg->received_last_index = parse_parameter(&echoback[6], 4);
00419     urg->received_skip_step = parse_parameter(&echoback[10], 2);
00420 
00421     return ret_type;
00422 }
00423 
00424 
00425 static urg_measurement_type_t parse_distance_echoback(urg_t *urg,
00426                                                       const char echoback[])
00427 {
00428     size_t line_length;
00429     urg_measurement_type_t ret_type = URG_UNKNOWN;
00430 
00431     if (!strcmp("QT", echoback)) {
00432         return URG_STOP;
00433     }
00434 
00435     line_length = strlen(echoback);
00436     if ((line_length == 12) &&
00437         ((echoback[0] == 'G') || (echoback[0] == 'H'))) {
00438         ret_type = parse_distance_parameter(urg, echoback);
00439 
00440     } else if ((line_length == 15) &&
00441                ((echoback[0] == 'M') || (echoback[0] == 'N'))) {
00442         ret_type = parse_distance_parameter(urg, echoback);
00443     }
00444     return ret_type;
00445 }
00446 
00447 
00448 static int receive_length_data(urg_t *urg, long length[],
00449                                unsigned short intensity[],
00450                                urg_measurement_type_t type, char buffer[])
00451 {
00452     int n;
00453     int step_filled = 0;
00454     int line_filled = 0;
00455     int multiecho_index = 0;
00456 
00457     int each_size =
00458         (urg->received_range_data_byte == URG_COMMUNICATION_2_BYTE) ? 2 : 3;
00459     int data_size = each_size;
00460     int is_intensity = URG_FALSE;
00461     int is_multiecho = URG_FALSE;
00462     int multiecho_max_size = 1;
00463 
00464     if ((type == URG_DISTANCE_INTENSITY) || (type == URG_MULTIECHO_INTENSITY)) {
00465         data_size *= 2;
00466         is_intensity = URG_TRUE;
00467     }
00468     if ((type == URG_MULTIECHO) || (type == URG_MULTIECHO_INTENSITY)) {
00469         is_multiecho = URG_TRUE;
00470         multiecho_max_size = URG_MAX_ECHO;
00471     }
00472 
00473     do {
00474         char *p = buffer;
00475         char *last_p;
00476 
00477         n = connection_readline(&urg->connection,
00478                                 &buffer[line_filled], BUFFER_SIZE - line_filled,
00479                                 urg->timeout);
00480 
00481         if (n > 0) {
00482             // チェックサムの評価
00483             if (buffer[line_filled + n - 1] !=
00484                 scip_checksum(&buffer[line_filled], n - 1)) {
00485                 ignore_receive_data_with_qt(urg, urg->timeout);
00486                 return set_errno_and_return(urg, URG_CHECKSUM_ERROR);
00487             }
00488         }
00489 
00490         if (n > 0) {
00491             line_filled += n - 1;
00492         }
00493         last_p = p + line_filled;
00494 
00495         while ((last_p - p) >= data_size) {
00496             int index;
00497 
00498             if (*p == '&') {
00499                 // 先頭文字が '&' だったときは、マルチエコーのデータとみなす
00500 
00501                 if ((last_p - (p + 1)) < data_size) {
00502                     // '&' を除いて、data_size 分データが無ければ抜ける
00503                     break;
00504                 }
00505 
00506                 --step_filled;
00507                 ++multiecho_index;
00508                 ++p;
00509                 --line_filled;
00510 
00511             } else {
00512                 // 次のデータ
00513                 multiecho_index = 0;
00514             }
00515 
00516             index = (step_filled * multiecho_max_size) + multiecho_index;
00517 
00518             if (step_filled >
00519                 (urg->received_last_index - urg->received_first_index)) {
00520                 // データが多過ぎる場合は、残りのデータを無視して戻る
00521                 ignore_receive_data_with_qt(urg, urg->timeout);
00522                 return set_errno_and_return(urg, URG_RECEIVE_ERROR);
00523             }
00524 
00525 
00526             if (is_multiecho && (multiecho_index == 0)) {
00527                 // マルチエコーのデータ格納先をダミーデータで埋める
00528                 int i;
00529                 if (length) {
00530                     for (i = 1; i < multiecho_max_size; ++i) {
00531                         length[index + i] = 0;
00532                     }
00533                 }
00534                 if (intensity) {
00535                     for (i = 1; i < multiecho_max_size; ++i) {
00536                         intensity[index + i] = 0;
00537                     }
00538                 }
00539             }
00540 
00541             // 距離データの格納
00542             if (length) {
00543                 length[index] = urg_scip_decode(p, 3);
00544             }
00545             p += 3;
00546 
00547             // 強度データの格納
00548             if (is_intensity) {
00549                 if (intensity) {
00550                     intensity[index] = (unsigned short)urg_scip_decode(p, 3);
00551                 }
00552                 p += 3;
00553             }
00554 
00555             ++step_filled;
00556             line_filled -= data_size;
00557         }
00558 
00559         // 次に処理する文字を退避
00560         memmove(buffer, p, line_filled);
00561     } while (n > 0);
00562 
00563     return step_filled;
00564 }
00565 
00566 
00568 static int receive_data(urg_t *urg, long data[], unsigned short intensity[],
00569                         long *time_stamp, unsigned long long *system_time_stamp)
00570 {
00571     urg_measurement_type_t type;
00572     char buffer[BUFFER_SIZE];
00573     int ret = 0;
00574     int n;
00575     int extended_timeout = urg->timeout
00576         + 2 * (urg->scan_usec * (urg->scanning_skip_scan) / 1000);
00577 
00578     // エコーバックの取得
00579     n = connection_readline(&urg->connection,
00580                             buffer, BUFFER_SIZE, extended_timeout);
00581     if (n <= 0) {
00582         return set_errno_and_return(urg, URG_NO_RESPONSE);
00583     }
00584     // エコーバックの解析
00585     type = parse_distance_echoback(urg, buffer);
00586 
00587     // 応答の取得
00588     n = connection_readline(&urg->connection,
00589                             buffer, BUFFER_SIZE, urg->timeout);
00590     if (n != 3) {
00591         ignore_receive_data_with_qt(urg, urg->timeout);
00592         return set_errno_and_return(urg, URG_INVALID_RESPONSE);
00593     }
00594 
00595     if (buffer[n - 1] != scip_checksum(buffer, n - 1)) {
00596         // チェックサムの評価
00597         ignore_receive_data_with_qt(urg, urg->timeout);
00598         return set_errno_and_return(urg, URG_CHECKSUM_ERROR);
00599     }
00600 
00601     if (type == URG_STOP) {
00602         // QT 応答の場合には、最後の改行を読み捨て、正常応答として処理する
00603         n = connection_readline(&urg->connection,
00604                                 buffer, BUFFER_SIZE, urg->timeout);
00605         if (n == 0) {
00606             return 0;
00607         } else {
00608             return set_errno_and_return(urg, URG_INVALID_RESPONSE);
00609         }
00610     }
00611 
00612     if (urg->specified_scan_times != 1) {
00613         if (!strncmp(buffer, "00", 2)) {
00614             // "00" 応答の場合は、エコーバック応答とみなし、
00615             // 最後の空行を読み捨て、次からのデータを返す
00616             n = connection_readline(&urg->connection,
00617                                     buffer, BUFFER_SIZE, urg->timeout);
00618 
00619             if (n != 0) {
00620                 ignore_receive_data_with_qt(urg, urg->timeout);
00621                 return set_errno_and_return(urg, URG_INVALID_RESPONSE);
00622             } else {
00623                 return receive_data(urg, data, intensity, time_stamp, system_time_stamp);
00624             }
00625         }
00626     }
00627 
00628     if (((urg->specified_scan_times == 1) && (strncmp(buffer, "00", 2))) ||
00629         ((urg->specified_scan_times != 1) && (strncmp(buffer, "99", 2)))) {
00630         if (urg->error_handler) {
00631             type = urg->error_handler(buffer, urg);
00632         }
00633 
00634         if (type == URG_UNKNOWN) {
00635             // Gx, Hx のときは 00P が返されたときがデータ
00636             // Mx, Nx のときは 99b が返されたときがデータ
00637             ignore_receive_data_with_qt(urg, urg->timeout);
00638             return set_errno_and_return(urg, URG_INVALID_RESPONSE);
00639         }
00640     }
00641 
00642     // タイムスタンプの取得
00643     n = connection_readline(&urg->connection,
00644                             buffer, BUFFER_SIZE, urg->timeout);
00645     if (n > 0) {
00646         if (time_stamp) {
00647             *time_stamp = urg_scip_decode(buffer, 4);
00648         }
00649         if (system_time_stamp) {
00650             urg_get_walltime(system_time_stamp);
00651         }
00652     }
00653 
00654     // データの取得
00655     switch (type) {
00656     case URG_DISTANCE:
00657     case URG_MULTIECHO:
00658         ret = receive_length_data(urg, data, NULL, type, buffer);
00659         break;
00660 
00661     case URG_DISTANCE_INTENSITY:
00662     case URG_MULTIECHO_INTENSITY:
00663         ret = receive_length_data(urg, data, intensity, type, buffer);
00664         break;
00665 
00666     case URG_STOP:
00667     case URG_UNKNOWN:
00668         ret = 0;
00669         break;
00670     }
00671 
00672     // specified_scan_times == 1 のときは Gx 系コマンドが使われるため
00673     // データを明示的に停止しなくてよい
00674     if ((urg->specified_scan_times > 1) && (urg->scanning_remain_times > 0)) {
00675         if (--urg->scanning_remain_times <= 0) {
00676             // データの停止のみを行う
00677             urg_stop_measurement(urg);
00678         }
00679     }
00680     return ret;
00681 }
00682 
00683 
00684 int urg_open(urg_t *urg, urg_connection_type_t connection_type,
00685              const char *device_or_address, long baudrate_or_port)
00686 {
00687     int ret;
00688     long baudrate = baudrate_or_port;
00689 
00690     urg->is_active = URG_FALSE;
00691     urg->is_sending = URG_TRUE;
00692     urg->last_errno = URG_NOT_CONNECTED;
00693     urg->timeout = MAX_TIMEOUT;
00694     urg->scanning_skip_scan = 0;
00695     urg->error_handler = NULL;
00696 
00697     // デバイスへの接続
00698     ret = connection_open(&urg->connection, connection_type,
00699                           device_or_address, baudrate_or_port);
00700 
00701     if (ret < 0) {
00702         switch (connection_type) {
00703         case URG_SERIAL:
00704             urg->last_errno = URG_SERIAL_OPEN_ERROR;
00705             break;
00706 
00707         case URG_ETHERNET:
00708             urg->last_errno = URG_ETHERNET_OPEN_ERROR;
00709             break;
00710 
00711         default:
00712             urg->last_errno = URG_INVALID_RESPONSE;
00713             break;
00714         }
00715         return urg->last_errno;
00716     }
00717 
00718     // 指定したボーレートで URG と通信できるように調整
00719     if (connection_type == URG_ETHERNET) {
00720         // Ethernet のときは仮の通信速度を指定しておく
00721         baudrate = 115200;
00722     }
00723 
00724     if (connect_urg_device(urg, baudrate) != URG_NO_ERROR) {
00725         return set_errno_and_return(urg, ret);
00726     }
00727     urg->is_sending = URG_FALSE;
00728 
00729     // 変数の初期化
00730     urg->last_errno = URG_NO_ERROR;
00731     urg->range_data_byte = URG_COMMUNICATION_3_BYTE;
00732     urg->specified_scan_times = 0;
00733     urg->scanning_remain_times = 0;
00734     urg->is_laser_on = URG_FALSE;
00735 
00736     // パラメータ情報を取得
00737     ret = receive_parameter(urg);
00738     if (ret == URG_NO_ERROR) {
00739         urg->is_active = URG_TRUE;
00740     }
00741     return ret;
00742 }
00743 
00744 
00745 void urg_close(urg_t *urg)
00746 {
00747     if (urg->is_active) {
00748         ignore_receive_data_with_qt(urg, urg->timeout);
00749     }
00750     connection_close(&urg->connection);
00751     urg->is_active = URG_FALSE;
00752 }
00753 
00754 
00755 void urg_set_timeout_msec(urg_t *urg, int msec)
00756 {
00757     urg->timeout = msec;
00758 }
00759 
00760 
00761 int urg_start_time_stamp_mode(urg_t *urg)
00762 {
00763     const int expected[] = { 0, EXPECTED_END };
00764     int n;
00765 
00766     if (!urg->is_active) {
00767         return set_errno_and_return(urg, URG_NOT_CONNECTED);
00768     }
00769 
00770     // TM0 を発行する
00771     n = scip_response(urg, "TM0\n", expected, urg->timeout, NULL, 0);
00772     if (n <= 0) {
00773         return set_errno_and_return(urg, URG_INVALID_RESPONSE);
00774     } else {
00775         return 0;
00776     }
00777 }
00778 
00779 
00780 long urg_time_stamp(urg_t *urg)
00781 {
00782     const int expected[] = { 0, EXPECTED_END };
00783     char buffer[BUFFER_SIZE];
00784     char *p;
00785     int ret;
00786 
00787     if (!urg->is_active) {
00788         return set_errno_and_return(urg, URG_NOT_CONNECTED);
00789     }
00790 
00791     ret = scip_response(urg, "TM1\n", expected,
00792                         urg->timeout, buffer, BUFFER_SIZE);
00793     if (ret < 0) {
00794         return ret;
00795     }
00796 
00797     // buffer からタイムスタンプを取得し、デコードして返す
00798     if (strcmp(buffer, "00P")) {
00799         // 最初の応答が "00P" でなければ戻る
00800         return set_errno_and_return(urg, URG_RECEIVE_ERROR);
00801     }
00802     p = buffer + 4;
00803     if (strlen(p) != 5) {
00804         return set_errno_and_return(urg, URG_RECEIVE_ERROR);
00805     }
00806     if (p[5] == scip_checksum(p, 4)) {
00807         return set_errno_and_return(urg, URG_CHECKSUM_ERROR);
00808     }
00809     return urg_scip_decode(p, 4);
00810 }
00811 
00812 
00813 int urg_stop_time_stamp_mode(urg_t *urg)
00814 {
00815     int expected[] = { 0, EXPECTED_END };
00816     int n;
00817 
00818     if (!urg->is_active) {
00819         return set_errno_and_return(urg, URG_NOT_CONNECTED);
00820     }
00821 
00822     // TM2 を発行する
00823     n = scip_response(urg, "TM2\n", expected, urg->timeout, NULL, 0);
00824     if (n <= 0) {
00825         return set_errno_and_return(urg, URG_INVALID_RESPONSE);
00826     } else {
00827         return 0;
00828     }
00829 }
00830 
00831 
00832 static int send_distance_command(urg_t *urg, int scan_times, int skip_scan,
00833                                  char single_scan_ch, char continuous_scan_ch,
00834                                  char scan_type_ch)
00835 {
00836     char buffer[BUFFER_SIZE];
00837     int write_size = 0;
00838     int front_index = urg->front_data_index;
00839     int n;
00840 
00841     urg->specified_scan_times = (scan_times < 0) ? 0 : scan_times;
00842     urg->scanning_remain_times = urg->specified_scan_times;
00843     urg->scanning_skip_scan = (skip_scan < 0) ? 0 : skip_scan;
00844     if (scan_times >= 100) {
00845         // 計測回数が 99 を越える場合は、無限回のスキャンを行う
00846         urg->specified_scan_times = 0;
00847     }
00848 
00849     if (urg->scanning_remain_times == 1) {
00850         // レーザ発光を指示
00851         urg_laser_on(urg);
00852 
00853         write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d\n",
00854                               single_scan_ch, scan_type_ch,
00855                               urg->scanning_first_step + front_index,
00856                               urg->scanning_last_step + front_index,
00857                               urg->scanning_skip_step);
00858     } else {
00859         write_size = snprintf(buffer, BUFFER_SIZE, "%c%c%04d%04d%02d%01d%02d\n",
00860                               continuous_scan_ch, scan_type_ch,
00861                               urg->scanning_first_step + front_index,
00862                               urg->scanning_last_step + front_index,
00863                               urg->scanning_skip_step,
00864                               skip_scan, urg->specified_scan_times);
00865         urg->is_sending = URG_TRUE;
00866     }
00867 
00868     n = connection_write(&urg->connection, buffer, write_size);
00869     if (n != write_size) {
00870         return set_errno_and_return(urg, URG_SEND_ERROR);
00871     }
00872 
00873     return 0;
00874 }
00875 
00876 
00877 int urg_start_measurement(urg_t *urg, urg_measurement_type_t type,
00878                           int scan_times, int skip_scan)
00879 {
00880     char range_byte_ch;
00881     int ret = 0;
00882 
00883     if (!urg->is_active) {
00884         return set_errno_and_return(urg, URG_NOT_CONNECTED);
00885     }
00886 
00887     if ((skip_scan < 0) || (skip_scan > 9)) {
00888         ignore_receive_data_with_qt(urg, urg->timeout);
00889         return set_errno_and_return(urg, URG_INVALID_PARAMETER);
00890     }
00891 
00892     // !!! Mx 系, Nx 系の計測中のときは、QT を発行してから
00893     // !!! 計測開始コマンドを送信するようにする
00894     // !!! ただし、MD 計測中に MD を発行するように、同じコマンドの場合は
00895     // !!! Mx 系, Nx 系の計測は上書きすることができるようにする
00896 
00897     // 指定されたタイプのパケットを生成し、送信する
00898     switch (type) {
00899     case URG_DISTANCE:
00900         range_byte_ch =
00901             (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ? 'S' : 'D';
00902         ret = send_distance_command(urg, scan_times, skip_scan,
00903                                     'G', 'M', range_byte_ch);
00904         break;
00905 
00906     case URG_DISTANCE_INTENSITY:
00907         ret = send_distance_command(urg, scan_times, skip_scan,
00908                                     'G', 'M', 'E');
00909         break;
00910 
00911     case URG_MULTIECHO:
00912         ret = send_distance_command(urg, scan_times, skip_scan,
00913                                     'H', 'N', 'D');
00914         break;
00915 
00916     case URG_MULTIECHO_INTENSITY:
00917         ret = send_distance_command(urg, scan_times, skip_scan,
00918                                     'H', 'N', 'E');
00919         break;
00920 
00921     case URG_STOP:
00922     case URG_UNKNOWN:
00923     default:
00924         ignore_receive_data_with_qt(urg, urg->timeout);
00925         urg->last_errno = URG_INVALID_PARAMETER;
00926         ret = urg->last_errno;
00927         break;
00928     }
00929 
00930     return ret;
00931 }
00932 
00933 
00934 int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp)
00935 {
00936     if (!urg->is_active) {
00937         return set_errno_and_return(urg, URG_NOT_CONNECTED);
00938     }
00939     return receive_data(urg, data, NULL, time_stamp, system_time_stamp);
00940 }
00941 
00942 
00943 int urg_get_distance_intensity(urg_t *urg,
00944                                long data[], unsigned short intensity[],
00945                                long *time_stamp, unsigned long long *system_time_stamp)
00946 {
00947     if (!urg->is_active) {
00948         return set_errno_and_return(urg, URG_NOT_CONNECTED);
00949     }
00950 
00951     return receive_data(urg, data, intensity, time_stamp, system_time_stamp);
00952 }
00953 
00954 
00955 int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp)
00956 {
00957     if (!urg->is_active) {
00958         return set_errno_and_return(urg, URG_NOT_CONNECTED);
00959     }
00960 
00961     return receive_data(urg, data_multi, NULL, time_stamp, system_time_stamp);
00962 }
00963 
00964 
00965 int urg_get_multiecho_intensity(urg_t *urg,
00966                                 long data_multi[],
00967                                 unsigned short intensity_multi[],
00968                                 long *time_stamp, unsigned long long *system_time_stamp)
00969 {
00970     if (!urg->is_active) {
00971         return set_errno_and_return(urg, URG_NOT_CONNECTED);
00972     }
00973 
00974     return receive_data(urg, data_multi, intensity_multi, time_stamp, system_time_stamp);
00975 }
00976 
00977 
00978 int urg_stop_measurement(urg_t *urg)
00979 {
00980     enum { MAX_READ_TIMES = 3 };
00981     int ret = URG_INVALID_RESPONSE;
00982     int n;
00983     int i;
00984 
00985     if (!urg->is_active) {
00986         return set_errno_and_return(urg, URG_NOT_CONNECTED);
00987     }
00988 
00989     // QT を発行する
00990     n = connection_write(&urg->connection, "QT\n", 3);
00991     if (n != 3) {
00992         return set_errno_and_return(urg, URG_SEND_ERROR);
00993     }
00994 
00995     for (i = 0; i < MAX_READ_TIMES; ++i) {
00996         // QT の応答が返されるまで、距離データを読み捨てる
00997         ret = receive_data(urg, NULL, NULL, NULL, NULL);
00998         if (ret == URG_NO_ERROR) {
00999             // 正常応答
01000             urg->is_laser_on = URG_FALSE;
01001             urg->is_sending = URG_FALSE;
01002             return set_errno_and_return(urg, URG_NO_ERROR);
01003         }
01004     }
01005     return ret;
01006 }
01007 
01008 
01009 int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step,
01010                                int skip_step)
01011 {
01012     // 設定の範囲外を指定したときは、エラーを返す
01013     if (((skip_step < 0) || (skip_step >= 100)) ||
01014         (first_step > last_step) ||
01015         (first_step < -urg->front_data_index) ||
01016         (last_step > (urg->last_data_index - urg->front_data_index))) {
01017         return set_errno_and_return(urg, URG_SCANNING_PARAMETER_ERROR);
01018     }
01019 
01020     urg->scanning_first_step = first_step;
01021     urg->scanning_last_step = last_step;
01022     urg->scanning_skip_step = skip_step;
01023 
01024     return set_errno_and_return(urg, URG_NO_ERROR);
01025 }
01026 
01027 
01028 int urg_set_connection_data_size(urg_t *urg,
01029                                  urg_range_data_byte_t data_byte)
01030 {
01031     if (!urg->is_active) {
01032         return set_errno_and_return(urg, URG_NOT_CONNECTED);
01033     }
01034 
01035     if ((data_byte != URG_COMMUNICATION_3_BYTE) ||
01036         (data_byte != URG_COMMUNICATION_2_BYTE)) {
01037         return set_errno_and_return(urg, URG_DATA_SIZE_PARAMETER_ERROR);
01038     }
01039 
01040     urg->range_data_byte = data_byte;
01041 
01042     return set_errno_and_return(urg, URG_NO_ERROR);
01043 }
01044 
01045 
01046 int urg_laser_on(urg_t *urg)
01047 {
01048     int expected[] = { 0, 2, EXPECTED_END };
01049     int ret;
01050 
01051     if (!urg->is_active) {
01052         return set_errno_and_return(urg, URG_NOT_CONNECTED);
01053     }
01054 
01055     if (urg->is_laser_on != URG_FALSE) {
01056         // 既にレーザが発光しているときは、コマンドを送信しないようにする
01057         urg->last_errno = 0;
01058         return urg->last_errno;
01059     }
01060 
01061     ret = scip_response(urg, "BM\n", expected, urg->timeout, NULL, 0);
01062     if (ret >= 0) {
01063         urg->is_laser_on = URG_TRUE;
01064         ret = 0;
01065     }
01066     return ret;
01067 }
01068 
01069 
01070 int urg_laser_off(urg_t *urg)
01071 {
01072     return urg_stop_measurement(urg);
01073 }
01074 
01075 
01076 int urg_reboot(urg_t *urg)
01077 {
01078     int expected[] = { 0, 1, EXPECTED_END };
01079     int ret;
01080     int i;
01081 
01082     if (!urg->is_active) {
01083         return set_errno_and_return(urg, URG_NOT_CONNECTED);
01084     }
01085 
01086     // 2回目の RB 送信後、接続を切断する
01087     for (i = 0; i < 2; ++i) {
01088         ret = scip_response(urg, "RB\n", expected, urg->timeout, NULL, 0);
01089         if (ret < 0) {
01090             return set_errno_and_return(urg, URG_INVALID_RESPONSE);
01091         }
01092     }
01093     urg->is_active = URG_FALSE;
01094     urg_close(urg);
01095 
01096     urg->last_errno = 0;
01097     return urg->last_errno;
01098 }
01099 
01100 
01101 void urg_sleep(urg_t *urg)
01102 {
01103     enum { RECEIVE_BUFFER_SIZE = 4 };
01104     int sl_expected[] = { 0, EXPECTED_END };
01105     char receive_buffer[RECEIVE_BUFFER_SIZE];
01106 
01107     if (urg_stop_measurement(urg) != URG_NO_ERROR) {
01108         return;
01109     }
01110 
01111     scip_response(urg, "%SL\n", sl_expected, MAX_TIMEOUT,
01112                   receive_buffer, RECEIVE_BUFFER_SIZE);
01113 }
01114 
01115 
01116 void urg_wakeup(urg_t *urg)
01117 {
01118     urg_stop_measurement(urg);
01119 }
01120 
01121 
01122 int urg_is_stable(urg_t *urg)
01123 {
01124     const char *stat = urg_sensor_status(urg);
01125     return strncmp("Stable", stat, 6) ? 0 : 1;
01126 }
01127 
01128 
01129 static char *copy_token(char *dest, char *receive_buffer,
01130                         const char *start_str, const char *end_ch, int lines)
01131 {
01132     size_t start_str_len = strlen(start_str);
01133     size_t end_ch_len = strlen(end_ch);
01134     int i;
01135     size_t j;
01136 
01137     for (j = 0; j < end_ch_len; ++j) {
01138         const char *p = receive_buffer;
01139 
01140         for (i = 0; i < lines; ++i) {
01141             if (!strncmp(p, start_str, start_str_len)) {
01142 
01143                 char *last_p = strchr(p + start_str_len, end_ch[j]);
01144                 if (last_p) {
01145                     *last_p = '\0';
01146                     memcpy(dest, p + start_str_len,
01147                            last_p - (p + start_str_len) + 1);
01148                     return dest;
01149                 }
01150             }
01151             p += strlen(p) + 1;
01152         }
01153     }
01154     return NULL;
01155 }
01156 
01157 
01158 static const char *receive_command_response(urg_t *urg,
01159                                             char *buffer, int buffer_size,
01160                                             const char* command,
01161                                             int response_lines)
01162 {
01163     const int vv_expected[] = { 0, EXPECTED_END };
01164     int ret;
01165 
01166     if (!urg->is_active) {
01167         return NOT_CONNECTED_MESSAGE;
01168     }
01169 
01170     ret = scip_response(urg, command, vv_expected, urg->timeout,
01171                         buffer, buffer_size);
01172     if (ret < response_lines) {
01173         return RECEIVE_ERROR_MESSAGE;
01174     }
01175 
01176     return NULL;
01177 }
01178 
01179 
01180 const char *urg_sensor_product_type(urg_t *urg)
01181 {
01182     enum {
01183         RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES,
01184     };
01185     char receive_buffer[RECEIVE_BUFFER_SIZE];
01186     const char *ret;
01187     char *p;
01188 
01189     ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
01190                                    "VV\n", VV_RESPONSE_LINES);
01191     if (ret) {
01192         return ret;
01193     }
01194 
01195     p = copy_token(urg->return_buffer,
01196                    receive_buffer, "PROD:", ";", VV_RESPONSE_LINES);
01197     return (p) ? p : RECEIVE_ERROR_MESSAGE;
01198 }
01199 
01200 
01201 const char *urg_sensor_serial_id(urg_t *urg)
01202 {
01203     enum {
01204         RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES,
01205     };
01206     char receive_buffer[RECEIVE_BUFFER_SIZE];
01207     const char *ret;
01208     char *p;
01209 
01210     ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
01211                                    "VV\n", VV_RESPONSE_LINES);
01212     if (ret) {
01213         return ret;
01214     }
01215 
01216     p = copy_token(urg->return_buffer,
01217                    receive_buffer, "SERI:", ";", VV_RESPONSE_LINES);
01218     return (p) ? p : RECEIVE_ERROR_MESSAGE;
01219 }
01220 
01221 const char *urg_sensor_vendor(urg_t *urg){
01222     enum {
01223         RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES,
01224     };
01225     char receive_buffer[RECEIVE_BUFFER_SIZE];
01226     const char *ret;
01227     char *p;
01228 
01229     if (!urg->is_active) {
01230         return NOT_CONNECTED_MESSAGE;
01231     }
01232 
01233     ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
01234                                    "VV\n", VV_RESPONSE_LINES);
01235     if (ret) {
01236         return ret;
01237     }
01238 
01239     p = copy_token(urg->return_buffer,
01240                    receive_buffer, "VEND:", ";", VV_RESPONSE_LINES);
01241     return (p) ? p : RECEIVE_ERROR_MESSAGE;
01242 }
01243 
01244 const char *urg_sensor_firmware_version(urg_t *urg)
01245 {
01246     enum {
01247         RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES,
01248     };
01249     char receive_buffer[RECEIVE_BUFFER_SIZE];
01250     const char *ret;
01251     char *p;
01252 
01253     if (!urg->is_active) {
01254         return NOT_CONNECTED_MESSAGE;
01255     }
01256 
01257     ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
01258                                    "VV\n", VV_RESPONSE_LINES);
01259     if (ret) {
01260         return ret;
01261     }
01262 
01263     p = copy_token(urg->return_buffer,
01264                    receive_buffer, "FIRM:", "(", VV_RESPONSE_LINES);
01265     return (p) ? p : RECEIVE_ERROR_MESSAGE;
01266 }
01267 
01268 const char *urg_sensor_firmware_date(urg_t *urg)
01269 {
01270     enum {
01271         RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES,
01272     };
01273     char receive_buffer[RECEIVE_BUFFER_SIZE];
01274     const char *ret;
01275     char *p;
01276 
01277     if (!urg->is_active) {
01278         return NOT_CONNECTED_MESSAGE;
01279     }
01280 
01281     // Get the firmware version and append a '(', this will be what's before the date
01282     char firmware_version[50];
01283     strcpy(firmware_version, urg_sensor_firmware_version(urg));
01284     strcat(firmware_version, "(");
01285 
01286     ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
01287                                    "VV\n", VV_RESPONSE_LINES);
01288     if (ret) {
01289         return ret;
01290     }
01291 
01292     // Strip out the actual date from between the '(' and ')'
01293     char *date;
01294     p = copy_token(urg->return_buffer,
01295                receive_buffer, "FIRM:", ";", VV_RESPONSE_LINES);
01296     date = copy_token(urg->return_buffer, p, firmware_version, ")", 1);
01297     return (date) ? date : RECEIVE_ERROR_MESSAGE;
01298 }
01299 
01300 const char *urg_sensor_protocol_version(urg_t *urg)
01301 {
01302     enum {
01303         RECEIVE_BUFFER_SIZE = BUFFER_SIZE * VV_RESPONSE_LINES,
01304     };
01305     char receive_buffer[RECEIVE_BUFFER_SIZE];
01306     const char *ret;
01307     char *p;
01308 
01309     if (!urg->is_active) {
01310         return NOT_CONNECTED_MESSAGE;
01311     }
01312 
01313     ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
01314                                    "VV\n", VV_RESPONSE_LINES);
01315     if (ret) {
01316         return ret;
01317     }
01318 
01319     p = copy_token(urg->return_buffer,
01320                    receive_buffer, "PROT:", ";", VV_RESPONSE_LINES);
01321     return (p) ? p : RECEIVE_ERROR_MESSAGE;
01322 }
01323 
01324 
01325 const char *urg_sensor_status(urg_t *urg)
01326 {
01327     enum {
01328         RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES,
01329     };
01330     char receive_buffer[RECEIVE_BUFFER_SIZE];
01331     const char *ret;
01332     char *p;
01333 
01334     if (!urg->is_active) {
01335         return NOT_CONNECTED_MESSAGE;
01336     }
01337 
01338     ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
01339                                    "II\n", II_RESPONSE_LINES);
01340     if (ret) {
01341         return ret;
01342     }
01343 
01344     p = copy_token(urg->return_buffer,
01345                    receive_buffer, "STAT:", ";", II_RESPONSE_LINES);
01346     return (p) ? p : RECEIVE_ERROR_MESSAGE;
01347 }
01348 
01349 
01350 const char *urg_sensor_state(urg_t *urg)
01351 {
01352     enum {
01353         RECEIVE_BUFFER_SIZE = BUFFER_SIZE * II_RESPONSE_LINES,
01354     };
01355     char receive_buffer[RECEIVE_BUFFER_SIZE];
01356     const char *ret;
01357     char *p;
01358 
01359     if (!urg->is_active) {
01360         return NOT_CONNECTED_MESSAGE;
01361     }
01362 
01363     ret = receive_command_response(urg, receive_buffer, RECEIVE_BUFFER_SIZE,
01364                                    "II\n", II_RESPONSE_LINES);
01365     if (ret) {
01366         return ret;
01367     }
01368 
01369     p = copy_token(urg->return_buffer,
01370                    receive_buffer, "MESM:", " (", II_RESPONSE_LINES);
01371     return (p) ? p : RECEIVE_ERROR_MESSAGE;
01372 }
01373 
01374 
01375 void urg_set_error_handler(urg_t *urg, urg_error_handler handler)
01376 {
01377     urg->error_handler = handler;
01378 }


urg_c
Author(s): Satofumi Kamimura , Katsumi Kimoto, Adrian Boeing
autogenerated on Thu Jun 6 2019 19:06:57