/*! \brief Get the frame range of the current strapdown integration data. */
XsRange MtwSdiData::frameRange() const
{
if (empty())
return XsRange();
return XsRange(m_firstFrameNumber, m_lastFrameNumber);
}
void XsDataPacket_assignFromLegacyDataPacket(struct XsDataPacket* thisPtr, struct LegacyDataPacket const* legacy, int index)
{
assert(legacy);
if (!legacy->isXbusSystem())
index = 0;
thisPtr->clear();
thisPtr->setDeviceId(legacy->deviceId(index));
thisPtr->setTimeOfArrival(legacy->timeOfArrival());
thisPtr->setPacketId(legacy->largePacketCounter());
XsDataFormat format = legacy->dataFormat(index);
if (legacy->containsRawData(index))
thisPtr->setRawData(legacy->rawData(index));
if (legacy->rawTemperatureChannelCount(index) == 4)
{
XsUShortVector rawGyroTemperatures;
rawGyroTemperatures[0] = legacy->rawTemperature(index, 1);
rawGyroTemperatures[1] = legacy->rawTemperature(index, 2);
rawGyroTemperatures[2] = legacy->rawTemperature(index, 3);
thisPtr->setRawGyroscopeTemperatureData(rawGyroTemperatures);
}
if (legacy->containsCalibratedAcceleration(index))
thisPtr->setCalibratedAcceleration(legacy->calibratedAcceleration(index));
if (legacy->containsCalibratedGyroscopeData(index))
{
//\todo Fix legacy watermarking
// 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;
// }
// Special copy to preserve watermarking
//thisPtr->message().setDataShort(XDI_RateOfTurn | di, thisPtr->message().getDataSize());
//thisPtr->message().setDataByte(3*thisPtr->getFPValueSize(di), thisPtr->message().getDataSize());
//thisPtr->itemCount()++;
//thisPtr->message().setDataBuffer(thisPtr->legacyMsg().getDataBuffer(info.m_calGyr), 3*thisPtr->getFPValueSize(di), XsDataPacket_itemOffsetExact(thisPtr, XDI_RateOfTurn | di));
thisPtr->setCalibratedGyroscopeData(legacy->calibratedGyroscopeData(index));
}
if (legacy->containsCalibratedMagneticField(index))
thisPtr->setCalibratedMagneticField(legacy->calibratedMagneticField(index));
if (legacy->containsOrientationQuaternion(index))
thisPtr->setOrientationQuaternion(legacy->orientationQuaternion(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu);
if (legacy->containsOrientationEuler(index))
thisPtr->setOrientationEuler(legacy->orientationEuler(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu);
if (legacy->containsOrientationMatrix(index))
thisPtr->setOrientationMatrix(legacy->orientationMatrix(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu);
if (legacy->containsPositionLLA(index))
thisPtr->setPositionLLA(legacy->positionLLA(index));
if (legacy->containsVelocity(index))
thisPtr->setVelocity(legacy->velocity(index), (format.m_outputSettings & XOS_Coordinates_Ned) ? XDI_CoordSysNed : XDI_CoordSysNwu);
if (legacy->containsStatus(index))
{
bool isDetailed = true;
uint32_t status = legacy->status(index, &isDetailed);
// For MTw's only, the status needs to be swapEndian32-ed
if (thisPtr->deviceId().isMtw1())
{
status = swapEndian32(status);
}
if (isDetailed)
{
thisPtr->setStatus(status);
}
else
{
thisPtr->setStatusByte((uint8_t) status);
}
}
if (legacy->containsGpsPvtData())
{
XsPressure tmp;
tmp.m_pressure = legacy->gpsPvtData(index).m_pressure * 2.0; // Convert to Pascal
tmp.m_pressureAge = legacy->gpsPvtData(index).m_pressureAge;
thisPtr->setPressure(tmp);
XsGpsPvtData pvt;
pvt = legacy->gpsPvtData(index);
thisPtr->setGpsPvtData(pvt);
}
if (legacy->containsUtcTime(index))
{
XsUtcTime time = legacy->utcTime(index);
thisPtr->setUtcTime(time);
}
if (legacy->containsSampleTimeFine(index))
{
uint32_t sampleTimeFine = legacy->sampleTimeFine(index);
thisPtr->setSampleTimeFine(sampleTimeFine);
}
// packet counter MUST go before anything that sets a frame range!
if (legacy->containsPacketCounter(index))
thisPtr->setPacketCounter(legacy->packetCounter(index));
if (legacy->containsMtwSdiData(index))
{
// not yet converted:
//uint8_t m_timeSync;
//XsVector3 m_currentBias;
MtwSdiData mtwsdi = legacy->mtwSdiData(index);
thisPtr->setDeviceId(mtwsdi.m_deviceId);
{
XsSdiData tmp(mtwsdi.orientationIncrement(), mtwsdi.velocityIncrement());
thisPtr->setSdiData(tmp);
//// Special copy to preserve watermarking
//thisPtr->message().setDataShort(XDI_DeltaQ | di, thisPtr->message().getDataSize());
//thisPtr->message().setDataByte(4*thisPtr->getFPValueSize(di), thisPtr->message().getDataSize());
//thisPtr->itemCount()++;
//thisPtr->message().setDataBuffer(thisPtr->legacyMsg().getDataBuffer(info.m_wOrientationIncrement), 4*thisPtr->getFPValueSize(di), XsDataPacket_itemOffsetExact(thisPtr, XDI_DeltaQ | di));
//thisPtr->message().setDataShort(XDI_DeltaV | di, thisPtr->message().getDataSize());
//thisPtr->message().setDataByte(3*thisPtr->getFPValueSize(di), thisPtr->message().getDataSize());
//thisPtr->itemCount()++;
//thisPtr->message().setDataBuffer(thisPtr->legacyMsg().getDataBuffer(info.m_wVelocityIncrement), 3*thisPtr->getFPValueSize(di), XsDataPacket_itemOffsetExact(thisPtr, XDI_DeltaV | di));
}
//if (mtwsdi.m_aidingData)
thisPtr->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;
thisPtr->setPressure(tmp);
}
thisPtr->setFrameRange(XsRange((int) mtwsdi.m_firstFrameNumber, (int) mtwsdi.m_lastFrameNumber));
thisPtr->setRssi(mtwsdi.m_rssi);
}
if (legacy->containsTemperature(index))
thisPtr->setTemperature(legacy->temperature(index));
// enable this when you experience weird crashes in XsByteArray_destruct or XsDataPacket_destruct
//validatePacket(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);
}