Diff: xbus/mtdata2.c

Revision:

6:28cdd6ee6b9d

Child:

15:558d279addd9

diff -r abc52dd88be2 -r 28cdd6ee6b9d xbus/mtdata2.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/xbus/mtdata2.c	Wed May 13 14:51:24 2015 +0200
@@ -0,0 +1,92 @@
+/*!
+ * \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, uint8_t const* data, uint16_t dataLength)
+{
+	uint8_t const* raw = getPointerToData(id, data, dataLength);
+	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;
+	}
+}
+