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
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
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
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
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
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
00244 ignore_receive_data(urg, MAX_TIMEOUT);
00245
00246
00247 ret = scip_response(urg, "SCIP2.0\n", scip20_expected,
00248 MAX_TIMEOUT, NULL, 0);
00249
00250
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
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
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
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
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
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
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
00636
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
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
00719 if (connection_type == URG_ETHERNET) {
00720
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
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
00798 if (strcmp(buffer, "00P")) {
00799
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
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
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
00893
00894
00895
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
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
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
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
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
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 }