cmtpacket.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

/*! \file cmtpacket.cpp

    For information about objects in this file, see the appropriate header:
    \ref Cmtpacket.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 "cmtpacket.h"

#ifdef _CMT_DLL_EXPORT
#include "xsens_math.h"
#endif

#ifdef _LOG_PACKET
#define PACKETLOG CMTLOG
#else
#define PACKETLOG(...)
#endif

namespace xsens
{
//////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////// Packet class
/////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////
Packet::Packet(uint16_t items, bool xbus)
{
    PACKETLOG("Create Packet %p\n", this);

    m_itemCount = items;
    m_infoList = nullptr;
    m_formatList = new CmtDataFormat[m_itemCount];
    m_toa = 0;
    m_rtc = 0;
    m_xm = xbus;
}

//////////////////////////////////////////////////////////////////////////////////////////
Packet::~Packet()
{
    PACKETLOG("Destroy Packet %p\n", this);
    LSTCHKDELNUL(m_formatList);
    LSTCHKDELNUL(m_infoList);
}

//////////////////////////////////////////////////////////////////////////////////////////
CmtDataFormat Packet::getDataFormat(const uint16_t index) const
{
    if (index >= m_itemCount || m_formatList == nullptr)
    {
        CmtDataFormat temp;
        return temp;
    }
    return m_formatList[index];
}

//////////////////////////////////////////////////////////////////////////////////////////
bool Packet::setDataFormat(const CmtDataFormat& format, const uint16_t index)
{
    if (index < m_itemCount)
    {
        m_formatList[index] = format;
        LSTCHKDELNUL(m_infoList);
        return true;
    }
    return false;
}

//////////////////////////////////////////////////////////////////////////////////////////
bool Packet::setDataFormat(
    const CmtOutputMode outputMode, const CmtOutputSettings outputSettings,
    const uint16_t index)
{
    if (index < m_itemCount)
    {
        m_formatList[index].m_outputMode = outputMode;
        m_formatList[index].m_outputSettings = outputSettings;
        LSTCHKDELNUL(m_infoList);
        return true;
    }
    return false;
}

//////////////////////////////////////////////////////////////////////////////////////////
bool Packet::getXbus(void) const { return m_xm; }
//////////////////////////////////////////////////////////////////////////////////////////
void Packet::setXbus(bool xbus, bool convert)
{
    if (xbus != m_xm)
    {
        if (convert)
        {
            CmtMtTimeStamp stamp = getSampleCounter(0);

            // move the time stamp value(s) around
            if (xbus)
            {
                // new version is Xbus, so old version is regular format ->
                // place timestamp at start of packet and remove from all
                // individual data units
                // remove the timestamp from all data units

                // insert timestamp at the start
                m_msg.insertData(2, 0);
                m_msg.setDataShort(stamp, 0);
            }
            else
            {
                // new version is regular, so old version is Xbus format ->
                // place timestamp at end of all individual data units and
                // remove from start of packet
                // append the timestamp to all data units

                // remove timestamp from the start
                m_msg.deleteData(2, 0);
            }
        }
        // update the state cache
        m_xm = xbus;
        LSTCHKDELNUL(m_infoList);
    }
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the floating/fixed point value size
uint16_t Packet::getFPValueSize(const uint16_t index) const
{
    uint16_t ds;
    switch (m_formatList[index].m_outputSettings &
            CMT_OUTPUTSETTINGS_DATAFORMAT_MASK)
    {
        case CMT_OUTPUTSETTINGS_DATAFORMAT_FLOAT:
            ds = 4;
            break;

        case CMT_OUTPUTSETTINGS_DATAFORMAT_DOUBLE:
            ds = 8;
            break;
        case CMT_OUTPUTSETTINGS_DATAFORMAT_FP1632:
            ds = 6;
            break;

        case CMT_OUTPUTSETTINGS_DATAFORMAT_F1220:
            ds = 4;
            break;

        default:
            ds = 0;
            break;
    }
    return ds;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the data size.
uint16_t Packet::getDataSize(const uint16_t index) const
{
    if (m_infoList == nullptr)
    {
        // allocate list
        m_infoList = new PacketInfo[m_itemCount];
        uint16_t totalOffset;
        if (m_xm)
            totalOffset = 2;
        else
            totalOffset = 0;

        // fill lists
        for (uint16_t i = 0; i < m_itemCount; ++i)
        {
            m_infoList[i].m_offset = totalOffset;
            m_infoList[i].m_size = 0;

            uint16_t ds = getFPValueSize(i);

            if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_RAW)
            {
                m_infoList[i].m_rawData = totalOffset;
                m_infoList[i].m_rawAcc = totalOffset;
                m_infoList[i].m_rawGyr = totalOffset + 6;
                m_infoList[i].m_rawMag = totalOffset + 12;
                m_infoList[i].m_rawTemp = totalOffset + 18;

                m_infoList[i].m_size += 20;
                totalOffset += 20;
            }
            else
            {
                m_infoList[i].m_rawData = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawAcc = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawGyr = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawMag = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawTemp = CMT_DATA_ITEM_NOT_AVAILABLE;
            }

            if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_RAWGPSPRINT)
            {
                m_infoList[i].m_rawGpsData = totalOffset;
                m_infoList[i].m_rawGpsPressure = totalOffset;
                m_infoList[i].m_rawGpsPressureAge = totalOffset + 2;
                m_infoList[i].m_size += 3;
                totalOffset += 3;

                m_infoList[i].m_rawGpsGpsData = totalOffset;
                m_infoList[i].m_rawGpsItow = totalOffset;
                m_infoList[i].m_rawGpsLatitude = totalOffset + 4;
                m_infoList[i].m_rawGpsLongitude = totalOffset + 8;
                m_infoList[i].m_rawGpsHeight = totalOffset + 12;
                m_infoList[i].m_rawGpsVeln = totalOffset + 16;
                m_infoList[i].m_rawGpsVele = totalOffset + 20;
                m_infoList[i].m_rawGpsVeld = totalOffset + 24;
                m_infoList[i].m_rawGpsHacc = totalOffset + 28;
                m_infoList[i].m_rawGpsVacc = totalOffset + 32;
                m_infoList[i].m_rawGpsSacc = totalOffset + 36;
                m_infoList[i].m_rawGpsGpsAge = totalOffset + 40;
                m_infoList[i].m_size += 41;
                totalOffset += 41;
            }
            else
            {
                m_infoList[i].m_rawGpsData = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawGpsPressure = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawGpsPressureAge = CMT_DATA_ITEM_NOT_AVAILABLE;

                m_infoList[i].m_rawGpsGpsData = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawGpsItow = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawGpsLatitude = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawGpsLongitude = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawGpsHeight = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawGpsVeln = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawGpsVele = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawGpsVeld = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawGpsHacc = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawGpsVacc = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawGpsSacc = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_rawGpsGpsAge = CMT_DATA_ITEM_NOT_AVAILABLE;
            }

            m_infoList[i].m_temp = CMT_DATA_ITEM_NOT_AVAILABLE;
            if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_TEMP)
            {
                m_infoList[i].m_temp = totalOffset;
                m_infoList[i].m_size += ds;
                totalOffset += ds;
            }

            if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_CALIB)
            {
                if ((m_formatList[i].m_outputSettings &
                     CMT_OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
                {
                    m_infoList[i].m_calAcc = totalOffset;
                    m_infoList[i].m_size += 3 * ds;
                    totalOffset += 3 * ds;
                }
                else
                    m_infoList[i].m_calAcc = CMT_DATA_ITEM_NOT_AVAILABLE;

                if ((m_formatList[i].m_outputSettings &
                     CMT_OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0)
                {
                    m_infoList[i].m_calGyr = totalOffset;
                    m_infoList[i].m_size += 3 * ds;
                    totalOffset += 3 * ds;
                }
                else
                    m_infoList[i].m_calGyr = CMT_DATA_ITEM_NOT_AVAILABLE;

                if ((m_formatList[i].m_outputSettings &
                     CMT_OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0)
                {
                    m_infoList[i].m_calMag = totalOffset;
                    m_infoList[i].m_size += 3 * ds;
                    totalOffset += 3 * ds;
                }
                else
                    m_infoList[i].m_calMag = CMT_DATA_ITEM_NOT_AVAILABLE;

                if ((m_formatList[i].m_outputSettings &
                     CMT_OUTPUTSETTINGS_CALIBMODE_ACCGYRMAG) == 0)
                    m_infoList[i].m_calData = m_infoList[i].m_calAcc;
                else
                    m_infoList[i].m_calData = CMT_DATA_ITEM_NOT_AVAILABLE;
            }
            else
            {
                m_infoList[i].m_calData = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_calAcc = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_calGyr = CMT_DATA_ITEM_NOT_AVAILABLE;
                m_infoList[i].m_calMag = CMT_DATA_ITEM_NOT_AVAILABLE;
            }

            m_infoList[i].m_oriEul = CMT_DATA_ITEM_NOT_AVAILABLE;
            m_infoList[i].m_oriQuat = CMT_DATA_ITEM_NOT_AVAILABLE;
            m_infoList[i].m_oriMat = CMT_DATA_ITEM_NOT_AVAILABLE;

            if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_ORIENT)
            {
                switch (m_formatList[i].m_outputSettings &
                        CMT_OUTPUTSETTINGS_ORIENTMODE_MASK)
                {
                    case CMT_OUTPUTSETTINGS_ORIENTMODE_EULER:
                        m_infoList[i].m_oriEul = totalOffset;
                        m_infoList[i].m_size += 3 * ds;
                        totalOffset += 3 * ds;
                        break;
                    case CMT_OUTPUTSETTINGS_ORIENTMODE_QUATERNION:
                        m_infoList[i].m_oriQuat = totalOffset;
                        m_infoList[i].m_size += 4 * ds;
                        totalOffset += 4 * ds;
                        break;
                    case CMT_OUTPUTSETTINGS_ORIENTMODE_MATRIX:
                        m_infoList[i].m_oriMat = totalOffset;
                        m_infoList[i].m_size += 9 * ds;
                        totalOffset += 9 * ds;
                        break;
                    default:
                        break;
                }
            }

            m_infoList[i].m_analogIn1 = CMT_DATA_ITEM_NOT_AVAILABLE;
            if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_AUXILIARY)
            {
                if ((m_formatList[i].m_outputSettings &
                     CMT_OUTPUTSETTINGS_AUXILIARYMODE_AIN1_MASK) == 0)
                {
                    m_infoList[i].m_analogIn1 = totalOffset;
                    m_infoList[i].m_size += 2;
                    totalOffset += 2;
                }
                else
                    m_infoList[i].m_analogIn1 = CMT_DATA_ITEM_NOT_AVAILABLE;

                if ((m_formatList[i].m_outputSettings &
                     CMT_OUTPUTSETTINGS_AUXILIARYMODE_AIN2_MASK) == 0)
                {
                    m_infoList[i].m_analogIn2 = totalOffset;
                    m_infoList[i].m_size += 2;
                    totalOffset += 2;
                }
                else
                    m_infoList[i].m_analogIn2 = CMT_DATA_ITEM_NOT_AVAILABLE;
            }

            m_infoList[i].m_posLLA = CMT_DATA_ITEM_NOT_AVAILABLE;
            if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_POSITION)
            {
                switch (m_formatList[i].m_outputSettings &
                        CMT_OUTPUTSETTINGS_POSITIONMODE_MASK)
                {
                    case CMT_OUTPUTSETTINGS_POSITIONMODE_LLA_WGS84:
                        m_infoList[i].m_posLLA = totalOffset;
                        m_infoList[i].m_size += 3 * ds;
                        totalOffset += 3 * ds;
                }
            }

            m_infoList[i].m_velNEDorNWU = CMT_DATA_ITEM_NOT_AVAILABLE;
            if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_VELOCITY)
            {
                switch (m_formatList[i].m_outputSettings &
                        CMT_OUTPUTSETTINGS_VELOCITYMODE_MASK)
                {
                    case CMT_OUTPUTSETTINGS_VELOCITYMODE_NED:
                        m_infoList[i].m_velNEDorNWU = totalOffset;
                        m_infoList[i].m_size += 3 * ds;
                        totalOffset += 3 * ds;
                }
            }

            m_infoList[i].m_status = CMT_DATA_ITEM_NOT_AVAILABLE;
            if (m_formatList[i].m_outputMode & CMT_OUTPUTMODE_STATUS)
            {
                m_infoList[i].m_status = totalOffset;
                m_infoList[i].m_size += 1;
                totalOffset += 1;
            }

            m_infoList[i].m_sc = CMT_DATA_ITEM_NOT_AVAILABLE;
            if (m_xm) m_infoList[i].m_sc = 0;
            if ((m_formatList[i].m_outputSettings &
                 CMT_OUTPUTSETTINGS_TIMESTAMP_MASK) ==
                CMT_OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)
            {
                if (!m_xm) m_infoList[i].m_sc = totalOffset;
                m_infoList[i].m_size += 2;
                totalOffset += 2;
            }

            // post-processing data is never available at this point
            m_infoList[i].m_acc_g = CMT_DATA_ITEM_NOT_AVAILABLE;
        }
    }

    if (index < m_itemCount) return m_infoList[index].m_size;
    return 0;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Raw Accelerometer component of a data item.
CmtShortVector Packet::getRawAcc(const uint16_t index) const
{
    CmtShortVector buffer;
    if (containsRawAcc(index))
        for (uint16_t i = 0; i < 3; ++i)
            buffer.m_data[i] =
                m_msg.getDataShort(m_infoList[index].m_rawAcc + (2 * i));

    return buffer;
}
bool Packet::containsRawAcc(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_rawAcc == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
    return true;
}
bool Packet::updateRawAcc(const CmtShortVector& vec, const uint16_t index)
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_rawAcc == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_rawAcc = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + 3 * 2);
        m_infoList[index].m_size += 3 * 2;
    }
    // update
    for (uint16_t i = 0; i < 3; ++i)
        m_msg.setDataShort(vec.m_data[i], m_infoList[index].m_rawAcc + (2 * i));
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Raw Gyroscope component of a data item.
CmtShortVector Packet::getRawGyr(const uint16_t index) const
{
    CmtShortVector buffer;
    if (containsRawGyr(index))
        for (uint16_t i = 0; i < 3; ++i)
            buffer.m_data[i] =
                m_msg.getDataShort(m_infoList[index].m_rawGyr + (2 * i));

    return buffer;
}
bool Packet::containsRawGyr(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_rawGyr == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
    return true;
}
bool Packet::updateRawGyr(const CmtShortVector& vec, const uint16_t index)
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_rawGyr == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_rawGyr = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + 3 * 2);
        m_infoList[index].m_size += 3 * 2;
    }
    // update
    for (uint16_t i = 0; i < 3; ++i)
        m_msg.setDataShort(vec.m_data[i], m_infoList[index].m_rawGyr + (2 * i));
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Raw Magnetometer component of a data item.
CmtShortVector Packet::getRawMag(const uint16_t index) const
{
    CmtShortVector buffer;
    if (containsRawMag(index))
        for (uint16_t i = 0; i < 3; ++i)
            buffer.m_data[i] =
                m_msg.getDataShort(m_infoList[index].m_rawMag + (2 * i));

    return buffer;
}
bool Packet::containsRawMag(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_rawMag == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
    return true;
}
bool Packet::updateRawMag(const CmtShortVector& vec, const uint16_t index)
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_rawMag == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_rawMag = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + 3 * 2);
        m_infoList[index].m_size += 3 * 2;
    }
    // update
    for (uint16_t i = 0; i < 3; ++i)
        m_msg.setDataShort(vec.m_data[i], m_infoList[index].m_rawMag + (2 * i));
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Raw Temperature component of a data item.
uint16_t Packet::getRawTemp(const uint16_t index) const
{
    if (!containsRawTemp(index)) return 0;

    return m_msg.getDataShort(m_infoList[index].m_rawTemp);
}
bool Packet::containsRawTemp(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_rawTemp == CMT_DATA_ITEM_NOT_AVAILABLE)
        return false;
    return true;
}
bool Packet::updateRawTemp(const uint16_t temp, const uint16_t index)
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_rawTemp == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_rawTemp = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + 2);
        m_infoList[index].m_size += 2;
    }
    // update
    m_msg.setDataShort(temp, m_infoList[index].m_rawTemp);
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Raw Data component of a data item.
CmtRawData Packet::getRawData(const uint16_t index) const
{
    CmtRawData buffer;
    if (containsRawData(index))
    {
        const uint8_t* tmp = m_msg.getDataBuffer(m_infoList[index].m_rawData);
        const uint16_t* sh = (const uint16_t*)tmp;
        uint16_t* bare = (uint16_t*)&buffer;

        for (uint16_t i = 0; i < 10; ++i, ++sh, ++bare)
            *bare = swapEndian16(
                *sh);  // m_msg.getDataShort(m_infoList[index].m_rawData +
        // (2*i));
    }
    return buffer;
}
bool Packet::containsRawData(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_rawData == CMT_DATA_ITEM_NOT_AVAILABLE)
        return false;
    return true;
}
bool Packet::updateRawData(const CmtRawData& data, const uint16_t index)
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_rawData == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_rawData = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + 3 * 3 * 2 + 2);
        m_infoList[index].m_rawAcc = m_infoList[index].m_rawData;
        m_infoList[index].m_rawGyr = m_infoList[index].m_rawData + 3 * 2;
        m_infoList[index].m_rawMag = m_infoList[index].m_rawData + 6 * 2;
        m_infoList[index].m_rawTemp = m_infoList[index].m_rawData + 9 * 2;
        m_infoList[index].m_size += 3 * 3 * 2 + 2;
    }
    // update
    int16_t* bare = (int16_t*)&data;
    for (uint16_t i = 0; i < 10; ++i)
        m_msg.setDataShort(bare[i], m_infoList[index].m_rawData + (2 * i));
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Raw Data component of a data item.
CmtRawGpsData Packet::getRawGpsData(const uint16_t index) const
{
    CmtRawGpsData buffer;
    if (containsRawGpsData(index))
    {
        // const uint8_t* tmp =
        // m_msg.getDataBuffer(m_infoList[index].m_rawGpsData);
        // const uint16_t* sh = (const uint16_t*) tmp;
        // uint16_t* bare = (uint16_t*) &buffer;

        // pressure data
        buffer.m_pressure =
            m_msg.getDataShort(m_infoList[index].m_rawGpsPressure);
        // pressAge
        buffer.m_pressureAge =
            m_msg.getDataByte(m_infoList[index].m_rawGpsPressureAge);

        // lon,lat,height,hacc,vacc,veln,vele,veld
        // tmp = m_msg.getDataBuffer(m_infoList[index].m_rawGpsGpsData);
        // const uint32_t* ln = (const uint32_t*) tmp;
        uint32_t* bareln = (uint32_t*)&buffer.m_itow;
        for (uint16_t i = 0; i < 10; ++i)
            bareln[i] =
                m_msg.getDataLong(m_infoList[index].m_rawGpsGpsData + (4 * i));

        // gpsAge
        buffer.m_gpsAge = m_msg.getDataByte(m_infoList[index].m_rawGpsGpsAge);
    }
    return buffer;
}
bool Packet::containsRawGpsData(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_rawGpsData == CMT_DATA_ITEM_NOT_AVAILABLE)
        return false;
    return true;
}
bool Packet::updateRawGpsData(const CmtRawGpsData& data, const uint16_t index)
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_rawGpsData == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_rawGpsData = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + (2 + 1) + (40 + 1));
        m_infoList[index].m_rawGpsPressure = m_infoList[index].m_rawGpsData;
        m_infoList[index].m_rawGpsPressureAge =
            m_infoList[index].m_rawGpsData + 2;

        m_infoList[index].m_rawGpsGpsData = m_infoList[index].m_rawGpsData + 3;
        m_infoList[index].m_rawGpsItow = m_infoList[index].m_rawGpsData + 3;
        m_infoList[index].m_rawGpsLatitude =
            m_infoList[index].m_rawGpsData + 3 + 4;
        m_infoList[index].m_rawGpsLongitude =
            m_infoList[index].m_rawGpsData + 3 + 8;
        m_infoList[index].m_rawGpsHeight =
            m_infoList[index].m_rawGpsData + 3 + 12;
        m_infoList[index].m_rawGpsVeln =
            m_infoList[index].m_rawGpsData + 3 + 16;
        m_infoList[index].m_rawGpsVele =
            m_infoList[index].m_rawGpsData + 3 + 20;
        m_infoList[index].m_rawGpsVeld =
            m_infoList[index].m_rawGpsData + 3 + 24;
        m_infoList[index].m_rawGpsHacc =
            m_infoList[index].m_rawGpsData + 3 + 28;
        m_infoList[index].m_rawGpsVacc =
            m_infoList[index].m_rawGpsData + 3 + 32;
        m_infoList[index].m_rawGpsSacc =
            m_infoList[index].m_rawGpsData + 3 + 36;
        m_infoList[index].m_rawGpsGpsAge =
            m_infoList[index].m_rawGpsData + 3 + 40;

        m_infoList[index].m_size += (2 + 1) + (40 + 1);
    }
    // update
    m_msg.setDataShort(data.m_pressure, m_infoList[index].m_rawGpsPressure);
    m_msg.setDataByte(
        data.m_pressureAge, m_infoList[index].m_rawGpsPressureAge);

    // lon,lat,height,hacc,vacc,veln,vele,veld
    int32_t* bareln = (int32_t*)&data.m_itow;
    for (uint16_t i = 0; i < 10; ++i)
        m_msg.setDataLong(
            bareln[i], m_infoList[index].m_rawGpsGpsData + (4 * i));

    // gpsAge
    m_msg.setDataByte(data.m_gpsAge, m_infoList[index].m_rawGpsGpsAge);
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Temperature component of a data item.
double Packet::getTemp(const uint16_t index) const
{
    if (containsTemp(index))
        return m_msg.getDataFPValue(
            m_formatList[index].m_outputSettings, m_infoList[index].m_temp);

    return 0.0;
}
bool Packet::containsTemp(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_temp == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
    return true;
}
bool Packet::updateTemp(const double& temp, const uint16_t index)
{
    if (getDataSize(index) == 0) return false;

    uint16_t ds = getFPValueSize(index);

    if (m_infoList[index].m_temp == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_temp = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + ds);
        m_infoList[index].m_size += ds;
    }
    // update
    m_msg.setDataFPValue(
        m_formatList[index].m_outputSettings, temp, m_infoList[index].m_temp);
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Calibrated Accelerometer component of a data item.
CmtVector Packet::getCalAcc(const uint16_t index) const
{
    CmtVector buffer;
    if (containsCalAcc(index))
        m_msg.getDataFPValue(
            &buffer.m_data[0], m_formatList[index].m_outputSettings,
            m_infoList[index].m_calAcc, 3);
    else
        memset(&buffer, 0, sizeof(buffer));
    return buffer;
}
bool Packet::containsCalAcc(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_calAcc == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
    return true;
}
bool Packet::updateCalAcc(const CmtVector& vec, const uint16_t index)
{
    const uint16_t numValues = 3;
    if (getDataSize(index) == 0) return false;

    uint16_t ds = getFPValueSize(index);

    if (m_infoList[index].m_calAcc == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_calAcc = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
        m_infoList[index].m_size += numValues * ds;
    }
    // update
    m_msg.setDataFPValue(
        m_formatList[index].m_outputSettings, &vec.m_data[0],
        m_infoList[index].m_calAcc, numValues);
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Calibrated Gyroscope component of a data item.
CmtVector Packet::getCalGyr(const uint16_t index) const
{
    CmtVector buffer;
    if (containsCalGyr(index))
        m_msg.getDataFPValue(
            &buffer.m_data[0], m_formatList[index].m_outputSettings,
            m_infoList[index].m_calGyr, 3);
    else
        memset(&buffer, 0, sizeof(buffer));
    return buffer;
}
bool Packet::containsCalGyr(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_calGyr == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
    return true;
}
bool Packet::updateCalGyr(const CmtVector& vec, const uint16_t index)
{
    const uint16_t numValues = 3;
    if (getDataSize(index) == 0) return false;

    uint16_t ds = getFPValueSize(index);

    if (m_infoList[index].m_calGyr == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_calGyr = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
        m_infoList[index].m_size += numValues * ds;
    }
    // update
    m_msg.setDataFPValue(
        m_formatList[index].m_outputSettings, &vec.m_data[0],
        m_infoList[index].m_calGyr, numValues);
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Calibrated Magnetometer component of a data item.
CmtVector Packet::getCalMag(const uint16_t index) const
{
    CmtVector buffer;
    if (containsCalMag(index))
        m_msg.getDataFPValue(
            &buffer.m_data[0], m_formatList[index].m_outputSettings,
            m_infoList[index].m_calMag, 3);
    else
        memset(&buffer, 0, sizeof(buffer));
    return buffer;
}
bool Packet::containsCalMag(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_calMag == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
    return true;
}
bool Packet::updateCalMag(const CmtVector& vec, const uint16_t index)
{
    const uint16_t numValues = 3;
    if (getDataSize(index) == 0) return false;

    uint16_t ds = getFPValueSize(index);

    if (m_infoList[index].m_calMag == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_calMag = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
        m_infoList[index].m_size += numValues * ds;
    }
    // update
    m_msg.setDataFPValue(
        m_formatList[index].m_outputSettings, &vec.m_data[0],
        m_infoList[index].m_calMag, numValues);
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Calibrated Accelerometer component of a data item.
CmtCalData Packet::getCalData(const uint16_t index) const
{
    CmtCalData buffer;
    double* bare = (double*)&buffer;
    if (containsCalData(index))
        m_msg.getDataFPValue(
            bare, m_formatList[index].m_outputSettings,
            m_infoList[index].m_calData, 9);
    else
        memset(&buffer, 0, sizeof(buffer));
    return buffer;
}
bool Packet::containsCalData(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_calData == CMT_DATA_ITEM_NOT_AVAILABLE)
        return false;
    return true;
}
bool Packet::updateCalData(const CmtCalData& data, const uint16_t index)
{
    const uint16_t numValues = 9;
    if (getDataSize(index) == 0) return false;

    uint16_t ds = getFPValueSize(index);

    if (m_infoList[index].m_calData == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_msg.m_autoUpdateChecksum = false;
        m_infoList[index].m_calData = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
        m_infoList[index].m_calAcc = m_infoList[index].m_calData;
        m_infoList[index].m_calGyr = m_infoList[index].m_calData + 3 * ds;
        m_infoList[index].m_calMag = m_infoList[index].m_calData + 6 * ds;
        m_infoList[index].m_size += numValues * ds;
    }
    // update

    double* bare = (double*)&data;
    m_msg.setDataFPValue(
        m_formatList[index].m_outputSettings, bare, m_infoList[index].m_calData,
        numValues);
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Orientation component of a data item as a Quaternion.
CmtQuat Packet::getOriQuat(const uint16_t index) const
{
    CmtQuat buffer;
    if (containsOriQuat(index))
        m_msg.getDataFPValue(
            &buffer.m_data[0], m_formatList[index].m_outputSettings,
            m_infoList[index].m_oriQuat, 4);
    else
        memset(&buffer, 0, sizeof(buffer));
    return buffer;
}

bool Packet::containsOriQuat(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_oriQuat == CMT_DATA_ITEM_NOT_AVAILABLE)
        return false;
    return true;
}
bool Packet::updateOriQuat(const CmtQuat& data, const uint16_t index)
{
    const uint16_t numValues = 4;
    if (getDataSize(index) == 0) return false;

    uint16_t ds = getFPValueSize(index);

    if (m_infoList[index].m_oriQuat == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_oriQuat = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
        m_infoList[index].m_size += numValues * ds;
    }
    // update
    m_msg.setDataFPValue(
        m_formatList[index].m_outputSettings, &data.m_data[0],
        m_infoList[index].m_oriQuat, numValues);
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Orientation component of a data item as CmtEuler angles.
CmtEuler Packet::getOriEuler(const uint16_t index) const
{
    CmtEuler buffer;
    if (containsOriEuler(index))
    {
        uint16_t ds = getFPValueSize(index);
        buffer.m_roll = m_msg.getDataFPValue(
            m_formatList[index].m_outputSettings, m_infoList[index].m_oriEul);
        buffer.m_pitch = m_msg.getDataFPValue(
            m_formatList[index].m_outputSettings,
            m_infoList[index].m_oriEul + ds);
        buffer.m_yaw = m_msg.getDataFPValue(
            m_formatList[index].m_outputSettings,
            m_infoList[index].m_oriEul + 2 * ds);
    }
    return buffer;
}
bool Packet::containsOriEuler(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_oriEul == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
    return true;
}
bool Packet::updateOriEuler(const CmtEuler& data, const uint16_t index)
{
    const uint16_t numValues = 3;
    if (getDataSize(index) == 0) return false;

    uint16_t ds = getFPValueSize(index);

    if (m_infoList[index].m_oriEul == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_oriEul = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
        m_infoList[index].m_size += numValues * ds;
    }
    // update
    m_msg.setDataFPValue(
        m_formatList[index].m_outputSettings, data.m_roll,
        m_infoList[index].m_oriEul);
    m_msg.setDataFPValue(
        m_formatList[index].m_outputSettings, data.m_pitch,
        m_infoList[index].m_oriEul + ds);
    m_msg.setDataFPValue(
        m_formatList[index].m_outputSettings, data.m_yaw,
        m_infoList[index].m_oriEul + 2 * ds);
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Orientation component of a data item as an Orientation Matrix.
CmtMatrix Packet::getOriMatrix(const uint16_t index) const
{
    CmtMatrix buffer;
    uint16_t k = 0;
    if (containsOriMatrix(index))
    {
        uint16_t ds = getFPValueSize(index);
        for (int32_t i = 0; i < 3; ++i)
            for (int32_t j = 0; j < 3; ++j, k += ds)
                buffer.m_data[i][j] = m_msg.getDataFPValue(
                    m_formatList[index].m_outputSettings,
                    m_infoList[index].m_oriMat + k);
    }
    else
        memset(&buffer, 0, sizeof(buffer));

    return buffer;
}
bool Packet::containsOriMatrix(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_oriMat == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
    return true;
}
bool Packet::updateOriMatrix(const CmtMatrix& data, const uint16_t index)
{
    const uint16_t numValues = 9;
    if (getDataSize(index) == 0) return false;

    uint16_t ds = getFPValueSize(index);

    if (m_infoList[index].m_oriMat == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_oriMat = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
        m_infoList[index].m_size += numValues * ds;
    }
    // update
    uint16_t k = 0;
    for (int32_t i = 0; i < 3; ++i)
        for (int32_t j = 0; j < 3; ++j, k += ds)
            m_msg.setDataFPValue(
                m_formatList[index].m_outputSettings, data.m_data[i][j],
                m_infoList[index].m_oriMat + k);
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the AnalogIn 1 component of a data item.
CmtAnalogInData Packet::getAnalogIn1(const uint16_t index) const
{
    CmtAnalogInData buffer;
    if (containsAnalogIn1(index))
        buffer.m_data = m_msg.getDataShort(m_infoList[index].m_analogIn1);

    return buffer;
}
bool Packet::containsAnalogIn1(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_analogIn1 == CMT_DATA_ITEM_NOT_AVAILABLE)
        return false;
    return true;
}
bool Packet::updateAnalogIn1(const CmtAnalogInData& data, const uint16_t index)
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_analogIn1 == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_analogIn1 = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + 2);
        m_infoList[index].m_size += 2;
    }
    // update
    m_msg.setDataShort(data.m_data, m_infoList[index].m_analogIn1);
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the AnalogIn 2 component of a data item.
CmtAnalogInData Packet::getAnalogIn2(const uint16_t index) const
{
    CmtAnalogInData buffer;
    if (containsAnalogIn2(index))
        buffer.m_data = m_msg.getDataShort(m_infoList[index].m_analogIn2);

    return buffer;
}
bool Packet::containsAnalogIn2(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_analogIn2 == CMT_DATA_ITEM_NOT_AVAILABLE)
        return false;
    return true;
}
bool Packet::updateAnalogIn2(const CmtAnalogInData& data, const uint16_t index)
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_analogIn2 == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_analogIn2 = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + 2);
        m_infoList[index].m_size += 2;
    }
    // update
    m_msg.setDataShort(data.m_data, m_infoList[index].m_analogIn2);
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Position LLA component of a data item.
CmtVector Packet::getPositionLLA(const uint16_t index) const
{
    CmtVector buffer;
    if (containsPositionLLA(index))
        m_msg.getDataFPValue(
            &buffer.m_data[0], m_formatList[index].m_outputSettings,
            m_infoList[index].m_posLLA, 3);
    else
        memset(&buffer, 0, sizeof(buffer));
    return buffer;
}
bool Packet::containsPositionLLA(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_posLLA == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
    return true;
}
bool Packet::updatePositionLLA(const CmtVector& data, const uint16_t index)
{
    const uint16_t numValues = 3;
    if (getDataSize(index) == 0) return false;

    uint16_t ds = getFPValueSize(index);

    if (m_infoList[index].m_posLLA == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_posLLA = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
        m_infoList[index].m_size += numValues * ds;
    }
    // update
    m_msg.setDataFPValue(
        m_formatList[index].m_outputSettings, &data.m_data[0],
        m_infoList[index].m_posLLA, numValues);
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Velocity NED component of a data item.
CmtVector Packet::getVelocity(const uint16_t index) const
{
    CmtVector buffer;
    if (containsVelocity(index))
        m_msg.getDataFPValue(
            &buffer.m_data[0], m_formatList[index].m_outputSettings,
            m_infoList[index].m_velNEDorNWU, 3);
    else
        memset(&buffer, 0, sizeof(buffer));

    return buffer;
}
bool Packet::containsVelocity(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_velNEDorNWU == CMT_DATA_ITEM_NOT_AVAILABLE)
        return false;
    return true;
}
bool Packet::updateVelocity(const CmtVector& data, const uint16_t index)
{
    const uint16_t numValues = 3;
    if (getDataSize(index) == 0) return false;

    uint16_t ds = getFPValueSize(index);

    if (m_infoList[index].m_velNEDorNWU == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_velNEDorNWU = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
        m_infoList[index].m_size += numValues * ds;
    }
    // update
    m_msg.setDataFPValue(
        m_formatList[index].m_outputSettings, &data.m_data[0],
        m_infoList[index].m_velNEDorNWU, numValues);
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Status component of a data item.
uint8_t Packet::getStatus(const uint16_t index) const
{
    if (containsStatus(index))
        return m_msg.getDataByte(m_infoList[index].m_status);
    return 0;
}
bool Packet::containsStatus(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_status == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
    return true;
}
bool Packet::updateStatus(const uint8_t data, const uint16_t index)
{
    if (getDataSize(index) == 0) return false;

    if (m_infoList[index].m_status == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_status = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + 1);
        m_infoList[index].m_size += 1;
    }
    // update
    m_msg.setDataByte(data, m_infoList[index].m_status);
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the Sample Counter component of the packet.
uint16_t Packet::getSampleCounter(const uint16_t index) const
{
    if (!containsSampleCounter(index)) return 0;
    return m_msg.getDataShort(m_infoList[index].m_sc);
}
bool Packet::containsSampleCounter(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_sc == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
    return true;
}
bool Packet::updateSampleCounter(const uint16_t counter, const uint16_t index)
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_sc == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add
        m_infoList[index].m_sc = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + 2);
        m_infoList[index].m_size += 2;
    }
    // update
    m_msg.setDataShort(counter, m_infoList[index].m_sc);
    return true;
}
TimeStamp Packet::getRtc(const uint16_t index) const
{
    MRPT_UNUSED_PARAM(index);
    return m_rtc;
}

//////////////////////////////////////////////////////////////////////////////////////////
// Return the XKF-3 Acc G component of the packet.
CmtVector Packet::getAccG(const uint16_t index) const
{
    CmtVector buffer;
    if (containsAccG(index))
        m_msg.getDataFPValue(
            &buffer.m_data[0], m_formatList[index].m_outputSettings,
            m_infoList[index].m_acc_g, 3);
    else
        memset(&buffer, 0, sizeof(buffer));

    return buffer;
}
bool Packet::containsAccG(const uint16_t index) const
{
    if (getDataSize(index) == 0) return false;
    if (m_infoList[index].m_acc_g == CMT_DATA_ITEM_NOT_AVAILABLE) return false;
    return true;
}
bool Packet::updateAccG(const CmtVector& g, const uint16_t index)
{
    const uint16_t numValues = 3;
    if (getDataSize(index) == 0) return false;

    uint16_t ds = getFPValueSize(index);

    if (m_infoList[index].m_acc_g == CMT_DATA_ITEM_NOT_AVAILABLE)
    {
        // add space
        m_infoList[index].m_acc_g = m_msg.getDataSize();
        m_msg.resizeData(m_msg.getDataSize() + numValues * ds);
        m_infoList[index].m_size += numValues * ds;
    }
    // update
    m_msg.setDataFPValue(
        m_formatList[index].m_outputSettings, &g.m_data[0],
        m_infoList[index].m_acc_g, numValues);
    return true;
}

Packet::Packet(const Packet& pack)
{
    PACKETLOG("Create new packet from Packet %p\n", &pack);

    m_itemCount = 0;
    m_formatList = nullptr;
    m_infoList = nullptr;
    *this = pack;
}

void Packet::operator=(const Packet& pack)
{
    PACKETLOG("Copy packet from Packet %p\n", this);

    if (m_itemCount != pack.m_itemCount)
    {
        LSTCHKDELNUL(m_formatList);
        m_itemCount = pack.m_itemCount;
        m_formatList = new CmtDataFormat[m_itemCount];
    }
    for (uint16_t i = 0; i < m_itemCount; ++i)
        m_formatList[i] = pack.m_formatList[i];
    LSTCHKDELNUL(m_infoList);
    m_toa = pack.m_toa;
    m_rtc = pack.m_rtc;
    m_msg = pack.m_msg;
    m_xm = pack.m_xm;
}

#ifdef _CMT_DLL_EXPORT
void Packet::interpolate(const Packet& pa, const Packet& pb, const double f)
{
    pa.getDataSize();
    pb.getDataSize();

    *this = pb;

    if (m_itemCount != pa.m_itemCount) return;

    getDataSize();
    CmtShortVector va, vb, vc;
    CmtVector fa, fb, fc;
    CmtQuat qf;
    CmtEuler ea, eb, ec;
    CmtMatrix mf;

    Quaternion qa, qb, qc;
    Matrix3x3 m;

    double g = 1.0 - f;
    double da, db, dc;
    uint16_t sa, sb, sc;

    // walk over all items and interpolate all data items
    for (uint16_t i = 0; i < m_itemCount; ++i)
    {
        //// raw data
        if (pa.m_infoList[i].m_rawAcc != CMT_DATA_ITEM_NOT_AVAILABLE &&
            pb.m_infoList[i].m_rawAcc != CMT_DATA_ITEM_NOT_AVAILABLE)
        {
            va = pa.getRawAcc(i);
            vb = pb.getRawAcc(i);
            vc.m_data[0] =
                (uint16_t)(g * (double)va.m_data[0] + f * (double)vb.m_data[0]);
            vc.m_data[1] =
                (uint16_t)(g * (double)va.m_data[1] + f * (double)vb.m_data[1]);
            vc.m_data[2] =
                (uint16_t)(g * (double)va.m_data[2] + f * (double)vb.m_data[2]);
            updateRawAcc(vc, i);
        }

        if (pa.m_infoList[i].m_rawGyr != CMT_DATA_ITEM_NOT_AVAILABLE &&
            pb.m_infoList[i].m_rawGyr != CMT_DATA_ITEM_NOT_AVAILABLE)
        {
            va = pa.getRawGyr(i);
            vb = pb.getRawGyr(i);
            vc.m_data[0] =
                (uint16_t)(g * (double)va.m_data[0] + f * (double)vb.m_data[0]);
            vc.m_data[1] =
                (uint16_t)(g * (double)va.m_data[1] + f * (double)vb.m_data[1]);
            vc.m_data[2] =
                (uint16_t)(g * (double)va.m_data[2] + f * (double)vb.m_data[2]);
            updateRawGyr(vc, i);
        }

        if (pa.m_infoList[i].m_rawMag != CMT_DATA_ITEM_NOT_AVAILABLE &&
            pb.m_infoList[i].m_rawMag != CMT_DATA_ITEM_NOT_AVAILABLE)
        {
            va = pa.getRawMag(i);
            vb = pb.getRawMag(i);
            vc.m_data[0] =
                (uint16_t)(g * (double)va.m_data[0] + f * (double)vb.m_data[0]);
            vc.m_data[1] =
                (uint16_t)(g * (double)va.m_data[1] + f * (double)vb.m_data[1]);
            vc.m_data[2] =
                (uint16_t)(g * (double)va.m_data[2] + f * (double)vb.m_data[2]);
            updateRawMag(vc, i);
        }

        if (pa.m_infoList[i].m_rawTemp != CMT_DATA_ITEM_NOT_AVAILABLE &&
            pb.m_infoList[i].m_rawTemp != CMT_DATA_ITEM_NOT_AVAILABLE)
        {
            da = (double)pa.getRawTemp(i);
            db = (double)pb.getRawTemp(i);
            dc = g * da + f * db;
            updateRawTemp((uint16_t)dc, i);
        }

        //// calibrated data
        if (pa.m_infoList[i].m_calAcc != CMT_DATA_ITEM_NOT_AVAILABLE &&
            pb.m_infoList[i].m_calAcc != CMT_DATA_ITEM_NOT_AVAILABLE)
        {
            fa = pa.getCalAcc(i);
            fb = pb.getCalAcc(i);
            fc.m_data[0] =
                (g * (double)fa.m_data[0] + f * (double)fb.m_data[0]);
            fc.m_data[1] =
                (g * (double)fa.m_data[1] + f * (double)fb.m_data[1]);
            fc.m_data[2] =
                (g * (double)fa.m_data[2] + f * (double)fb.m_data[2]);
            updateCalAcc(fc, i);
        }

        if (pa.m_infoList[i].m_calGyr != CMT_DATA_ITEM_NOT_AVAILABLE &&
            pb.m_infoList[i].m_calGyr != CMT_DATA_ITEM_NOT_AVAILABLE)
        {
            fa = pa.getCalGyr(i);
            fb = pb.getCalGyr(i);
            fc.m_data[0] =
                (g * (double)fa.m_data[0] + f * (double)fb.m_data[0]);
            fc.m_data[1] =
                (g * (double)fa.m_data[1] + f * (double)fb.m_data[1]);
            fc.m_data[2] =
                (g * (double)fa.m_data[2] + f * (double)fb.m_data[2]);
            updateCalGyr(fc, i);
        }

        if (pa.m_infoList[i].m_calMag != CMT_DATA_ITEM_NOT_AVAILABLE &&
            pb.m_infoList[i].m_calMag != CMT_DATA_ITEM_NOT_AVAILABLE)
        {
            fa = pa.getCalMag(i);
            fb = pb.getCalMag(i);
            fc.m_data[0] =
                (g * (double)fa.m_data[0] + f * (double)fb.m_data[0]);
            fc.m_data[1] =
                (g * (double)fa.m_data[1] + f * (double)fb.m_data[1]);
            fc.m_data[2] =
                (g * (double)fa.m_data[2] + f * (double)fb.m_data[2]);
            updateCalMag(fc, i);
        }

        //// orientations
        if (pa.m_infoList[i].m_oriQuat != CMT_DATA_ITEM_NOT_AVAILABLE &&
            pb.m_infoList[i].m_oriQuat != CMT_DATA_ITEM_NOT_AVAILABLE)
        {
            qf = pa.getOriQuat(i);
            qa.isArray(qf.m_data);

            qf = pb.getOriQuat(i);
            qb.isArray(qf.m_data);

            qc.isSlerp(qa, qb, f);
            qc.setArray(qf.m_data);

            updateOriQuat(qf, i);
        }

        if (pa.m_infoList[i].m_oriEul != CMT_DATA_ITEM_NOT_AVAILABLE &&
            pb.m_infoList[i].m_oriEul != CMT_DATA_ITEM_NOT_AVAILABLE)
        {
            ea = pa.getOriEuler(i);
            eb = pb.getOriEuler(i);
            ec.m_roll = (g * (double)ea.m_roll + f * (double)eb.m_roll);
            ec.m_pitch = (g * (double)ea.m_pitch + f * (double)eb.m_pitch);
            ec.m_yaw = (g * (double)ea.m_yaw + f * (double)eb.m_yaw);
            updateOriEuler(ec, i);
        }

        if (pa.m_infoList[i].m_oriMat != CMT_DATA_ITEM_NOT_AVAILABLE &&
            pb.m_infoList[i].m_oriMat != CMT_DATA_ITEM_NOT_AVAILABLE)
        {
            mf = pa.getOriMatrix(i);
            m.isArray(&mf.m_data[0][0]);
            qa.isRmat(m);

            mf = pb.getOriMatrix(i);
            m.isArray(&mf.m_data[0][0]);
            qb.isRmat(m);

            qc.isSlerp(qa, qb, f);
            m.isQuat(qc);

            m.setArray(&mf.m_data[0][0]);
            updateOriMatrix(mf, i);
        }

        //// other
        if (pa.m_infoList[i].m_sc != CMT_DATA_ITEM_NOT_AVAILABLE &&
            pb.m_infoList[i].m_sc != CMT_DATA_ITEM_NOT_AVAILABLE)
        {
            sa = pa.getSampleCounter(i);
            sb = pb.getSampleCounter(i);
            sc = sb - sa;
            dc = f * (double)sc;
            sc = sa + (uint16_t)doubleToLong(dc);
            updateSampleCounter(sc, i);
        }

        if (pa.m_infoList[i].m_acc_g != CMT_DATA_ITEM_NOT_AVAILABLE &&
            pb.m_infoList[i].m_acc_g != CMT_DATA_ITEM_NOT_AVAILABLE)
        {
            fa = pa.getAccG(i);
            fb = pb.getAccG(i);
            fc.m_data[0] =
                (g * (double)fa.m_data[0] + f * (double)fb.m_data[0]);
            fc.m_data[1] =
                (g * (double)fa.m_data[1] + f * (double)fb.m_data[1]);
            fc.m_data[2] =
                (g * (double)fa.m_data[2] + f * (double)fb.m_data[2]);
            updateAccG(fc, i);
        }
    }
    // some remaining stuff
    // TOA remains equal to pb TOA
    m_rtc = (TimeStamp)(g * (double)pa.m_rtc + f * (double)pb.m_rtc);
}
#endif

}  // end of xsens namespace