Revision 6:28cdd6ee6b9d, committed 2015-05-13

Comitter:

Alex Young

Date:

Wed May 13 14:51:24 2015 +0200

Parent:

5:abc52dd88be2

Child:

7:c913a7cd5231

Commit message:

Basic implementation for extracting items from MTData2 packets

Changed in this revision

--- /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;
+	}
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/xbus/mtdata2.h	Wed May 13 14:51:24 2015 +0200
@@ -0,0 +1,48 @@
+/*!
+ * \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.
+ */
+
+#ifndef __MTDATA2_H
+#define __MTDATA2_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*! \brief Xbus message ID for MTData2 packets. */
+#define MTDATA2_MESSAGE_ID (0x36)
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+enum XsDataIdentifier
+{
+	XDI_PacketCounter  = 0x1020,
+	XDI_SampleTimeFine = 0x1060,
+	XDI_Quaternion     = 0x2010,
+	XDI_DeltaV         = 0x4010,
+	XDI_Acceleration   = 0x4020,
+	XDI_RateOfTurn     = 0x8020,
+	XDI_DeltaQ         = 0x8030,
+	XDI_MagneticField  = 0xC020,
+	XDI_StatusWord     = 0xE020,
+};
+
+bool MtData2_getItem(void* item, enum XsDataIdentifier id, uint8_t const* data, uint16_t dataLength);
+
+#ifdef __cplusplus
+}
+#endif // extern "C"
+
+#endif // __MTDATA2_H