SickPLSMessage.cc

Go to the documentation of this file.

/* Auto-generated header */
//#include "SickConfig.hh"

/* Implementation dependencies */
#include <iomanip>
#include <iostream>

#include "SickPLSMessage.hh"
#include "SickPLSUtility.hh"

/* Associate the namespace */
namespace SickToolbox
{

SickPLSMessage::SickPLSMessage( )
{

    /* Initialize the object */
    Clear();
}

SickPLSMessage::SickPLSMessage( const uint8_t dest_address, const uint8_t * const payload_buffer, const unsigned int payload_length ) :
    SickMessage< SICK_PLS_MSG_HEADER_LEN, SICK_PLS_MSG_PAYLOAD_MAX_LEN, SICK_PLS_MSG_TRAILER_LEN >()
{

    /* Build the message */
    BuildMessage(dest_address,payload_buffer,payload_length);
}

SickPLSMessage::SickPLSMessage( uint8_t * const message_buffer ) :
    SickMessage< SICK_PLS_MSG_HEADER_LEN, SICK_PLS_MSG_PAYLOAD_MAX_LEN, SICK_PLS_MSG_TRAILER_LEN >()
{

    /* Parse the byte sequence into a message object */
    ParseMessage(message_buffer);
}

void SickPLSMessage::BuildMessage( const uint8_t dest_address, 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_PLS_MSG_HEADER_LEN, SICK_PLS_MSG_PAYLOAD_MAX_LEN, SICK_PLS_MSG_TRAILER_LEN >
    ::BuildMessage(payload_buffer,payload_length);

    /*
     * Set the message header!
     */
    _message_buffer[0] = 0x02;         // Start of transmission (stx)
    _message_buffer[1] = dest_address; // Sick PLS address

    /* Include the payload length in the header */
    uint16_t payload_length_16 = host_to_sick_pls_byte_order((uint16_t)_payload_length);
    memcpy(&_message_buffer[2],&payload_length_16,2);

    /*
     * Set the message trailer!
     */
    _checksum = host_to_sick_pls_byte_order(_computeCRC(_message_buffer,_payload_length+4));
    memcpy(&_message_buffer[_payload_length+4],&_checksum,2);

}

void SickPLSMessage::ParseMessage( const uint8_t * const message_buffer )
{

    /* Call the parent method!
     * NOTE: This method resets the object and assigns _populated as true
     */
    SickMessage< SICK_PLS_MSG_HEADER_LEN, SICK_PLS_MSG_PAYLOAD_MAX_LEN, SICK_PLS_MSG_TRAILER_LEN >
    ::ParseMessage(message_buffer);

    /* Extract the payload length */
    uint16_t payload_length_16 = 0;
    memcpy(&payload_length_16,&message_buffer[2],2);
    _payload_length = (unsigned int)sick_pls_to_host_byte_order(payload_length_16);

    /* Compute the total message length */
    _message_length = MESSAGE_HEADER_LENGTH + MESSAGE_TRAILER_LENGTH + _payload_length;

    /* Copy the give message into the buffer */
    memcpy(_message_buffer, message_buffer,_message_length);

    /* Extract the checksum from the frame */
    memcpy(&_checksum,&_message_buffer[_payload_length+MESSAGE_HEADER_LENGTH],2);
    _checksum = sick_pls_to_host_byte_order(_checksum);

}

void SickPLSMessage::Clear( )
{

    /* Call the parent method and clear out class' protected members */
    SickMessage< SICK_PLS_MSG_HEADER_LEN, SICK_PLS_MSG_PAYLOAD_MAX_LEN, SICK_PLS_MSG_TRAILER_LEN >::Clear();

    /* Reset the class' additional fields */
    _checksum = 0;

}

void SickPLSMessage::Print( ) const
{

    std::cout.setf(std::ios::hex,std::ios::basefield);
    std::cout << "Checksum: " << (unsigned int) GetChecksum() << std::endl;
    std::cout << "Dest. Addr.: " << (unsigned int) GetDestAddress() << std::endl;
    std::cout << "Command Code: " << (unsigned int) GetCommandCode() << std::endl;
    std::cout << std::flush;

    /* Call parent's print function */
    SickMessage< SICK_PLS_MSG_HEADER_LEN, SICK_PLS_MSG_PAYLOAD_MAX_LEN, SICK_PLS_MSG_TRAILER_LEN >::Print();
}

uint16_t SickPLSMessage::_computeCRC( uint8_t * data, unsigned int data_length ) const
{

    uint16_t uCrc16;
    uint8_t abData[2];
    uCrc16 = abData[0] = 0;
    while (data_length-- )
    {
        abData[1] = abData[0];
        abData[0] = *data++;
        if(uCrc16 & 0x8000)
        {
            uCrc16 = (uCrc16 & 0x7fff) << 1;
            uCrc16 ^= CRC16_GEN_POL;
        }
        else
        {
            uCrc16 <<= 1;
        }
        uCrc16 ^= MKSHORT(abData[0],abData[1]);
    }
    return uCrc16;
}

SickPLSMessage::~SickPLSMessage( ) { }

} /* namespace SickToolbox */