xsmessage.c

Go to the documentation of this file.

 
//  Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
//  All rights reserved.
//  
//  Redistribution and use in source and binary forms, with or without modification,
//  are permitted provided that the following conditions are met:
//  
//  1.  Redistributions of source code must retain the above copyright notice,
//      this list of conditions, and the following disclaimer.
//  
//  2.  Redistributions in binary form must reproduce the above copyright notice,
//      this list of conditions, and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//  
//  3.  Neither the names of the copyright holders nor the names of their contributors
//      may be used to endorse or promote products derived from this software without
//      specific prior written permission.
//  
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
//  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
//  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
//  THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
//  SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
//  OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
//  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR
//  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS 
//  SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES 
//  OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE 
//  ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
//  
 
 
//  Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
//  All rights reserved.
//  
//  Redistribution and use in source and binary forms, with or without modification,
//  are permitted provided that the following conditions are met:
//  
//  1.  Redistributions of source code must retain the above copyright notice,
//      this list of conditions, and the following disclaimer.
//  
//  2.  Redistributions in binary form must reproduce the above copyright notice,
//      this list of conditions, and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//  
//  3.  Neither the names of the copyright holders nor the names of their contributors
//      may be used to endorse or promote products derived from this software without
//      specific prior written permission.
//  
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
//  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
//  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
//  THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
//  SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
//  OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
//  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR
//  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS 
//  SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES 
//  OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE 
//  ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
//  
 
#include "xsmessage.h"
#include <stdlib.h>
#include <memory.h>             // memset
#include "xsbusid.h"
#include <stdio.h>
 
// Field message indices
#define XS_IND_PREAMBLE                         0
#define XS_IND_BID                                              1
#define XS_IND_MID                                              2
#define XS_IND_LEN                                              3
#define XS_IND_DATA0                                    4
#define XS_IND_LENEXTH                                  4
#define XS_IND_LENEXTL                                  5
#define XS_IND_DATAEXT0                         6
 
#define XS_SELFTEST_OK                                  0x1FF
 
// message data lengths
#define XS_LEN_TRANSPORTMODE                    1
#define XS_LEN_DEVICEID                         4
#define XS_LEN_INITBUSRESULTS                   4
#define XS_LEN_PERIOD                                   2
#define XS_LEN_BUSPWR                                   2
#define XS_LEN_DATALENGTH                               2
#define XS_LEN_CONFIGURATION                    118
#define XS_LEN_FIRMWAREREV                              3
#define XS_LEN_BTDISABLE                                1
#define XS_LEN_OPMODE                                   1
#define XS_LEN_BAUDRATE                         1
#define XS_LEN_SYNCMODE                         1
#define XS_LEN_PRODUCTCODE                              20
#define XS_LEN_PROCESSINGFLAGS                  2
#define XS_LEN_XMPWROFF                         0
#define XS_LEN_OUTPUTMODE                               2
#define XS_LEN_OUTPUTSETTINGS                   4
#define XS_LEN_OUTPUTSKIPFACTOR         2
#define XS_LEN_SYNCINMODE                               2
#define XS_LEN_SYNCINSKIPFACTOR         2
#define XS_LEN_SYNCINOFFSET                     4
#define XS_LEN_SYNCOUTMODE                              2
#define XS_LEN_SYNCOUTSKIPFACTOR                2
#define XS_LEN_SYNCOUTOFFSET                    4
#define XS_LEN_SYNCOUTPULSEWIDTH                4
#define XS_LEN_ERRORMODE                                2
#define XS_LEN_TRANSMITDELAY                    2
#define XS_LEN_OBJECTALIGNMENT                  36
#define XS_LEN_XMERRORMODE                              2
#define XS_LEN_BUFFERSIZE                               2
#define XS_LEN_HEADING                                  4
#define XS_LEN_MAGNETICFIELD                    12
#define XS_LEN_LOCATIONID                               2
#define XS_LEN_EXTOUTPUTMODE                    2
#define XS_LEN_INITTRACKMODE                    2
#define XS_LEN_STOREFILTERSTATE                 0
#define XS_LEN_UTCTIME                                  12
#define XS_LEN_FILTERPROFILELABEL                       20
#define XS_LEN_FILTERPROFILEFULL                        (1+1+XS_LEN_FILTERPROFILELABEL)
#define XS_LEN_AVAILABLEFILTERPROFILES          (XS_MAX_FILTERPROFILES_IN_MT*XS_LEN_FILTERPROFILEFULL)
#define XS_LEN_REQFILTERPROFILEACK                      2
#define XS_LEN_SETFILTERPROFILE                         2
#define XS_LEN_GRAVITYMAGNITUDE         4
#define XS_LEN_GNSSLEVERARM                             12
#define XS_LEN_LATLONALT                                18
#define XS_LEN_SETNOROTATION                    2
#define XS_LEN_FILTERSETTINGS                   4
#define XS_LEN_AMD                                              2
#define XS_LEN_RESETORIENTATION         2
#define XS_LEN_GNSSSTATUS                               (1+5*16)
#define XS_LEN_CLIENTUSAGE                              1
#define XS_LEN_CLIENTPRIORITY                   1
#define XS_LEN_WIRELESSCONFIG                   6
#define XS_LEN_INFOREQUEST                              1
#define XS_LEN_SETOUTPUTTRIGGER         10
#define XS_LEN_SETINPUTTRIGGER                  10
 
// MTData defines
// Length of data blocks in bytes
#define XS_LEN_RAWDATA                                  20
#define XS_LEN_CALIBDATA                                36
#define XS_LEN_CALIB_ACCDATA                    12
#define XS_LEN_CALIB_GYRDATA                    12
#define XS_LEN_CALIB_MAGDATA                    12
#define XS_LEN_ORIENT_QUATDATA                  16
#define XS_LEN_ORIENT_EULERDATA         12
#define XS_LEN_ORIENT_MATRIXSTA         36
#define XS_LEN_SAMPLECNT                                2
#define XS_LEN_TEMPDATA                         4
 
// Length of data blocks in floats
#define XS_LEN_CALIBDATA_FLT                    9
#define XS_LEN_TEMPDATA_FLT                     1
#define XS_LEN_ORIENT_QUATDATA_FLT              4
#define XS_LEN_ORIENT_EULERDATA_FLT     3
#define XS_LEN_ORIENT_MATRIXSTA_FLT     9
 
#pragma pack(push, 1)
// 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;
};
#pragma pack(pop)
 
 
typedef union Itypes Itypes;
 
uint8_t byteSum(const uint8_t* buffer, XsSize count)
{
        register uint8_t sum = 0;
        for (; count; --count, sum += *(buffer++));
        return sum;
}
 
static inline uint8_t* XsMessage_dataAtOffset(XsMessage* thisPtr, XsSize offset)
{
        XsMessageHeader* hdr;
 
        assert(thisPtr->m_message.m_data);
        assert(offset < XsMessage_dataSize(thisPtr));
 
        hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data;
        if (hdr->m_length != 255)
                return hdr->m_datlen.m_data + offset;
        return hdr->m_datlen.m_extended.m_data + offset;
}
 
static inline const uint8_t* XsMessage_cdataAtOffset(XsMessage const* thisPtr, XsSize offset)
{
        return XsMessage_dataAtOffset((XsMessage*)thisPtr, offset);
}
 
static inline void XsMessage_ensureDataSize(XsMessage* thisPtr, XsSize offset, XsSize sizeofValue)
{
        if (XsMessage_dataSize(thisPtr) < offset + sizeofValue)
                XsMessage_resizeData(thisPtr, offset + sizeofValue);
}
 
static inline void XsMessage_updateChecksumWithValue(XsMessage* thisPtr, const void* value, XsSize sizeofValue, XsSize offset)
{
        if (thisPtr->m_autoUpdateChecksum)
        {
                thisPtr->m_checksum[0] += byteSum(XsMessage_getDataBuffer(thisPtr, offset), sizeofValue);
                thisPtr->m_checksum[0] -= byteSum(value, sizeofValue);
        }
}
 
static inline void swapEndian(void* data, const XsSize size)
{
        switch (size)
        {
                case sizeof(char):
                        break;
                case sizeof(uint16_t):
                {
                        uint16_t i16;
                        memcpy((void*)&i16, data, sizeof(uint16_t));
                        i16 = swapEndian16(i16);
                        memcpy(data, (void*)&i16, sizeof(uint16_t));
                        break;
                }
                case sizeof(uint32_t):
                {
                        uint32_t i32;
                        memcpy((void*)&i32, data, sizeof(uint32_t));
                        i32 = swapEndian32(i32);
                        memcpy(data, (void*)&i32, sizeof(uint32_t));
                        break;
                }
                case sizeof(uint64_t):
                {
                        uint64_t i64;
                        memcpy((void*)&i64, data, sizeof(uint64_t));
                        i64 = swapEndian64(i64);
                        memcpy(data, (void*)&i64, sizeof(uint64_t));
                        break;
                }
                default:
                        assert(0);
        }
}
 
void XsMessage_getEndianCorrectData(XsMessage const* thisPtr, void* value, XsSize size, XsSize offset)
{
        memcpy(value, (void const*) XsMessage_cdataAtOffset(thisPtr, offset), size);
        swapEndian(value, size);
}
 
void XsMessage_setEndianCorrectData(XsMessage* thisPtr, void const* value, XsSize size, XsSize offset)
{
        void* dest;
        XsMessage_ensureDataSize(thisPtr, offset, size);
        XsMessage_updateChecksumWithValue(thisPtr, value, size, offset);
        dest = XsMessage_dataAtOffset(thisPtr, offset);
        memcpy(dest, value, size);
        swapEndian(dest, size);
}
 
void XsMessage_constructSized(XsMessage* thisPtr, XsSize dataSize)
{
        XsSize msgSize;
        XsMessageHeader* hdr;
 
        if (dataSize < 255)
                msgSize = dataSize + 5;
        else
                msgSize = dataSize + 7;
 
        XsByteArray_construct(&thisPtr->m_message, msgSize, 0);
        memset(thisPtr->m_message.m_data, 0, msgSize);
        hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data;
        hdr->m_preamble = XS_PREAMBLE;
        hdr->m_messageId = 0;
        hdr->m_busId = XS_BID_MASTER;
 
        if (dataSize < XS_EXTLENCODE)
        {
                hdr->m_length = (uint8_t) dataSize;
                *((uint8_t**) &thisPtr->m_checksum) = &hdr->m_datlen.m_data[dataSize];
                thisPtr->m_checksum[0] = (uint8_t) - (int8_t)(uint8_t)dataSize;
        }
        else
        {
                hdr->m_length = XS_EXTLENCODE;
                hdr->m_datlen.m_extended.m_length.m_high = (uint8_t)(dataSize >> 8);
                hdr->m_datlen.m_extended.m_length.m_low = (uint8_t) dataSize;
                *((uint8_t**) &thisPtr->m_checksum) = &hdr->m_datlen.m_extended.m_data[dataSize];
                thisPtr->m_checksum[0] = (uint8_t) - (hdr->m_datlen.m_extended.m_length.m_high + hdr->m_datlen.m_extended.m_length.m_low + XS_EXTLENCODE);
        }
        thisPtr->m_checksum[0] -= hdr->m_busId;
}
 
void XsMessage_construct(XsMessage* thisPtr)
{
        XsMessage_constructSized(thisPtr, 0);
}
 
void XsMessage_copyConstruct(XsMessage* thisPtr, XsMessage const* src)
{
        if (!src)
                XsMessage_construct(thisPtr);
        else
        {
                XsMessageHeader* hdr;
                XsSize dataSize;
                XsArray_copyConstruct(&thisPtr->m_message, &src->m_message);
                thisPtr->m_autoUpdateChecksum = src->m_autoUpdateChecksum;
                hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data;
                dataSize = XsMessage_dataSize(thisPtr);
                if (dataSize >= 255)
                        *((uint8_t**) &thisPtr->m_checksum) = &hdr->m_datlen.m_extended.m_data[dataSize];
                else
                        *((uint8_t**) &thisPtr->m_checksum) = &hdr->m_datlen.m_data[dataSize];
        }
}
 
void XsMessage_assign(XsMessage* thisPtr, XsSize dataSize)
{
        XsMessage_destruct(thisPtr);
        XsMessage_constructSized(thisPtr, dataSize);
}
 
void XsMessage_load(XsMessage* thisPtr, XsSize msgSize, unsigned char const* src)
{
        XsByteArray_construct(&thisPtr->m_message, msgSize, src);
        *((uint8_t**) &thisPtr->m_checksum) = &thisPtr->m_message.m_data[XsMessage_getTotalMessageSize(thisPtr) - 1];
}
 
void XsMessage_destruct(XsMessage* thisPtr)
{
        XsArray_destruct(&thisPtr->m_message);
        *((uint8_t**) &thisPtr->m_checksum) = 0;
}
 
void XsMessage_copy(XsMessage* copy, XsMessage const* thisPtr)
{
        XsArray_copy(&copy->m_message, &thisPtr->m_message);
        *((uint8_t**) &copy->m_checksum) = &copy->m_message.m_data[XsMessage_getTotalMessageSize(copy) - 1];
        copy->m_autoUpdateChecksum = thisPtr->m_autoUpdateChecksum;
}
 
XsSize XsMessage_dataSize(XsMessage const* thisPtr)
{
        XsMessageHeader const* hdr;
 
        if (!thisPtr->m_message.m_data)
                return 0;
 
        hdr = (XsMessageHeader const*)(void const*) thisPtr->m_message.m_data;
        if (hdr->m_length == 255)
                return (((XsSize) hdr->m_datlen.m_extended.m_length.m_high) << 8) + hdr->m_datlen.m_extended.m_length.m_low;
        else
                return ((XsSize) hdr->m_length);
}
 
const uint8_t* XsMessage_constData(XsMessage const* thisPtr, XsSize offset)
{
        if (!thisPtr->m_message.m_data)
                return 0;
        return XsMessage_cdataAtOffset(thisPtr, offset);
}
 
const uint8_t* XsMessage_getMessageStart(XsMessage const* thisPtr)
{
        return thisPtr->m_message.m_data;
}
 
XsSize XsMessage_getTotalMessageSize(XsMessage const* thisPtr)
{
        XsMessageHeader* hdr;
 
        if (!thisPtr->m_message.m_data)
                return 0;
 
        hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data;
        if (hdr->m_length == 255)
                return (((XsSize) hdr->m_datlen.m_extended.m_length.m_high) << 8) + hdr->m_datlen.m_extended.m_length.m_low + 7;
        else
                return ((XsSize) hdr->m_length) + 5;
}
 
uint8_t XsMessage_getDataByte(XsMessage const* thisPtr, XsSize offset)
{
        return *XsMessage_cdataAtOffset(thisPtr, offset);
}
 
uint16_t XsMessage_getDataShort(XsMessage const* thisPtr, XsSize offset)
{
        uint16_t ret;
        XsMessage_getEndianCorrectData(thisPtr, &ret, sizeof(ret), offset);
        return ret;
}
 
uint32_t XsMessage_getDataLong(XsMessage const* thisPtr, XsSize offset)
{
        uint32_t ret;
        XsMessage_getEndianCorrectData(thisPtr, &ret, sizeof(ret), offset);
        return ret;
}
 
uint64_t XsMessage_getDataLongLong(XsMessage const* thisPtr, XsSize offset)
{
        uint64_t ret;
        XsMessage_getEndianCorrectData(thisPtr, &ret, sizeof(ret), offset);
        return ret;
}
 
float XsMessage_getDataFloat(XsMessage const* thisPtr, XsSize offset)
{
        float ret;
        XsMessage_getEndianCorrectData(thisPtr, &ret, sizeof(ret), offset);
        return ret;
}
 
double XsMessage_getDataDouble(XsMessage const* thisPtr, XsSize offset)
{
        double ret;
        XsMessage_getEndianCorrectData(thisPtr, &ret, sizeof(ret), offset);
        return ret;
}
 
double XsMessage_getDataF1220(XsMessage const* thisPtr, XsSize offset)
{
        int32_t tmp;
        Itypes rv;
        tmp = (int32_t) XsMessage_getDataLong(thisPtr, offset);
 
        rv.d = ((double) tmp) / 1048576.0;
        rv.i64 = (rv.i64 & ~1LL) | (tmp & 1);
        return rv.d;
}
 
double XsMessage_getDataFP1632(XsMessage const* thisPtr, XsSize offset)
{
        int16_t fpint;
        int32_t fpfrac;
        Itypes fp, rv;
 
        fpfrac = (int32_t) XsMessage_getDataLong(thisPtr, offset);
        fpint = (int16_t) XsMessage_getDataShort(thisPtr, offset + 4);
 
        fp.i32.i0 = fpint;
        fp.i32.i1 = fpfrac;
 
        rv.d = (double) fp.i64 / 4294967296.0;
        rv.i64 = (rv.i64 & ~1LL) | (fpfrac & 1);
        return rv.d;
}
 
const uint8_t* XsMessage_getDataBuffer(XsMessage const* thisPtr, XsSize offset)
{
        return XsMessage_constData(thisPtr, offset);
}
 
void XsMessage_setDataByte(XsMessage* thisPtr, uint8_t value, XsSize offset)
{
        XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset);
}
 
void XsMessage_setDataShort(XsMessage* thisPtr, uint16_t value, XsSize offset)
{
        XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset);
}
 
void XsMessage_setDataLong(XsMessage* thisPtr, uint32_t value, XsSize offset)
{
        XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset);
}
 
void XsMessage_setDataLongLong(XsMessage* thisPtr, uint64_t value, XsSize offset)
{
        XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset);
}
 
 
void XsMessage_setDataFloat(XsMessage* thisPtr, float value, XsSize offset)
{
        XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset);
}
 
void XsMessage_setDataDouble(XsMessage* thisPtr, double value, XsSize offset)
{
        XsMessage_setEndianCorrectData(thisPtr, &value, sizeof(value), offset);
}
 
void XsMessage_setDataF1220(XsMessage* thisPtr, double value, XsSize offset)
{
        Itypes fp;
        uint32_t val;
 
        fp.d = value;
        val = (uint32_t)(int32_t)(value * 1048576.0);
 
        XsMessage_setDataLong(thisPtr, (val & ~1UL) | (fp.i64 & 1), offset);
}
 
void XsMessage_setDataFP1632(XsMessage* thisPtr, double value, XsSize offset)
{
        Itypes fp;
        int16_t fpint;
        int32_t fpfrac;
        int32_t dexp;
        uint32_t b;
 
        fp.d = value;
        b = (uint32_t)(fp.i64 & 1);
        dexp = ((fp.i32.i0 & (0x7fffffffL)) >> 20) - 1023;
 
        if (dexp <= 14)
        {
                fp.i16.s0 = (fp.i16.s0 & 0x000F) | 0x0010;
                if (value < 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 (value < 0)
                {
                        fpint = ((int16_t)(uint16_t)(0x8000));
                        fpfrac = 0;
                }
                else
                {
                        fpint = 0x7fff;
                        fpfrac = -1;
                }
        }
 
        XsMessage_setDataLong(thisPtr, (fpfrac & ~1L) | b, offset);
        XsMessage_setDataShort(thisPtr, (uint16_t) fpint, offset + (XsSize)4);
}
 
void XsMessage_setDataBuffer(XsMessage* thisPtr, const uint8_t* buffer, XsSize size, XsSize offset)
{
        XsMessage_ensureDataSize(thisPtr, offset, size);
        XsMessage_updateChecksumWithValue(thisPtr, buffer, size, offset);
        memcpy(XsMessage_dataAtOffset(thisPtr, offset), buffer, size);
}
 
uint8_t XsMessage_getFPValueSize(XsDataIdentifier id)
{
        switch (id & XDI_SubFormatMask)
        {
                case XDI_SubFormatFloat:
                        return 4;
 
                case XDI_SubFormatDouble:
                        return 8;
 
                case XDI_SubFormatFp1632:
                        return 6;
 
                case XDI_SubFormatFp1220:
                        return 4;
 
                default:
                        return 0;
        }
}
 
static double convertFromFloat(float f)
{
        Itypes rv, tmp;
        tmp.f32.f0 = f;
        rv.d = (double) f;
        rv.i64 = (rv.i64 & ~1LL) | (tmp.i32.i0 & 1);
        return rv.d;
}
 
static float convertToFloat(double d)
{
        Itypes rv, tmp;
        tmp.d = d;
        rv.f32.f0 = (float) d;
        rv.i32.i0 = (rv.i32.i0 & ~1L) | (tmp.i64 & 1);
        return rv.f32.f0;
}
 
void XsMessage_getDataFPValuesById(XsMessage const* thisPtr, XsDataIdentifier dataIdentifier, double* dest, XsSize offset, XsSize numValues)
{
        XsSize i;
        for (i = 0; i < numValues; i++)
        {
                switch (dataIdentifier & XDI_SubFormatMask)
                {
                        case XDI_SubFormatFloat:
                                *dest++ = convertFromFloat(XsMessage_getDataFloat(thisPtr, offset));
                                offset += 4;
                                break;
 
                        case XDI_SubFormatDouble:
                                *dest++ = XsMessage_getDataDouble(thisPtr, offset);
                                offset += 8;
                                break;
 
                        case XDI_SubFormatFp1632:
                                *dest++ = XsMessage_getDataFP1632(thisPtr, offset);
                                offset += 6;
                                break;
 
                        case XDI_SubFormatFp1220:
                                *dest++ = XsMessage_getDataF1220(thisPtr, offset);
                                offset += 4;
                                break;
 
                        default:
                                *dest++ = 0;
                                break;
                }
        }
}
 
void XsMessage_setDataFPValuesById(XsMessage* thisPtr, XsDataIdentifier dataIdentifier, double const* data, XsSize offset, XsSize numValues)
{
        XsSize i;
        for (i = 0; i < numValues; i++)
        {
                switch (dataIdentifier & XDI_SubFormatMask)
                {
                        case XDI_SubFormatFloat:
                                XsMessage_setDataFloat(thisPtr, convertToFloat(data[i]), offset);
                                offset += 4;
                                break;
 
                        case XDI_SubFormatDouble:
                                XsMessage_setDataDouble(thisPtr, data[i], offset);
                                offset += 8;
                                break;
 
                        case XDI_SubFormatFp1632:
                                XsMessage_setDataFP1632(thisPtr, data[i], offset);
                                offset += 6;
                                break;
 
                        case XDI_SubFormatFp1220:
                                XsMessage_setDataF1220(thisPtr, data[i], offset);
                                offset += 4;
                                break;
 
                        default:
                                break;
                }
        }
}
 
void XsMessage_getDataRealValuesById(XsMessage const* thisPtr, XsDataIdentifier dataIdentifier, XsReal* dest, XsSize offset, XsSize numValues)
{
        XsSize i;
        for (i = 0; i < numValues; i++)
        {
                switch (dataIdentifier & XDI_SubFormatMask)
                {
                        case XDI_SubFormatFloat:
#ifdef XSENS_SINGLE_PRECISION
                                *dest++ = XsMessage_getDataFloat(thisPtr, offset);
#else
                                *dest++ = convertFromFloat(XsMessage_getDataFloat(thisPtr, offset));
#endif
                                offset += 4;
                                break;
 
                        case XDI_SubFormatDouble:
#ifdef XSENS_SINGLE_PRECISION
                                *dest++ = convertToFloat(XsMessage_getDataDouble(thisPtr, offset));
#else
                                *dest++ = XsMessage_getDataDouble(thisPtr, offset);
#endif
                                offset += 8;
                                break;
 
                        case XDI_SubFormatFp1632:
#ifdef XSENS_SINGLE_PRECISION
                                *dest++ = convertToFloat(XsMessage_getDataFP1632(thisPtr, offset));
#else
                                *dest++ = XsMessage_getDataFP1632(thisPtr, offset);
#endif
                                offset += 6;
                                break;
 
                        case XDI_SubFormatFp1220:
#ifdef XSENS_SINGLE_PRECISION
                                *dest++ = convertToFloat(XsMessage_getDataF1220(thisPtr, offset));
#else
                                *dest++ = XsMessage_getDataF1220(thisPtr, offset);
#endif
                                offset += 4;
                                break;
 
                        default:
                                *dest++ = 0;
                                break;
                }
        }
}
 
void XsMessage_setDataRealValuesById(XsMessage* thisPtr, XsDataIdentifier dataIdentifier, XsReal const* data, XsSize offset, XsSize numValues)
{
        XsSize i;
        for (i = 0; i < numValues; i++)
        {
                switch (dataIdentifier & XDI_SubFormatMask)
                {
                        case XDI_SubFormatFloat:
                                XsMessage_setDataFloat(thisPtr, convertToFloat(data[i]), offset);
                                offset += 4;
                                break;
 
                        case XDI_SubFormatDouble:
                                XsMessage_setDataDouble(thisPtr, data[i], offset);
                                offset += 8;
                                break;
 
                        case XDI_SubFormatFp1632:
                                XsMessage_setDataFP1632(thisPtr, data[i], offset);
                                offset += 6;
                                break;
 
                        case XDI_SubFormatFp1220:
                                XsMessage_setDataF1220(thisPtr, data[i], offset);
                                offset += 4;
                                break;
 
                        default:
                                break;
                }
        }
}
 
uint8_t XsMessage_computeChecksum(XsMessage const* thisPtr)
{
        XsSize i, msgSize;
        uint8_t cs = 0;
 
        msgSize = XsMessage_getTotalMessageSize(thisPtr) - 1;
        for (i = 1; i < msgSize; ++i)
                cs -= thisPtr->m_message.m_data[i];
 
        return cs;
}
 
void XsMessage_recomputeChecksum(XsMessage* thisPtr)
{
        assert(thisPtr->m_checksum);
        thisPtr->m_checksum[0] = XsMessage_computeChecksum(thisPtr);
}
 
int XsMessage_isChecksumOk(XsMessage const* thisPtr)
{
        assert(thisPtr->m_checksum);
        return thisPtr->m_checksum[0] == XsMessage_computeChecksum(thisPtr);
}
 
XsMessageHeader* XsMessage_getHeader(XsMessage* thisPtr)
{
        return (XsMessageHeader*)(void*) thisPtr->m_message.m_data;
}
 
const XsMessageHeader* XsMessage_getConstHeader(XsMessage const* thisPtr)
{
        return (const XsMessageHeader*)(void*) thisPtr->m_message.m_data;
}
 
int XsMessage_empty(XsMessage const* thisPtr)
{
        XsMessageHeader* hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data;
        if (!hdr)
                return 1;
        return (hdr->m_messageId == 0 && hdr->m_busId == XS_BID_MASTER);
}
 
void XsMessage_resizeData(XsMessage* thisPtr, XsSize newSize)
{
        XsSize index, oldSize;
        XsByteArray old = XSBYTEARRAY_INITIALIZER;
        XsMessageHeader* oldHdr, *newHdr;
        uint8_t* oldData, *newData;
 
        oldSize = XsMessage_dataSize(thisPtr);
        if (oldSize == newSize)
                return;
 
        XsArray_swap(&thisPtr->m_message, &old);
        XsMessage_constructSized(thisPtr, newSize);
 
        newHdr = (XsMessageHeader*) thisPtr->m_message.m_data;
        oldHdr = (XsMessageHeader*) old.m_data;
        if (!oldHdr)            // our original message may have been empty / uninitialized
                return;
 
        if (thisPtr->m_autoUpdateChecksum)
                thisPtr->m_checksum[0] += newHdr->m_busId;
 
        newHdr->m_busId = oldHdr->m_busId;
        newHdr->m_preamble = oldHdr->m_preamble;
        newHdr->m_messageId = oldHdr->m_messageId;
 
        if (thisPtr->m_autoUpdateChecksum)
                thisPtr->m_checksum[0] -= newHdr->m_busId + newHdr->m_messageId;
 
        if (oldSize >= XS_EXTLENCODE)
                oldData = oldHdr->m_datlen.m_extended.m_data;
        else
                oldData = oldHdr->m_datlen.m_data;
 
        if (newSize >= XS_EXTLENCODE)
                newData = newHdr->m_datlen.m_extended.m_data;
        else
                newData = newHdr->m_datlen.m_data;
 
        if (oldSize > newSize)
                oldSize = newSize;      // speed up the loop
        for (index = 0; index < oldSize; ++index)
                newData[index] = oldData[index];
 
        if (thisPtr->m_autoUpdateChecksum)
                thisPtr->m_checksum[0] -= byteSum(oldData, oldSize);
 
        XsArray_destruct(&old);
}
 
void XsMessage_setBusId(XsMessage* thisPtr, uint8_t busId)
{
        XsMessageHeader* hdr;
        if (!thisPtr->m_message.m_data)
                XsMessage_construct(thisPtr);
 
        hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data;
        if (thisPtr->m_autoUpdateChecksum)
                thisPtr->m_checksum[0] += hdr->m_busId;
        hdr->m_busId = busId;
        if (thisPtr->m_autoUpdateChecksum)
                thisPtr->m_checksum[0] -= hdr->m_busId;
}
 
void XsMessage_setMessageId(XsMessage* thisPtr, enum XsXbusMessageId msgId)
{
        XsMessageHeader* hdr;
        if (!thisPtr->m_message.m_data)
                XsMessage_construct(thisPtr);
 
        hdr = (XsMessageHeader*)(void*) thisPtr->m_message.m_data;
        if (thisPtr->m_autoUpdateChecksum)
                thisPtr->m_checksum[0] += hdr->m_messageId;
        hdr->m_messageId = (uint8_t) msgId;
        if (thisPtr->m_autoUpdateChecksum)
                thisPtr->m_checksum[0] -= hdr->m_messageId;
}
 
void XsMessage_insertData(XsMessage* thisPtr, XsSize count, XsSize offset)
{
        XsSize index, oldSize, newSize;
        XsByteArray old = XSBYTEARRAY_INITIALIZER;
        XsMessageHeader* oldHdr, *newHdr;
        uint8_t* oldData, *newData;
 
        if (!count)
                return;
 
        oldSize = XsMessage_dataSize(thisPtr);
        newSize = oldSize + count;
 
        if (newSize < offset + count)
                newSize = offset + count;
 
        XsArray_swap(&thisPtr->m_message, &old);
        XsMessage_constructSized(thisPtr, newSize);
 
        newHdr = (XsMessageHeader*) thisPtr->m_message.m_data;
        oldHdr = (XsMessageHeader*) old.m_data;
        if (!oldHdr)            // our original message may have been empty / uninitialized
                return;
 
        newHdr->m_busId = oldHdr->m_busId;
        newHdr->m_preamble = oldHdr->m_preamble;
        newHdr->m_messageId = oldHdr->m_messageId;
 
        if (oldSize >= XS_EXTLENCODE)
                oldData = &oldHdr->m_datlen.m_extended.m_data[0];
        else
                oldData = &oldHdr->m_datlen.m_data[0];
 
        if (newSize >= XS_EXTLENCODE)
                newData = &newHdr->m_datlen.m_extended.m_data[0];
        else
                newData = &newHdr->m_datlen.m_data[0];
 
        if (offset <= oldSize)
        {
                for (index = 0; index < offset; ++index)
                        newData[index] = oldData[index];
        }
        else
        {
                for (index = 0; index < oldSize; ++index)
                        newData[index] = oldData[index];
                for (; index < offset; ++index)
                        newData[index] = 0;
        }
        for (; index < oldSize; ++index)
                newData[index + count] = oldData[index];
 
        if (thisPtr->m_autoUpdateChecksum)
                thisPtr->m_checksum[0] -= byteSum(oldData, oldSize);
 
        XsArray_destruct(&old);
}
 
void XsMessage_deleteData(XsMessage* thisPtr, XsSize count, XsSize offset)
{
        XsSize index, oldSize, newSize;
        XsByteArray old = XSBYTEARRAY_INITIALIZER;
        XsMessageHeader* oldHdr, *newHdr;
        uint8_t* oldData, *newData;
 
        oldSize = XsMessage_dataSize(thisPtr);
        if (!count || offset >= oldSize)
                return;
 
        if (offset + count >= oldSize)
        {
                XsMessage_resizeData(thisPtr, offset);
                return;
        }
        newSize = oldSize - count;
 
        XsArray_swap(&thisPtr->m_message, &old);
        XsMessage_constructSized(thisPtr, newSize);
 
        newHdr = (XsMessageHeader*) thisPtr->m_message.m_data;
        oldHdr = (XsMessageHeader*) old.m_data;
        if (!oldHdr)            // our original message may have been empty / uninitialized
                return;
 
        newHdr->m_busId = oldHdr->m_busId;
        newHdr->m_preamble = oldHdr->m_preamble;
        newHdr->m_messageId = oldHdr->m_messageId;
 
        if (oldSize >= XS_EXTLENCODE)
                oldData = oldHdr->m_datlen.m_extended.m_data;
        else
                oldData = oldHdr->m_datlen.m_data;
 
        if (newSize >= XS_EXTLENCODE)
                newData = newHdr->m_datlen.m_extended.m_data;
        else
                newData = newHdr->m_datlen.m_data;
 
        for (index = 0; index < offset; ++index)
                newData[index] = oldData[index];
        for (; index < newSize; ++index)
                newData[index] = oldData[index + count];
 
        if (thisPtr->m_autoUpdateChecksum)
                thisPtr->m_checksum[0] -= byteSum(newData, newSize);
 
        XsArray_destruct(&old);
}
 
void XsMessage_swap(XsMessage* a, XsMessage* b)
{
        XsMessage tmp;
 
        *((uint8_t**)&tmp.m_checksum) = a->m_checksum;
        tmp.m_autoUpdateChecksum = a->m_autoUpdateChecksum;
 
        *((uint8_t**)&a->m_checksum) = b->m_checksum;
        a->m_autoUpdateChecksum = b->m_autoUpdateChecksum;
 
        *((uint8_t**)&b->m_checksum) = tmp.m_checksum;
        b->m_autoUpdateChecksum = tmp.m_autoUpdateChecksum;
 
        XsArray_swap(&a->m_message, &b->m_message);
}
 
int XsMessage_compare(XsMessage const* a, XsMessage const* b)
{
        return XsArray_compare(&a->m_message, &b->m_message);
}
 
void XsMessage_toHexString(XsMessage const* thisPtr, XsSize maxBytes, XsString* resultValue)
{
        char* s;
        XsSize i = XsMessage_getTotalMessageSize(thisPtr);
        if (maxBytes == 0 || maxBytes > i)
                maxBytes = i;
 
        if (maxBytes)
        {
                XsString_resize(resultValue, maxBytes * 3 - 1);
                s = (char*) resultValue->m_data;
                for (i = 0; i < maxBytes - 1; ++i)
                        sprintf(s + (i * 3), "%02X ", (unsigned int)((uint8_t const*) thisPtr->m_message.m_data)[i]);
                sprintf(s + ((maxBytes - 1) * 3), "%02X", (unsigned int)((uint8_t const*) thisPtr->m_message.m_data)[maxBytes - 1]);
        }
        else
                XsString_resize(resultValue, 0);
}