Revision 16:4bdcdac223d8, committed 2015-05-19

Comitter:

Alex Young

Date:

Tue May 19 13:40:33 2015 +0200

Parent:

15:558d279addd9

Child:

17:680f28e00d17

Commit message:

Move MtData2 parsing to xbus message module

Changed in this revision

--- a/main.cpp	Tue May 19 13:34:37 2015 +0200
+++ b/main.cpp	Tue May 19 13:40:33 2015 +0200
@@ -1,6 +1,5 @@
 #include "mbed.h"
 #include "xbusparser.h"
-#include "mtdata2.h"
 #include "xbusmessage.h"
 
 static Serial pc(PA_2, PA_3);
@@ -60,18 +59,18 @@
 	{
 		pc.printf("MTData2:");
 		uint16_t counter;
-		if (MtData2_getItem(&counter, XDI_PacketCounter, message))
+		if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
 		{
 			pc.printf(" Packet counter: %5d", counter);
 		}
 		float ori[4];
-		if (MtData2_getItem(ori, XDI_Quaternion, message))
+		if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
 		{
 			pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1],
 					ori[2], ori[3]);
 		}
 		uint32_t status;
-		if (MtData2_getItem(&status, XDI_StatusWord, message))
+		if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
 		{
 			pc.printf(" Status:%X", status);
 		}

--- a/xbus/mtdata2.c	Tue May 19 13:34:37 2015 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,92 +0,0 @@
-/*!
- * \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;
-	}
-}
-

--- a/xbus/mtdata2.h	Tue May 19 13:34:37 2015 +0200
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,46 +0,0 @@
-/*!
- * \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>
-#include "xbusmessage.h"
-
-#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, struct XbusMessage const* message);
-
-#ifdef __cplusplus
-}
-#endif // extern "C"
-
-#endif // __MTDATA2_H

--- a/xbus/xbusmessage.c	Tue May 19 13:34:37 2015 +0200
+++ b/xbus/xbusmessage.c	Tue May 19 13:40:33 2015 +0200
@@ -54,3 +54,77 @@
 	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)
+	{
+		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 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:
+				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;
+	}
+}
+

--- a/xbus/xbusmessage.h	Tue May 19 13:34:37 2015 +0200
+++ b/xbus/xbusmessage.h	Tue May 19 13:40:33 2015 +0200
@@ -18,6 +18,7 @@
 
 #include <stddef.h>
 #include <stdint.h>
+#include <stdbool.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -32,6 +33,19 @@
 	XMID_MtData2            = 0x36
 };
 
+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,
+};
+
 struct XbusMessage
 {
 	enum XsMessageId mid;
@@ -40,6 +54,7 @@
 };
 
 size_t XbusMessage_format(uint8_t* raw, struct XbusMessage const* message);
+bool XbusMessage_getDataItem(void* item, enum XsDataIdentifier id, struct XbusMessage const* message);
 
 #ifdef __cplusplus
 }