Exemplo n.º 1
0
/*! \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);
}
Exemplo n.º 2
0
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);
}
Exemplo n.º 3
0
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);
}