cmtmessage.cpp

/*! \file Cmtmessage.cpp

        For information about objects in this file, see the appropriate header:
        \ref Cmtmessage.h

        \section FileCopyright Copyright Notice 
        Copyright (C) Xsens Technologies B.V., 2006.  All rights reserved.
        
        This source code is intended for use only by Xsens Technologies BV and
        those that have explicit written permission to use it from
        Xsens Technologies BV.
        
        THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
        KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
        IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
        PARTICULAR PURPOSE.

*/

#include "cmtmessage.h"

namespace xsens {

#ifdef _LOG_CMT_MSG
#define MSGLOG                CMTLOG
#else
#define MSGLOG(...)
#endif

#ifndef CMT_OUTPUTSETTINGS_DATAFORMAT_DOUBLE
#define CMT_OUTPUTSETTINGS_DATAFORMAT_DOUBLE                0x00000300
#endif

//////////////////////////////////////////////////////////////////////////////////////////
// Create a Message object with the given data length and message Id.
Message::Message(const uint8_t msgId, const uint16_t length, const uint16_t maxLength)
{
        if (maxLength < CMT_MAXMSGLEN)
                m_maxLength = CMT_MAXMSGLEN;
        else
                m_maxLength = maxLength;
        m_buffer = (MessageHeader*) new char[m_maxLength];//(MessageHeader*) malloc(m_maxLength);
        MSGLOG("Message(%02x, %hu, %hu): buffer = %p\n",(int32_t) msgId, length, m_maxLength,m_buffer);
        memset(m_buffer,0,m_maxLength);

        m_buffer->m_preamble = CMT_PREAMBLE;
        m_buffer->m_messageId = msgId;
        m_buffer->m_busId = CMT_BID_MASTER;

        if (length >= 255)
        {
                m_buffer->m_length = CMT_EXTLENCODE;

                m_buffer->m_datlen.m_extended.m_length.m_high = (uint8_t) (length >> 8);
                m_buffer->m_datlen.m_extended.m_length.m_low = (uint8_t) length;

                m_checksum = &(((uint8_t*) m_buffer)[length + CMT_LEN_MSGEXTHEADER]);
                m_checksum[0] = -(msgId + CMT_EXTLENCODE + (uint8_t)length + (length >> 8));
        }
        else
        {
                m_buffer->m_length = (uint8_t) length;
                m_checksum = &(((uint8_t*) m_buffer)[length + CMT_LEN_MSGHEADER]);
                m_checksum[0] = -(msgId + (uint8_t) length);
        }
        m_checksum[0] -= CMT_BID_MASTER;

        m_autoUpdateChecksum = true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Create a message from the given source string
Message::Message(const uint8_t* source, const uint16_t size, const uint16_t maxLength)
{
        const MessageHeader* tmp = (MessageHeader*) source;
        uint16_t length;

        if (maxLength < CMT_MAXMSGLEN)
                m_maxLength = CMT_MAXMSGLEN;
        else
                m_maxLength = maxLength;

        if (tmp->m_length == CMT_EXTLENCODE)
                length = ((uint16_t) tmp->m_datlen.m_extended.m_length.m_high * 256 + (uint16_t) tmp->m_datlen.m_extended.m_length.m_low) + CMT_LEN_MSGEXTHEADERCS;
        else
                length = tmp->m_length + CMT_LEN_MSGHEADERCS;
        if (size && size < length)
                length = size;

        if ((uint32_t) length > m_maxLength)
                m_maxLength = length;

        m_buffer = (MessageHeader*) new char[m_maxLength];//(MessageHeader*) malloc(m_maxLength);
        MSGLOG("Message(%02x%02x%02x%02x%02x%02x, %hu, %hu): buffer = %p\n",source[0],source[1],source[2],source[3],source[4],source[5], size, m_maxLength,m_buffer);
        if (length < m_maxLength)
                memset(&(((uint8_t*) m_buffer)[length]),0,m_maxLength-length);

        memcpy(m_buffer,source,length);

        m_checksum = &(((uint8_t*) m_buffer)[length-1]);

        m_autoUpdateChecksum = true;
}

Message::Message(const Message& src)
{
        m_maxLength = src.m_maxLength;
        m_buffer = (MessageHeader*) new char[m_maxLength];
        memcpy(m_buffer,src.m_buffer,m_maxLength);
        ptrdiff_t add = (uint8_t*) src.m_checksum - (uint8_t*) src.m_buffer;
        m_checksum = (uint8_t*) m_buffer + add;
        m_autoUpdateChecksum = true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Destroy a Message object.
Message::~Message()
{
        MSGLOG("~Message(): buffer = %p\n",m_buffer);
        LSTCHKDELNUL(m_buffer);//free(m_buffer);
}

//////////////////////////////////////////////////////////////////////////////////////////
// Clear all data in the message
void Message::clear(void)
{
        memset(m_buffer,0,m_maxLength);
        m_checksum = &(((uint8_t*) m_buffer)[CMT_LEN_MSGHEADER]);
        m_buffer->m_preamble = CMT_PREAMBLE;
        m_buffer->m_busId = CMT_BID_MASTER;
        m_checksum[0] = (uint8_t) (-CMT_BID_MASTER);
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the current value of the data as a double (64 bits).
double Message::getDataDouble(const uint16_t offset) const
{
        double ret;
        uint8_t* dest = (uint8_t*) &ret;
        uint8_t* src = &(getDataStart()[offset]);
        dest[0] = src[7];        dest[1] = src[6];        dest[2] = src[5];        dest[3] = src[4];
        dest[4] = src[3];        dest[5] = src[2];        dest[6] = src[1];        dest[7] = src[0];
        
        return ret;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the current value of the data as a float (32 bits).
float Message::getDataFloat(const uint16_t offset) const
{
        float ret;
        uint8_t* dest = (uint8_t*) &ret;
        uint8_t* src = &(getDataStart()[offset]);
        dest[0] = src[3];
        dest[1] = src[2];
        dest[2] = src[1];
        dest[3] = src[0];
        
        return ret;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the current value of the data as a double(64 bits), after converting it from F1220.
double Message::getDataF1220(const uint16_t offset) const
{
        double ret;
        int32_t tmp;
        uint8_t* dest = (uint8_t*) &tmp;
        uint8_t* src = &(getDataStart()[offset]);
        dest[0] = src[3];
        dest[1] = src[2];
        dest[2] = src[1];
        dest[3] = src[0];

        ret = ((double) tmp)/1048576.0;
        return ret;
}

// little endian
union Itypes {
        int64_t i64;
        struct {
                int32_t i1,i0;
        } i32;
        struct {
                int16_t s3,s2,s1,s0;
        } i16;
        struct {
                signed char b7,b6,b5,b4,b3,b2,b1,b0;
        } i8;

        double d;
        struct {
                float f1,f0;
        } f32;
};

//////////////////////////////////////////////////////////////////////////////////////////
// Return the current value of the data as a double(64 bits), after converting it from FP1632.
double Message::getDataFP1632(const uint16_t offset) const
{

        int16_t fpint;
        int32_t fpfrac;

        uint8_t* dest = (uint8_t*) &fpfrac;
        uint8_t* src = &(getDataStart()[offset]);
        dest[3] = *(src++);
        dest[2] = *(src++);
        dest[1] = *(src++);
        dest[0] = *(src++);

        dest = (uint8_t*) &fpint;
        dest[1] = *(src++);
        dest[0] = *(src++);

        Itypes fp;
        fp.i32.i0 = fpint;
        fp.i32.i1 = fpfrac;

        return (double) fp.i64 / 4294967296.0;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return current data value as a double(64 bits), after converting it from
// float, double, FP1632 or FP1220 depending on outputSettings
double Message::getDataFPValue(const uint64_t outputSettings, const uint16_t offset) const
{
        switch (outputSettings & CMT_OUTPUTSETTINGS_DATAFORMAT_MASK)
        {
                case CMT_OUTPUTSETTINGS_DATAFORMAT_FLOAT:
                        return getDataFloat(offset);

                case CMT_OUTPUTSETTINGS_DATAFORMAT_DOUBLE:
                        return getDataDouble(offset);

                case CMT_OUTPUTSETTINGS_DATAFORMAT_FP1632:
                        return getDataFP1632(offset);

                case CMT_OUTPUTSETTINGS_DATAFORMAT_F1220:
                        return getDataF1220(offset);
        }
        return 0.0; // should never happen
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return current data values as a double(64 bits), after converting it from
// float, double, FP1632 or FP1220 depending on outputSettings
void Message::getDataFPValue(double *dest, const uint64_t outputSettings, uint16_t offset, const int16_t numValues) const
{
        for (uint16_t i=0; i<numValues; i++) {
                switch (outputSettings & CMT_OUTPUTSETTINGS_DATAFORMAT_MASK)
                {
                        case CMT_OUTPUTSETTINGS_DATAFORMAT_FLOAT:
                                *dest++ = getDataFloat(offset);
                                offset += 4;
                                break;

                        case CMT_OUTPUTSETTINGS_DATAFORMAT_DOUBLE:
                                *dest++ = getDataDouble(offset);
                                offset += 8;
                                break;

                        case CMT_OUTPUTSETTINGS_DATAFORMAT_FP1632:
                                *dest++ = getDataFP1632(offset);
                                offset += 6;
                                break;

                        case CMT_OUTPUTSETTINGS_DATAFORMAT_F1220:
                                *dest++ = getDataF1220(offset);
                                offset += 4;
                }
        }
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the current value of the data as an uint32_t (32 bits).
uint32_t Message::getDataLong(const uint16_t offset) const
{
        uint32_t ret;
        uint8_t* dest = (uint8_t*) &ret;
        uint8_t* src = &(getDataStart()[offset]);
        dest[0] = src[3];
        dest[1] = src[2];
        dest[2] = src[1];
        dest[3] = src[0];
        
        return ret;
}
        
//////////////////////////////////////////////////////////////////////////////////////////
// Return the current value of the data as an uint16_t (16 bits).
uint16_t Message::getDataShort(const uint16_t offset) const
{
        uint16_t ret;
        uint8_t* dest = (uint8_t*) &ret;
        uint8_t* src = &(getDataStart()[offset]);
        dest[0] = src[1];
        dest[1] = src[0];
        
        return ret;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the length of the message buffer.
uint16_t Message::getDataSize(void) const
{
        if (m_buffer->m_length == 255)
                return ((uint16_t) m_buffer->m_datlen.m_extended.m_length.m_high * 256 + (uint16_t) m_buffer->m_datlen.m_extended.m_length.m_low);
        else
                return m_buffer->m_length;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Internal function to get the start of the data buffer.
uint8_t* Message::getDataStart(void) const
{
        if (m_buffer->m_length == 255)
        {
                return const_cast<uint8_t*>(m_buffer->m_datlen.m_extended.m_data);
        }
        else
                return const_cast<uint8_t*>(m_buffer->m_datlen.m_data);
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the length of the message buffer.
uint16_t Message::getTotalMessageSize(void) const
{
        if (m_buffer->m_length == 255)
                return ((uint16_t) m_buffer->m_datlen.m_extended.m_length.m_high * 256 + (uint16_t) m_buffer->m_datlen.m_extended.m_length.m_low) + CMT_LEN_MSGEXTHEADERCS;
        else
                return m_buffer->m_length + CMT_LEN_MSGHEADERCS;
}

//////////////////////////////////////////////////////////////////////////////////////////
bool Message::isChecksumOk(void) const
{
        if (getTotalMessageSize() > m_maxLength)
                return false;
        return calcChecksum() == m_checksum[0];
}

//////////////////////////////////////////////////////////////////////////////////////////
// Read the entire message from the given source string
XsensResultValue Message::loadFromString(const uint8_t* source, const uint16_t size)
{
        MSGLOG(__FUNCTION__ " size=%hu, first 7 bytes: %02x %02x %02x %02x %02x %02x %02x\n",size,source[0],source[1],source[2],source[3],source[4],source[5],source[6]);
        if (size > m_maxLength)
        {
                MSGLOG(__FUNCTION__ " returns XRV_PARAMINVALID\n");
                return XRV_PARAMINVALID;
        }
        memcpy(m_buffer,source,size);
        m_checksum = ((uint8_t*) m_buffer) + size-1;
        // check the size
        if (m_buffer->m_length == CMT_EXTLENCODE)
        {
                if (((uint16_t) m_buffer->m_datlen.m_extended.m_length.m_high * 256 + (uint16_t) m_buffer->m_datlen.m_extended.m_length.m_low) > size-CMT_LEN_MSGEXTHEADERCS)
                {
                        MSGLOG(__FUNCTION__ " returns XRV_DATACORRUPT (extended length)\n");
                        return XRV_DATACORRUPT;
                }
        }
        else
        {
                if (m_buffer->m_length > size-CMT_LEN_MSGHEADERCS)
                {
                        MSGLOG(__FUNCTION__ " returns XRV_DATACORRUPT (normal length)\n");
                        return XRV_DATACORRUPT;
                }
        }

        if (m_checksum[0] != calcChecksum())
        {
                MSGLOG(__FUNCTION__ " returns XRV_CHECKSUMFAULT\n");
                return XRV_CHECKSUMFAULT;
        }

        MSGLOG(__FUNCTION__ " returns XRV_OK\n");
        return XRV_OK;
}

//////////////////////////////////////////////////////////////////////////////////////////
void Message::resizeData(const uint16_t newSize)
{
        int32_t index, oldLength;

        MSGLOG("Message.resizeData(%hu): buffer = %p\n",newSize,m_buffer);

        if ((uint32_t) (newSize + CMT_LEN_MSGEXTHEADERCS) > m_maxLength)
        {
                uint16_t newLen = (uint16_t) (m_maxLength+m_maxLength);
                if ((newSize + CMT_LEN_MSGEXTHEADERCS) > newLen)
                        newLen = newSize + CMT_LEN_MSGEXTHEADERCS;
                m_buffer = (MessageHeader*) realloc(m_buffer,newLen);
                m_maxLength = newLen;
        }

        MSGLOG("Message.resizeData after realloc(%hu): buffer = %p\n",m_maxLength,m_buffer);

        if (newSize >= CMT_EXTLENCODE)
        {
                if (m_buffer->m_length < CMT_EXTLENCODE)
                {
                        // make extended, shift all data 2 bytes up
                        for (index = (int32_t) m_buffer->m_length; index >= 0; --index)
                                m_buffer->m_datlen.m_extended.m_data[index] =
                                                                m_buffer->m_datlen.m_data[index];
        
                        oldLength = m_buffer->m_length;
                        m_buffer->m_length = CMT_EXTLENCODE;
                }
                else
                {
                        oldLength = ((uint16_t) m_buffer->m_datlen.m_extended.m_length.m_high * 256 + (uint16_t) m_buffer->m_datlen.m_extended.m_length.m_low);
                }
                m_buffer->m_datlen.m_extended.m_length.m_high = (uint8_t) (newSize >> 8);
                m_buffer->m_datlen.m_extended.m_length.m_low = (uint8_t) newSize;

                for (index = oldLength; index < newSize; ++index)
                        m_buffer->m_datlen.m_extended.m_data[index] = 0;

                m_checksum = &m_buffer->m_datlen.m_extended.m_data[newSize];
        }
        else
        {
                if (m_buffer->m_length == CMT_EXTLENCODE)
                {
                        oldLength = ((uint16_t) m_buffer->m_datlen.m_extended.m_length.m_high * 256 + (uint16_t) m_buffer->m_datlen.m_extended.m_length.m_low);
                        // un-extend, shift all data 2 bytes down
                        for (index = 0; index < newSize; ++index)
                                m_buffer->m_datlen.m_data[index] =
                                                                m_buffer->m_datlen.m_extended.m_data[index];
                }
                else
                        oldLength = m_buffer->m_length;
                m_buffer->m_length = (uint8_t) newSize;
                for (index = oldLength; index < newSize; ++index)
                        m_buffer->m_datlen.m_data[index] = 0;
                m_checksum = &m_buffer->m_datlen.m_data[newSize];
        }
        if (m_autoUpdateChecksum)
                recomputeChecksum();
        MSGLOG("Message.resizeData end(%hu): buffer = %p\n",m_maxLength,m_buffer);
}

//////////////////////////////////////////////////////////////////////////////////////////
// Set the new value of the m_busId field and update the checksum.
void Message::setBusId(const uint8_t busId)
{
        if (m_autoUpdateChecksum)
                m_checksum[0] += m_buffer->m_busId - busId;
        m_buffer->m_busId = busId;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Write a string of bytes into the data buffer.
void Message::setDataBuffer(const uint8_t* data, const uint16_t offset,
                                                const uint16_t count)
{
        if (getDataSize() < offset+count)
                resizeData(offset+count);

        if (count > 0)
        {
                uint8_t * dest = &(getDataStart()[offset]);
                for (uint16_t i = 0;i < count;++i)
                {
                        if (m_autoUpdateChecksum)
                                m_checksum[0] += dest[i] - data[i];
                        dest[i] = data[i];
                }
        }
}

//////////////////////////////////////////////////////////////////////////////////////////
void Message::setDataByte(const uint8_t data, const uint16_t offset)
{
        if (getDataSize() < offset + 1)
                resizeData(offset+1);

        uint8_t* dest = &(getDataStart()[offset]);
        if (m_autoUpdateChecksum)
                m_checksum[0] += dest[0] - data;
        dest[0]        = data;
}

//////////////////////////////////////////////////////////////////////////////////////////
void Message::setDataDouble(const double data, const uint16_t offset)
{
        if (getDataSize() < offset+8)
                resizeData(offset+8);

        uint8_t* dest = &(getDataStart()[offset]);
        const uint8_t* cdata = (const uint8_t*) &data;
        if (m_autoUpdateChecksum)
                m_checksum[0] += dest[0]  + dest[1]  + dest[2]  + dest[3]
                                          +         dest[4]  + dest[5]  + dest[6]  + dest[7]
                                          -  cdata[0] - cdata[1] - cdata[2] - cdata[3]
                                          -  cdata[4] - cdata[5] - cdata[6] - cdata[7];

        const uint8_t* src = (const uint8_t*) &data;
        dest[0] = src[7];        dest[1] = src[6];        dest[2] = src[5];        dest[3] = src[4];
        dest[4] = src[3];        dest[5] = src[2];        dest[6] = src[1];        dest[7] = src[0];
}

//////////////////////////////////////////////////////////////////////////////////////////
void Message::setDataFloat(const float data, const uint16_t offset)
{
        if (getDataSize() < offset+4)
                resizeData(offset+4);

        uint8_t* dest = &(getDataStart()[offset]);
        const uint8_t* cdata = (const uint8_t*) &data;
        if (m_autoUpdateChecksum)
                m_checksum[0] += dest[0]  + dest[1]  + dest[2]  + dest[3]
                                          -  cdata[0] - cdata[1] - cdata[2] - cdata[3];

        const uint8_t* src = (const uint8_t*) &data;
        dest[0] = src[3];
        dest[1] = src[2];
        dest[2] = src[1];
        dest[3] = src[0];
}

//////////////////////////////////////////////////////////////////////////////////////////
void Message::setDataF1220(const double data, const uint16_t offset)
{
        int32_t val = (int32_t) (data*1048576.0);
        setDataLong(val,offset);
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the current value of the data as a double(64 bits), after converting it from F1220.
void Message::setDataFP1632(const double data, const uint16_t offset)
{
        if (getDataSize() < offset+6)
                resizeData(offset+6);

        Itypes fp;
        fp.d = data;
        int32_t dexp = ((fp.i32.i0 & (0x7fffffffL)) >> 20) - 1023;

        int16_t fpint;
        int32_t fpfrac;

        if (dexp <= 14)
        {
                fp.i16.s0 = (fp.i16.s0 & 0x000F) | 0x0010;
                if (data < 0)
                        fp.i64 = -fp.i64;
                if (dexp > -32)
                        fp.i64 = fp.i64 >> (20-dexp);        // 52-32 - exponent
                else
                        fp.i64 = fp.i64 >> 52;        // this could be optimized?
                fpint = fp.i16.s1;
                fpfrac = fp.i32.i1;
        }
        else
        {
                if (data < 0)
                {
                        fpint = ((int16_t) (0x8000));
                        fpfrac = 0;
                }
                else
                {
                        fpint = 0x7fff;
                        fpfrac = -1;
                }
        }

        uint8_t* src = (uint8_t*) &fpfrac;
        uint8_t* dest = &(getDataStart()[offset]);

        *(dest++) = src[3];
        *(dest++) = src[2];
        *(dest++) = src[1];
        *(dest++) = src[0];

        src = (uint8_t*) &fpint;
        *(dest++) = src[1];
        *(dest++) = src[0];
}

//////////////////////////////////////////////////////////////////////////////////////////
void Message::setDataFPValue(const uint64_t outputSettings, const double data, const uint16_t offset)
{
        switch (outputSettings & CMT_OUTPUTSETTINGS_DATAFORMAT_MASK)
        {
                case CMT_OUTPUTSETTINGS_DATAFORMAT_FLOAT:
                        return setDataFloat((const float)data, offset);

                case CMT_OUTPUTSETTINGS_DATAFORMAT_DOUBLE:
                        return setDataDouble(data, offset);

                case CMT_OUTPUTSETTINGS_DATAFORMAT_FP1632:
                        return setDataFP1632(data, offset);

                case CMT_OUTPUTSETTINGS_DATAFORMAT_F1220:
                        return setDataF1220(data, offset);
        }
}

//////////////////////////////////////////////////////////////////////////////////////////
void Message::setDataFPValue(const uint64_t outputSettings, const double *data, uint16_t offset, const uint16_t numValues)
{
        for (uint16_t i=0; i<numValues; i++) {
                switch (outputSettings & CMT_OUTPUTSETTINGS_DATAFORMAT_MASK)
                {
                        case CMT_OUTPUTSETTINGS_DATAFORMAT_FLOAT:
                                setDataFloat((float)data[i], offset);
                                offset += 4;
                                break;

                        case CMT_OUTPUTSETTINGS_DATAFORMAT_DOUBLE:
                                setDataDouble(data[i], offset);
                                offset += 8;
                                break;

                        case CMT_OUTPUTSETTINGS_DATAFORMAT_FP1632:
                                setDataFP1632(data[i], offset);
                                offset += 6;
                                break;

                        case CMT_OUTPUTSETTINGS_DATAFORMAT_F1220:
                                setDataF1220(data[i], offset);
                                offset += 4;
                }
        }
}

//////////////////////////////////////////////////////////////////////////////////////////
void Message::setDataLong(const uint32_t data, const uint16_t offset)
{
        if (getDataSize() < offset+4)
                resizeData(offset+4);

        uint8_t* dest = &(getDataStart()[offset]);
        if (m_autoUpdateChecksum)
                m_checksum[0] +=(uint8_t)(dest[0] + dest[1] + dest[2] + dest[3] - (data & 0xFF)-
                                                ((data >> 8) & 0xFF) - ((data >> 16) & 0xFF) - ((data >> 24) & 0xFF));

        const uint8_t* src = (const uint8_t*) &data;
        dest[0] = src[3];
        dest[1] = src[2];
        dest[2] = src[1];
        dest[3] = src[0];
}

//////////////////////////////////////////////////////////////////////////////////////////
void Message::setDataShort(const uint16_t data, const uint16_t offset)
{
        if (getDataSize() < offset+2)
                resizeData(offset+2);

        uint8_t* dest = &(getDataStart()[offset]);
        if (m_autoUpdateChecksum)
                m_checksum[0] += dest[0] + dest[1] - (data & 255) - (data >> 8);

        const uint8_t* src = (const uint8_t*) &data;
        dest[0] = src[1];
        dest[1] = src[0];
}

//////////////////////////////////////////////////////////////////////////////////////////
// Set the new value of the m_messageId field and update the checksum.
void Message::setMessageId(const uint8_t msgId)
{
        if (m_autoUpdateChecksum)
                m_checksum[0] += m_buffer->m_messageId - msgId;
        m_buffer->m_messageId =msgId;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Copy message src into this
void Message::operator = (const Message& src)
{
        if (m_maxLength != src.m_maxLength)
        {
                LSTCHKDELNUL(m_buffer);
                m_maxLength = src.m_maxLength;
                m_buffer = (MessageHeader*) new char[m_maxLength];
        }
        memcpy(m_buffer,src.m_buffer,m_maxLength);
        ptrdiff_t add = (uint8_t*) src.m_checksum - (uint8_t*) src.m_buffer;
        m_checksum = (uint8_t*) m_buffer + add;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Remove a number of bytes from the message (this will reduce the message size)
void Message::deleteData(uint16_t size, uint16_t offset)
{
        if (size == 0)
                return;
        uint16_t oldSize = getDataSize();
        uint16_t newSize;
        if (offset >= oldSize)
                return;

        if (oldSize > offset + size)
                newSize = oldSize - size;
        else
                newSize = offset;

        // shift all bytes after the offset down by size positions
        uint8_t* address = getDataBuffer();
        for (uint16_t i = offset; i < newSize; ++i)
                address[i] = address[i+size];

        // reduce the message length
        resizeData(newSize);
}

//////////////////////////////////////////////////////////////////////////////////////////
// Insert a number of bytes into the message (this will increase the message size)
void Message::insertData(uint16_t size, uint16_t offset)
{
        if (size == 0)
                return;
        // stretch the message
        uint16_t oldSize = getDataSize();
        resizeData(oldSize + size);
        // shift the bytes after the offset up by size positions
        uint8_t* address = getDataBuffer();
        for (uint16_t i = oldSize-1; i >= offset && i < oldSize; --i)
                address[i+size] = address[i];
}

//////////////////////////////////////////////////////////////////////////////////////////
// Compute the checksum of the given byte string.
uint8_t computeChecksum(const uint8_t* buffer, uint32_t length)
{
        uint8_t cs = 0;
        while (length--)
                cs -= *(buffer++);

        return cs;
}

} // end of xsens namespace