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