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