int_xsdatapacket.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#include "int_xsdatapacket.h"
#include <xsens/xsbusid.h>
#include <xsens/xsdatapacket.h>
#include <xsens/xsgpspvtdata.h>
#include <xsens/xsmessage.h>
#include <xsens/xstypesconfig.h>
#include "legacydatapacket.h"
#include "mtwsdidata.h"
#include "packetfixeddata.h"

// lint -esym(1512, XsDataPacket)
/*! \internal
    \brief Hack class to access internals of XsDataPacket in C++
*/
class InternalDataPacket : public XsDataPacket
{
    // lint --e(1511, 1509, 1536)
   public:
    //! \brief \copybrief XsDataPacket::m_msg
    inline XsMessage& msg() { return m_msg; }
    //! \brief \copybrief XsDataPacket::m_legacyMsg
    inline XsMessage& legacyMsg() { return m_legacyMsg; }
    //! \brief \copybrief XsDataPacket::m_deviceId
    inline XsDeviceId& deviceId() { return m_deviceId; }
    //! \brief \copybrief XsDataPacket::m_lastFoundId
    inline XsDataIdentifier& lastFoundId() { return m_lastFoundId; }
    //! \brief \copybrief XsDataPacket::m_lastFoundOffset
    inline int& lastFoundOffset() { return m_lastFoundOffset; }
    //! \brief \copybrief XsDataPacket::m_itemCount
    inline uint16_t& itemCount() { return m_itemCount; }
    //! \brief \copybrief XsDataPacket::m_originalMessageLength
    inline uint16_t& originalMessageLength() { return m_originalMessageLength; }
    //! \brief \copybrief XsDataPacket::m_toa
    inline XsTimeStamp& toa() { return m_toa; }
    //! \brief \copybrief XsDataPacket::m_packetId
    inline XsTimeStamp& packetId() { return m_packetId; }
};

/*! \class XsDataPacket
    \brief Contains data received from a device or read from a file

    This class is used by XDA for easy access to data contained in a message. It
   analyzes its internal
    XsMessage upon contruction to give access to the individual contained pieces
   of data.
    New data can also be added to the %XsDataPacket or updated if it already
   existed.
*/
extern "C"
{
    void validatePacket(XsDataPacket* thisPtr);

    void XsDataPacket_assignFromXsLegacyDataPacket(
        struct XsDataPacket* thisPtr, struct LegacyDataPacket const* pack,
        int index)
    {
        assert(pack);

        auto* hacket = (InternalDataPacket*)thisPtr;

        hacket->legacyMsg() = pack->message();

        if (!pack->isXbusSystem()) index = 0;

        hacket->message().clear();
        if (pack->isXbusSystem())
            hacket->message().setBusId(index + 1);
        else
            hacket->message().setBusId(XS_BID_MASTER);

        hacket->message().setMessageId(XMID_MtData2);
        // hacket->m_legacyMsg = pack->message();
        hacket->deviceId() = pack->deviceId(index);
        hacket->itemCount() = 1;
        hacket->toa() = pack->timeOfArrival();
        hacket->originalMessageLength() =
            (uint16_t)pack->originalMessage().getDataSize();
        hacket->packetId() = pack->largePacketCounter();
        PacketInfo info = pack->packetInfo(index);
        XsDataFormat format = pack->dataFormat(index);
        XsDataIdentifier di;
        switch (format.m_outputSettings & XOS_Dataformat_Mask)
        {
            case XOS_Dataformat_Float:
                di = XDI_SubFormatFloat;
                break;
            case XOS_Dataformat_Double:
                di = XDI_SubFormatDouble;
                break;
            case XOS_Dataformat_Fp1632:
                di = XDI_SubFormatFp1632;
                break;
            case XOS_Dataformat_F1220:
                di = XDI_SubFormatFp1220;
                break;
            default:
                di = XDI_None;
                break;
        }
        if (pack->containsRawData(index))
            hacket->setRawData(pack->rawData(index));
        if (pack->rawTemperatureChannelCount(index) == 4)
        {
            XsUShortVector rawGyroTemperatures;
            rawGyroTemperatures[0] = pack->rawTemperature(index, 1);
            rawGyroTemperatures[1] = pack->rawTemperature(index, 2);
            rawGyroTemperatures[2] = pack->rawTemperature(index, 3);
            hacket->setRawGyroscopeTemperatureData(rawGyroTemperatures);
        }
        if (pack->containsCalibratedAcceleration(index))
            hacket->setCalibratedAcceleration(
                pack->calibratedAcceleration(index));
        if (pack->containsCalibratedGyroscopeData(index))
        {
            // Special copy to preserve watermarking
            hacket->message().setDataShort(
                XDI_RateOfTurn | di, hacket->message().getDataSize());
            hacket->message().setDataByte(
                3 * hacket->getFPValueSize(di),
                hacket->message().getDataSize());
            hacket->itemCount()++;
            hacket->message().setDataBuffer(
                hacket->legacyMsg().getDataBuffer(info.m_calGyr),
                3 * hacket->getFPValueSize(di),
                XsDataPacket_itemOffsetExact(thisPtr, XDI_RateOfTurn | di));
        }
        if (pack->containsCalibratedMagneticField(index))
            hacket->setCalibratedMagneticField(
                pack->calibratedMagneticField(index));
        if (pack->containsOrientationQuaternion(index))
            hacket->setOrientationQuaternion(
                pack->orientationQuaternion(index),
                (format.m_outputSettings & XOS_Coordinates_Ned)
                    ? XDI_CoordSysNed
                    : XDI_CoordSysNwu);
        if (pack->containsOrientationEuler(index))
            hacket->setOrientationEuler(
                pack->orientationEuler(index),
                (format.m_outputSettings & XOS_Coordinates_Ned)
                    ? XDI_CoordSysNed
                    : XDI_CoordSysNwu);
        if (pack->containsOrientationMatrix(index))
            hacket->setOrientationMatrix(
                pack->orientationMatrix(index),
                (format.m_outputSettings & XOS_Coordinates_Ned)
                    ? XDI_CoordSysNed
                    : XDI_CoordSysNwu);
        if (pack->containsPositionLLA(index))
            hacket->setPositionLLA(pack->positionLLA(index));
        if (pack->containsVelocity(index))
            hacket->setVelocity(
                pack->velocity(index),
                (format.m_outputSettings & XOS_Coordinates_Ned)
                    ? XDI_CoordSysNed
                    : XDI_CoordSysNwu);
        if (pack->containsStatus(index))
        {
            bool isDetailed = true;
            uint32_t status = pack->status(index, &isDetailed);

            // For MTw's only, the status needs to be swapEndian32-ed
            if (hacket->deviceId().isMtw())
            {
                status = swapEndian32(status);
            }

            if (isDetailed)
            {
                hacket->setStatus(status);
            }
            else
            {
                hacket->setStatusByte((uint8_t)status);
            }
        }
        if (pack->containsGpsPvtData())
        {
            XsPressure tmp;
            tmp.m_pressure =
                pack->gpsPvtData(index).m_pressure * 2.0;  // Convert to Pascal
            tmp.m_pressureAge = pack->gpsPvtData(index).m_pressureAge;
            hacket->setPressure(tmp);
            XsGpsPvtData pvt;
            pvt = pack->gpsPvtData(index);
            hacket->setGpsPvtData(pvt);
        }
        if (pack->containsUtcTime(index))
        {
            XsUtcTime time = pack->utcTime(index);
            hacket->setUtcTime(time);
        }
        if (pack->containsMtwSdiData(index))
        {
            // not yet converted:
            // uint8_t          m_timeSync;
            // XsVector3        m_currentBias;

            MtwSdiData mtwsdi = pack->mtwSdiData(index);
            hacket->deviceId() = mtwsdi.m_deviceId;
            {
                // Special copy to preserve watermarking
                hacket->message().setDataShort(
                    XDI_DeltaQ | di, hacket->message().getDataSize());
                hacket->message().setDataByte(
                    4 * hacket->getFPValueSize(di),
                    hacket->message().getDataSize());
                hacket->itemCount()++;
                hacket->message().setDataBuffer(
                    hacket->legacyMsg().getDataBuffer(
                        info.m_wOrientationIncrement),
                    4 * hacket->getFPValueSize(di),
                    XsDataPacket_itemOffsetExact(thisPtr, XDI_DeltaQ | di));

                hacket->message().setDataShort(
                    XDI_DeltaV | di, hacket->message().getDataSize());
                hacket->message().setDataByte(
                    3 * hacket->getFPValueSize(di),
                    hacket->message().getDataSize());
                hacket->itemCount()++;
                hacket->message().setDataBuffer(
                    hacket->legacyMsg().getDataBuffer(
                        info.m_wVelocityIncrement),
                    3 * hacket->getFPValueSize(di),
                    XsDataPacket_itemOffsetExact(thisPtr, XDI_DeltaV | di));
            }

            // if (mtwsdi.m_aidingData)
            hacket->setCalibratedMagneticField(mtwsdi.m_magnetoMeter);
            if (mtwsdi.m_barometer)
            {
                XsPressure tmp;
                tmp.m_pressure = mtwsdi.m_barometer *
                                 100.0;  // convert from millibar to pascal
                tmp.m_pressureAge = mtwsdi.m_barometer ? 0 : 255;
                hacket->setPressure(tmp);
            }
            hacket->setFrameRange(XsRange(
                (int)mtwsdi.m_firstFrameNumber, (int)mtwsdi.m_lastFrameNumber));
            hacket->setRssi(mtwsdi.m_rssi);
        }
        if (pack->containsTemperature(index))
            hacket->setTemperature(pack->temperature(index));
        if (pack->containsPacketCounter(index))
            hacket->setPacketCounter(pack->packetCounter(index));

        // enable this when you experience weird crashes in XsByteArray_destruct
        // or XsDataPacket_destruct validatePacket(thisPtr);
    }

}  // extern "C"