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


urg_c
Author(s): Satofumi Kamimura , Katsumi Kimoto, Adrian Boeing
autogenerated on Wed Aug 26 2015 16:38:27