00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00026 #include "kb_lrf.h"
00027 #include "korebot.h"
00028
00029
00030 enum {
00031 Timeout = 800,
00032 LineLength = 16 + 64 + 1 + 1 + 1,
00033 };
00034
00035
00036 #define GPIO_POWER 16 // power switch for the LRF module
00037
00038 #define BAUDRATE 19200 // not used for USB devices
00039
00040 static int HComm=-1;
00041
00042 static char* ErrorMessage = "no error.";
00043
00044
00045
00046
00047
00048
00049 typedef struct {
00050 enum parameters_t {
00051 MODL = 0,
00052 DMIN,
00053 DMAX,
00054 ARES,
00055 AMIN,
00056 AMAX,
00057 AFRT,
00058 SCAN,
00059 } parameter;
00060 char model[128];
00061 long distance_min;
00062 long distance_max;
00063 int area_total;
00064 int area_min;
00065 int area_max;
00066 int area_front;
00067 int scan_rpm;
00068
00069 int first;
00070 int last;
00071 int max_size;
00072 long last_timestamp;
00073 } urg_state_t;
00074
00075 urg_state_t urg_state;
00076
00077
00078 long kb_lrf_DistanceData[LRF_DATA_NB];
00079
00080 long kb_lrf_DistanceDataSensor[LRF_DATA_NB];
00081 long kb_lrf_DistanceDataSum[LRF_DATA_NB];
00082 int kb_lrf_DistanceGoodCounter[LRF_DATA_NB];
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095 static int com_connect(const char* device, long baudrate) {
00096
00097 struct termios options;
00098
00099 if ((HComm=open(device,O_RDWR | O_NOCTTY | O_NDELAY))<0) {
00100 printf("ERROR: can't open serial port %s\n",device);
00101 return -1;
00102 }
00103
00104 fcntl(HComm, F_SETFL, 0);
00105
00106
00107 tcgetattr(HComm, &options);
00108
00109
00110 options.c_cflag |= (CLOCAL | CREAD);
00111 options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
00112 options.c_oflag &= ~OPOST;
00113 options.c_cc[VMIN] = 0;
00114 options.c_cc[VTIME] = Timeout/100;
00115
00116
00117 tcsetattr(HComm, TCSANOW, &options);
00118
00119
00120
00121
00122
00123
00124 return 0;
00125 }
00126
00127
00128
00136 static int com_send(const char* data, int size) {
00137 int n;
00138
00139 tcflush (HComm, TCIFLUSH);
00140 n=write( HComm , data , size );
00141
00142 return n;
00143 }
00144
00145
00146
00155 static int com_recv(char* data, int max_size, int timeout) {
00156
00157 int filled = 0;
00158 int readable_size = 0;
00159 struct termios options;
00160
00161 fcntl(HComm, F_SETFL, FNDELAY);
00162
00163 memset (data, 0,max_size);
00164
00165 do {
00166
00167
00168
00169
00170 ioctl(HComm, FIONREAD, &readable_size);
00171
00172 int read_n = (readable_size > max_size) ? max_size : readable_size;
00173
00174 int n;
00175
00176 n=read( HComm , &data[filled] ,read_n );
00177 filled += n;
00178 readable_size -= n;
00179
00180 if (filled >= max_size) {
00181 return filled;
00182 }
00183 } while (readable_size > 0);
00184
00185
00186
00187
00188
00189
00190 if (timeout > 0) {
00191
00192
00193 tcgetattr(HComm, &options);
00194 options.c_cc[VTIME] = timeout/100;
00195
00196
00197 tcsetattr(HComm, TCSANOW, &options);
00198
00199
00200 fcntl(HComm, F_SETFL, 0);
00201
00202 int n;
00203 while (1) {
00204
00205 n=read(HComm, &data[filled], 1);
00206 if (n < 1) {
00207
00208 tcflush (HComm, TCIFLUSH);
00209
00210 return filled;
00211 }
00212 filled += n;
00213 if (filled >= max_size) {
00214 return filled;
00215 }
00216
00217 }
00218 }
00219 }
00220
00221
00222
00223
00224
00230 static int urg_sendTag(const char* tag) {
00231
00232 char send_message[LineLength];
00233 sprintf(send_message, "%s\n", tag);
00234 int send_size = strlen(send_message);
00235 com_send(send_message, send_size);
00236
00237 return send_size;
00238 }
00239
00240
00241
00242
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00282 static int urg_sendMessage(const char* command, int timeout, int* recv_n) {
00283
00284 int send_size = urg_sendTag(command);
00285 int recv_size = send_size + 2 + 1 + 2;
00286 char buffer[LineLength];
00287
00288 sleep(1);
00289 int n = com_recv(buffer, recv_size, timeout);
00290 *recv_n = n;
00291
00292 if (n < recv_size) {
00293 printf("Error:received size did not matched!\n");
00294 return -1;
00295 }
00296
00297 if (strncmp(buffer, command, send_size -1)) {
00298 printf("Error:command not matched!\n");
00299 return -2;
00300 }
00301
00302
00303
00304
00305 char reply_str[3] = "00";
00306 reply_str[0] = buffer[send_size];
00307 reply_str[1] = buffer[send_size + 1];
00308 return strtol(reply_str, NULL, 16);
00309 }
00310
00311
00312
00313
00320 static int urg_getParameters(urg_state_t* state) {
00321
00322
00323 char buffer[LineLength];
00324 int line_index = 0;
00325 enum {
00326 TagReply = 0,
00327 DataReply,
00328 Other,
00329 };
00330 int line_length;
00331
00332
00333 urg_sendTag("PP");
00334
00335 sleep(1);
00336 for (; (line_length = urg_readLine(buffer)) > 0; ++line_index) {
00337
00338 if (line_index == Other + MODL) {
00339 buffer[line_length - 2] = '\0';
00340
00341 strcpy(state->model,&buffer[5]);
00342
00343 printf("model: %s\n",state->model);
00344
00345 } else if (line_index == Other + DMIN) {
00346 state->distance_min = atoi(&buffer[5]);
00347
00348 } else if (line_index == Other + DMAX) {
00349 state->distance_max = atoi(&buffer[5]);
00350
00351 } else if (line_index == Other + ARES) {
00352 state->area_total = atoi(&buffer[5]);
00353
00354 } else if (line_index == Other + AMIN) {
00355 state->area_min = atoi(&buffer[5]);
00356 state->first = state->area_min;
00357
00358 } else if (line_index == Other + AMAX) {
00359 state->area_max = atoi(&buffer[5]);
00360 state->last = state->area_max;
00361
00362 } else if (line_index == Other + AFRT) {
00363 state->area_front = atoi(&buffer[5]);
00364
00365 } else if (line_index == Other + SCAN) {
00366 state->scan_rpm = atoi(&buffer[5]);
00367 printf("scan_rpm: %d\n",state->scan_rpm);
00368 }
00369 }
00370
00371 if (line_index <= Other + SCAN) {
00372 return -1;
00373 }
00374
00375 state->max_size = state->area_max +1;
00376
00377 return 0;
00378 }
00379
00380
00381
00382
00394 static int urg_connect(urg_state_t* state,
00395 const char* port, const long baudrate) {
00396
00397 if (com_connect(port, baudrate) < 0) {
00398 ErrorMessage = "Cannot connect COM device!";
00399 return -1;
00400 }
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410 int recv_n = 0;
00411 urg_sendMessage("SCIP2.0", Timeout, &recv_n);
00412 if (recv_n <= 0) {
00413 printf("If there is no reply it is considered as baud rate incompatibility, try with different baud rate!\n");
00414 close(HComm);
00415 ErrorMessage = "Cannot change to SCIP2.0 mode!";
00416 return -2;
00417
00418 }
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431 if (urg_getParameters(state) < 0) {
00432 ErrorMessage = "PP command fail.";
00433 close(HComm);
00434 return -3;
00435 }
00436
00437 state->last_timestamp = 0;
00438
00439 printf("URG is detected, port %s\n\n",port);
00440
00441
00442 return 0;
00443 }
00444
00445
00446
00453 static int urg_captureByGD(const urg_state_t* state) {
00454
00455 char send_message[LineLength];
00456 sprintf(send_message, "GD%04d%04d%02d", state->first, state->last, 0);
00457
00458 return urg_sendTag(send_message);
00459 }
00460
00461
00462
00471 static int urg_captureByMD(const urg_state_t* state, int capture_times) {
00472
00473 char send_message[LineLength];
00474 sprintf(send_message, "MD%04d%04d%02d%01d%02d",
00475 state->first, state->last, 0, 0, capture_times);
00476
00477 return urg_sendTag(send_message);
00478 }
00479
00480
00481
00489 static long urg_decode(const char* data, int data_byte) {
00490 long value = 0;
00491 int i;
00492
00493 for (i=0; i < data_byte; ++i) {
00494 value <<= 6;
00495 value &= ~0x3f;
00496 value |= data[i] - 0x30;
00497 }
00498 return value;
00499 }
00500
00501
00509 static int urg_addRecvData(const char buffer[], long data[], int* filled) {
00510
00511 static int remain_byte = 0;
00512 static char remain_data[3];
00513 const int data_byte = 3;
00514
00515 const char* pre_p = buffer;
00516 const char* p = pre_p;
00517
00518 if (remain_byte > 0) {
00519 memmove(&remain_data[remain_byte], buffer, data_byte - remain_byte);
00520 data[*filled] = urg_decode(remain_data, data_byte);
00521 ++(*filled);
00522 pre_p = &buffer[data_byte - remain_byte];
00523 p = pre_p;
00524 remain_byte = 0;
00525 }
00526
00527 do {
00528 ++p;
00529 if ((int)(p - pre_p) >= data_byte) {
00530 data[*filled] = urg_decode(pre_p, data_byte);
00531 ++(*filled);
00532 pre_p = p;
00533 }
00534 } while (*p != '\0');
00535 remain_byte = p - pre_p;
00536 memmove(remain_data, pre_p, remain_byte);
00537
00538 return 0;
00539 }
00540
00541
00550 static int urg_receiveData(urg_state_t* state, long data[], size_t max_size) {
00551
00552 int filled = 0;
00553 int i;
00554 int ret;
00555 size_t sti;
00556 char message_type = 'M';
00557 char buffer[LineLength];
00558 int line_length;
00559
00560
00561
00562
00563
00564
00565
00566 for (i = 0; (line_length = urg_readLine(buffer)) >= 0; ++i) {
00567
00568
00569 if ((i >= 6) && (line_length == 0)) {
00570
00571
00572 for (sti = filled; sti < max_size; ++sti) {
00573
00574 data[filled++] = 19;
00575 }
00576 return filled;
00577
00578 } else if (i == 0) {
00579
00580 if ((buffer[0] != 'M') && (buffer[0] != 'G')) {
00581 return -1;
00582 }
00583 message_type = buffer[0];
00584
00585 } else if (! strncmp(buffer, "99b", 3)) {
00586
00587 i = 4;
00588
00589 } else if ((i == 1) && (message_type == 'G')) {
00590 i = 4;
00591
00592 } else if (i == 4) {
00593
00594 if (strncmp(buffer, "99b", 3)) {
00595 return -2;
00596 }
00597
00598 } else if (i == 5) {
00599 state->last_timestamp = urg_decode(buffer, 4);
00600
00601 } else if (i >= 6) {
00602
00603 if (line_length > (64 + 1)) {
00604 line_length = (64 + 1);
00605 }
00606 buffer[line_length -1] = '\0';
00607 ret = urg_addRecvData(buffer, data, &filled);
00608 if (ret < 0) {
00609 return ret;
00610 }
00611 }
00612 }
00613 return -3;
00614 }
00615
00616
00617
00618
00619
00620
00621
00622
00628 long kb_lrf_Get_Timestamp(void)
00629 {
00630 return urg_state.last_timestamp;
00631 }
00632
00633
00634
00635
00639 void kb_lrf_Laser_On(int LRF_DeviceHandle)
00640 {
00641 int recv_n;
00642
00643 if (LRF_DeviceHandle>=0) {
00644 urg_sendMessage("BM", Timeout, &recv_n);
00645 }
00646 }
00647
00648
00652 void kb_lrf_Laser_Off(int LRF_DeviceHandle)
00653 {
00654 int recv_n;
00655
00656 if (LRF_DeviceHandle>=0) {
00657 urg_sendMessage("QT", Timeout, &recv_n);
00658 }
00659 }
00660
00661
00665 void kb_lrf_Power_On(void)
00666 {
00667 kb_gpio_init();
00668
00669
00670 kb_gpio_function(GPIO_POWER,0);
00671 kb_gpio_dir(GPIO_POWER,0);
00672 kb_gpio_set(GPIO_POWER);
00673 kb_gpio_cleanup();
00674
00675 usleep(1500000);
00676 }
00677
00678
00682 void kb_lrf_Power_Off(void)
00683 {
00684 kb_gpio_init();
00685
00686
00687 kb_gpio_function(GPIO_POWER,0);
00688 kb_gpio_dir(GPIO_POWER,0);
00689 kb_gpio_clear(GPIO_POWER);
00690 kb_gpio_cleanup();
00691
00692 }
00693
00694
00701 int kb_lrf_Init(char *LRF_DeviceName)
00702 {
00703 int LRF_DeviceHandle=-1;
00704 int recv_n = 0;
00705 int ret=0;
00706
00707
00708
00709 ret = urg_connect(&urg_state, LRF_DeviceName, BAUDRATE);
00710 if (ret < 0) {
00711
00712 printf("Error: %s\n", ErrorMessage);
00713 KB_ERROR("kb_lrf_Init",KB_ERROR_OPENLRF, LRF_DeviceName);
00714
00715
00716 return -1;
00717 }
00718
00719 LRF_DeviceHandle=HComm;
00720
00721
00722
00723 kb_lrf_Laser_On(LRF_DeviceHandle);
00724
00725
00726 return(LRF_DeviceHandle);
00727 }
00728
00729
00730
00737 void kb_lrf_Close(int LRF_DeviceHandle)
00738 {
00739 if (LRF_DeviceHandle>=0)
00740 {
00741 kb_lrf_Laser_Off(LRF_DeviceHandle);
00742 close(LRF_DeviceHandle);
00743 }
00744 }
00745
00746
00747
00748
00761 int kb_lrf_GetDistances(int LRF_DeviceHandle)
00762 {
00763 if (LRF_DeviceHandle<0)
00764 {
00765 return -1;
00766 }
00767
00768 if(urg_captureByMD(&urg_state, 1)<0)
00769 {
00770 return -2;
00771 }
00772
00773 if(urg_receiveData(&urg_state, kb_lrf_DistanceData, LRF_DATA_NB)<0)
00774 {
00775 return -3;
00776 }
00777
00778 return 0;
00779
00780
00781 }
00782
00783
00797 int kb_lrf_GetDistances_Averaged(int LRF_DeviceHandle, int average)
00798 {
00799 int i, j,ret;
00800
00801 memset (kb_lrf_DistanceDataSum, 0,LRF_DATA_NB*sizeof(long));
00802 memset (kb_lrf_DistanceGoodCounter, 0,LRF_DATA_NB*sizeof(int));
00803
00804
00805 for (i=0; i<average; i++)
00806 {
00807 if ((ret=kb_lrf_GetDistances(LRF_DeviceHandle))<0)
00808 return ret;
00809 for (j=0; j<LRF_DATA_NB; j++)
00810 {
00811
00812
00813 kb_lrf_DistanceDataSum[j] += kb_lrf_DistanceData[j];
00814 kb_lrf_DistanceGoodCounter[j]++;
00815
00816 }
00817 }
00818
00819
00820 for (j=0; j<LRF_DATA_NB; j++)
00821 {
00822 if (kb_lrf_DistanceGoodCounter[j] > 0)
00823 {
00824 kb_lrf_DistanceData[j] = kb_lrf_DistanceDataSum[j]/kb_lrf_DistanceGoodCounter[j];
00825 } else {
00826 kb_lrf_DistanceData[j] = 0;
00827 }
00828 }
00829
00830 return 0;
00831 }
00832