SickLDMessage.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 #include <arpa/inet.h> 
00023 
00024 #include <sicktoolbox/SickLDMessage.hh>
00025 #include <sicktoolbox/SickLDUtility.hh> // for byye-order conversions where necessary
00026 
00027 /* Associate the namespace */
00028 namespace SickToolbox {
00029 
00033   SickLDMessage::SickLDMessage( ) :
00034     SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >()  {
00035 
00036     /* Initialize the object */
00037     Clear(); 
00038   }
00039   
00045   SickLDMessage::SickLDMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) :
00046     SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >()  {
00047 
00048     /* Build the message object (implicit initialization) */
00049     BuildMessage(payload_buffer,payload_length); 
00050   }
00051   
00056   SickLDMessage::SickLDMessage( const uint8_t * const message_buffer ) :
00057     SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >()  {
00058 
00059     /* Parse the message into the container (implicit initialization) */
00060     ParseMessage(message_buffer); 
00061   }
00062   
00068   void SickLDMessage::BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) {
00069 
00070     /* Call the parent method
00071      * NOTE: The parent method resets the object and assigns _message_length, _payload_length,
00072      *       _populated and copies the payload into the message buffer at the correct position
00073      */
00074     SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >
00075       ::BuildMessage(payload_buffer,payload_length);
00076     
00077     /*
00078      * Set the message header!
00079      */
00080     _message_buffer[0] = 0x02; // STX
00081     _message_buffer[1] = 'U';  // User
00082     _message_buffer[2] = 'S';  // Service
00083     _message_buffer[3] = 'P';  // Protocol
00084     
00085     /* Include the payload length in the header */
00086     uint32_t payload_length_32 = host_to_sick_ld_byte_order((uint32_t)_payload_length);
00087     memcpy(&_message_buffer[4],&payload_length_32,4);
00088     
00089     /*
00090      * Set the message trailer (just a checksum)!
00091      */
00092     _message_buffer[_message_length-1] = _computeXOR(&_message_buffer[8],(uint32_t)_payload_length);
00093   }
00094   
00099   void SickLDMessage::ParseMessage( const uint8_t * const message_buffer ) {
00100     
00101     /* Call the parent method
00102      * NOTE: This method resets the object and assigns _populated as true
00103      */
00104     SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >
00105       ::ParseMessage(message_buffer);
00106     
00107     /* Extract the payload length */
00108     uint32_t payload_length_32 = 0;
00109     memcpy(&payload_length_32,&message_buffer[4],4);
00110     _payload_length = (unsigned int)sick_ld_to_host_byte_order(payload_length_32);
00111 
00112     /* Compute the total message length */
00113     _message_length = MESSAGE_HEADER_LENGTH + MESSAGE_TRAILER_LENGTH + _payload_length;
00114     
00115     /* Copy the given packet into the buffer */
00116     memcpy(_message_buffer,message_buffer,_message_length);
00117   }
00118 
00122   void SickLDMessage::Print( ) const {
00123 
00124     std::cout.setf(std::ios::hex,std::ios::basefield);
00125     std::cout << "Checksum: " << (unsigned int) GetChecksum() << std::endl;  
00126     std::cout << "Service code: " << (unsigned int) GetServiceCode() << std::endl;
00127     std::cout << "Service subcode: " << (unsigned int) GetServiceSubcode() << std::endl;
00128     std::cout << std::flush;
00129 
00130     /* Call parent's print function */
00131     SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >::Print();    
00132   }
00133   
00139   uint8_t SickLDMessage::_computeXOR( const uint8_t * const data, const uint32_t length ) {
00140     
00141     /* Compute the XOR by summing all of the bytes */
00142     uint8_t checksum = 0;
00143     for (uint32_t i = 0; i < length; i++) {
00144       checksum ^= data[i]; // NOTE: this is equivalent to simply summing all of the bytes
00145     }
00146     
00147     /* done */
00148     return checksum;
00149   }
00150   
00151   SickLDMessage::~SickLDMessage( ) { }
00152   
00153 } /* namespace SickToolbox */


sicktoolbox
Author(s): Jason Derenick , Thomas Miller
autogenerated on Thu Aug 27 2015 15:17:16