File content as of revision 21:6015b8be3a00:

/*!
 * \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 "xbusmessage.h"
#include "xbusdef.h"
#include "xbusutility.h"

size_t XbusMessage_format(uint8_t* raw, struct XbusMessage const* message)
{
	uint8_t* dptr = raw;
	*dptr++ = XBUS_PREAMBLE;

	*dptr = XBUS_BUS_ID_STANDALONE;
	uint8_t checksum = 0;
	checksum -= *dptr++;

	*dptr = message->mid;
	checksum -= *dptr++;

	if (message->length < XBUS_EXTENDED_LENGTH)
	{
		*dptr = message->length;
		checksum -= *dptr++;
	}
	else
	{
		*dptr = XBUS_EXTENDED_LENGTH;
		checksum -= *dptr++;
		*dptr = message->length >> 8;
		checksum -= *dptr++;
		*dptr = message->length & 0xFF;
		checksum -= *dptr++;
	}

	uint8_t* sptr = message->data;
	for (int i = 0; i < message->length; ++i)
	{
		*dptr = *sptr++;
		checksum -= *dptr++;
	}
	*dptr++ = checksum;

	return dptr - raw;
}

static uint8_t const* getPointerToData(enum XsDataIdentifier id, uint8_t const* data, uint16_t dataLength)
{
	uint8_t const* dptr = data;
	while (dptr < data + dataLength)
	{
		uint16_t itemId;
		uint8_t itemSize;
		dptr = XbusUtility_readU16(&itemId, dptr);
		dptr = XbusUtility_readU8(&itemSize, dptr);

		if (id == itemId)
			return dptr;

		dptr += itemSize;
	}
	return NULL;
}

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

bool XbusMessage_getDataItem(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:
				raw = XbusUtility_readU16(item, raw);
				break;

			case XDI_SampleTimeFine:
			case XDI_StatusWord:
				raw = XbusUtility_readU32(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;
	}
}

char const* XbusMessage_dataDescription(enum XsDataIdentifier id)
{
	switch (id)
	{
		case XDI_PacketCounter:
			return "Packet counter";

		case XDI_SampleTimeFine:
			return "Sample time fine";

		case XDI_Quaternion:
			return "Quaternion";

		case XDI_DeltaV:
			return "Velocity increment";

		case XDI_Acceleration:
			return "Acceleration";

		case XDI_RateOfTurn:
			return "Rate of turn";

		case XDI_DeltaQ:
			return "Orientation increment";

		case XDI_MagneticField:
			return "Magnetic field";

		case XDI_StatusWord:
			return "Status word";

		default:
			return "Unknown data type";
	}
}