00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include "kb_lrf.h"
00011 #include "kb_gpio.h"
00012 #include "korebot.h"
00013
00029 #define DISTANCE_READINGS_TO_AVERAGE 3
00030
00031 #define MAX_TIMEOUT_IN_MS 500
00032 #define MAX_DATA_BYTES 246
00033
00034 #define device "/dev/tts/2"
00035
00036 #define PXA_INT0 8
00037 #define OUT 0
00038
00039
00040 unsigned short calcTable[] = {
00041 0x0000, 0x0002, 0x0003, 0x0004, 0x0006, 0x0007, 0x0008, 0x000A, 0x000B, 0x000C,
00042 0x000E, 0x000F, 0x0010, 0x0012, 0x0013, 0x0014, 0x0016, 0x0017, 0x0018, 0x001A,
00043 0x001B, 0x001C, 0x001E, 0x001F, 0x0020, 0x0022, 0x0023, 0x0024, 0x0026, 0x0027,
00044 0x0028, 0x002A, 0x002B, 0x002C, 0x002E, 0x002F, 0x0030, 0x0032, 0x0033, 0x0034,
00045 0x0036, 0x0037, 0x0038, 0x003A, 0x003B, 0x003C, 0x003E, 0x003F, 0x0040, 0x0042,
00046 0x0043, 0x0044, 0x0046, 0x0047, 0x0048, 0x004A, 0x004B, 0x004C, 0x004E, 0x004F,
00047 0x0050, 0x0052, 0x0053, 0x0054, 0x0056, 0x0057, 0x0058, 0x005A, 0x005B, 0x005C,
00048 0x005E, 0x005F, 0x0060, 0x0062, 0x0063, 0x0064, 0x0066, 0x0067, 0x0068, 0x006A,
00049 0x006B, 0x006C, 0x006E, 0x006F, 0x0070, 0x0072, 0x0073, 0x0074, 0x0076, 0x0077,
00050 0x0078, 0x007A, 0x007B, 0x007C, 0x007E, 0x007F, 0x0080, 0x0082, 0x0083, 0x0084,
00051 0x0086, 0x0087, 0x0088, 0x008A, 0x008B, 0x008C, 0x008E, 0x008F, 0x0090, 0x0092,
00052 0x0093, 0x0094, 0x0096, 0x0097, 0x0098, 0x009A, 0x009B, 0x009C, 0x009E, 0x009F,
00053 0x00A0, 0x00A2, 0x00A3, 0x00A4, 0x00A6, 0x00A7, 0x00A8, 0x00AA, 0x00AB, 0x00AC,
00054 0x00AE, 0x00AF, 0x00B0, 0x00B2, 0x00B3, 0x00B4, 0x00B6, 0x00B7, 0x00B8, 0x00BA,
00055 0x00BB, 0x00BC, 0x00BE, 0x00BF, 0x00C0, 0x00C2, 0x00C3, 0x00C4, 0x00C6, 0x00C7,
00056 0x00C8, 0x00CA, 0x00CB, 0x00CC, 0x00CE, 0x00CF, 0x00D0, 0x00D2, 0x00D3, 0x00D4,
00057 0x00D6, 0x00D7, 0x00D8, 0x00DA, 0x00DB, 0x00DC, 0x00DE, 0x00DF, 0x00E0, 0x00E2,
00058 0x00E3, 0x00E4, 0x00E6, 0x00E7, 0x00E8, 0x00EA, 0x00EB, 0x00EC, 0x00EE, 0x00EF,
00059 0x00F0, 0x00F2, 0x00F3, 0x00F4, 0x00F6, 0x00F7, 0x00F8, 0x00FA, 0x00FB, 0x00FC,
00060 0x00FE, 0x00FF, 0x0100, 0x0102, 0x0103, 0x0104, 0x0106, 0x0107, 0x0108, 0x010A,
00061 0x010B, 0x010C, 0x010E, 0x010F, 0x0110, 0x0112, 0x0113, 0x0114, 0x0116, 0x0117,
00062 0x0118, 0x011A, 0x011B, 0x011C, 0x011E, 0x011F, 0x0120, 0x0122, 0x0123, 0x0124,
00063 0x0126, 0x0127, 0x0128, 0x012A, 0x012B, 0x012C, 0x012E, 0x012F, 0x0130, 0x0132,
00064 0x0133, 0x0134, 0x0136, 0x0137, 0x0138, 0x013A, 0x013B, 0x013C, 0x013E, 0x013F,
00065 0x0140, 0x0142, 0x0143, 0x0144, 0x0146, 0x0147, 0x0148, 0x014A, 0x014B, 0x014C,
00066 0x014E, 0x014F, 0x0150, 0x0152, 0x0153 };
00067
00068
00069
00070
00071 #define STX 2
00072 #define ETX 3
00073
00074
00075 #define LRF_PORT "/dev/tts/2"
00076
00077
00078 unsigned char kb_lrf_Data[MAX_DATA_BYTES];
00079
00080 unsigned short kb_lrf_DistanceData[121];
00081
00082 unsigned short kb_lrf_DistanceDataSensor[121];
00083 unsigned long kb_lrf_DistanceDataSum[121];
00084 int kb_lrf_DistanceGoodCounter[121];
00085
00086 unsigned short CertifiedCode;
00087
00088 #ifdef kt_debug_lowlevel
00089
00090
00091
00092
00093 char *Bin2Asc(char *str, int bin, int sz)
00094 {
00095 int i, j;
00096
00097 for( j = (sz - 1); j > -1; j-- )
00098 {
00099 str[j] = (bin & 1) + 0x30;
00100 bin = bin >> 1;
00101 }
00102
00103 return str;
00104
00105 }
00106
00107 char *Array2Raw(char *str, int sz)
00108 {
00109 int i, j;
00110 char raw[sz+1];
00111
00112
00113 for( i = 0, j = (sz-1); i < sz; i++, j-- )
00114 {
00115 raw[i] = (str[j] == 0x30) ? 0x31 : 0x30;
00116 }
00117
00118 str[0] = 0x31;
00119
00120 for( i = 0; i < sz-1; i++)
00121 {
00122 str[i+1] = raw[i];
00123 }
00124
00125 return str;
00126 }
00127
00128 char *makeStream(char *outStream, char *inMsg, int inSz)
00129 {
00130 int i, j;
00131 char *buf;
00132
00133
00134 buf = (char *)malloc(190477);
00135 memset(buf, 0, 190477);
00136
00137 *outStream = 0;
00138
00139 for( i = 0; i < inSz; i++ )
00140 {
00141 Bin2Asc(buf, inMsg[i], 8);
00142 Array2Raw(buf, 8);
00143
00144 strcat(outStream, buf);
00145 memset(buf, 0, 190477);
00146 }
00147
00148 free(buf);
00149
00150 return outStream;
00151 }
00152
00153 #endif
00154
00155
00163 int kb_lrf_OpenComPort(void)
00164 {
00165 int LRF_DeviceHandle;
00166 struct termios tty;
00167
00168 if ((LRF_DeviceHandle = open(LRF_PORT, O_RDWR)) < 0)
00169 {
00170
00171 KB_ERROR("kb_lrf_OpenComPort",KB_ERROR_OPENLRF, LRF_PORT);
00172 return(-1);
00173 }
00174
00175
00176 tcgetattr(LRF_DeviceHandle, &tty);
00177 cfsetospeed(&tty, (speed_t) B57600);
00178 cfsetispeed(&tty, (speed_t) B57600);
00179 tty.c_cflag = (tty.c_cflag & ~(CSIZE)) | CS7 | CSTOPB | CLOCAL | CREAD ;
00180 tty.c_iflag = IGNBRK;
00181 tty.c_lflag = 0;
00182 tty.c_oflag = 0;
00183 tty.c_cc[VMIN] = 1;
00184 tty.c_cc[VTIME] = 5;
00185 tcsetattr(LRF_DeviceHandle, TCSANOW, &tty);
00186
00187 return(LRF_DeviceHandle);
00188 }
00189
00190
00191
00198 void kb_lrf_FlushSerPort(int LRF_DeviceHandle)
00199 {
00200 fd_set fd_set1;
00201 struct timeval tv;
00202 char InputDummy;
00203
00204 tv.tv_sec = 0;
00205 tv.tv_usec = 0;
00206 FD_ZERO(&fd_set1);
00207 FD_SET(LRF_DeviceHandle, &fd_set1);
00208
00209 while (select(FD_SETSIZE, &fd_set1, NULL, NULL, &tv) > 0)
00210 {
00211 (void) read(LRF_DeviceHandle, &InputDummy, 1);
00212 FD_ZERO(&fd_set1);
00213 FD_SET(LRF_DeviceHandle, &fd_set1);
00214 }
00215 }
00216
00217
00224 #define HKO_POLY 0x1021
00225 unsigned char kb_lrf_lsb_to_msb(unsigned char lsb)
00226 {
00227 unsigned char msb=0;
00228 unsigned char i, tmp;
00229
00230 for(i=0; i<8; i++)
00231 {
00232 if (i<4)
00233 tmp = (lsb & (1<<i)) << (((3-i)*2)+1);
00234 else
00235 tmp = (lsb & (1<<i)) >> (((i-4)*2)+1);
00236
00237 msb |= tmp;
00238 }
00239 return msb;
00240 }
00241
00242
00250 unsigned short kb_lrf_GenerateCRC(unsigned char *buf, int count)
00251 {
00252 unsigned short ch, i, crc, j, v, xor_flag;
00253 unsigned short crc_msb;
00254
00255 crc = 0x0000;
00256 for(i=0; i< count; i++)
00257 {
00258 ch = buf[i];
00259
00260 v = 0x01;
00261
00262 for (j=0; j<8; j++)
00263 {
00264 if (crc & 0x8000)
00265 {
00266 xor_flag= 1;
00267 }
00268 else
00269 {
00270 xor_flag= 0;
00271 }
00272 crc = crc << 1;
00273
00274 if (ch & v)
00275 {
00276 crc= crc + 1;
00277 }
00278
00279 if (xor_flag)
00280 {
00281 crc = crc ^ HKO_POLY;
00282 }
00283
00284 v = v << 1;
00285 }
00286 }
00287
00288 for (i=0; i<16; i++)
00289 {
00290 if (crc & 0x8000)
00291 {
00292 xor_flag= 1;
00293 }
00294 else
00295 {
00296 xor_flag= 0;
00297 }
00298 crc = crc << 1;
00299
00300 if (xor_flag)
00301 {
00302 crc = crc ^ HKO_POLY;
00303 }
00304 }
00305
00306 crc_msb = kb_lrf_lsb_to_msb((unsigned char) (crc >> 8));
00307 crc_msb = crc_msb << 8;
00308 crc_msb = crc_msb | kb_lrf_lsb_to_msb((unsigned char) (crc & 0x00FF));
00309
00310 return crc_msb;
00311 }
00312
00313
00329 int kb_lrf_GetDataFromLRF(int LRF_DeviceHandle, int TotalBytes)
00330 {
00331
00332 fd_set fd_set0;
00333 struct timeval tv;
00334
00335 int i, j;
00336 int d0, d1, d2, d3;
00337
00338 int BytesRead;
00339 int TotalBytesRaw;
00340 int TotalBytesRead;
00341 int TotalBytesMissing;
00342
00343 char DummyInputChar;
00344 char DataRaw[2*MAX_DATA_BYTES];
00345
00346 unsigned short CalculatedCRC;
00347 unsigned short ReceivedCRC;
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358 TotalBytesRaw = calcTable[TotalBytes];
00359
00360
00361 TotalBytesRaw += 2;
00362
00363
00364
00365
00366
00367 TotalBytesRead = 0;
00368 while(TotalBytesRead < TotalBytesRaw)
00369 {
00370 tv.tv_sec = 0;
00371 tv.tv_usec = MAX_TIMEOUT_IN_MS*1000;
00372 FD_ZERO(&fd_set0);
00373 FD_SET(LRF_DeviceHandle, &fd_set0);
00374
00375 if(select(FD_SETSIZE, &fd_set0, NULL, NULL, &tv) <= 0)
00376 {
00377
00378 return(-3);
00379 }
00380
00381 TotalBytesMissing = TotalBytesRaw - TotalBytesRead;
00382 BytesRead = read(LRF_DeviceHandle, DataRaw+TotalBytesRead, TotalBytesMissing);
00383 TotalBytesRead += BytesRead;
00384 }
00385
00386
00387 if(TotalBytesRead != TotalBytesRaw)
00388 {
00389
00390 return(-4);
00391 }
00392
00393
00394 if(DataRaw[0] != STX)
00395 {
00396 KB_ERROR("GetDataFromLRF",KB_ERROR_LRFNOSTX);
00397 return(-5);
00398 }
00399
00400
00401 if(DataRaw[TotalBytesRead-1] != ETX)
00402 {
00403 KB_ERROR("kb_lrf_GetDataFromLRF",KB_ERROR_LRFNOETX);
00404 return(-6);
00405 }
00406
00407
00408
00409
00410
00411
00412
00413 i=0;
00414 j=1;
00415
00416 while (i<TotalBytes+1)
00417
00418
00419 {
00420
00421
00422 d0 = DataRaw[j ]-0x20;
00423 d1 = DataRaw[j+1]-0x20;
00424 d2 = DataRaw[j+2]-0x20;
00425 d3 = DataRaw[j+3]-0x20;
00426
00427
00428 kb_lrf_Data[i ] = ((d0 & 0x3F)<<2) | ((d1 & 0x30)>>4);
00429 kb_lrf_Data[i+1] = ((d1 & 0x0F)<<4) | ((d2 & 0x3C)>>2);
00430 kb_lrf_Data[i+2] = ((d2 & 0x03)<<6) | ((d3 & 0x3F) );
00431
00432
00433
00434 i += 3;
00435 j += 4;
00436 }
00437
00438
00439
00440
00441 CalculatedCRC = kb_lrf_GenerateCRC(kb_lrf_Data, TotalBytes-2);
00442
00443
00444 ReceivedCRC = ((unsigned short) (kb_lrf_Data[TotalBytes-2]<<8) | (kb_lrf_Data[TotalBytes-1]));
00445
00446
00447 if (CalculatedCRC != ReceivedCRC)
00448 {
00449 KB_DEBUG( "kb_lrf_GetDataFromLRF", 0, "CRC_CALCULATED = 0x%4.4X, CRC_RECEIVED = 0x%4.4X",
00450 CalculatedCRC, ReceivedCRC );
00451 return(-7);
00452 }
00453
00454
00455 return(1);
00456 }
00457
00458
00459
00460
00471 int kb_lrf_SendCommand(int LRF_DeviceHandle, char *LRF_Command, int LRF_CommandLength)
00472 {
00473 fd_set fd_set1;
00474 struct timeval tv;
00475
00476 int i, j;
00477 int d0, d1, d2;
00478 int BytesToSend, RawBytesToSend, BytesWritten, ceilInt;
00479
00480 char Data[MAX_DATA_BYTES];
00481 char DataRaw[2*MAX_DATA_BYTES];
00482
00483 unsigned short CalculatedCRC;
00484 float nbrBytes, ceilVal;
00485
00486 BytesToSend = LRF_CommandLength;
00487
00488 if (BytesToSend+2 > MAX_DATA_BYTES)
00489 {
00490 KB_ERROR("kb_lrf_SendCommand",KB_ERROR_LRFDATA2BIG, BytesToSend+2);
00491 return(-1);
00492 }
00493
00494
00495
00496 kb_lrf_FlushSerPort(LRF_DeviceHandle);
00497
00498
00499 for (i=0; i<BytesToSend; i++)
00500 {
00501 Data[i] = LRF_Command[i];
00502 }
00503 CalculatedCRC = kb_lrf_GenerateCRC(LRF_Command, BytesToSend);
00504 Data[i] = (CalculatedCRC & 0xFF00)>>8;
00505 Data[i+1] = (CalculatedCRC & 0x00FF);
00506 BytesToSend += 2;
00507
00508
00509 i=0;
00510 j=0;
00511
00512 while (i<BytesToSend)
00513 {
00514 d0 = Data[i ];
00515 d1 = Data[i+1];
00516 d2 = Data[i+2];
00517
00518 DataRaw[1+j ] = 0x20 + ((d0 & 0xFC) >> 2) ;
00519 DataRaw[1+j+1] = 0x20 + (((d0 & 0x03) << 4) | ((d1 & 0xF0) >> 4));
00520 DataRaw[1+j+2] = 0x20 + (((d1 & 0x0F) << 2) | ((d2 & 0xC0) >> 6));
00521 DataRaw[1+j+3] = 0x20 + ((d2 & 0x3F) ) ;
00522
00523 i += 3;
00524 j += 4;
00525 }
00526
00527 nbrBytes = (float) BytesToSend;
00528 nbrBytes /= 3.0;
00529 nbrBytes *= 4.0;
00530
00531 KB_DEBUG( "kb_lrf_SendCommand", 0, "nbrBytes=%f", nbrBytes );
00532
00533
00534
00535
00536
00537
00538
00539
00540 RawBytesToSend = calcTable[BytesToSend];
00541
00542
00543
00544 DataRaw[0]=0x02;
00545 DataRaw[RawBytesToSend+1]=0x03;
00546 RawBytesToSend += 2;
00547
00548
00549 tv.tv_sec = 0;
00550 tv.tv_usec = 1000*MAX_TIMEOUT_IN_MS;
00551 FD_ZERO(&fd_set1);
00552 FD_SET(LRF_DeviceHandle, &fd_set1);
00553 if (select(FD_SETSIZE, NULL, &fd_set1, NULL, &tv) <= 0)
00554 {
00555 KB_ERROR("kb_lrf_SendCommand",KB_ERROR_LRFNODATA);
00556 return(-2);
00557 }
00558
00559 BytesWritten = write(LRF_DeviceHandle, DataRaw, RawBytesToSend);
00560
00561 if (BytesWritten != RawBytesToSend)
00562 {
00563 KB_ERROR("kb_lrf_SendCommand", KB_ERROR_LRFSENDDATA);
00564 return(-3);
00565 }
00566
00567 KB_DEBUG( "kb_lrf_SendCommand", 0, "bytes sent=%d", RawBytesToSend );
00568
00569 return(0);
00570 }
00571
00572
00573
00582 int kb_lrf_AcquireCertifiedCode(int LRF_DeviceHandle)
00583 {
00584 char LRF_CMD[6];
00585
00586
00587 sprintf (LRF_CMD, "%c%c", 0xA0, 0x69);
00588 KB_DEBUG( "kb_lrf_AcquireCertifiedCode", 0, "command=%c%c", 0xA0, 0x69 );
00589
00590 kb_lrf_SendCommand(LRF_DeviceHandle, LRF_CMD, 2);
00591
00592 if (kb_lrf_GetDataFromLRF(LRF_DeviceHandle, 12) < 0)
00593 {
00594 KB_ERROR("kb_lrf_AcquireCertifiedCode", KB_ERROR_LRFNOCERT);
00595 return(-1);
00596 }
00597 if ((kb_lrf_Data[0] != 0xA0) || (kb_lrf_Data[1] != 0x69))
00598 {
00599 KB_ERROR("kb_lrf_AcquireCertifiedCode", KB_ERROR_LRFNODATA);
00600 return(-2);
00601 }
00602
00603 CertifiedCode = kb_lrf_GenerateCRC(kb_lrf_Data+2, 8);
00604
00605 return(0);
00606 }
00607
00608
00618 int kb_lrf_CertifyLink(int LRF_DeviceHandle)
00619 {
00620 char LRF_CMD[6];
00621
00622 sprintf (LRF_CMD, "%c%c%c%c%c", 0xA0, 0x5A, 0x01,
00623 ((CertifiedCode & 0xFF00)>>8), (CertifiedCode & 0x00FF));
00624 KB_DEBUG( "kb_lrf_CertifyLink", 0, "command=%c%c%c%c%c", 0xA0, 0x5A, 0x01,
00625 ((CertifiedCode & 0xFF00)>>8), (CertifiedCode & 0x00FF));
00626
00627 kb_lrf_SendCommand(LRF_DeviceHandle, LRF_CMD, 5);
00628
00629 if (kb_lrf_GetDataFromLRF(LRF_DeviceHandle, 5) < 0)
00630 {
00631 KB_ERROR("kb_lrf_CertifyLink", KB_ERROR_LRFNOCERT);
00632 return(-1);
00633 }
00634 if ((kb_lrf_Data[0] != 0xA0) || (kb_lrf_Data[1] != 0x5a) || (kb_lrf_Data[2] != 0x01))
00635 {
00636 KB_ERROR("kb_lrf_SendCommand",KB_ERROR_LRFNODATA);
00637 return(-2);
00638 }
00639
00640 return(0);
00641 }
00642
00643
00644
00648 void kb_lrf_pwrOn(void)
00649 {
00650 kb_gpio_set(PXA_INT0);
00651 }
00652
00653
00657 void kb_lrf_pwrOff(void)
00658 {
00659 kb_gpio_clear(PXA_INT0);
00660 }
00661
00662
00669 int kb_lrf_Init(char *LRF_DeviceName)
00670 {
00671 int LRF_DeviceHandle;
00672 int rc;
00673
00674
00675 LRF_DeviceHandle = kb_lrf_OpenComPort();
00676 if (LRF_DeviceHandle < 0)
00677 {
00678 KB_ERROR("kb_lrf_OpenComPort",KB_ERROR_OPENLRF, LRF_DeviceName);
00679 return(-1);
00680 }
00681
00682
00683 if((rc = kb_init( 0 , NULL )) < 0 )
00684 return 1;
00685
00686
00687 kb_gpio_init();
00688
00689
00690
00691 kb_gpio_function(PXA_INT0,0);
00692
00693
00694 kb_gpio_dir(PXA_INT0, OUT);
00695
00696
00697 kb_lrf_pwrOn();
00698
00699
00700 usleep(1500*1000);
00701
00702
00703 kb_lrf_AcquireCertifiedCode(LRF_DeviceHandle);
00704 kb_lrf_CertifyLink(LRF_DeviceHandle);
00705
00706
00707 sleep(0.5);
00708
00709 return(LRF_DeviceHandle);
00710 }
00711
00712
00713
00720 void kb_lrf_Close(int LRF_DeviceHandle)
00721 {
00722
00723 kb_lrf_pwrOff();
00724 close(LRF_DeviceHandle);
00725 }
00726
00727
00728
00738 int kb_lrf_FetchDistanceFromLRF(int LRF_DeviceHandle)
00739 {
00740 char LRF_CMD[2];
00741 int i;
00742
00743
00744 LRF_CMD[0]=0xA2;
00745 LRF_CMD[1]=0x69;
00746 if (kb_lrf_SendCommand(LRF_DeviceHandle, LRF_CMD, 2) < 0)
00747 {
00748 KB_ERROR("kb_lrf_FetchDistanceFromLRF",KB_ERROR_LRFCMDTX);
00749 return(-1);
00750 }
00751
00752
00753 if (kb_lrf_GetDataFromLRF(LRF_DeviceHandle, 246) < 0)
00754 {
00755 KB_ERROR("kb_lrf_FetchDistanceFromLRF",KB_ERROR_LRFDISTRX);
00756 return(-2);
00757 }
00758
00759
00760 if ((kb_lrf_Data[0] != 0xA2) || (kb_lrf_Data[1] != 0x69))
00761 {
00762 KB_ERROR("kb_lrf_FetchDistanceFromLRF",KB_ERROR_LRFDATARX);
00763 return(-3);
00764 }
00765
00766
00767 for (i=0; i<121; i++)
00768 {
00769 kb_lrf_DistanceDataSensor[i] = (kb_lrf_Data[(2*i)+3] << 8) +
00770 kb_lrf_Data[(2*i)+2];
00771 }
00772
00773 return(0);
00774 }
00775
00776
00777
00787 int kb_lrf_GetDistances(int LRF_DeviceHandle)
00788 {
00789 int i, j;
00790
00791
00792 if (kb_lrf_CertifyLink(LRF_DeviceHandle) < 0)
00793 {
00794 kb_lrf_AcquireCertifiedCode(LRF_DeviceHandle);
00795
00796 if (kb_lrf_CertifyLink(LRF_DeviceHandle) < 0)
00797 {
00798 KB_ERROR("kb_lrf_GetDistances", KB_ERROR_LRFNOCERT);
00799 return (-1);
00800 }
00801 }
00802
00803
00804 for (j=0; j<121; j++)
00805 {
00806 kb_lrf_DistanceDataSum[j] = 0;
00807 kb_lrf_DistanceGoodCounter[j] = 0;
00808 }
00809
00810
00811 for (i=0; i<DISTANCE_READINGS_TO_AVERAGE; i++)
00812 {
00813 (void) kb_lrf_FetchDistanceFromLRF(LRF_DeviceHandle);
00814 for (j=0; j<121; j++)
00815 {
00816 if (kb_lrf_DistanceDataSensor[j] < 0xF000)
00817 {
00818 kb_lrf_DistanceDataSum[j] += kb_lrf_DistanceDataSensor[j];
00819 kb_lrf_DistanceGoodCounter[j]++;
00820 }
00821 }
00822 }
00823
00824
00825 for (j=0; j<121; j++)
00826 {
00827 if (kb_lrf_DistanceGoodCounter[j] > 0)
00828 {
00829 kb_lrf_DistanceData[j] = kb_lrf_DistanceDataSum[j]/kb_lrf_DistanceGoodCounter[j];
00830 } else {
00831 kb_lrf_DistanceData[j] = 0;
00832 }
00833 }
00834
00835 return(0);
00836 }
00837