File content as of revision 15:558d279addd9:

/*!
 * \file
 * \copyright
 * Copyright (C) Xsens Technologies B.V., 2015.  All rights reserved.
 *
 * This source code is intended for use only by Xsens Technologies BV and
 * those that have explicit written permission to use it from
 * Xsens Technologies BV.
 *
 * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
 * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
 * PARTICULAR PURPOSE.
 */

#include "mtdata2.h"
#include <stddef.h>

static uint8_t const* getPointerToData(enum XsDataIdentifier id, uint8_t const* data, uint16_t dataLength)
{
	uint8_t const* dptr = data;
	while (dptr < data + dataLength)
	{
		const uint16_t itemId = (dptr[0] << 8) | dptr[1];
		dptr += sizeof(itemId);
		const uint8_t itemSize = *dptr++;

		if (id == itemId)
			return dptr;

		dptr += itemSize;
	}
	return NULL;
}

static void readShort(uint16_t* out, uint8_t const* raw)
{
	*out = (raw[0] << 8) | raw[1];
}

static void readInt(uint32_t* out, uint8_t const* raw)
{
	*out = (raw[0] << 24) | (raw[1] << 16) | (raw[2] << 8) | raw[3];
}

static void readFloats(float* out, uint8_t const* raw, uint8_t floats)
{
	for (int i = 0; i < floats; ++i)
	{
		readInt((uint32_t*)&out[i], &raw[i * sizeof(float)]);
	}
}

bool MtData2_getItem(void* item, enum XsDataIdentifier id, struct XbusMessage const* message)
{
	uint8_t const* raw = getPointerToData(id, message->data, message->length);
	if (raw)
	{
		switch (id)
		{
			case XDI_PacketCounter:
				readShort(item, raw);
				break;

			case XDI_SampleTimeFine:
			case XDI_StatusWord:
				readInt(item, raw);
				break;

			case XDI_Quaternion:
			case XDI_DeltaQ:
				readFloats(item, raw, 4);
				break;

			case XDI_DeltaV:
			case XDI_Acceleration:
			case XDI_RateOfTurn:
			case XDI_MagneticField:
				readFloats(item, raw, 3);
				break;

			default:
				return false;
		}
		return true;
	}
	else
	{
		return false;
	}
}