SickLMS2xxMessage.cc
Go to the documentation of this file.
00001 
00016 /* Auto-generated header */
00017 #include <sicktoolbox/SickConfig.hh>
00018 
00019 /* Implementation dependencies */
00020 #include <iomanip>
00021 #include <iostream>
00022 
00023 #include <sicktoolbox/SickLMS2xxMessage.hh>
00024 #include <sicktoolbox/SickLMS2xxUtility.hh>
00025 
00026 /* Associate the namespace */
00027 namespace SickToolbox {
00028 
00032   SickLMS2xxMessage::SickLMS2xxMessage( ) {
00033 
00034     /* Initialize the object */
00035     Clear(); 
00036   }
00037 
00044   SickLMS2xxMessage::SickLMS2xxMessage( const uint8_t dest_address, const uint8_t * const payload_buffer, const unsigned int payload_length ) :
00045     SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >()  {
00046 
00047     /* Build the message */
00048     BuildMessage(dest_address,payload_buffer,payload_length);
00049   }
00050 
00055   SickLMS2xxMessage::SickLMS2xxMessage( uint8_t * const message_buffer ) :
00056     SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >()  {
00057 
00058     /* Parse the byte sequence into a message object */
00059     ParseMessage(message_buffer);
00060   }
00061 
00068   void SickLMS2xxMessage::BuildMessage( const uint8_t dest_address, const uint8_t * const payload_buffer,
00069                                      const unsigned int payload_length ) {
00070 
00071     /* Call the parent method!
00072      * NOTE: The parent method resets the object and assigns _message_length, _payload_length,
00073      *       _populated and copies the payload into the message buffer at the correct position
00074      */
00075     SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >
00076       ::BuildMessage(payload_buffer,payload_length);
00077 
00078     /*
00079      * Set the message header!
00080      */
00081     _message_buffer[0] = 0x02;         // Start of transmission (stx)
00082     _message_buffer[1] = dest_address; // Sick LMS address
00083         
00084     /* Include the payload length in the header */
00085     uint16_t payload_length_16 = host_to_sick_lms_2xx_byte_order((uint16_t)_payload_length);
00086     memcpy(&_message_buffer[2],&payload_length_16,2);
00087 
00088     /*
00089      * Set the message trailer!
00090      */    
00091     _checksum = host_to_sick_lms_2xx_byte_order(_computeCRC(_message_buffer,_payload_length+4));
00092     memcpy(&_message_buffer[_payload_length+4],&_checksum,2);
00093 
00094   }
00095 
00100   void SickLMS2xxMessage::ParseMessage( const uint8_t * const message_buffer ) {
00101 
00102     /* Call the parent method!
00103      * NOTE: This method resets the object and assigns _populated as true
00104      */
00105     SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >
00106       ::ParseMessage(message_buffer);
00107     
00108     /* Extract the payload length */
00109     uint16_t payload_length_16 = 0;
00110     memcpy(&payload_length_16,&message_buffer[2],2);
00111     _payload_length = (unsigned int)sick_lms_2xx_to_host_byte_order(payload_length_16);
00112 
00113     /* Compute the total message length */    
00114     _message_length = MESSAGE_HEADER_LENGTH + MESSAGE_TRAILER_LENGTH + _payload_length;
00115 
00116     /* Copy the give message into the buffer */
00117     memcpy(_message_buffer, message_buffer,_message_length);
00118 
00119     /* Extract the checksum from the frame */
00120     memcpy(&_checksum,&_message_buffer[_payload_length+MESSAGE_HEADER_LENGTH],2);
00121     _checksum = sick_lms_2xx_to_host_byte_order(_checksum);
00122 
00123   }
00124 
00128   void SickLMS2xxMessage::Clear( ) {    
00129 
00130     /* Call the parent method and clear out class' protected members */
00131     SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >::Clear();
00132 
00133     /* Reset the class' additional fields */
00134     _checksum = 0;
00135     
00136   }
00137   
00141   void SickLMS2xxMessage::Print( ) const {
00142     
00143     std::cout.setf(std::ios::hex,std::ios::basefield);
00144     std::cout << "Checksum: " << (unsigned int) GetChecksum() << std::endl;  
00145     std::cout << "Dest. Addr.: " << (unsigned int) GetDestAddress() << std::endl;
00146     std::cout << "Command Code: " << (unsigned int) GetCommandCode() << std::endl;
00147     std::cout << std::flush;
00148 
00149     /* Call parent's print function */
00150     SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >::Print();    
00151   }
00152   
00159   uint16_t SickLMS2xxMessage::_computeCRC( uint8_t * data, unsigned int data_length ) const {
00160   
00161     uint16_t uCrc16;
00162     uint8_t abData[2];
00163     uCrc16 = abData[0] = 0;
00164     while (data_length-- ) {
00165       abData[1] = abData[0];
00166       abData[0] = *data++;
00167       if(uCrc16 & 0x8000) {
00168         uCrc16 = (uCrc16 & 0x7fff) << 1;
00169         uCrc16 ^= CRC16_GEN_POL;
00170       }
00171       else {
00172         uCrc16 <<= 1;
00173       }
00174       uCrc16 ^= MKSHORT(abData[0],abData[1]);
00175     }
00176     return uCrc16;
00177   }
00178 
00179   SickLMS2xxMessage::~SickLMS2xxMessage( ) { }
00180 
00181 } /* namespace SickToolbox */


sicktoolbox
Author(s): Jason Derenick , Thomas Miller
autogenerated on Sun May 5 2019 02:28:23