Diff: xbus/xbusmessage.c

Revision:

16:4bdcdac223d8

Parent:

10:18a6661b7e59

Child:

18:2073072bad51

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