sick_lms400.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
00003  * Code based on the LGPL Player SICK LMS400 driver by Nico Blodow and Radu Bogdan Rusu
00004  *
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are met:
00009  *
00010  *     * Redistributions of source code must retain the above copyright
00011  *       notice, this list of conditions and the following disclaimer.
00012  *     * Redistributions in binary form must reproduce the above copyright
00013  *       notice, this list of conditions and the following disclaimer in the
00014  *       documentation and/or other materials provided with the distribution.
00015  *
00016  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00017  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00018  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00019  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00020  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00021  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00022  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00023  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00024  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00025  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00026  * POSSIBILITY OF SUCH DAMAGE.
00027  *
00028  * $Id: sick_lms400.cpp 657 2010-09-25 23:31:31Z moesenle $
00029  *
00030  */
00031 
00032 #include <sick_lms400/sick_lms400.h>
00033 #include <angles/angles.h>
00034 
00035 const int CMD_BUFFER_SIZE = 255;
00036 
00037 
00039 // Constructor.
00040 sick_lms400::SickLMS400::SickLMS400 (const char* host, int port, int debug_mode)
00041 {
00042   portno_   = port;
00043   hostname_ = host;
00044   verbose_  = debug_mode;
00045   memset (command_, 0, BUF_SIZE);
00046   MeasurementQueue_ = new std::vector<MeasurementQueueElement_t >;
00047 }
00048 
00050 // Connect to the LMS400 unit using hostname:portno
00051 // Returns 0 if connection was successful, -1 otherwise
00052 int
00053   sick_lms400::SickLMS400::Connect ()
00054 {
00055   // Create a socket
00056   sockfd_ = socket (AF_INET, SOCK_STREAM, 0);
00057   if (sockfd_ < 0)
00058     return (-1);
00059 
00060   // Get the network host entry
00061   memset (&serv_addr_, 0, sizeof (serv_addr_));
00062   serv_addr_.sin_port = htons (portno_);
00063   serv_addr_.sin_family = AF_INET;
00064 #if defined (HAVE_GETADDRINFO)
00065   addr_ptr = NULL;
00066   if (getaddrinfo (hostname_, NULL, NULL, &(addr_ptr)))
00067   {
00068     ROS_ERROR ("getaddrinfo() failed with error");
00069     return (-1);
00070   }
00071   assert (addr_ptr);
00072   assert (addr_ptr->ai_addr);
00073   if ((addr_ptr->ai_addr->sa_family) != AF_INET)
00074   {
00075     ROS_ERROR ("unsupported internet address family");
00076     return (-1);
00077   }
00078   serv_addr.sin_addr.s_addr = (reinterpret_cast<struct sockaddr_in*> (addr_ptr->ai_addr))->sin_addr.s_addr;
00079   freeaddrinfo (addr_ptr);
00080   addr_ptr = NULL;
00081 #else
00082   server_ = gethostbyname (hostname_);
00083   if ((server_) == NULL)
00084     return (-1);
00085   memcpy (&(serv_addr_.sin_addr.s_addr), server_->h_addr, server_->h_length);
00086 #endif
00087 
00088   // Attempt to connect
00089   if (connect (sockfd_, reinterpret_cast<struct sockaddr*> (&serv_addr_), sizeof (serv_addr_)) < 0)
00090     return (-1);
00091 
00092   return (0);
00093 }
00094 
00096 // Disconnect from the LMS400 unit
00097 // Returns 0 if connection was successful, -1 otherwise
00098 int
00099   sick_lms400::SickLMS400::Disconnect ()
00100 {
00101   return (close (sockfd_));
00102 }
00103 
00105 // Enable/Disable extended RIS (Remission Information System) detectivity
00106 int
00107   sick_lms400::SickLMS400::EnableRIS (int onoff)
00108 {
00109   char cmd[CMD_BUFFER_SIZE];
00110   snprintf (cmd, CMD_BUFFER_SIZE, "sWN MDblex %i", onoff);
00111   SendCommand (cmd);
00112 
00113   if (ReadAnswer () != 0)
00114     return (-1);
00115   ExtendedRIS_ = onoff;
00116   return (0);
00117 }
00118 
00120 // Set the mean filter parameters
00121 int
00122   sick_lms400::SickLMS400::SetMeanFilterParameters (int num_scans)
00123 {
00124   char cmd[CMD_BUFFER_SIZE];
00125   snprintf (cmd, CMD_BUFFER_SIZE, "sWN FLmean 0 %i", num_scans);
00126   SendCommand (cmd);
00127 
00128   if (ReadAnswer () != 0)
00129     return (-1);
00130   MeanFilterNumScans_ = num_scans;
00131   return (0);
00132 }
00133 
00135 // Set the range filter parameters
00136 int
00137   sick_lms400::SickLMS400::SetRangeFilterParameters (float range_min, float range_max)
00138 {
00139   char cmd[CMD_BUFFER_SIZE];
00140   snprintf (cmd, CMD_BUFFER_SIZE, "sWN FLrang %+f %+f", (float)range_min, (float)range_max);
00141   SendCommand (cmd);
00142 
00143   if (ReadAnswer () != 0)
00144     return (-1);
00145   RangeFilterBottomLimit_ = range_min;
00146   RangeFilterTopLimit_    = range_max;
00147   return (0);
00148 }
00149 
00151 // Enable filters using a filter mask
00152 int
00153   sick_lms400::SickLMS400::EnableFilters (int filter_mask)
00154 {
00155   char cmd[CMD_BUFFER_SIZE];
00156   snprintf (cmd, CMD_BUFFER_SIZE, "sWN FLsel %+i", filter_mask);
00157   SendCommand (cmd);
00158 
00159   if (ReadAnswer () != 0)
00160     return (-1);
00161   FilterMask_ = filter_mask;
00162   return (0);
00163 }
00164 
00166 // Takes a string containing an ip adress and returns an array of 4 u_chars
00167 unsigned char*
00168   sick_lms400::SickLMS400::ParseIP (char* ip)
00169 {
00170   char* tmp = (char*) malloc (strlen (ip) + 1);
00171   unsigned char* _ip = (unsigned char*) malloc (4);
00172 
00173   strcpy (tmp, ip);
00174   _ip[0] = atoi (strtok (tmp, "."));
00175   for (int i = 1; i < 4; i++)
00176     _ip[i] = atoi (strtok (NULL, "."));
00177 
00178   free (tmp);
00179   return (_ip);
00180 }
00181 
00183 // Set the desired userlevel by logging in with the appropriate password
00184 int
00185   sick_lms400::SickLMS400::SetUserLevel (int8_t userlevel, const char* password)
00186 {
00187   char cmd[CMD_BUFFER_SIZE];
00188   snprintf (cmd, CMD_BUFFER_SIZE, "sMN SetAccessMode %d %s", userlevel, password);
00189   SendCommand (cmd);
00190   return (ReadConfirmationAndAnswer ());
00191 }
00192 
00194 // Fills string pointed to by macadress with the MAC adress read from the sensor
00195 int
00196   sick_lms400::SickLMS400::GetMACAddress (char** macaddress)
00197 {
00198   char *mac = (char*) malloc (20);
00199   int index = 0;
00200   char* tmp;
00201 
00202   SendCommand ("sRN EImac ");
00203   if (ReadAnswer () != 0)
00204     return (-1);
00205 
00206   strtok ((char*) buffer_, " ");
00207   strtok (NULL, " ");
00208 
00209   for (int i = 0; i < 6; i++)
00210   {
00211     tmp = strtok (NULL, "-");
00212     strncpy (&mac[index], tmp, 2);
00213     index += 2;
00214     mac[index++] = ':';
00215   }
00216 
00217   mac[--index] = 0;
00218   *macaddress = mac;
00219   return (0);
00220 }
00221 
00223 // Set the IP address of the LMS400
00224 int
00225   sick_lms400::SickLMS400::SetIP (char* ip)
00226 {
00227   unsigned char* ip_str;
00228   ip_str = ParseIP (ip);
00229   char cmd[80];
00230 
00231   snprintf (cmd, 80, "sWN EIip %X %X %X %X", ip_str[0], ip_str[1], ip_str[2], ip_str[3]);
00232   free (ip_str);
00233   SendCommand (cmd);
00234 
00235   return (ReadAnswer ());
00236 }
00237 
00239 // Set the gateway address for the Ethernet interface
00240 int
00241   sick_lms400::SickLMS400::SetGateway (char* gw)
00242 {
00243   unsigned char* gw_str;
00244   gw_str = ParseIP (gw);
00245   char cmd[CMD_BUFFER_SIZE];
00246 
00247   snprintf (cmd, CMD_BUFFER_SIZE, "sWN EIgate %X %X %X %X", gw_str[0], gw_str[1], gw_str[2], gw_str[3]);
00248   free (gw_str);
00249   SendCommand (cmd);
00250 
00251   return (ReadAnswer ());
00252 }
00253 
00255 // Set the subnet mask for the Ethernet interface
00256 int
00257   sick_lms400::SickLMS400::SetNetmask (char* mask)
00258 {
00259   unsigned char* mask_str;
00260   mask_str = ParseIP (mask);
00261   char cmd[CMD_BUFFER_SIZE];
00262 
00263   snprintf (cmd, CMD_BUFFER_SIZE, "sWN EImask %X %X %X %X", mask_str[0], mask_str[1], mask_str[2], mask_str[3]);
00264   free (mask_str);
00265   SendCommand (cmd);
00266 
00267   return (ReadAnswer ());
00268 }
00269 
00271 // Set port for TCP/IP communication
00272 int
00273   sick_lms400::SickLMS400::SetPort (uint16_t port)
00274 {
00275   char cmd[CMD_BUFFER_SIZE];
00276 
00277   snprintf (cmd,CMD_BUFFER_SIZE, "sWN EIport %04X", port);
00278   SendCommand (cmd);
00279 
00280   return (ReadAnswer ());
00281 }
00282 
00284 // Reset the LMS400 unit
00285 int
00286   sick_lms400::SickLMS400::ResetDevice ()
00287 {
00288   const char* cmd = "sMN mDCreset ";
00289   SendCommand (cmd);
00290 
00291   return (ReadAnswer ());
00292 }
00293 
00295 // Terminate configuration and change back to userlevel 0
00296 int
00297   sick_lms400::SickLMS400::TerminateConfiguration ()
00298 {
00299   const char* cmd = "sMN Run";
00300   SendCommand (cmd);
00301 
00302   return (ReadConfirmationAndAnswer ());
00303 }
00304 
00306 // Set the laser angular resolution. Requires userlevel 2. Unused for now.
00307 int
00308   sick_lms400::SickLMS400::SetAngularResolution (const char* password, float ang_res,
00309                                                  float angle_start, float angle_range)
00310 {
00311   char cmd[CMD_BUFFER_SIZE];
00312   snprintf (cmd, CMD_BUFFER_SIZE, "sMN mSCconfigbyang 04 %s %+f 01 %+f %+f",
00313            password, ang_res, angle_start, angle_range);
00314   SendCommand (cmd);
00315 
00316   return (ReadConfirmationAndAnswer ());
00317 }
00318 
00320 // Set the laser scanning frequency. Requires userlevel 2. Unused for now.
00321 int
00322   sick_lms400::SickLMS400::SetScanningFrequency (const char* password, float freq,
00323                                     float angle_start, float angle_range)
00324 {
00325   char cmd[CMD_BUFFER_SIZE];
00326   snprintf (cmd, CMD_BUFFER_SIZE, "sMN mSCconfigbyfreq 04 %s %+f 01 %+f %+f",
00327            password, freq, angle_start, angle_range);
00328   SendCommand (cmd);
00329 
00330   return (ReadConfirmationAndAnswer ());
00331 }
00332 
00334 // Set both resolution and frequency without going to a higher user level (?)
00335 int
00336   sick_lms400::SickLMS400::SetResolutionAndFrequency (float freq, float ang_res,
00337                                          float angle_start, float angle_range)
00338 {
00339   char cmd[CMD_BUFFER_SIZE];
00340   snprintf (cmd, CMD_BUFFER_SIZE, "sMN mSCsetscanconfig %+.2f %+.2f %+.2f %+.2f",
00341     freq, ang_res, angle_start, angle_range);
00342   SendCommand (cmd);
00343 
00344   int error = ReadConfirmationAndAnswer ();
00345 
00346   // If no error, parse the results
00347   if (error == 0)
00348   {
00349     strtok ((char*)buffer_, " "); strtok (NULL, " ");
00350     int ErrorCode = strtol (strtok (NULL, " "), NULL, 16);
00351     long int sf = strtol (strtok (NULL, " "), NULL, 16);
00352     long int re = strtol (strtok (NULL, " "), NULL, 16);
00353 
00354     if ((ErrorCode != 0) && (verbose_ > 0))
00355       printf (">> Warning: got an error code %d\n", ErrorCode);
00356 
00357     scanning_frequency_ = sf;
00358     resolution_         = re;
00359 
00360     if (verbose_ > 0)
00361       printf (">> Measured value quality is: %ld [5-10]\n",
00362         strtol (strtok (NULL, " "), NULL, 16));
00363   }
00364 
00365   return (error);
00366 }
00367 
00369 // Start a measurement for both distance and intensity or just distance.
00370 int
00371   sick_lms400::SickLMS400::StartMeasurement (bool intensity)
00372 {
00373   char cmd[CMD_BUFFER_SIZE];
00374   if (intensity)
00375     snprintf (cmd, CMD_BUFFER_SIZE, "sMN mLRreqdata %x", 0x20);
00376   else
00377     snprintf (cmd, CMD_BUFFER_SIZE, "sMN mLRreqdata %x", 0x21);
00378 
00379   SendCommand (cmd);
00380 
00381   return (ReadConfirmationAndAnswer ());
00382 }
00383 
00385 // Read a measurement
00386 sensor_msgs::LaserScan
00387   sick_lms400::SickLMS400::ReadMeasurement ()
00388 {
00389   sensor_msgs::LaserScan scan;
00390 
00391   char cs_read = 0, cs_calc = 0;
00392   int length  = 0;
00393   int current = 0;
00394 
00395   memset (buffer_, 0, 256);
00396   if (!MeasurementQueue_->empty ())
00397   {
00398     if (verbose_ > 0)
00399       ROS_DEBUG (">>> Reading from queue...\n");
00400     memcpy (buffer_, (char*) MeasurementQueue_->front ().string, MeasurementQueue_->front ().length + 1);
00401     free (MeasurementQueue_->front ().string);
00402     MeasurementQueue_->erase (MeasurementQueue_->begin ());
00403   }
00404   else
00405   {
00406     if (verbose_ == 2)
00407       ROS_DEBUG (">>> Queue empty. Reading from socket...\n");
00408     n_ = read (sockfd_, buffer_, 8);
00409     if (n_ < 0)
00410     {
00411       if (verbose_ > 0)
00412         ROS_DEBUG (">>> E: error reading from socket!\n");
00413       return (scan);
00414     }
00415     if (buffer_[0] != 0x02 || buffer_[1] != 0x02 || buffer_[2] != 0x02 || buffer_[3] != 0x02)
00416     {
00417       if (verbose_ > 0)
00418         ROS_DEBUG (">>> E: error expected 4 bytes STX's!\n");
00419       n_ = read (sockfd_, buffer_, 255);
00420       return (scan);
00421     }
00422 
00423     // find message length
00424     length = ( (buffer_[4] << 24) | (buffer_[5] << 16) | (buffer_[6] <<  8) | (buffer_[7]) );
00425     do
00426     {
00427       n_ = read (sockfd_, &buffer_[current], length-current);
00428       current += n_;
00429     } while (current < length);
00430 
00431     // read checksum:
00432     int ret = read (sockfd_, &cs_read, 1);
00433     if (ret < 1)
00434     {
00435       ROS_ERROR ("LMS400 didnt get any data in read %d",ret);
00436       return (scan);
00437     }
00438 
00439     for (int i = 0; i < length; i++)
00440       cs_calc ^= buffer_[i];
00441 
00442     if (cs_calc != cs_read)
00443     {
00444       if (verbose_ > 0)
00445         ROS_WARN (">>> E: checksums do not match!\n");
00446       return (scan);
00447     }
00448   }
00449 
00450   // parse measurements header and fill in the configuration parameters
00451   MeasurementHeader_t meas_header;
00452   memcpy (&meas_header, (void *)buffer_, sizeof (MeasurementHeader_t));
00453 
00454   float min_angle  = meas_header.StartingAngle / 10000.0;
00455   float resolution = meas_header.AngularStepWidth / 10000.0;
00456   float max_angle  = ((float) meas_header.NumberMeasuredValues) * resolution + min_angle;
00457   //float scanning_frequency = meas_header.ScanningFrequency;
00458 
00459   if (verbose_ == 2)
00460     ROS_DEBUG (">>> Reading %d values from %f to %f", meas_header.NumberMeasuredValues, meas_header.StartingAngle / 10000.0,
00461                ((float) meas_header.NumberMeasuredValues) * resolution + min_angle);
00462 
00463   uint16_t distance;
00464   uint8_t remission;
00465   int index = sizeof (MeasurementHeader_t);
00466 
00467   // Fill in the appropriate values
00468   scan.angle_min       = angles::from_degrees (min_angle);
00469   scan.angle_max       = angles::from_degrees (max_angle);
00470   scan.angle_increment = angles::from_degrees (resolution);
00471   scan.range_min       = 0.7;
00472   scan.range_max       = 3.6;
00473   scan.ranges.resize (meas_header.NumberMeasuredValues);
00474   scan.intensities.resize (meas_header.NumberMeasuredValues);
00475 
00476   memcpy (&scan.scan_time, &buffer_[sizeof(MeasurementHeader_t) + meas_header.NumberMeasuredValues * 3 + 14], 2);
00477 
00478   // Parse the read buffer and copy values into our distance/intensity buffer
00479   for (int i = 0; i < meas_header.NumberMeasuredValues; i++)
00480   {
00481     if (meas_header.Format == 0x20 || meas_header.Format == 0x21)
00482     {
00483       memcpy (&distance, (void *)&buffer_[index], sizeof (uint16_t) );
00484       index += sizeof (uint16_t);
00485     }
00486     if (meas_header.Format == 0x20 || meas_header.Format == 0x22)
00487     {
00488       memcpy (&remission, (void *)&buffer_[index], sizeof (uint8_t) );
00489       index += sizeof (uint8_t);
00490     }
00491     scan.ranges[i]      = distance * meas_header.DistanceScaling / 1000.0;
00492     scan.intensities[i] = remission * meas_header.RemissionScaling;
00493 
00494     if (verbose_ == 2)
00495       ROS_DEBUG (" >>> [%i] dist: %i\t remission: %i", i, distance * meas_header.DistanceScaling, remission * meas_header.RemissionScaling);
00496   }
00497 
00498   scan.header.frame_id = "lms400_tilt_laser";
00499   scan.header.stamp = ros::Time::now ();
00500 
00501   return (scan);
00502 }
00503 
00505 // Stop a measurement
00506 int
00507   sick_lms400::SickLMS400::StopMeasurement ()
00508 {
00509   char cmd[CMD_BUFFER_SIZE];
00510   snprintf (cmd, CMD_BUFFER_SIZE, "sMN mLRstopdata");
00511   SendCommand (cmd);
00512 
00513   return (ReadConfirmationAndAnswer ());
00514 }
00515 
00517 // Send a command to the laser unit. Returns -1 on error.
00518 int
00519   sick_lms400::SickLMS400::SendCommand (const char* cmd)
00520 {
00521   if (verbose_ > 0)
00522     ROS_DEBUG (">> Sent: \"%s\"\n", cmd);
00523   AssembleCommand ((unsigned char *) cmd, strlen (cmd));
00524 
00525   n_ = write (sockfd_, command_, commandlength_);
00526   if (n_ < 0)
00527     return (-1);
00528 
00529   return (0);
00530 }
00531 
00533 // Read a result from the laser unit.
00534 int
00535   sick_lms400::SickLMS400::ReadResult ()
00536 {
00537   memset (buffer_, 0, 256);
00538   n_ = read (sockfd_, buffer_, 8);
00539   if (n_ < 0)
00540     return (-1);
00541 
00542   if (buffer_[0] != 0x02 || buffer_[1] != 0x02 || buffer_[2] != 0x02 || buffer_[3] != 0x02)
00543   {
00544     if (verbose_ > 0)
00545       ROS_WARN ("> E: expected 4 bytes STX's!");
00546     n_ = read (sockfd_, buffer_, 255);
00547     return (-1);
00548   }
00549 
00550   // Find message length
00551   int length = ( (buffer_[4] << 24) | (buffer_[5] << 16) | (buffer_[6] <<  8) | (buffer_[7]) );
00552   int current = 0;
00553   do
00554   {
00555     n_ = read (sockfd_, &buffer_[current], length-current);
00556     current += n_;
00557   } while (current < length);
00558 
00559   bufferlength_ = length;
00560   if ((verbose_ > 0) && (buffer_[0] != 0x20))
00561     ROS_DEBUG (">> Received: \"%s\"\n", buffer_);
00562 
00563   // Check for error
00564   if (strncmp ((const char*)buffer_, "sFA", 3) == 0)
00565   {
00566     strtok ((char*)buffer_, " ");
00567     ROS_DEBUG (">> E: Got an error message with code 0x%s\n", strtok (NULL, " "));
00568   }
00569 
00570   // Read checksum:
00571   char cs_read = 0;
00572   int ret = read (sockfd_, &cs_read, 1);
00573   if (ret < 1)
00574   {
00575     ROS_ERROR ("LMS400 didnt get any data in read %d",ret);
00576     return (-1);
00577   }
00578 
00579   if (buffer_[0] == 's')
00580     return (0);
00581   else if (buffer_[0] == 0x20)
00582     return (ReadResult ());
00583   else if (bufferlength_ > sizeof (MeasurementHeader_t))
00584   {
00585     if (verbose_ > 0)
00586       ROS_DEBUG (">>>> ReadResult: probably found a data packet!\n>>>>             %s\n", buffer_);
00587     // Don't throw away our precious measurement, queue it for later use :)
00588     unsigned char* tmp = (unsigned char*) malloc (bufferlength_ + 1);
00589     memcpy (tmp, buffer_, bufferlength_ + 1);
00590     MeasurementQueueElement_t q;
00591     q.string = tmp;
00592     q.length = bufferlength_;
00593     MeasurementQueue_->push_back (q);
00594     // and then, try to read what we actually wanted to read...
00595     return (ReadResult ());
00596   }
00597 
00598   return (0);
00599 }
00600 
00602 // Read an answer from the laser unit
00603 int
00604   sick_lms400::SickLMS400::ReadAnswer ()
00605 {
00606   return (ReadResult ());
00607 }
00608 
00610 // Read a confirmation and an answer from the laser unit
00611 int
00612   sick_lms400::SickLMS400::ReadConfirmationAndAnswer ()
00613 {
00614   ReadResult ();
00615   if (buffer_[0] == 's' && buffer_[1] == 'F' && buffer_[2] == 'A')
00616     return (-1);
00617   else
00618     return (ReadResult ());
00619 }
00620 
00622 // adds a header and the checksum to the command to be sent
00623 int
00624   sick_lms400::SickLMS400::AssembleCommand (unsigned char* cmd, int len)
00625 {
00626   unsigned char checksum = 0;
00627   int index = 0;
00628 
00629   command_[0]  = 0x02;  // Messages start with 4 STX's
00630   command_[1]  = 0x02;
00631   command_[2]  = 0x02;
00632   command_[3]  = 0x02;
00633   command_[4]  = (len >> 24) & 0xff; // then message length
00634   command_[5]  = (len >> 16) & 0xff;
00635   command_[6]  = (len >>  8) & 0xff;
00636   command_[7]  = (len      ) & 0xff;
00637 
00638   for (index = 0; index < len; index++)
00639   {
00640     command_[index + 8]  = cmd[index];
00641     checksum ^= cmd[index];
00642   }
00643   command_[8 + len] = checksum;
00644   command_[9 + len] = 0x00;
00645 
00646   commandlength_ = 9 + len;
00647   return (0);
00648 }


sick_lms400
Author(s): Radu Bogdan Rusu (rusu@cs.tum.edu), Dejan Pangercic (dejan.pangercic@cs.tum.edu)
autogenerated on Mon Oct 6 2014 09:36:31