Revision 18:2073072bad51, committed 2015-05-19

Comitter:

Alex Young

Date:

Tue May 19 14:45:05 2015 +0200

Parent:

17:680f28e00d17

Child:

19:46e88d37ecef

Commit message:

Extract functions for reading from xbus buffers to utility module

Changed in this revision

--- a/xbus/xbusmessage.c	Tue May 19 14:01:33 2015 +0200
+++ b/xbus/xbusmessage.c	Tue May 19 14:45:05 2015 +0200
@@ -1,130 +1,122 @@
-/*!
- * \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"
-
-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)
-	{
-		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;
-	}
-}
-
+/*!
+ * \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;
+	}
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/xbus/xbusutility.c	Tue May 19 14:45:05 2015 +0200
@@ -0,0 +1,34 @@
+/*!
+ * \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 "xbusutility.h"
+
+uint8_t const* XbusUtility_readU8(uint8_t* out, uint8_t const* in)
+{
+	*out = *in;
+	return ++in;
+}
+
+uint8_t const* XbusUtility_readU16(uint16_t* out, uint8_t const* in)
+{
+	*out = (in[0] << 8) | in[1];
+	return in + sizeof(uint16_t);
+}
+
+uint8_t const* XbusUtility_readU32(uint32_t* out, uint8_t const* in)
+{
+	*out = (in[0] << 24) | (in[1] << 16) | (in[2] << 8) | in[3];
+	return in + sizeof(uint32_t);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/xbus/xbusutility.h	Tue May 19 14:45:05 2015 +0200
@@ -0,0 +1,25 @@
+/*!
+ * \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 __XBUSUTILITY_H
+#define __XBUSUTILITY_H
+
+#include <stdint.h>
+
+uint8_t const* XbusUtility_readU8(uint8_t* out, uint8_t const* in);
+uint8_t const* XbusUtility_readU16(uint16_t* out, uint8_t const* in);
+uint8_t const* XbusUtility_readU32(uint32_t* out, uint8_t const* in);
+
+#endif // __XBUSUTILITY_H