Diff: main.cpp

Revision:

7:c913a7cd5231

Parent:

5:abc52dd88be2

Child:

8:77cd45916596

--- a/main.cpp	Wed May 13 14:51:24 2015 +0200
+++ b/main.cpp	Wed May 13 14:53:07 2015 +0200
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "xbusparser.h"
+#include "mtdata2.h"
 
 static Serial pc(PA_2, PA_3);
 static Serial mt(PB_9, PB_8);
@@ -21,7 +22,31 @@
 
 static void mtDataHandler(uint8_t mid, uint16_t dataLength, uint8_t const* data)
 {
-	pc.printf("Received Xbus message. MID=%X, length=%d\n", mid, dataLength);
+	if (mid == MTDATA2_MESSAGE_ID)
+	{
+		pc.printf("Data packet received\n");
+		uint16_t counter;
+		if (MtData2_getItem(&counter, XDI_PacketCounter, data, dataLength))
+		{
+			pc.printf("\tPacket counter:\t%d\n", counter);
+		}
+		float ori[4];
+		if (MtData2_getItem(ori, XDI_Quaternion, data, dataLength))
+		{
+			pc.printf("\tOrientation\t%.3f, %.3f, %.3f, %.3f\n", ori[0], ori[1],
+					ori[2], ori[3]);
+		}
+		uint32_t status;
+		if (MtData2_getItem(&status, XDI_StatusWord, data, dataLength))
+		{
+			pc.printf("\tFilter status:\t%d\n", status);
+		}
+		pc.printf("\n\n");
+	}
+	else
+	{
+		pc.printf("Received Xbus message. MID=%X, length=%d\n", mid, dataLength);
+	}
 }