xsdatapacket.cpp

Go to the documentation of this file.

 
//  Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
//  All rights reserved.
//  
//  Redistribution and use in source and binary forms, with or without modification,
//  are permitted provided that the following conditions are met:
//  
//  1.  Redistributions of source code must retain the above copyright notice,
//      this list of conditions, and the following disclaimer.
//  
//  2.  Redistributions in binary form must reproduce the above copyright notice,
//      this list of conditions, and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//  
//  3.  Neither the names of the copyright holders nor the names of their contributors
//      may be used to endorse or promote products derived from this software without
//      specific prior written permission.
//  
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
//  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
//  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
//  THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
//  SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
//  OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
//  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR
//  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS 
//  SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES 
//  OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE 
//  ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
//  
 
 
//  Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
//  All rights reserved.
//  
//  Redistribution and use in source and binary forms, with or without modification,
//  are permitted provided that the following conditions are met:
//  
//  1.  Redistributions of source code must retain the above copyright notice,
//      this list of conditions, and the following disclaimer.
//  
//  2.  Redistributions in binary form must reproduce the above copyright notice,
//      this list of conditions, and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//  
//  3.  Neither the names of the copyright holders nor the names of their contributors
//      may be used to endorse or promote products derived from this software without
//      specific prior written permission.
//  
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
//  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
//  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
//  THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
//  SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
//  OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
//  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR
//  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS 
//  SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES 
//  OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE 
//  ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
//  
 
#include "xsdatapacket.h"
#include "datapacket_p.h"
#include "xsmatrix3x3.h"
#include "xsvector.h"
#include "xsrssi.h"
#include "xsmath.h"
#include <string.h>
 
#define MAP     (*thisPtr->d)
 
static const uint64_t coarseFactor = 10000ULL;
 
using namespace XsDataPacket_Private;
 
static void detach(XsDataPacket* thisPtr)
{
        if (thisPtr->d->m_refCount == 1)
                return;
        DataPacketPrivate* old = thisPtr->d;
        thisPtr->d = new DataPacketPrivate(*old);
        if (--old->m_refCount == 0)     // this can happen in some concurrent situations
                delete old;
}
 
Variant* createVariant(XsDataIdentifier id)
{
        // It may be faster to create a static map with construct functions instead of this switch, but this is a much simpler implementation
        switch (id & XDI_FullTypeMask)
        {
                //XDI_TemperatureGroup          = 0x0800,
                case XDI_Temperature:
                        return new SimpleVariant<double>(id);
                //case XDI_TimestampGroup               :// 0x1000,
                case XDI_UtcTime                                :// 0x1010,
                        return new XsTimeInfoVariant(id);
                case XDI_PacketCounter                  :// 0x1020,
                        return new SimpleVariant<uint16_t>(id);
                case XDI_Itow                                   :// 0x1030,
                        return new SimpleVariant<uint32_t>(id);
                case XDI_GnssAge                                :// 0x1040,
                        return new SimpleVariant<uint8_t>(id);
                case XDI_PressureAge                    :// 0x1050,
                        return new SimpleVariant<uint8_t>(id);
                case XDI_SampleTimeFine                 :// 0x1060,
                case XDI_SampleTimeCoarse               :// 0x1070,
                        return new SimpleVariant<uint32_t>(id);
                case XDI_FrameRange                             :// 0x1080,     // add for MTw (if needed)
                        return new XsRangeVariant(id);
                case XDI_PacketCounter8                 :// 0x1090,
                        return new SimpleVariant<uint8_t>(id);
                case XDI_SampleTime64                   :// 0x10A0,
                        return new SimpleVariant<uint64_t>(id);
 
                //case XDI_OrientationGroup             :// 0x2000,
                case XDI_Quaternion                             :// 0x2010,
                        return new XsQuaternionVariant(id);
                case XDI_RotationMatrix                 :// 0x2020,
                        return new XsMatrixVariant(id);
                case XDI_EulerAngles                    :// 0x2030,
                        return new XsEulerVariant(id);
 
                //case XDI_PressureGroup                :// 0x3000,
                case XDI_BaroPressure                   :// 0x3010,
                        return new SimpleVariant<uint32_t>(id);
 
                //case XDI_AccelerationGroup    :// 0x4000,
                case XDI_DeltaV                                 :// 0x4010,
                case XDI_Acceleration                   :// 0x4020,
                case XDI_FreeAcceleration               :// 0x4030,
                case XDI_AccelerationHR                 :// 0x4040,
                        return new XsVector3Variant(id);
 
                //case XDI_PositionGroup                :// 0x5000,
                case XDI_AltitudeMsl                    :// 0x5010,
                case XDI_AltitudeEllipsoid              :// 0x5020,
                        return new SimpleVariant<double>(id);
                case XDI_PositionEcef                   :// 0x5030,
                        return new XsVector3Variant(id);
                case XDI_LatLon                                 :// 0x5040,
                        return new XsVector2Variant(id);
 
                //case XDI_SnapshotGroup                :// 0xC800,
                //case XDI_RetransmissionMask   :// 0x0001,
                //case XDI_RetransmissionFlag   :// 0x0001,
                case XDI_AwindaSnapshot         :// 0xC810,
                        return new XsAwindaSnapshotVariant(id);
                case XDI_FullSnapshot                   :// 0xC820,
                        return new XsFullSnapshotVariant(id);
 
                //case XDI_GnssGroup                    :// 0x7000,
                case XDI_GnssPvtData                    :// 0x7010,
                        return new XsRawGnssPvtDataVariant(id);
                case XDI_GnssSatInfo                    :// 0x7020,
                        return new XsRawGnssSatInfoVariant(id);
                case XDI_GnssPvtPulse                   :// 0x7030
                        return new SimpleVariant<uint32_t>(id);
 
                //case XDI_AngularVelocityGroup :// 0x8000,
                case XDI_RateOfTurn                             :// 0x8020,
                case XDI_RateOfTurnHR                   :// 0x8040,
                        return new XsVector3Variant(id);
                case XDI_DeltaQ                                 :// 0x8030,
                        return new XsQuaternionVariant(id);
 
                //case XDI_RawSensorGroup               :// 0xA000,
                //case XDI_RawUnsigned                  :// 0x0000, //!< Tracker produces unsigned raw values, usually fixed behavior
                //case XDI_RawSigned                    :// 0x0001, //!< Tracker produces signed raw values, usually fixed behavior
                case XDI_RawAccGyrMagTemp               :// 0xA010,
                        return new XsScrDataVariant(id);
 
                case XDI_RawGyroTemp                    :// 0xA020,
                case XDI_RawAcc                                 :// 0xA030,
                case XDI_RawGyr                                 :// 0xA040,
                case XDI_RawMag                                 :// 0xA050,
                        return new XsUShortVectorVariant(id);
 
                case XDI_RawDeltaQ                              :// 0xA060,
                        return new XsQuaternionVariant(id);
                case XDI_RawDeltaV                              :// 0xA070,
                        return new XsVector3Variant(id);
 
                //case XDI_AnalogInGroup                :// 0xB000,
                case XDI_AnalogIn1                              :// 0xB010,
                case XDI_AnalogIn2                              :// 0xB020,
                        return new SimpleVariant<uint16_t>(id);
 
                //case XDI_MagneticGroup                :// 0xC000,
                case XDI_MagneticField                  :// 0xC020,
                case XDI_MagneticFieldCorrected :// 0xC040,
 
                //case XDI_VelocityGroup                :// 0xD000,
                case XDI_VelocityXYZ                    :// 0xD010,
                        return new XsVector3Variant(id);
 
                //case XDI_StatusGroup                  :// 0xE000,
                case XDI_StatusByte                             :// 0xE010,
                        return new SimpleVariant<uint8_t>(id);
                case XDI_StatusWord                             :// 0xE020,
                        return new SimpleVariant<uint32_t>(id);
                case XDI_Rssi                                   :// 0xE040,
                        return new SimpleVariant<uint8_t>(id);
                case XDI_DeviceId                               :// 0xE080,
                        return new SimpleVariant<uint32_t>(id);
                case XDI_LocationId                             :// 0xE090
                        return new SimpleVariant<uint16_t>(id);
 
                //case XDI_IndicationGroup              :// 0x4800, // 0100.1000 -> bit reverse = 0001.0010 -> type 18
                case XDI_TriggerIn1                             :// 0x4810,
                case XDI_TriggerIn2                             :// 0x4820,
                        return new XsTriggerIndicationDataVariant(id);
 
                case XDI_RawBlob                                :// 0xA080
                        return new XsByteArrayVariant(id);
 
                case XDI_GloveSnapshotLeft:                     // 0xC830
                case XDI_GloveSnapshotRight:            // 0xC840
                        return new XsGloveSnapshotVariant(id);
 
                case XDI_GloveDataLeft:                         // 0xC930
                case XDI_GloveDataRight:                        // 0xC940
                        return new XsGloveDataVariant(id);
 
                default:
                        //JLERRORG("Unknown id: " << id);
                        assert(0);
                        return nullptr;
        }
}
 
XsUShortVector* rawVector(const XsDataPacket* thisPtr, XsUShortVector* returnVal, XsDataIdentifier id, XsUShortVector XsScrData::* field)
{
        assert(returnVal);
        auto it = MAP.find(XDI_RawAccGyrMagTemp);
        if (it != MAP.end())
                *returnVal = it->second->toDerived<XsScrDataVariant>().m_data.*field;
        else
        {
                it = MAP.find(id);
                if (it != MAP.end())
                        *returnVal = it->second->toDerived<XsUShortVectorVariant>().m_data;
        }
        return returnVal;
}
 
void setRawVector(XsDataPacket* thisPtr, const XsUShortVector* vec, XsDataIdentifier id, XsUShortVector XsScrData::* field)
{
        detach(thisPtr);
        auto it = MAP.find(XDI_RawAccGyrMagTemp);
        if (it != MAP.end())
                it->second->toDerived<XsScrDataVariant>().m_data.*field = *vec;
        else
        {
                it = MAP.find(id);
                if (it != MAP.end())
                        it->second->toDerived<XsUShortVectorVariant>().m_data = *vec;
                else
                        MAP.insert(id, new XsUShortVectorVariant(id, *vec));
        }
}
 
template <typename T, typename V>
T* genericGet(const XsDataPacket* thisPtr, T* returnVal, XsDataIdentifier id, T const& failValue = T())
{
        assert(returnVal);
        auto it = MAP.find(id);
        if (it != MAP.end())
                *returnVal = it->second->toDerived<V>().m_data;
        else
                *returnVal = failValue;
        return returnVal;
}
 
template <typename T, typename V>
void genericSet(XsDataPacket* thisPtr, T const* val, XsDataIdentifier id)
{
        detach(thisPtr);
        assert(val);
        auto it = MAP.find(id);
        if (it != MAP.end())
        {
                it->second->toDerived<V>().m_data = *val;
                it->second->setDataId(id);
        }
        else
                MAP.insert(id, new V(id, *val));
}
 
template <typename T, typename V = SimpleVariant<T>>
struct GenericSimple
{
        static T get(const XsDataPacket* thisPtr, XsDataIdentifier id, T const& failValue = T())
        {
                auto it = MAP.find(id);
                if (it != MAP.end())
                        return it->second->toDerived<V>().m_data;
                return failValue;
        }
 
        static void set(XsDataPacket* thisPtr, T val, XsDataIdentifier id)
        {
                detach(thisPtr);
                auto it = MAP.find(id);
                if (it != MAP.end())
                        it->second->toDerived<V>().m_data = val;
                else
                        MAP.insert(id, new V(id, val));
        }
};
 
inline bool genericContains(const XsDataPacket* thisPtr, XsDataIdentifier id)
{
        return MAP.find(id) != MAP.end();
}
 
int XsDataPacket_containsOrientationQuaternion(const XsDataPacket* thisPtr)
{
        return genericContains(thisPtr, XDI_Quaternion);
}
 
int XsDataPacket_containsOrientationEuler(const XsDataPacket* thisPtr)
{
        return genericContains(thisPtr, XDI_EulerAngles);
}
 
int XsDataPacket_containsOrientationMatrix(const XsDataPacket* thisPtr)
{
        return genericContains(thisPtr, XDI_RotationMatrix);
}
 
XsQuaternion preRotFromXdi(XsDataIdentifier actual, XsDataIdentifier desired)
{
        static const XsQuaternion q_id(1, 0, 0, 0);                                                                             // x -> x
        static const XsQuaternion q_nwu2ned(0, 1, 0, 0);                                                                        // nwu -> ned = 180 degrees x
        static const XsQuaternion q_enu2nwu(1.4142135623730950488016887242097 * 0.5, 0, 0, -1.4142135623730950488016887242097 * 0.5);   // enu -> nwu = 90 degrees z
        static const XsQuaternion q_enu2ned(0, -1.4142135623730950488016887242097 * 0.5, -1.4142135623730950488016887242097 * 0.5, 0);  // enu -> ned = 90 degrees z followed by 180 degrees x
 
        static const XsQuaternion q_ned2nwu(0, -1, 0, 0);
        static const XsQuaternion q_nwu2enu(1.4142135623730950488016887242097 * 0.5, 0, 0, 1.4142135623730950488016887242097 * 0.5);
        static const XsQuaternion q_ned2enu(0, 1.4142135623730950488016887242097 * 0.5, 1.4142135623730950488016887242097 * 0.5, 0);
 
        switch (desired & XDI_CoordSysMask)
        {
                default:
                case XDI_CoordSysEnu:
                        switch (actual & XDI_CoordSysMask)
                        {
                                default:
                                case XDI_CoordSysEnu:
                                        return q_id;
 
                                case XDI_CoordSysNed:
                                        return q_ned2enu;
 
                                case XDI_CoordSysNwu:
                                        return q_nwu2enu;
                        }
 
                case XDI_CoordSysNed:
                        switch (actual & XDI_CoordSysMask)
                        {
                                default:
                                case XDI_CoordSysEnu:
                                        return q_enu2ned;
 
                                case XDI_CoordSysNed:
                                        return q_id;
 
                                case XDI_CoordSysNwu:
                                        return q_nwu2ned;
                        }
 
                case XDI_CoordSysNwu:
                        switch (actual & XDI_CoordSysMask)
                        {
                                default:
                                case XDI_CoordSysEnu:
                                        return q_enu2nwu;
 
                                case XDI_CoordSysNed:
                                        return q_ned2nwu;
 
                                case XDI_CoordSysNwu:
                                        return q_id;
                        }
        }
}
 
extern "C" {
 
        void XsDataPacket_construct(XsDataPacket* thisPtr)
        {
                thisPtr->d = new DataPacketPrivate;
                thisPtr->m_deviceId = 0;
                thisPtr->m_toa = 0;
                thisPtr->m_packetId = -1;
                thisPtr->m_etos = 0;
        }
 
        void XsDataPacket_copyConstruct(XsDataPacket* thisPtr, XsDataPacket const* src)
        {
                ++src->d->m_refCount;
                thisPtr->d = src->d;
                thisPtr->m_deviceId = src->m_deviceId;
                thisPtr->m_toa = src->m_toa;
                thisPtr->m_packetId = src->m_packetId;
                thisPtr->m_etos = src->m_etos;
        }
 
        void XsDataPacket_destruct(XsDataPacket* thisPtr)
        {
                if (thisPtr->d && --thisPtr->d->m_refCount == 0)
                        delete thisPtr->d;
                thisPtr->d = nullptr;
        }
 
        void XsDataPacket_clear(XsDataPacket* thisPtr, XsDataIdentifier id)
        {
                detach(thisPtr);
                if (id == XDI_None)
                {
                        XsDataPacket_destruct(thisPtr);
                        XsDataPacket_construct(thisPtr);
                }
                else
                        MAP.erase(id);
        }
 
        void XsDataPacket_copy(XsDataPacket* copy, XsDataPacket const* src)
        {
                if (copy->d != src->d)
                {
                        ++src->d->m_refCount;
                        if (--copy->d->m_refCount == 0)
                                delete copy->d;
                        copy->d = src->d;
                }
                copy->m_deviceId = src->m_deviceId;
                copy->m_toa = src->m_toa;
                copy->m_packetId = src->m_packetId;
                copy->m_etos = src->m_etos;
        }
 
        void XsDataPacket_swap(XsDataPacket* thisPtr, XsDataPacket* other)
        {
                using std::swap;
                swap(thisPtr->d, other->d);
                swap(thisPtr->m_deviceId, other->m_deviceId);
                swap(thisPtr->m_toa, other->m_toa);
                swap(thisPtr->m_packetId, other->m_packetId);
                swap(thisPtr->m_etos, other->m_etos);
        }
 
        int XsDataPacket_empty(const XsDataPacket* thisPtr)
        {
                return 0 != MAP.empty();
        }
 
        int XsDataPacket_itemCount(const XsDataPacket* thisPtr)
        {
                assert(thisPtr);
                return (int) MAP.size();
        }
 
        XsDataIdentifier XsDataPacket_dataFormat(const XsDataPacket* thisPtr, XsDataIdentifier id)
        {
                auto it = MAP.find(id);
                if (it == MAP.end())
                        return XDI_None;
                return it->second->dataId() & XDI_SubFormatMask;
        }
 
        static XsReal signed_cast(uint16_t v)
        {
                return (int16_t)v;
        }
 
        static XsReal unsigned_cast(uint16_t v)
        {
                return v;
        }
 
        XsUShortVector* XsDataPacket_rawAcceleration(const XsDataPacket* thisPtr, XsUShortVector* returnVal)
        {
                return rawVector(thisPtr, returnVal, XDI_RawAcc, &XsScrData::m_acc);
        }
 
        int XsDataPacket_containsRawAcceleration(const XsDataPacket* thisPtr)
        {
                return  MAP.find(XDI_RawAccGyrMagTemp) != MAP.end() ||
                        MAP.find(XDI_RawAcc) != MAP.end();
        }
 
        void XsDataPacket_setRawAcceleration(XsDataPacket* thisPtr, const XsUShortVector* vec)
        {
                setRawVector(thisPtr, vec, XDI_RawAcc, &XsScrData::m_acc);
        }
 
        XsUShortVector* XsDataPacket_rawGyroscopeData(const XsDataPacket* thisPtr, XsUShortVector* returnVal)
        {
                return rawVector(thisPtr, returnVal, XDI_RawGyr, &XsScrData::m_gyr);
        }
 
        int XsDataPacket_containsRawGyroscopeData(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_RawAccGyrMagTemp) || genericContains(thisPtr, XDI_RawGyr);
        }
 
        void XsDataPacket_setRawGyroscopeData(XsDataPacket* thisPtr, const XsUShortVector* vec)
        {
                setRawVector(thisPtr, vec, XDI_RawGyr, &XsScrData::m_gyr);
        }
 
        XsUShortVector* XsDataPacket_rawMagneticField(const XsDataPacket* thisPtr, XsUShortVector* returnVal)
        {
                return rawVector(thisPtr, returnVal, XDI_RawMag, &XsScrData::m_mag);
        }
 
        int XsDataPacket_containsRawMagneticField(const XsDataPacket* thisPtr)
        {
                return  MAP.find(XDI_RawAccGyrMagTemp) != MAP.end() ||
                        MAP.find(XDI_RawMag) != MAP.end();
        }
 
        void XsDataPacket_setRawMagneticField(XsDataPacket* thisPtr, const XsUShortVector* vec)
        {
                setRawVector(thisPtr, vec, XDI_RawMag, &XsScrData::m_mag);
        }
 
        uint16_t XsDataPacket_rawTemperature(const XsDataPacket* thisPtr)
        {
                auto it = MAP.find(XDI_RawAccGyrMagTemp);
                if (it != MAP.end())
                        return it->second->toDerived<XsScrDataVariant>().m_data.m_temp;
                else
                        return 0;
        }
 
        int XsDataPacket_containsRawTemperature(const XsDataPacket* thisPtr)
        {
                return  MAP.find(XDI_RawAccGyrMagTemp) != MAP.end();
        }
 
        void XsDataPacket_setRawTemperature(XsDataPacket* thisPtr, uint16_t temp)
        {
                detach(thisPtr);
                auto it = MAP.find(XDI_RawAccGyrMagTemp);
                if (it != MAP.end())
                        it->second->toDerived<XsScrDataVariant>().m_data.m_temp = temp;
                else
                {
                        auto v = new XsScrDataVariant(XDI_RawAccGyrMagTemp);
                        v->m_data.m_temp = temp;
                        MAP.insert(XDI_RawAccGyrMagTemp, v);
                }
        }
 
        XsUShortVector* XsDataPacket_rawGyroscopeTemperatureData(const XsDataPacket* thisPtr, XsUShortVector* returnVal)
        {
                assert(returnVal);
                auto it = MAP.find(XDI_RawGyroTemp);
                if (it != MAP.end())
                        *returnVal = it->second->toDerived<XsUShortVectorVariant>().m_data;
                return returnVal;
        }
 
        int XsDataPacket_containsRawGyroscopeTemperatureData(const XsDataPacket* thisPtr)
        {
                return MAP.find(XDI_RawGyroTemp) != MAP.end();
        }
 
        void XsDataPacket_setRawGyroscopeTemperatureData(XsDataPacket* thisPtr, const XsUShortVector* vec)
        {
                detach(thisPtr);
                auto it = MAP.find(XDI_RawGyroTemp);
                if (it != MAP.end())
                        it->second->toDerived<XsUShortVectorVariant>().m_data = *vec;
                else
                {
                        auto v = new XsUShortVectorVariant(XDI_RawGyroTemp);
                        v->m_data = *vec;
                        MAP.insert(XDI_RawGyroTemp, v);
                }
        }
 
        XsScrData* XsDataPacket_rawData(const XsDataPacket* thisPtr, XsScrData* returnVal)
        {
                assert(returnVal);
                auto it = MAP.find(XDI_RawAccGyrMagTemp);
                if (it != MAP.end())
                        *returnVal = it->second->toDerived<XsScrDataVariant>().m_data;
                else
                {
                        for (XsSize i = 0; i < 3; ++i)
                        {
                                returnVal->m_acc[i] = 0;
                                returnVal->m_gyr[i] = 0;
                                returnVal->m_mag[i] = 0;
                        }
                        returnVal->m_temp = 0;
                }
                return returnVal;       // not found
        }
 
        int XsDataPacket_containsRawData(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_RawAccGyrMagTemp);
        }
 
        void XsDataPacket_setRawData(XsDataPacket* thisPtr, const XsScrData* data)
        {
                detach(thisPtr);
                auto it = MAP.find(XDI_RawAccGyrMagTemp);
                if (it != MAP.end())
                        it->second->toDerived<XsScrDataVariant>().m_data = *data;
                else
                {
                        auto v = new XsScrDataVariant(XDI_RawAccGyrMagTemp);
                        v->m_data = *data;
                        MAP.insert(XDI_RawAccGyrMagTemp, v);
                }
        }
 
        XsVector* XsDataPacket_velocityIncrement(const XsDataPacket* thisPtr, XsVector* returnVal)
        {
                return genericGet<XsVector, XsVector3Variant>(thisPtr, returnVal, XDI_DeltaV);
        }
 
        int XsDataPacket_containsVelocityIncrement(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_DeltaV);
        }
 
        void XsDataPacket_setVelocityIncrement(XsDataPacket* thisPtr, const XsVector* vec)
        {
                genericSet<XsVector, XsVector3Variant>(thisPtr, vec, XDI_DeltaV | XDI_SubFormatDouble);
        }
 
        XsVector* XsDataPacket_calibratedAcceleration(const XsDataPacket* thisPtr, XsVector* returnVal)
        {
                return genericGet<XsVector, XsVector3Variant>(thisPtr, returnVal, XDI_Acceleration);
        }
 
        int XsDataPacket_containsCalibratedAcceleration(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_Acceleration);
        }
 
        void XsDataPacket_setCalibratedAcceleration(XsDataPacket* thisPtr, const XsVector* vec)
        {
                genericSet<XsVector, XsVector3Variant>(thisPtr, vec, XDI_Acceleration | XDI_SubFormatDouble);
        }
 
 
        XsQuaternion* XsDataPacket_orientationIncrement(const XsDataPacket* thisPtr, XsQuaternion* returnVal)
        {
                return genericGet<XsQuaternion, XsQuaternionVariant>(thisPtr, returnVal, XDI_DeltaQ);
        }
 
        int XsDataPacket_containsOrientationIncrement(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_DeltaQ);
        }
 
        void XsDataPacket_setOrientationIncrement(XsDataPacket* thisPtr, const XsQuaternion* quat)
        {
                genericSet<XsQuaternion, XsQuaternionVariant>(thisPtr, quat, XDI_DeltaQ | XDI_SubFormatDouble);
        }
 
        XsVector* XsDataPacket_calibratedGyroscopeData(const XsDataPacket* thisPtr, XsVector* returnVal)
        {
                return genericGet<XsVector, XsVector3Variant>(thisPtr, returnVal, XDI_RateOfTurn);
        }
 
        int XsDataPacket_containsCalibratedGyroscopeData(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_RateOfTurn);
        }
 
        void XsDataPacket_setCalibratedGyroscopeData(XsDataPacket* thisPtr, const XsVector* vec)
        {
                genericSet<XsVector, XsVector3Variant>(thisPtr, vec, XDI_RateOfTurn | XDI_SubFormatDouble);
        }
 
        XsVector* XsDataPacket_calibratedMagneticField(const XsDataPacket* thisPtr, XsVector* returnVal)
        {
                return genericGet<XsVector, XsVector3Variant>(thisPtr, returnVal, XDI_MagneticField);
        }
 
        int XsDataPacket_containsCalibratedMagneticField(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_MagneticField);
        }
 
        void XsDataPacket_setCalibratedMagneticField(XsDataPacket* thisPtr, const XsVector* vec)
        {
                genericSet<XsVector, XsVector3Variant>(thisPtr, vec, XDI_MagneticField | XDI_SubFormatDouble);
        }
 
        XsCalibratedData* XsDataPacket_calibratedData(const XsDataPacket* thisPtr, XsCalibratedData* returnVal)
        {
                assert(returnVal);
 
                XsDataPacket_calibratedAcceleration(thisPtr, &returnVal->m_acc);
                XsDataPacket_calibratedGyroscopeData(thisPtr, &returnVal->m_gyr);
                XsDataPacket_calibratedMagneticField(thisPtr, &returnVal->m_mag);
 
                return returnVal;
        }
 
        int XsDataPacket_containsCalibratedData(const XsDataPacket* thisPtr)
        {
                // Note: calibrated data is only present if *all* components are present
                return  genericContains(thisPtr, XDI_Acceleration) &&
                        genericContains(thisPtr, XDI_RateOfTurn) &&
                        genericContains(thisPtr, XDI_MagneticField);
        }
 
        void XsDataPacket_setCalibratedData(XsDataPacket* thisPtr, const XsCalibratedData* data)
        {
                XsDataPacket_setCalibratedAcceleration(thisPtr, &data->m_acc);
                XsDataPacket_setCalibratedGyroscopeData(thisPtr, &data->m_gyr);
                XsDataPacket_setCalibratedMagneticField(thisPtr, &data->m_mag);
        }
 
        XsVector* XsDataPacket_correctedMagneticField(const XsDataPacket* thisPtr, XsVector* returnVal)
        {
                return genericGet<XsVector, XsVector3Variant>(thisPtr, returnVal, XDI_MagneticFieldCorrected);
        }
 
        int XsDataPacket_containsCorrectedMagneticField(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_MagneticFieldCorrected);
        }
 
        void XsDataPacket_setCorrectedMagneticField(XsDataPacket* thisPtr, const XsVector* vec)
        {
                genericSet<XsVector, XsVector3Variant>(thisPtr, vec, XDI_MagneticFieldCorrected | XDI_SubFormatDouble);
        }
 
        XsQuaternion* XsDataPacket_orientationQuaternion(const XsDataPacket* thisPtr, XsQuaternion* returnVal, XsDataIdentifier coordinateSystem)
        {
                assert(returnVal);
                auto it = MAP.find(XDI_Quaternion);
                if (it != MAP.end())
                {
                        *returnVal = it->second->toDerived<XsQuaternionVariant>().m_data;
                        XsDataIdentifier foundId = it->second->dataId();
                        if ((coordinateSystem & XDI_CoordSysMask) != (foundId & XDI_CoordSysMask))
                        {
                                XsQuaternion rot;
                                rot = preRotFromXdi(foundId, coordinateSystem);
                                XsQuaternion_multiply(&rot, returnVal, returnVal);
                        }
                }
                else if (XsDataPacket_containsOrientationMatrix(thisPtr))
                {
                        XsMatrix3x3 m;
                        XsDataPacket_orientationMatrix(thisPtr, &m, coordinateSystem);
                        returnVal->fromRotationMatrix(m);
                }
                else if (XsDataPacket_containsOrientationEuler(thisPtr))
                {
                        XsEuler eul;
                        XsDataPacket_orientationEuler(thisPtr, &eul, coordinateSystem);
                        returnVal->fromEulerAngles(eul);
                }
                //else
                //      memset(returnVal->m_data, 0, 4*sizeof(XsReal));
 
                return returnVal;
        }
 
        static void removeAllOrientations(XsDataPacket* thisPtr)
        {
                detach(thisPtr);
                MAP.erase(XDI_Quaternion);
                MAP.erase(XDI_RotationMatrix);
                MAP.erase(XDI_EulerAngles);
        }
 
        void XsDataPacket_setOrientationQuaternion(XsDataPacket* thisPtr, const XsQuaternion* data, XsDataIdentifier coordinateSystem)
        {
                // always create a new one
                removeAllOrientations(thisPtr);
                MAP.insert(XDI_Quaternion, new XsQuaternionVariant(XDI_Quaternion | XDI_SubFormatDouble | (coordinateSystem & XDI_CoordSysMask), *data));
        }
 
        XsEuler* XsDataPacket_orientationEuler(const XsDataPacket* thisPtr, XsEuler* returnVal, XsDataIdentifier coordinateSystem)
        {
                assert(returnVal);
                auto it = MAP.find(XDI_EulerAngles);
                if (it != MAP.end())
                {
                        *returnVal = it->second->toDerived<XsEulerVariant>().m_data;
                        XsDataIdentifier foundId = it->second->dataId();
                        if ((coordinateSystem & XDI_CoordSysMask) != (foundId & XDI_CoordSysMask))
                        {
                                XsQuaternion rot, q;
                                rot = preRotFromXdi(foundId, coordinateSystem);
                                XsQuaternion_fromEulerAngles(&q, returnVal);
                                XsQuaternion_multiply(&rot, &q, &q);
                                XsEuler_fromQuaternion(returnVal, &q);
                        }
                }
                else if (XsDataPacket_containsOrientationMatrix(thisPtr))
                {
                        XsMatrix3x3 m;
                        XsDataPacket_orientationMatrix(thisPtr, &m, coordinateSystem);
                        XsQuaternion q;
                        q.fromRotationMatrix(m);
                        returnVal->fromQuaternion(q);
                }
                else if (XsDataPacket_containsOrientationQuaternion(thisPtr))
                {
                        XsQuaternion q;
                        XsDataPacket_orientationQuaternion(thisPtr, &q, coordinateSystem);
                        returnVal->fromQuaternion(q);
                }
                //else
                //{
                //      returnVal->m_x = XsMath_zero;
                //      returnVal->m_y = XsMath_zero;
                //      returnVal->m_z = XsMath_zero;
                //}
 
                return returnVal;
        }
 
        void XsDataPacket_setOrientationEuler(XsDataPacket* thisPtr, const XsEuler* data, XsDataIdentifier coordinateSystem)
        {
                // always create a new one
                removeAllOrientations(thisPtr);
                MAP.insert(XDI_EulerAngles, new XsEulerVariant(XDI_EulerAngles | XDI_SubFormatDouble | (coordinateSystem & XDI_CoordSysMask), *data));
        }
 
        XsMatrix* XsDataPacket_orientationMatrix(const XsDataPacket* thisPtr, XsMatrix* returnVal, XsDataIdentifier coordinateSystem)
        {
                assert(returnVal);
                auto it = MAP.find(XDI_RotationMatrix);
                if (it != MAP.end())
                {
                        *returnVal = it->second->toDerived<XsMatrixVariant>().m_data;
                        XsDataIdentifier foundId = it->second->dataId();
                        if ((coordinateSystem & XDI_CoordSysMask) != (foundId & XDI_CoordSysMask))
                        {
                                XsQuaternion rot, q;
                                rot = preRotFromXdi(foundId, coordinateSystem);
                                XsQuaternion_fromRotationMatrix(&q, returnVal);
                                XsQuaternion_multiply(&rot, &q, &q);
                                XsMatrix_fromQuaternion(returnVal, &q);
                        }
                }
                else if (XsDataPacket_containsOrientationQuaternion(thisPtr))
                {
                        XsQuaternion q;
                        XsDataPacket_orientationQuaternion(thisPtr, &q, coordinateSystem);
                        returnVal->fromQuaternion(q);
                }
                else if (XsDataPacket_containsOrientationEuler(thisPtr))
                {
                        XsEuler eul;
                        XsDataPacket_orientationEuler(thisPtr, &eul, coordinateSystem);
                        XsQuaternion q;
                        q.fromEulerAngles(eul);
                        returnVal->fromQuaternion(q);
                }
                //else
                //      memset(returnVal->m_data, 0, 4*sizeof(XsReal));
 
                return returnVal;
        }
 
        void XsDataPacket_setOrientationMatrix(XsDataPacket* thisPtr, const XsMatrix* data, XsDataIdentifier coordinateSystem)
        {
                // always create a new one
                removeAllOrientations(thisPtr);
                MAP.insert(XDI_RotationMatrix, new XsMatrixVariant(XDI_RotationMatrix | XDI_SubFormatDouble | (coordinateSystem & XDI_CoordSysMask), *data));
        }
 
        int XsDataPacket_containsOrientation(const XsDataPacket* thisPtr)
        {
                return  genericContains(thisPtr, XDI_Quaternion) ||
                        genericContains(thisPtr, XDI_EulerAngles) ||
                        genericContains(thisPtr, XDI_RotationMatrix);
        }
 
        XsDataIdentifier XsDataPacket_orientationIdentifier(const XsDataPacket* thisPtr)
        {
                auto it = MAP.find(XDI_Quaternion);
                if (it != MAP.end())
                        return it->second->dataId();
 
                it = MAP.find(XDI_EulerAngles);
                if (it != MAP.end())
                        return it->second->dataId();
 
                it = MAP.find(XDI_RotationMatrix);
                if (it != MAP.end())
                        return it->second->dataId();
 
                return XDI_None;
        }
 
        XsDataIdentifier XsDataPacket_coordinateSystemOrientation(const XsDataPacket* thisPtr)
        {
                return XsDataPacket_orientationIdentifier(thisPtr) & XDI_CoordSysMask;
        }
 
        int XsDataPacket_containsPressure(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_BaroPressure);
        }
 
        int XsDataPacket_containsPressureAge(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_PressureAge);
        }
 
        XsPressure* XsDataPacket_pressure(const XsDataPacket* thisPtr, XsPressure* returnVal)
        {
                assert(returnVal);
                auto it = MAP.find(XDI_BaroPressure);
                if (it != MAP.end())
                {
                        returnVal->m_pressure = it->second->toDerived<SimpleVariant<uint32_t>>().m_data;
                        returnVal->m_pressureAge = 0;
                }
 
                it = MAP.find(XDI_PressureAge);
                if (it != MAP.end())
                        returnVal->m_pressureAge = it->second->toDerived<SimpleVariant<uint8_t>>().m_data;
 
                return returnVal;
        }
 
        void XsDataPacket_setPressure(XsDataPacket* thisPtr, const XsPressure* data)
        {
                GenericSimple<uint32_t>::set(thisPtr, (uint32_t) XsMath::doubleToLong(data->m_pressure), XDI_BaroPressure);
                GenericSimple<uint8_t>::set(thisPtr, data->m_pressureAge, XDI_PressureAge);
        }
 
        XsSdiData* XsDataPacket_sdiData(const XsDataPacket* thisPtr, XsSdiData* returnVal)
        {
                assert(returnVal);
                auto it = MAP.find(XDI_DeltaQ);
                if (it != MAP.end())
                        returnVal->setOrientationIncrement(it->second->toDerived<XsQuaternionVariant>().m_data);
 
                it = MAP.find(XDI_DeltaV);
                if (it != MAP.end())
                        returnVal->setVelocityIncrement(it->second->toDerived<XsVector3Variant>().m_data);
 
                return returnVal;
        }
 
        int XsDataPacket_containsSdiData(const XsDataPacket* thisPtr)
        {
                return  genericContains(thisPtr, XDI_DeltaQ) &&
                        genericContains(thisPtr, XDI_DeltaV);
        }
 
        void XsDataPacket_setSdiData(XsDataPacket* thisPtr, const XsSdiData* data)
        {
                genericSet<XsQuaternion, XsQuaternionVariant>(thisPtr, &data->orientationIncrement(), XDI_DeltaQ | XDI_SubFormatDouble);
                genericSet<XsVector3, XsVector3Variant>(thisPtr, &data->velocityIncrement(), XDI_DeltaV | XDI_SubFormatDouble);
        }
 
        XsGloveData* XsDataPacket_gloveData(const XsDataPacket* thisPtr, XsGloveData* returnVal, XsHandId hand)
        {
                switch (hand)
                {
                        case XHI_LeftHand:
                                return genericGet<XsGloveData, XsGloveDataVariant>(thisPtr, returnVal, XDI_GloveDataLeft);
                        case XHI_RightHand:
                                return genericGet<XsGloveData, XsGloveDataVariant>(thisPtr, returnVal, XDI_GloveDataRight);
                        case XHI_Unknown:
                        default:
                                XsGloveData_destruct(returnVal);
                                return returnVal;
                }
        }
 
        int XsDataPacket_containsGloveData(const XsDataPacket* thisPtr, XsHandId hand)
        {
                switch (hand)
                {
                        case XHI_LeftHand:
                                return genericContains(thisPtr, XDI_GloveDataLeft);
                        case XHI_RightHand:
                                return genericContains(thisPtr, XDI_GloveDataRight);
                        case XHI_Unknown:
                                return genericContains(thisPtr, XDI_GloveDataLeft) || genericContains(thisPtr, XDI_GloveDataRight);
                        default:
                                return false;
                }
        }
 
        void XsDataPacket_setGloveData(XsDataPacket* thisPtr, const XsGloveData* data, XsHandId hand)
        {
                switch (hand)
                {
                        case XHI_LeftHand:
                                genericSet<XsGloveData, XsGloveDataVariant>(thisPtr, data, XDI_GloveDataLeft);
                                break;
                        case XHI_RightHand:
                                genericSet<XsGloveData, XsGloveDataVariant>(thisPtr, data, XDI_GloveDataRight);
                                break;
                        case XHI_Unknown:
                        default:
                                break;
                }
        }
 
        XsDeviceId* XsDataPacket_storedDeviceId(const XsDataPacket* thisPtr, XsDeviceId* returnVal)
        {
                return genericGet<XsDeviceId, SimpleVariant<uint32_t>>(thisPtr, returnVal, XDI_DeviceId);
        }
 
        int XsDataPacket_containsStoredDeviceId(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_DeviceId);
        }
 
        void XsDataPacket_setStoredDeviceId(XsDataPacket* thisPtr, const XsDeviceId* data)
        {
                GenericSimple<uint32_t>::set(thisPtr, data->legacyDeviceId(), XDI_DeviceId);
        }
 
        uint16_t XsDataPacket_storedLocationId(const XsDataPacket* thisPtr)
        {
                return GenericSimple<uint16_t>::get(thisPtr, XDI_LocationId);
        }
 
        int XsDataPacket_containsStoredLocationId(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_LocationId);
        }
 
        void XsDataPacket_setStoredLocationId(XsDataPacket* thisPtr, uint16_t data)
        {
                GenericSimple<uint16_t>::set(thisPtr, data, XDI_LocationId);
        }
 
        double XsDataPacket_temperature(const XsDataPacket* thisPtr)
        {
                return GenericSimple<double>::get(thisPtr, XDI_Temperature);
        }
 
        int XsDataPacket_containsTemperature(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_Temperature);
        }
 
        void XsDataPacket_setTemperature(XsDataPacket* thisPtr, double temperature)
        {
                GenericSimple<double>::set(thisPtr, temperature, XDI_Temperature | XDI_SubFormatDouble);
        }
 
        XsAnalogInData* XsDataPacket_analogIn1Data(const XsDataPacket* thisPtr, XsAnalogInData* returnVal)
        {
                return genericGet<XsAnalogInData, SimpleVariant<uint16_t>>(thisPtr, returnVal, XDI_AnalogIn1);
        }
 
        int XsDataPacket_containsAnalogIn1Data(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_AnalogIn1);
        }
 
        void XsDataPacket_setAnalogIn1Data(XsDataPacket* thisPtr, const XsAnalogInData* data)
        {
                GenericSimple<uint16_t>::set(thisPtr, data->m_data, XDI_AnalogIn1);
        }
 
        XsAnalogInData* XsDataPacket_analogIn2Data(const XsDataPacket* thisPtr, XsAnalogInData* returnVal)
        {
                return genericGet<XsAnalogInData, SimpleVariant<uint16_t>>(thisPtr, returnVal, XDI_AnalogIn2);
        }
 
        int XsDataPacket_containsAnalogIn2Data(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_AnalogIn2);
        }
 
        void XsDataPacket_setAnalogIn2Data(XsDataPacket* thisPtr, const XsAnalogInData* data)
        {
                GenericSimple<uint16_t>::set(thisPtr, data->m_data, XDI_AnalogIn2);
        }
 
        XsVector* XsDataPacket_positionLLA(const XsDataPacket* thisPtr, XsVector* returnVal)
        {
                assert(returnVal);
                returnVal->setSize(3);
 
                XsVector latlon(2);
                genericGet<XsVector, XsVector2Variant>(thisPtr, &latlon, XDI_LatLon);
                if (!latlon.empty())
                {
                        (*returnVal)[0] = latlon[0];
                        (*returnVal)[1] = latlon[1];
                }
                else
                {
                        (*returnVal)[0] = 0;
                        (*returnVal)[1] = 0;
                }
                // This triggers a warning when XsReal is float, which is deliberately not silenced
                (*returnVal)[2] = GenericSimple<double>::get(thisPtr, XDI_AltitudeEllipsoid);
 
                return returnVal;
        }
 
        int XsDataPacket_containsPositionLLA(const XsDataPacket* thisPtr)
        {
                return  genericContains(thisPtr, XDI_LatLon) &&
                        genericContains(thisPtr, XDI_AltitudeEllipsoid);
        }
 
        void XsDataPacket_setPositionLLA(XsDataPacket* thisPtr, const XsVector* data)
        {
                XsVector latlon(2);
                latlon[0] = (*data)[0];
                latlon[1] = (*data)[1];
 
                genericSet<XsVector, XsVector2Variant>(thisPtr, &latlon, XDI_LatLon | XDI_SubFormatDouble);
                GenericSimple<double>::set(thisPtr, (*data)[2], XDI_AltitudeEllipsoid | XDI_SubFormatDouble);
        }
 
        XsVector* XsDataPacket_latitudeLongitude(const XsDataPacket* thisPtr, XsVector* returnVal)
        {
                return genericGet<XsVector, XsVector2Variant>(thisPtr, returnVal, XDI_LatLon);
        }
 
        int XsDataPacket_containsLatitudeLongitude(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_LatLon);
        }
 
        void XsDataPacket_setLatitudeLongitude(XsDataPacket* thisPtr, const XsVector* data)
        {
                genericSet<XsVector, XsVector2Variant>(thisPtr, data, XDI_LatLon | XDI_SubFormatDouble);
        }
 
        double XsDataPacket_altitude(const XsDataPacket* thisPtr)
        {
                return GenericSimple<double>::get(thisPtr, XDI_AltitudeEllipsoid);
        }
 
        int XsDataPacket_containsAltitude(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_AltitudeEllipsoid);
        }
 
        void XsDataPacket_setAltitude(XsDataPacket* thisPtr, double data)
        {
                GenericSimple<double>::set(thisPtr, data, XDI_AltitudeEllipsoid | XDI_SubFormatDouble);
        }
 
        double XsDataPacket_altitudeMsl(const XsDataPacket* thisPtr)
        {
                return GenericSimple<double>::get(thisPtr, XDI_AltitudeMsl);
        }
 
        int XsDataPacket_containsAltitudeMsl(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_AltitudeMsl);
        }
 
        void XsDataPacket_setAltitudeMsl(XsDataPacket* thisPtr, double data)
        {
                GenericSimple<double>::set(thisPtr, data, XDI_AltitudeMsl | XDI_SubFormatDouble);
        }
 
        XsVector* XsDataPacket_velocity(const XsDataPacket* thisPtr, XsVector* returnVal, XsDataIdentifier coordinateSystem)
        {
                assert(returnVal);
                auto it = MAP.find(XDI_VelocityXYZ);
                if (it != MAP.end())
                {
                        *returnVal = it->second->toDerived<XsVector3Variant>().m_data;
                        XsDataIdentifier actualCsys = (it->second->dataId() & XDI_CoordSysMask);
                        XsDataIdentifier desiredCsys = (coordinateSystem & XDI_CoordSysMask);
                        if (actualCsys != desiredCsys)
                        {
                                XsVector3 vel(*returnVal);
                                XsVector& rv = *returnVal;
 
                                switch (desiredCsys)
                                {
                                        case XDI_CoordSysEnu:
                                        {
                                                if (actualCsys == XDI_CoordSysNwu)
                                                {
                                                        rv[0] = -vel[1];
                                                        rv[1] = vel[0];
                                                        rv[2] = vel[2];
                                                }
                                                else if (actualCsys == XDI_CoordSysNed)
                                                {
                                                        rv[0] = vel[1];
                                                        rv[1] = vel[0];
                                                        rv[2] = -vel[2];
                                                }
                                        }
                                        break;
 
                                        case XDI_CoordSysNwu:
                                        {
                                                if (actualCsys == XDI_CoordSysEnu)
                                                {
                                                        rv[0] = vel[1];
                                                        rv[1] = -vel[0];
                                                        rv[2] = vel[2];
                                                }
                                                else if (actualCsys == XDI_CoordSysNed)
                                                {
                                                        rv[0] = vel[0];
                                                        rv[1] = -vel[1];
                                                        rv[2] = -vel[2];
                                                }
                                        }
                                        break;
 
                                        case XDI_CoordSysNed:
                                        {
                                                if (actualCsys == XDI_CoordSysEnu)
                                                {
                                                        rv[0] = vel[1];
                                                        rv[1] = vel[0];
                                                        rv[2] = -vel[2];
                                                }
                                                else if (actualCsys == XDI_CoordSysNwu)
                                                {
                                                        rv[0] = vel[0];
                                                        rv[1] = -vel[1];
                                                        rv[2] = -vel[2];
                                                }
                                        }
                                        break;
 
                                        default:
                                        {
                                                rv[0] = vel[0];
                                                rv[1] = vel[1];
                                                rv[2] = vel[2];
                                        }
                                        break;
                                }
                        }
                }
 
                return returnVal;
        }
 
        int XsDataPacket_containsVelocity(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_VelocityXYZ);
        }
 
        void XsDataPacket_setVelocity(XsDataPacket* thisPtr, const XsVector* data, XsDataIdentifier coordinateSystem)
        {
                detach(thisPtr);
                auto it = MAP.find(XDI_VelocityXYZ);
                if (it != MAP.end())
                        MAP.erase(it);
 
                genericSet<XsVector, XsVector3Variant>(thisPtr, data, XDI_VelocityXYZ | XDI_SubFormatDouble | (coordinateSystem & XDI_CoordSysMask));
        }
 
        XsDataIdentifier XsDataPacket_velocityIdentifier(const XsDataPacket* thisPtr)
        {
                auto it = MAP.find(XDI_VelocityXYZ);
                if (it != MAP.end())
                        return it->second->dataId();
                return XDI_None;
        }
 
        XsDataIdentifier XsDataPacket_coordinateSystemVelocity(const XsDataPacket* thisPtr)
        {
                return XsDataPacket_velocityIdentifier(thisPtr) & XDI_CoordSysMask;
        }
 
        uint32_t XsDataPacket_status(const XsDataPacket* thisPtr)
        {
                auto it = MAP.find(XDI_StatusWord);
                if (it != MAP.end())
                        return it->second->toDerived<SimpleVariant<uint32_t>>().m_data;
 
                it = MAP.find(XDI_StatusByte);
                if (it != MAP.end())
                        return it->second->toDerived<SimpleVariant<uint8_t>>().m_data;
 
                return 0;
        }
 
        int XsDataPacket_containsDetailedStatus(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_StatusWord);
        }
 
        int XsDataPacket_containsStatus(const XsDataPacket* thisPtr)
        {
                return  genericContains(thisPtr, XDI_StatusWord) ||
                        genericContains(thisPtr, XDI_StatusByte);
        }
 
        void XsDataPacket_setStatusByte(XsDataPacket* thisPtr, uint8_t data)
        {
                detach(thisPtr);
                if (genericContains(thisPtr, XDI_StatusWord))
                {
                        uint32_t word = GenericSimple<uint32_t>::get(thisPtr, XDI_StatusWord);
                        word = ((word & (~0xFF)) | data);
                        XsDataPacket_setStatus(thisPtr, word);
                }
                else
                        GenericSimple<uint8_t>::set(thisPtr, data, XDI_StatusByte);
        }
 
        void XsDataPacket_setStatus(XsDataPacket* thisPtr, uint32_t data)
        {
                detach(thisPtr);
                auto it = MAP.find(XDI_StatusByte);
                if (it != MAP.end())
                        MAP.erase(it);
 
                GenericSimple<uint32_t>::set(thisPtr, data, XDI_StatusWord);
        }
 
        XsTriggerIndicationData* XsDataPacket_triggerIndication(const XsDataPacket* thisPtr, XsDataIdentifier triggerId, XsTriggerIndicationData* returnVal)
        {
                return genericGet<XsTriggerIndicationData, XsTriggerIndicationDataVariant>(thisPtr, returnVal, triggerId);
        }
 
        int XsDataPacket_containsTriggerIndication(const XsDataPacket* thisPtr, XsDataIdentifier triggerId)
        {
                return genericContains(thisPtr, triggerId);
        }
 
        void XsDataPacket_setTriggerIndication(XsDataPacket* thisPtr, XsDataIdentifier triggerId, const XsTriggerIndicationData* triggerIndicationData)
        {
                genericSet<XsTriggerIndicationData, XsTriggerIndicationDataVariant>(thisPtr, triggerIndicationData, triggerId);
        }
 
        uint8_t XsDataPacket_packetCounter8(const XsDataPacket* thisPtr)
        {
                return GenericSimple<uint8_t>::get(thisPtr, XDI_PacketCounter8);
        }
 
        int XsDataPacket_containsPacketCounter8(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_PacketCounter8);
        }
 
        void XsDataPacket_setPacketCounter8(XsDataPacket* thisPtr, uint8_t counter)
        {
                GenericSimple<uint8_t>::set(thisPtr, counter, XDI_PacketCounter8);
        }
 
        uint16_t XsDataPacket_packetCounter(const XsDataPacket* thisPtr)
        {
                auto it = MAP.find(XDI_PacketCounter);
                if (it != MAP.end())
                        return it->second->toDerived<SimpleVariant<uint16_t>>().m_data;
 
                it = MAP.find(XDI_FrameRange);
                if (it != MAP.end())
                        return static_cast<uint16_t>(static_cast<unsigned int>(it->second->toDerived<XsRangeVariant>().m_data.last()));
 
                return 0;
        }
 
        int XsDataPacket_containsPacketCounter(const XsDataPacket* thisPtr)
        {
                return  genericContains(thisPtr, XDI_PacketCounter) ||
                        genericContains(thisPtr, XDI_FrameRange);
        }
 
        void XsDataPacket_setPacketCounter(XsDataPacket* thisPtr, uint16_t counter)
        {
                GenericSimple<uint16_t>::set(thisPtr, counter, XDI_PacketCounter);
                MAP.erase(XDI_FrameRange);
        }
 
        uint32_t XsDataPacket_sampleTimeFine(const XsDataPacket* thisPtr)
        {
                return GenericSimple<uint32_t>::get(thisPtr, XDI_SampleTimeFine);
        }
 
        int XsDataPacket_containsSampleTimeFine(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_SampleTimeFine);
        }
 
        void XsDataPacket_setSampleTimeFine(XsDataPacket* thisPtr, uint32_t counter)
        {
                GenericSimple<uint32_t>::set(thisPtr, counter, XDI_SampleTimeFine);
                auto it = MAP.find(XDI_SampleTime64);
                if (it != MAP.end())
                {
                        auto& var = it->second->toDerived<SimpleVariant<uint64_t>>();
                        var.m_data = coarseFactor * (var.m_data / coarseFactor) + (counter % coarseFactor);
                }
        }
 
        uint32_t XsDataPacket_sampleTimeCoarse(const XsDataPacket* thisPtr)
        {
                return GenericSimple<uint32_t>::get(thisPtr, XDI_SampleTimeCoarse);
        }
 
        int XsDataPacket_containsSampleTimeCoarse(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_SampleTimeCoarse);
        }
 
        void XsDataPacket_setSampleTimeCoarse(XsDataPacket* thisPtr, uint32_t counter)
        {
                GenericSimple<uint32_t>::set(thisPtr, counter, XDI_SampleTimeCoarse);
                auto it = MAP.find(XDI_SampleTime64);
                if (it != MAP.end())
                {
                        auto& var = it->second->toDerived<SimpleVariant<uint64_t>>();
                        var.m_data = (coarseFactor * counter) + (var.m_data % coarseFactor);
                }
        }
 
        uint64_t XsDataPacket_sampleTime64(const XsDataPacket* thisPtr)
        {
                auto it = MAP.find(XDI_SampleTime64);
                if (it != MAP.end())
                        return it->second->toDerived<SimpleVariant<uint64_t>>().m_data;
 
                uint64_t rv = 0;
 
                bool hasSampletimeCoarse = false;
                it = MAP.find(XDI_SampleTimeCoarse);
                if (it != MAP.end())
                {
                        hasSampletimeCoarse = true;
                        rv += ((uint64_t) it->second->toDerived<SimpleVariant<uint32_t>>().m_data) * coarseFactor;
                }
 
                it = MAP.find(XDI_SampleTimeFine);
                if (it != MAP.end())
                {
                        uint32_t sampletimeFine = it->second->toDerived<SimpleVariant<uint32_t>>().m_data;
                        if (hasSampletimeCoarse)
                                sampletimeFine = sampletimeFine % coarseFactor;
                        rv += sampletimeFine;
                }
 
                return rv;
        }
 
        int XsDataPacket_containsSampleTime64(const XsDataPacket* thisPtr)
        {
                return  genericContains(thisPtr, XDI_SampleTime64) ||
                        (genericContains(thisPtr, XDI_SampleTimeCoarse) && genericContains(thisPtr, XDI_SampleTimeFine));
        }
 
        void XsDataPacket_setSampleTime64(XsDataPacket* thisPtr, uint64_t counter)
        {
                GenericSimple<uint64_t>::set(thisPtr, counter, XDI_SampleTime64);
                GenericSimple<uint32_t>::set(thisPtr, (uint32_t)(counter / coarseFactor), XDI_SampleTimeCoarse);
                GenericSimple<uint32_t>::set(thisPtr, (uint32_t)(counter % coarseFactor), XDI_SampleTimeFine);
        }
 
        XsTimeInfo* XsDataPacket_utcTime(const XsDataPacket* thisPtr, XsTimeInfo* returnVal)
        {
                return genericGet<XsTimeInfo, XsTimeInfoVariant>(thisPtr, returnVal, XDI_UtcTime);
        }
 
        int XsDataPacket_containsUtcTime(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_UtcTime);
        }
 
        void XsDataPacket_setUtcTime(XsDataPacket* thisPtr, const XsTimeInfo* data)
        {
                genericSet<XsTimeInfo, XsTimeInfoVariant>(thisPtr, data, XDI_UtcTime);
        }
 
        XsVector* XsDataPacket_freeAcceleration(const XsDataPacket* thisPtr, XsVector* returnVal)
        {
                return genericGet<XsVector, XsVector3Variant>(thisPtr, returnVal, XDI_FreeAcceleration);
        }
 
        int XsDataPacket_containsFreeAcceleration(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_FreeAcceleration);
        }
 
        void XsDataPacket_setFreeAcceleration(XsDataPacket* thisPtr, const XsVector* g)
        {
                genericSet<XsVector, XsVector3Variant>(thisPtr, g, XDI_FreeAcceleration);
        }
 
        XsRange* XsDataPacket_frameRange(const XsDataPacket* thisPtr, XsRange* returnVal)
        {
                return genericGet<XsRange, XsRangeVariant>(thisPtr, returnVal, XDI_FrameRange);
        }
 
        int XsDataPacket_containsFrameRange(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_FrameRange);
        }
 
        void XsDataPacket_setFrameRange(XsDataPacket* thisPtr, const XsRange* r)
        {
                genericSet<XsRange, XsRangeVariant>(thisPtr, r, XDI_FrameRange);
                MAP.erase(XDI_PacketCounter);
        }
 
        int XsDataPacket_rssi(const XsDataPacket* thisPtr)
        {
                return (int)(int8_t) GenericSimple<uint8_t>::get(thisPtr, XDI_Rssi, (uint8_t) XS_RSSI_UNKNOWN);
        }
 
        int XsDataPacket_containsRssi(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_Rssi);
        }
 
        void XsDataPacket_setRssi(XsDataPacket* thisPtr, int r)
        {
                GenericSimple<uint8_t>::set(thisPtr, (uint8_t)(int8_t)r, XDI_Rssi);
        }
 
        XsRawGnssPvtData* XsDataPacket_rawGnssPvtData(const XsDataPacket* thisPtr, XsRawGnssPvtData* returnVal)
        {
                return genericGet<XsRawGnssPvtData, XsRawGnssPvtDataVariant>(thisPtr, returnVal, XDI_GnssPvtData);
        }
 
        int XsDataPacket_containsRawGnssPvtData(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_GnssPvtData);
        }
 
        void XsDataPacket_setRawGnssPvtData(XsDataPacket* thisPtr, const XsRawGnssPvtData* r)
        {
                genericSet<XsRawGnssPvtData, XsRawGnssPvtDataVariant>(thisPtr, r, XDI_GnssPvtData);
        }
 
        uint32_t XsDataPacket_gnssPvtPulse(const XsDataPacket* thisPtr)
        {
                return GenericSimple<uint32_t>::get(thisPtr, XDI_GnssPvtPulse);
        }
 
        int XsDataPacket_containsGnssPvtPulse(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_GnssPvtPulse);
        }
 
        void XsDataPacket_setGnssPvtPulse(XsDataPacket* thisPtr, uint32_t counter)
        {
                GenericSimple<uint32_t>::set(thisPtr, counter, XDI_GnssPvtPulse);
        }
 
        uint8_t XsDataPacket_gnssAge(const XsDataPacket* thisPtr)
        {
                return GenericSimple<uint8_t>::get(thisPtr, XDI_GnssAge, 255);
        }
 
        int XsDataPacket_containsGnssAge(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_GnssAge);
        }
 
        void XsDataPacket_setGnssAge(XsDataPacket* thisPtr, uint8_t age)
        {
                GenericSimple<uint8_t>::set(thisPtr, age, XDI_GnssAge);
        }
 
        XsRawGnssSatInfo* XsDataPacket_rawGnssSatInfo(const XsDataPacket* thisPtr, XsRawGnssSatInfo* returnVal)
        {
                return genericGet<XsRawGnssSatInfo, XsRawGnssSatInfoVariant>(thisPtr, returnVal, XDI_GnssSatInfo);
        }
 
        int XsDataPacket_containsRawGnssSatInfo(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_GnssSatInfo);
        }
 
        void XsDataPacket_setRawGnssSatInfo(XsDataPacket* thisPtr, const XsRawGnssSatInfo* data)
        {
                genericSet<XsRawGnssSatInfo, XsRawGnssSatInfoVariant>(thisPtr, data, XDI_GnssSatInfo);
        }
 
        XsDataPacket* XsDataPacket_merge(XsDataPacket* thisPtr, const XsDataPacket* other, int overwrite)
        {
                detach(thisPtr);
                bool over = !!overwrite;
                thisPtr->d->merge(*other->d, over);
 
                // do additional handling for 'unique' items
                auto keepOne = [&](XsDataIdentifier id1, XsDataIdentifier id2)
                {
                        if (genericContains(thisPtr, id1) &&
                                genericContains(thisPtr, id2))
                        {
                                bool gc = genericContains(other, id1);
                                if ((gc && !over) || (!gc && over))     // logical xor does not exist in C++, write it explicitly
                                        MAP.erase(id1);
                                else
                                        MAP.erase(id2);
                        }
                };
 
                keepOne(XDI_Quaternion, XDI_RotationMatrix);
                keepOne(XDI_Quaternion, XDI_EulerAngles);
                keepOne(XDI_EulerAngles, XDI_RotationMatrix);
                keepOne(XDI_PacketCounter, XDI_FrameRange);
                keepOne(XDI_SampleTime64, XDI_SampleTimeCoarse);
                keepOne(XDI_SampleTime64, XDI_SampleTimeFine);
 
                if (over)
                {
                        thisPtr->m_deviceId = other->m_deviceId;
                        thisPtr->m_toa = other->m_toa;
                        thisPtr->m_packetId = other->m_packetId;
                        thisPtr->m_etos = other->m_etos;
                }
 
                return thisPtr;
        }
 
        void XsDataPacket_setMessage(XsDataPacket* thisPtr, const XsMessage* msg)
        {
                XsDataPacket_clear(thisPtr, XDI_None);
 
                XsSize offset = 0;
                XsSize sz = msg->getDataSize();
 
                while (offset + 3 <= sz)        // minimum size of an item is 2(ID) + 1(size) + 0(minimum size)
                {
                        XsDataIdentifier id = static_cast<XsDataIdentifier>(XsMessage_getDataShort(msg, offset));
                        XsSize itemSize = XsMessage_getDataByte(msg, offset + 2);
                        if (offset + itemSize + 3 > sz)
                                break;  // the item is corrupt
 
                        Variant* var = createVariant(id);
                        if (var)
                        {
                                itemSize = var->readFromMessage(*msg, offset + 3, itemSize);
                                MAP.insert(id, var);
                        }
                        offset += 3 + itemSize; // never use var->sizeInMsg() here, since it _may_ differ
                }
                if (offset < sz)
                {
                        // if we get here then we detected some kind of corruption
                        // so we should not trust anything we read
                        XsDataPacket_clear(thisPtr, XDI_None);
                        return;
                }
        }
 
        void XsDataPacket_toMessage(const XsDataPacket* thisPtr, XsMessage* msg)
        {
                msg->resizeData(0);     // clear the data part while leaving header intact
                msg->setMessageId(XMID_MtData2);
 
                XsSize offset = 0;
                msg->resizeData(2048);  // prevent constant message resizing by pre-allocating a large message and later reducing its size
                for (auto const& i : MAP)
                {
                        XsSize sz = i.second->sizeInMsg();
                        if (sz < 255)
                        {
                                msg->setDataShort((uint16_t) i.second->dataId(), offset);
                                msg->setDataByte((uint8_t)sz, offset + 2);
                                i.second->writeToMessage(*msg, offset + 3);
                                offset += 3 + sz;
                        }
                        else
                        {
                                XsSize sz2 = sz;
                                XsSize offset2 = offset;
                                while (sz2 >= 255)
                                {
                                        msg->setDataShort((uint16_t) i.second->dataId(), offset2);
                                        msg->setDataByte((uint8_t)255, offset2 + 2);
                                        offset2 += 258;
                                        sz2 -= 255;
                                }
                                msg->setDataShort((uint16_t) i.second->dataId(), offset2);
                                msg->setDataByte((uint8_t)sz2, offset2 + 2);    // note that this size may be 0
                                i.second->writeToMessage(*msg, offset + 3);     // individual write functions should takke extended size into account
                                offset = offset2 + 3 + sz2;
                        }
                }
                msg->resizeData(offset);
        }
 
        XsSnapshot* XsDataPacket_fullSnapshot(const XsDataPacket* thisPtr, XsSnapshot* returnVal)
        {
                genericGet<XsSnapshot, XsFullSnapshotVariant>(thisPtr, returnVal, XDI_FullSnapshot);
                if (!returnVal->m_deviceId.isValid() && thisPtr->m_deviceId.isValid())
                        returnVal->m_deviceId = thisPtr->m_deviceId;
                return returnVal;
        }
 
        int XsDataPacket_containsFullSnapshot(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_FullSnapshot);
        }
 
        void XsDataPacket_setFullSnapshot(XsDataPacket* thisPtr, XsSnapshot const* data, int retransmission)
        {
                genericSet<XsSnapshot, XsFullSnapshotVariant>(thisPtr, data, XDI_FullSnapshot | (retransmission ? XDI_RetransmissionFlag : XDI_None));
        }
 
        XsSnapshot* XsDataPacket_awindaSnapshot(const XsDataPacket* thisPtr, XsSnapshot* returnVal)
        {
                return genericGet<XsSnapshot, XsAwindaSnapshotVariant>(thisPtr, returnVal, XDI_AwindaSnapshot);
        }
 
        int XsDataPacket_containsAwindaSnapshot(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_AwindaSnapshot);
        }
 
        void XsDataPacket_setAwindaSnapshot(XsDataPacket* thisPtr, XsSnapshot const* data, int retransmission)
        {
                genericSet<XsSnapshot, XsAwindaSnapshotVariant>(thisPtr, data, XDI_AwindaSnapshot | (retransmission ? XDI_RetransmissionFlag : XDI_None));
        }
 
        int XsDataPacket_isAwindaSnapshotARetransmission(const XsDataPacket* thisPtr)
        {
                if (!thisPtr->d)
                        return false;
                auto it = MAP.find(XDI_AwindaSnapshot);
                if (it == MAP.end())
                        return false;
                return (it->second->dataId() & XDI_RetransmissionMask) == XDI_RetransmissionFlag;
        }
 
        XsGloveSnapshot* XsDataPacket_gloveSnapshot(const XsDataPacket* thisPtr, XsGloveSnapshot* returnVal, XsHandId hand)
        {
                switch (hand)
                {
                        case XHI_LeftHand:
                                return genericGet<XsGloveSnapshot, XsGloveSnapshotVariant>(thisPtr, returnVal, XDI_GloveSnapshotLeft);
                        case XHI_RightHand:
                                return genericGet<XsGloveSnapshot, XsGloveSnapshotVariant>(thisPtr, returnVal, XDI_GloveSnapshotRight);
                        case XHI_Unknown:
                        default:
                                memset(returnVal, 0, sizeof(XsGloveSnapshot));
                                return returnVal;
                }
        }
 
        int XsDataPacket_containsGloveSnapshot(const XsDataPacket* thisPtr, XsHandId hand)
        {
                switch (hand)
                {
                        case XHI_LeftHand:
                                return genericContains(thisPtr, XDI_GloveSnapshotLeft);
                        case XHI_RightHand:
                                return genericContains(thisPtr, XDI_GloveSnapshotRight);
                        case XHI_Unknown:
                                return genericContains(thisPtr, XDI_GloveSnapshotLeft) || genericContains(thisPtr, XDI_GloveSnapshotRight);
                        default:
                                return false;
                }
        }
 
        void XsDataPacket_setGloveSnapshot(XsDataPacket* thisPtr, XsGloveSnapshot const* data, int retransmission, XsHandId hand)
        {
                switch (hand)
                {
                        case XHI_LeftHand:
                                genericSet<XsGloveSnapshot, XsGloveSnapshotVariant>(thisPtr, data, XDI_GloveSnapshotLeft | (retransmission ? XDI_RetransmissionFlag : XDI_None));
                                break;
                        case XHI_RightHand:
                                genericSet<XsGloveSnapshot, XsGloveSnapshotVariant>(thisPtr, data, XDI_GloveSnapshotRight | (retransmission ? XDI_RetransmissionFlag : XDI_None));
                                break;
                        case XHI_Unknown:
                        default:
                                break;
                }
        }
 
        static void convertRawVector(XsUShortVector const& input, XsDataIdentifier id, XsVector& returnVal)
        {
                XsReal(*caster)(uint16_t) = unsigned_cast;
                returnVal.setSize(3);
                if ((id & XDI_DataFormatMask) == XDI_RawSigned)
                        caster = signed_cast;
                for (XsSize i = 0; i < 3; i++)
                        returnVal[i] = caster(input[i]);
        }
 
        static XsVector* convertedVector(const XsDataPacket* thisPtr, XsVector* returnVal, XsDataIdentifier id, XsUShortVector XsScrData::* field)
        {
                assert(returnVal);
 
                XsUShortVector tmp;
 
                auto it = MAP.find(XDI_RawAccGyrMagTemp);
                if (it != MAP.end())
                        tmp = it->second->toDerived<XsScrDataVariant>().m_data.*field;
                else
                {
                        it = MAP.find(id);
                        if (it != MAP.end())
                                tmp = it->second->toDerived<XsUShortVectorVariant>().m_data;
                }
 
                convertRawVector(tmp, it->second->dataId(), *returnVal);
                return returnVal;
        }
 
        XsVector* XsDataPacket_rawAccelerationConverted(const XsDataPacket* thisPtr, XsVector* returnVal)
        {
                return convertedVector(thisPtr, returnVal, XDI_RawAcc, &XsScrData::m_acc);
        }
 
        XsVector* XsDataPacket_rawGyroscopeDataConverted(const XsDataPacket* thisPtr, XsVector* returnVal)
        {
                return convertedVector(thisPtr, returnVal, XDI_RawGyr, &XsScrData::m_gyr);
        }
 
        XsVector* XsDataPacket_rawMagneticFieldConverted(const XsDataPacket* thisPtr, XsVector* returnVal)
        {
                return convertedVector(thisPtr, returnVal, XDI_RawMag, &XsScrData::m_mag);
        }
 
        XsVector* XsDataPacket_rawGyroscopeTemperatureDataConverted(const XsDataPacket* thisPtr, XsVector* returnVal)
        {
                assert(returnVal);
                auto it = MAP.find(XDI_RawGyroTemp);
                XsUShortVector tmp;
                if (it != MAP.end())
                        tmp = it->second->toDerived<XsUShortVectorVariant>().m_data;
                convertRawVector(tmp, it->second->dataId(), *returnVal);
                return returnVal;
        }
 
        XsByteArray* XsDataPacket_rawBlob(const XsDataPacket* thisPtr, XsByteArray* returnVal)
        {
                return genericGet<XsByteArray, XsByteArrayVariant>(thisPtr, returnVal, XDI_RawBlob);
        }
 
        int XsDataPacket_containsRawBlob(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_RawBlob);
        }
 
        void XsDataPacket_setRawBlob(XsDataPacket* thisPtr, const XsByteArray* data)
        {
                genericSet<XsByteArray, XsByteArrayVariant>(thisPtr, data, XDI_RawBlob);
        }
 
        XsVector* XsDataPacket_accelerationHR(const XsDataPacket* thisPtr, XsVector* returnVal)
        {
                return genericGet<XsVector, XsVector3Variant>(thisPtr, returnVal, XDI_AccelerationHR);
        }
 
        int XsDataPacket_containsAccelerationHR(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_AccelerationHR);
        }
 
        void XsDataPacket_setAccelerationHR(XsDataPacket* thisPtr, const XsVector* vec)
        {
                genericSet<XsVector, XsVector3Variant>(thisPtr, vec, XDI_AccelerationHR | XDI_SubFormatDouble);
        }
 
        XsVector* XsDataPacket_rateOfTurnHR(const XsDataPacket* thisPtr, XsVector* returnVal)
        {
                return genericGet<XsVector, XsVector3Variant>(thisPtr, returnVal, XDI_RateOfTurnHR);
        }
 
        int XsDataPacket_containsRateOfTurnHR(const XsDataPacket* thisPtr)
        {
                return genericContains(thisPtr, XDI_RateOfTurnHR);
        }
 
        void XsDataPacket_setRateOfTurnHR(XsDataPacket* thisPtr, const XsVector* vec)
        {
                genericSet<XsVector, XsVector3Variant>(thisPtr, vec, XDI_RateOfTurnHR | XDI_SubFormatDouble);
        }
 
        int XsDataPacket_privateCount()
        {
                return DataPacketPrivate::creationDiff();
        }
 
}       // extern "C"