int_xsdatapacket.cpp

Go to the documentation of this file.

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

    InternalDataPacket* 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"