SickLDMessage.cc

Go to the documentation of this file.

/* Auto-generated header */
#include <sicktoolbox/SickConfig.hh>

/* Implementation dependencies */
#include <iomanip>
#include <iostream>
#include <arpa/inet.h> 

#include <sicktoolbox/SickLDMessage.hh>
#include <sicktoolbox/SickLDUtility.hh> // for byye-order conversions where necessary

/* Associate the namespace */
namespace SickToolbox {

  SickLDMessage::SickLDMessage( ) :
    SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >()  {

    /* Initialize the object */
    Clear(); 
  }
  
  SickLDMessage::SickLDMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) :
    SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >()  {

    /* Build the message object (implicit initialization) */
    BuildMessage(payload_buffer,payload_length); 
  }
  
  SickLDMessage::SickLDMessage( const uint8_t * const message_buffer ) :
    SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >()  {

    /* Parse the message into the container (implicit initialization) */
    ParseMessage(message_buffer); 
  }
  
  void SickLDMessage::BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) {

    /* Call the parent method
     * NOTE: The parent method resets the object and assigns _message_length, _payload_length,
     *       _populated and copies the payload into the message buffer at the correct position
     */
    SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >
      ::BuildMessage(payload_buffer,payload_length);
    
    /*
     * Set the message header!
     */
    _message_buffer[0] = 0x02; // STX
    _message_buffer[1] = 'U';  // User
    _message_buffer[2] = 'S';  // Service
    _message_buffer[3] = 'P';  // Protocol
    
    /* Include the payload length in the header */
    uint32_t payload_length_32 = host_to_sick_ld_byte_order((uint32_t)_payload_length);
    memcpy(&_message_buffer[4],&payload_length_32,4);
    
    /*
     * Set the message trailer (just a checksum)!
     */
    _message_buffer[_message_length-1] = _computeXOR(&_message_buffer[8],(uint32_t)_payload_length);
  }
  
  void SickLDMessage::ParseMessage( const uint8_t * const message_buffer ) {
    
    /* Call the parent method
     * NOTE: This method resets the object and assigns _populated as true
     */
    SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >
      ::ParseMessage(message_buffer);
    
    /* Extract the payload length */
    uint32_t payload_length_32 = 0;
    memcpy(&payload_length_32,&message_buffer[4],4);
    _payload_length = (unsigned int)sick_ld_to_host_byte_order(payload_length_32);

    /* Compute the total message length */
    _message_length = MESSAGE_HEADER_LENGTH + MESSAGE_TRAILER_LENGTH + _payload_length;
    
    /* Copy the given packet into the buffer */
    memcpy(_message_buffer,message_buffer,_message_length);
  }

  void SickLDMessage::Print( ) const {

    std::cout.setf(std::ios::hex,std::ios::basefield);
    std::cout << "Checksum: " << (unsigned int) GetChecksum() << std::endl;  
    std::cout << "Service code: " << (unsigned int) GetServiceCode() << std::endl;
    std::cout << "Service subcode: " << (unsigned int) GetServiceSubcode() << std::endl;
    std::cout << std::flush;

    /* Call parent's print function */
    SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >::Print();    
  }
  
  uint8_t SickLDMessage::_computeXOR( const uint8_t * const data, const uint32_t length ) {
    
    /* Compute the XOR by summing all of the bytes */
    uint8_t checksum = 0;
    for (uint32_t i = 0; i < length; i++) {
      checksum ^= data[i]; // NOTE: this is equivalent to simply summing all of the bytes
    }
    
    /* done */
    return checksum;
  }
  
  SickLDMessage::~SickLDMessage( ) { }
  
} /* namespace SickToolbox */