WURDE: /home/fwph/code/wurde/modules/sickLaser/sick.c Source File

00001 
00002 #include "serial.h"
00003 #include <termios.h>
00004 #include <stdio.h>
00005 #include <errno.h>
00006 #include <sys/time.h>
00007 #include <stdlib.h>
00008 #include <string.h>
00009 #include <unistd.h>
00010 #define TRUE 1
00011 #define FALSE 0
00012 #include <fcntl.h>
00013 #define carmen_test_alloc(X) do {if ((void*)(X) == NULL) {fprintf(stderr, "Out of memory in %s, (%s:%d).\n",__FUNCTION__, __FILE__, __LINE__);abort();}}while(0)
00014 double
00015 carmen_get_time_ms(void)
00016 {
00017   struct timeval tv;
00018   double t;
00019 
00020   if (gettimeofday(&tv, NULL) < 0)
00021     fprintf(stderr, "carmen_get_time_ms encountered error in gettimeofday : %s\n",
00022                 strerror(errno));
00023   t = tv.tv_sec + tv.tv_usec/1000000.0;
00024   return t;
00025 }
00026 //End naughtyness
00027 
00028 //#include "laser_ipc.h"
00029 #include "sick.h"
00030 
00031 /* fancy serial functions */
00032 
00033 int iParity(parity_t par)
00034 {
00035   if(par == N)
00036     return(IGNPAR);
00037   else
00038     return(INPCK);
00039 }
00040 
00041 int iSoftControl(int flowcontrol)
00042 {
00043   if(flowcontrol)
00044     return(IXON);
00045   else
00046     return(IXOFF);
00047 }
00048 
00049 int cDataSize(int numbits)
00050 {
00051   switch(numbits) {
00052   case 5:
00053     return(CS5);
00054     break;
00055   case 6:
00056     return(CS6);
00057     break;
00058   case 7:
00059     return(CS7);
00060     break;
00061   case 8:
00062     return(CS8);
00063     break;
00064   default:
00065     return(CS8);
00066     break;
00067   }
00068 }
00069 
00070 int cStopSize(int numbits)
00071 {
00072   if(numbits == 2)
00073     return(CSTOPB);
00074   else
00075     return(0);
00076 }
00077 
00078 int cFlowControl(int flowcontrol)
00079 {
00080   if(flowcontrol)
00081     return(CRTSCTS);
00082   else
00083     return(CLOCAL);
00084 }
00085 
00086 int cParity(parity_t par)
00087 {
00088   if(par != N) {
00089     if(par == O)
00090       return(PARENB | PARODD);
00091     else
00092       return(PARENB);
00093   }
00094   else
00095     return(0);
00096 }
00097 
00098 int cBaudrate(int baudrate)
00099 {
00100   switch(baudrate) {
00101   case 0:
00102     return(B0);
00103     break;
00104   case 300:
00105     return(B300);
00106     break;
00107   case 600:
00108     return(B600);
00109     break;
00110   case 1200:
00111     return(B1200);
00112     break;
00113   case 2400:
00114     return(B2400);
00115     break;
00116   case 4800:
00117     return(B4800);
00118     break;
00119   case 9600:
00120     return(B9600);
00121     break;
00122   case 19200:
00123     return(B19200);
00124     break;
00125   case 38400:
00126     return(B38400);
00127     break;
00128   case 57600:
00129     return(B57600);
00130     break;
00131   case 115200:
00132     return(B115200);
00133     break;
00134   case 500000:
00135     /* to use 500k you have to change the entry of B460800 in you kernel:
00136        /usr/src/linux/drivers/usb/serial/ftdi_sio.h:
00137        ftdi_8U232AM_48MHz_b460800 = 0x0006    */
00138     return(B460800);
00139     break;
00140   default:
00141     return(B9600);
00142     break;
00143   }
00144 }
00145 
00146 void sick_set_serial_params(sick_laser_p laser)
00147 {
00148   struct termios  ctio;
00149   
00150   tcgetattr(laser->dev.fd, &ctio); /* save current port settings */
00151   ctio.c_iflag = iSoftControl(laser->dev.swf) | iParity(laser->dev.parity);
00152   ctio.c_oflag = 0;
00153   ctio.c_cflag = CREAD | cFlowControl(laser->dev.hwf || laser->dev.swf) |
00154     cParity(laser->dev.parity) | cDataSize(laser->dev.databits) | 
00155     cStopSize(laser->dev.stopbits);
00156   ctio.c_lflag = 0;
00157   ctio.c_cc[VTIME] = 0;     /* inter-character timer unused */
00158   ctio.c_cc[VMIN] = 0;      /* blocking read until 0 chars received */
00159   cfsetispeed(&ctio, (speed_t)cBaudrate(laser->dev.baudrate));
00160   cfsetospeed(&ctio, (speed_t)cBaudrate(laser->dev.baudrate));
00161   tcflush(laser->dev.fd, TCIFLUSH);
00162   tcsetattr(laser->dev.fd, TCSANOW, &ctio);
00163 }
00164 
00165 void sick_set_baudrate(sick_laser_p laser, int brate)
00166 {
00167   struct termios  ctio;
00168   
00169   tcgetattr(laser->dev.fd, &ctio); /* save current port settings */
00170   cfsetispeed(&ctio, (speed_t)cBaudrate(brate));
00171   cfsetospeed(&ctio, (speed_t)cBaudrate(brate));
00172   tcflush(laser->dev.fd, TCIFLUSH);
00173   tcsetattr(laser->dev.fd, TCSANOW, &ctio);
00174 }
00175 
00176 int sick_serial_connect(sick_laser_p laser)
00177 {
00178   if((laser->dev.fd = open(laser->dev.ttyport, O_RDWR | O_NOCTTY, 0)) < 0)
00179     return(-1);
00180   sick_set_serial_params(laser);
00181   return(laser->dev.fd);
00182 }
00183 
00184 /* sick_compute_checksum - Compute the CRC checksum of a segment of data. */
00185 
00186 static int sick_compute_checksum(unsigned char *CommData, int uLen)
00187 {
00188   unsigned char abData[2] = {0, 0}, uCrc16[2] = {0, 0};
00189   
00190   while(uLen--) {
00191     abData[0] = abData[1];
00192     abData[1] = *CommData++;
00193     if(uCrc16[0] & 0x80) {
00194       uCrc16[0] <<= 1;
00195       if(uCrc16[1] & 0x80)
00196         uCrc16[0] |= 0x01;
00197       uCrc16[1] <<= 1;
00198       uCrc16[0] ^= CRC16_GEN_POL0;
00199       uCrc16[1] ^= CRC16_GEN_POL1;
00200     } 
00201     else {
00202       uCrc16[0] <<= 1;
00203       if(uCrc16[1] & 0x80)
00204         uCrc16[0] |= 0x01;
00205       uCrc16[1] <<= 1;
00206     }
00207     uCrc16[0] ^= abData[0];
00208     uCrc16[1] ^= abData[1];
00209   }
00210   return (((int)uCrc16[0]) * 256 + ((int)uCrc16[1]));
00211 }
00212 
00213 int sick_read_data(sick_laser_p laser, unsigned char *data, double timeout)
00214 {
00215   static int val, i, j, l, pos, chk1, chk2;
00216   double start_time = carmen_get_time_ms();
00217 
00218   l    = BUFFER_SIZE;
00219   pos  = 0;
00220   chk1 = FALSE;
00221   chk2 = FALSE;
00222   while(carmen_get_time_ms() - start_time < timeout) {
00223     val = carmen_serial_numChars(laser->dev.fd);
00224     if(val > 0) {
00225       if(pos + val >= BUFFER_SIZE)
00226         return(0);
00227       if(pos + val >= l + 6)
00228         val = l + 6 - pos;
00229       read(laser->dev.fd, &(data[pos]), val);
00230       pos += val;
00231       if(!chk1 && pos > 2) {
00232         if(data[0] != STX || data[1] != LID) {
00233           for(i = 1; i < pos - 1; i++) {
00234             if(data[i] == STX && data[i+1] == LID) {
00235               for(j = i; j < pos; j++) {
00236                 data[j - i] = data[j];
00237               }
00238               pos -= i;
00239               chk1 = TRUE;
00240               break;
00241             }
00242           }
00243           if(!chk1)
00244             pos = 0;
00245         } 
00246         else
00247           chk1 = TRUE;
00248       }
00249       if(!chk2 && pos > 4) {
00250         l = data[3] * 256 + data[2];
00251         chk2 = TRUE;
00252       }
00253     }
00254     if(pos == l + 6)
00255       return(l + 6);
00256     usleep(1000);
00257   }
00258   return(0);
00259 }
00260 
00261 int sick_write_command(sick_laser_p laser, unsigned char command,
00262                        unsigned char *argument, int arg_length)
00263 {
00264   unsigned char  buffer[MAX_COMMAND_SIZE];
00265   int pos = 0, i, check, length, loop, answer = 0, counter = 0;
00266   int val = 0;
00267 
00268   /* SYNC CHARS */
00269   buffer[pos++] = 0x02;
00270   /* ADDRESS    */
00271   buffer[pos++] = 0x00;
00272   /* MESSAGE LENGTH */
00273   length = 1 + arg_length;
00274   buffer[pos++] = length & 0x00ff;
00275   buffer[pos++] = length / 256; /* I wonder if that works */
00276   /* COMMAND */
00277   buffer[pos++] = command;
00278   /* ARGUMENT */
00279   if(arg_length > 0)
00280     for(i=0; i < arg_length; i++)
00281       buffer[pos++] = argument[i];
00282   /* CHECKSUM */
00283   check = sick_compute_checksum(buffer, length + 4);
00284   buffer[pos++] = check & 0x00ff;
00285   buffer[pos++] = check / 256;
00286   carmen_serial_writen(laser->dev.fd, (char*)buffer, pos);
00287 
00288   /* wait for acknowledgement */
00289   loop = 1;
00290   answer = INI;
00291   while(loop) {
00292     counter++;
00293     val = carmen_serial_numChars(laser->dev.fd);
00294     if(val > 0) {
00295       read(laser->dev.fd, &buffer, val);
00296       switch(buffer[0]) {
00297       case 0x02:
00298         answer = STX;
00299         break;
00300       case 0x06:
00301         answer = ACK;
00302         break;
00303       case 0x10:
00304         answer = DLE;
00305         break;
00306       case 0x15:
00307         answer = NAK;
00308         break;
00309       default:
00310         answer = UKN;
00311       }
00312       loop = 0;
00313     } 
00314     else if(counter > 5) {
00315       answer = TIO;
00316       loop = 0;
00317     }
00318     usleep(1000); 
00319   }
00320   return answer;
00321 }
00322 
00323 void sick_request_status(sick_laser_p laser)
00324 {
00325   sick_write_command(laser, 0x31, NULL, 0);
00326 }
00327 
00328 void sick_request_sensor(sick_laser_p laser)
00329 {
00330   static unsigned char args[2] = {0x05, 0xb4};
00331 
00332   sick_write_command(laser, 0x30, args, 2);
00333 }
00334 
00335 int sick_set_laser_baudrate(sick_laser_p laser, int brate)
00336 {
00337   unsigned char args[1];
00338   int result;
00339 
00340   if(brate == 500000)
00341     args[0] = 0x48;
00342   else if(brate == 38400)
00343     args[0] = 0x40;
00344   else if(brate == 19200)
00345     args[0] = 0x41;
00346   else
00347     args[0] = 0x42;
00348   result = sick_write_command(laser, 0x20, args, 1);
00349   return (result == ACK);
00350 }
00351 
00352 int sick_set_config_mode(sick_laser_p laser)
00353 {
00354   unsigned char data[MAX_COMMAND_SIZE], args[MAX_COMMAND_SIZE];
00355   int i, result;
00356 
00357   for(i = 0; i < 8; i++)
00358     args[i + 1] = (unsigned char)laser->dev.passwd[i];
00359   args[0] = 0x00;
00360   carmen_serial_ClearInputBuffer(laser->dev.fd);
00361   result = sick_write_command(laser, 0x20, args, 9);
00362   if(result == ACK)
00363     return(sick_read_data(laser, data, MAX_TIME_FOR_CONFIG));
00364   else
00365     return(FALSE);
00366 }
00367 
00368 int sick_set_lms_resolution(sick_laser_p laser)
00369 {
00370   unsigned char args[4];
00371   int result;
00372 
00373   if(laser->settings.angle_range == 100) {
00374     args[0] = 0x64; args[1] = 0x00;
00375     if(laser->settings.angle_resolution == RES_1_00_DEGREE) {
00376       args[2] = 0x64; args[3] = 0x00;
00377     } 
00378     else if(laser->settings.angle_resolution == RES_0_50_DEGREE) {
00379       args[2] = 0x32; args[3] = 0x00;
00380     } 
00381     else {
00382       args[2] = 0x19; args[3] = 0x00;
00383     }
00384   } 
00385   else {
00386     args[0] = 0xB4; args[1] = 0x00;
00387     if(laser->settings.angle_resolution == RES_1_00_DEGREE) {
00388       args[2] = 0x64; args[3] = 0x00;
00389     } else {
00390       args[2] = 0x32; args[3] = 0x00;
00391     }
00392   }
00393   result = sick_write_command(laser, 0x3B, args, 4);
00394   return(result == ACK);
00395 }
00396 
00397 int sick_request_lms_config(sick_laser_p laser)
00398 {
00399   int result;
00400   
00401   result = sick_write_command(laser, 0x74, NULL, 0);
00402   return(result == ACK);
00403 }
00404 
00405 int sick_set_lms_config(sick_laser_p laser, unsigned char *data, int len)
00406 {
00407   int result;
00408 
00409   if(len == 32) {
00410     result = sick_write_command(laser, 0x77, data, len);
00411     return(result == ACK);
00412   } 
00413   else
00414     return(FALSE);
00415 }
00416 
00417 int sick_parse_conf_data(sick_laser_p laser, unsigned char *buf, int length)
00418 {
00419   int check, i;
00420   unsigned char data[32];
00421   
00422   if(length < 4)
00423     return(FALSE);
00424   else
00425     check = sick_compute_checksum(buf, length - 2);
00426   if((length != 42 && length != 40) || buf[4] != 0xF4 ||
00427      buf[length - 2] != (check & 0x00ff) || buf[length - 1] != (check / 256))
00428     return(FALSE);
00429   for(i = 0; i < 32; i++)
00430     data[i] = buf[i + 5];
00431   if((laser->settings.range_res == CM && data[6] != 0) ||
00432      (laser->settings.range_res == MM && data[6] != 1) ||
00433      (laser->settings.range_res == DM && data[6] != 2) ||
00434      (laser->settings.range_dist == RANGE80M  && data[5] != 1) ||
00435      (laser->settings.range_dist == RANGE160M && data[5] != 3) ||
00436      (laser->settings.range_dist == RANGE320M && data[5] != 5)) {
00437     fprintf(stderr, "ok\n");
00438     fprintf(stderr, "INFO: set LASER in config-mode ........ ");
00439     do {} while (!sick_set_config_mode(laser));
00440     fprintf(stderr, "ok\n");
00441   
00442     switch(laser->settings.range_dist) {
00443     case RANGE80M:
00444       data[5] = 1; 
00445       break;
00446     case RANGE160M:
00447       data[5] = 3;
00448       break;
00449     case RANGE320M:
00450       data[6] = 5;
00451       break;
00452     default:
00453       data[6] = 1;
00454       break;
00455     }
00456     switch(laser->settings.range_res) {
00457     case CM:
00458       data[6] = 0;
00459       break;
00460     case MM:
00461       data[6] = 1;
00462       break;
00463     case DM:
00464       data[6] = 2;
00465       break;
00466     default:
00467       data[6] = 0;
00468       break;
00469     }
00470     fprintf(stderr, "INFO: set LMS configuration ........... ");
00471     do {} while (!sick_set_lms_config(laser, data, 32));
00472     fprintf(stderr, "ok\n");
00473   }
00474   return(TRUE);
00475 }
00476 
00477 int sick_set_lms_range(sick_laser_p laser)
00478 {
00479   int l = 0;
00480   unsigned char data[BUFFER_SIZE];
00481 
00482   if(sick_request_lms_config(laser)) {
00483     usleep(100000);
00484     l = sick_read_data(laser, data, MAX_TIME_FOR_GETTING_CONF);
00485     return(sick_parse_conf_data(laser, data, l));
00486   } 
00487   else
00488     return(FALSE);
00489 }
00490 
00491 void sick_start_continuous_mode(sick_laser_p laser)
00492 {
00493   unsigned char lmsarg[1] = {0x24}, pls180arg[1] = {0x20};
00494   unsigned char pls360arg[1] = {0x24};
00495   int result = 0;
00496   
00497   do {
00498     if(laser->settings.type == LMS)
00499       result = sick_write_command(laser, 0x20, lmsarg, 1);
00500     else if(laser->settings.type == PLS &&
00501             laser->settings.angle_resolution == RES_1_00_DEGREE)
00502       result = sick_write_command(laser, 0x20, pls180arg, 1);
00503     else if(laser->settings.type == PLS &&
00504             laser->settings.angle_resolution == RES_0_50_DEGREE)
00505       result = sick_write_command(laser, 0x20, pls360arg, 1);
00506   } while(result != ACK);
00507 }
00508 
00509 void sick_stop_continuous_mode(sick_laser_p laser)
00510 {
00511   unsigned char args[1] = {0x25};
00512   int result;
00513 
00514   do {
00515     result = sick_write_command(laser, 0x20, args, 1);
00516   } while(result != ACK);
00517 }
00518 
00519 /* sick_test_baudrate - Test a combination of baudrate and parity 
00520    of the laser. */
00521 
00522 int sick_testBaudrate(sick_laser_p laser, int brate)
00523 {
00524   unsigned char data[BUFFER_SIZE], ReqLaser[2] = {5, 180};
00525   int response;
00526 
00527   sick_set_baudrate(laser, brate);
00528   
00529   response = sick_write_command(laser, 0x30, ReqLaser, 2);
00530   if(response == NAK) {
00531     fprintf(stderr, "Error : Could not send command to laser.\n");
00532     return FALSE;
00533   }
00534   if(sick_read_data(laser, data, MAX_TIME_FOR_TESTING_BAUDRATE))
00535     return TRUE;
00536   return FALSE;
00537 }
00538 
00539 int sick_detect_baudrate(sick_laser_p laser)
00540 {
00541   fprintf(stderr, "INFO: detect connected baudrate: ...... 9600");
00542   if(sick_testBaudrate(laser, 9600)) {
00543     fprintf(stderr, "\n");
00544     return(9600);
00545   } 
00546   else {
00547     fprintf(stderr, "\rINFO: detect connected baudrate: ...... 19200");
00548     if(sick_testBaudrate(laser, 19200)) {
00549       fprintf(stderr, "\n");
00550       return(19200);
00551     } 
00552     else {
00553       fprintf(stderr, "\rINFO: detect connected baudrate: ...... 38400");
00554       if(sick_testBaudrate(laser, 38400)) {
00555         fprintf(stderr, "\n");
00556         return(38400);
00557       } 
00558       else {
00559         if(laser->settings.use_highspeed) {
00560           fprintf(stderr, "\rINFO: detect connected baudrate: ...... 500000");
00561           if(sick_testBaudrate(laser, 500000)) {
00562             fprintf(stderr, "\n");
00563             return(500000);
00564           } 
00565           else {
00566             fprintf(stderr, "\rINFO: detect connected baudrate: ...... failed\n");
00567             return(0);
00568           }
00569         } 
00570         else {
00571           fprintf(stderr, "\rINFO: detect connected baudrate: ...... failed\n");          
00572           return(0);
00573         }
00574       }
00575     }
00576   }
00577 }
00578 
00579 int sick_check_baudrate(sick_laser_p laser, int brate)
00580 {
00581   fprintf(stderr, "check baudrate:\n");
00582   fprintf(stderr, "    %d ... ", brate);
00583   if(sick_testBaudrate(laser, brate)) {
00584     fprintf(stderr, "yes\n");
00585     return(TRUE);
00586   } 
00587   else {
00588     fprintf(stderr, "no\n");
00589     return(FALSE);
00590   }
00591 }
00592 
00593 void sick_install_settings(sick_laser_p laser)
00594 {
00595   laser->dev.type = laser->settings.type;
00596   laser->dev.baudrate = laser->settings.start_baudrate;
00597   laser->dev.parity = laser->settings.parity;
00598   laser->dev.fd = -1;
00599   laser->dev.databits = laser->settings.databits;
00600   laser->dev.stopbits = laser->settings.stopbits;
00601   laser->dev.hwf = laser->settings.hwf;
00602   laser->dev.swf = laser->settings.swf;
00603   strncpy((char *)laser->dev.passwd, (char*)laser->settings.password, 8);
00604   laser->dev.ttyport =
00605     (char *)malloc((strlen(laser->settings.device_name) + 1) * sizeof(char));
00606   carmen_test_alloc(laser->dev.ttyport);
00607   strcpy(laser->dev.ttyport, laser->settings.device_name);
00608 }
00609 
00610 void sick_allocate_laser(sick_laser_p laser)
00611 {
00612   int i;
00613   
00614   laser->numvalues = laser->settings.num_values;
00615   laser->range = (double *)malloc(laser->settings.num_values * sizeof(double));
00616   carmen_test_alloc(laser->range);
00617   laser->glare = (int *)malloc(laser->settings.num_values * sizeof(int));
00618   carmen_test_alloc(laser->glare);
00619   laser->wfv = (int *)malloc(laser->settings.num_values * sizeof(int));
00620   carmen_test_alloc(laser->wfv);
00621   laser->sfv = (int *)malloc(laser->settings.num_values * sizeof(int));
00622   carmen_test_alloc(laser->sfv);
00623   laser->buffer = (unsigned char *)malloc(LASER_BUFFER_SIZE);
00624   carmen_test_alloc(laser->buffer);
00625   laser->new_reading = 0;
00626   laser->buffer_position = 0;
00627   laser->processed_mark = 0;
00628   for(i = 0; i < laser->settings.num_values; i++) {
00629     laser->range[i] = 0.0;
00630     laser->glare[i] = 0;
00631     laser->wfv[i] = 0;
00632     laser->sfv[i] = 0;
00633   }
00634 }
00635 
00636 int sick_connect_device(sick_laser_p laser)
00637 {
00638   sick_install_settings(laser);
00639   sick_allocate_laser(laser);
00640   fprintf(stderr, "INFO: connect TTY %-14s ...... ", laser->dev.ttyport);
00641   sick_serial_connect(laser);
00642   if(laser->dev.fd == -1) {
00643     fprintf(stderr, "failed\n");
00644     return -1;
00645   }
00646   fprintf(stderr, "ok\n");
00647   fprintf(stderr, "INFO: set port param %6d:%d%c%d ....... ",
00648           laser->dev.baudrate, laser->dev.databits,
00649           (laser->dev.parity == N ? 'N' : laser->dev.parity == E ? 'E' : 'O'),
00650           laser->dev.stopbits);
00651   sick_set_serial_params(laser);
00652   fprintf(stderr, "ok\n");
00653   return 0;
00654 }
00655 
00656 int sick_start_laser(sick_laser_p laser)
00657 {
00658   int brate = 0;
00659 
00660   fprintf(stderr, "###########################################\n");
00661   fprintf(stderr, "INFO: LASER type ...................... ");
00662   fprintf(stderr, "%s\n", (laser->settings.type == PLS) ? "PLS" : "LMS");
00663   
00664   /* open the serial port */
00665   if (sick_connect_device(laser) < 0) {
00666           return -1;
00667   }
00668   
00669   /* make sure the baudrate is set correctly, change it if necessary */
00670   if(laser->settings.detect_baudrate) {
00671     brate = sick_detect_baudrate(laser);
00672     if(!brate)
00673       return -1;
00674   } 
00675   else if(!sick_check_baudrate(laser, laser->settings.start_baudrate)) {
00676     fprintf(stderr, "ERROR: communication does not work!\n");
00677     return -1;
00678   }
00679   if(brate != laser->settings.set_baudrate) {
00680     fprintf(stderr, "INFO: set LASER in config-mode ........ ");
00681     while(!sick_set_config_mode(laser));
00682     fprintf(stderr, "ok\n");
00683     fprintf(stderr, "INFO: set LASER baudrate to %6d .... ",
00684             laser->settings.set_baudrate);
00685     while(!sick_set_laser_baudrate(laser, laser->settings.set_baudrate));
00686     sick_set_baudrate(laser, laser->settings.set_baudrate);
00687     fprintf(stderr, "ok\n");
00688   }
00689   
00690   /* set the resolution of the blue lasers */
00691   if(laser->settings.type == LMS) {
00692     fprintf(stderr, "INFO: angle range ..................... %3d\n",
00693             laser->settings.angle_range);
00694     fprintf(stderr, "INFO: angle resolution ................ %1.2f\n",
00695             (laser->settings.angle_resolution == RES_1_00_DEGREE ? 1.0 :
00696              laser->settings.angle_resolution == RES_0_50_DEGREE ? 0.5 : 
00697              0.25));
00698     fprintf(stderr, "INFO: set LASER mode .................. ");
00699     while(!sick_set_lms_resolution(laser));
00700     fprintf(stderr, "ok\n");
00701     usleep(100000);
00702     fprintf(stderr, "INFO: get LMS configuration ........... ");
00703     while(!sick_set_lms_range(laser));
00704     fprintf(stderr, "ok\n");
00705   }
00706   
00707   /* set the laser to continuous mode */
00708   fprintf(stderr, "INFO: start LASER continuous mode ..... ");
00709   sick_start_continuous_mode(laser);
00710   fprintf(stderr, "ok\n");
00711   fprintf(stderr, "###########################################\n");
00712   return 0;
00713 }
00714 
00715 /* sick_valid_packet - This function returns 1 if a valid packet is
00716    detected in a chunk of data.  An offset and packet length are 
00717    returned. */
00718 
00719 int sick_valid_packet(unsigned char *data, long size,
00720                       long *offset, long *len)
00721 {
00722   int i, check, packet_size = 0, theo_size = 0;
00723 
00724   for(i = 0; i < size; i++) {
00725     if(packet_size == 0 && *data == 0x02)
00726       packet_size++;
00727     else if(packet_size == 1 && *data == 0x80)
00728       packet_size++;
00729     else if(packet_size == 1)
00730       packet_size = 0;
00731     else if(packet_size == 2) {
00732       theo_size = data[0];
00733       packet_size++;
00734     } 
00735     else if(packet_size == 3) {
00736       theo_size += (data[0] << 8) + 6;
00737       if(size >= theo_size + (i - packet_size)) {       // check the crc
00738         check = data[theo_size - 3 - 2];
00739         check += data[theo_size - 3 - 1] << 8;                                
00740         if(check != sick_compute_checksum(data - 3, theo_size - 2)) {
00741           i -= 2;
00742           data -= 2;
00743           packet_size = 0;
00744         }
00745         else {
00746           *offset = i - packet_size;
00747           *len = theo_size;
00748           return 1;
00749         }
00750       } 
00751       else
00752         packet_size = 0;
00753     }
00754     data++;
00755   }
00756   return 0;
00757 }
00758 
00759 /* sick_process_packet - Interpret packets received from the laser.  If
00760    the packets contain laser data, expand the data into a useful form. */
00761 
00762 static void sick_process_packet(sick_laser_p laser, unsigned char *packet)
00763 {
00764   int i = 0, LoB = 0, HiB = 0, bit14, bit15, numMeasurements;
00765   float conversion = 1.0;
00766 
00767   if(packet[0] == 0xb0) {
00768     /* Check the number of measurements */
00769     numMeasurements = ((packet[2] << 8) + packet[1]) & 0x3FFF;
00770 
00771     /* Extract the distance conversion factor */
00772     bit14 = packet[2] & 0x40;
00773     bit15 = packet[2] & 0x80;
00774     if(laser->settings.type == LMS) {
00775       if(!bit15)
00776         if(!bit14)
00777           conversion = 1.0;
00778         else
00779           conversion = 0.1;
00780       else
00781         conversion = 10.0;
00782     }
00783     
00784     /* Compute range values */
00785     laser->new_reading = 1;
00786     for (i = 0; i < numMeasurements; i++) {
00787       LoB = packet[i * 2 + 3]; 
00788       HiB = packet[i * 2 + 4];
00789       laser->range[i] = ((HiB & 0x1f) * 256 + LoB) * conversion;
00790       laser->glare[i] = (HiB & 0x20) / 32; 
00791       laser->wfv[i] = (HiB & 0x40) / 64;  
00792       laser->sfv[i] = (HiB & 0x80) / 128;
00793     }
00794     if(laser->raw){
00795       for(i=0; i<numMeasurements; i++){
00796         LoB = packet[i * 2 + 3]; 
00797         HiB = packet[i * 2 + 4];
00798         laser->raw[i] = ((float)(((HiB & 0x1f) * 256 + LoB)))/100.0;    
00799       }
00800     }
00801     laser->timestamp = carmen_get_time_ms();
00802   }
00803 }
00804 
00805 /* sick_handle_laser - Process any data that is available from the 
00806    laser. Attempt to detect valid packets in the data. */
00807 
00808 void sick_handle_laser(sick_laser_p laser)
00809 {
00810   int bytes_available, bytes_read;
00811   int leftover;
00812   
00813   laser->new_reading = 0;
00814 
00815   /* read what is available in the buffer */
00816   bytes_available = carmen_serial_numChars(laser->dev.fd);
00817   if(bytes_available > LASER_BUFFER_SIZE - laser->buffer_position)
00818     bytes_available = LASER_BUFFER_SIZE - laser->buffer_position;
00819   bytes_read = carmen_serial_readn(laser->dev.fd, (char*)(laser->buffer +
00820                                    laser->buffer_position), bytes_available);
00821 
00822   /* process at most one laser reading */
00823   if(bytes_read > 0) {
00824     laser->buffer_position += bytes_read;
00825     if(sick_valid_packet(laser->buffer + laser->processed_mark,
00826                          laser->buffer_position - laser->processed_mark,
00827                          &(laser->packet_offset), &(laser->packet_length))) {
00828       sick_process_packet(laser, laser->buffer + laser->processed_mark +
00829                           laser->packet_offset + 4);
00830       leftover = laser->buffer_position - laser->processed_mark - 
00831         laser->packet_length;
00832       laser->processed_mark += laser->packet_offset + laser->packet_length;
00833       if(leftover == 0) {
00834         laser->buffer_position = 0;
00835         laser->processed_mark = 0;
00836       }  
00837     }
00838   }
00839 
00840   /* shift everything forward in the buffer, if necessary */
00841   if(laser->buffer_position > LASER_BUFFER_SIZE / 2) {
00842     memmove(laser->buffer, laser->buffer + laser->processed_mark,
00843             laser->buffer_position - laser->processed_mark);
00844     laser->buffer_position = laser->buffer_position - laser->processed_mark;
00845     laser->processed_mark = 0;
00846   } 
00847 }
00848 
00849 void sick_stop_laser(sick_laser_p laser)
00850 {
00851   fprintf(stderr, "\nINFO: stop LASER continuous mode ....... ");
00852   sick_stop_continuous_mode(laser);
00853   fprintf(stderr, "ok\n");
00854   close(laser->dev.fd);
00855 }