Diff: main.cpp

Revision:

24:2cc49dc854e3

Parent:

22:3eab999c5076

Child:

25:01356fb59467

--- a/main.cpp	Wed May 20 12:39:11 2015 +0200
+++ b/main.cpp	Wed May 20 12:47:50 2015 +0200
@@ -61,28 +61,33 @@
 	}
 }
 
-static void mtDataHandler(struct XbusMessage const* message)
+static void handleDataMessage(struct XbusMessage const* message)
+{
+	pc.printf("MTData2:");
+	uint16_t counter;
+	if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
+	{
+		pc.printf(" Packet counter: %5d", counter);
+	}
+	float ori[4];
+	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 (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
+	{
+		pc.printf(" Status:%X", status);
+	}
+	pc.printf("\n");
+}
+
+static void mtMessageHandler(struct XbusMessage const* message)
 {
 	if (message->mid == XMID_MtData2)
 	{
-		pc.printf("MTData2:");
-		uint16_t counter;
-		if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
-		{
-			pc.printf(" Packet counter: %5d", counter);
-		}
-		float ori[4];
-		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 (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
-		{
-			pc.printf(" Status:%X", status);
-		}
-		pc.printf("\n");
+		handleDataMessage(message);
 	}
 	else if (message->mid == XMID_DeviceId)
 	{
@@ -119,7 +124,7 @@
 {
 	XbusParserCallback xbusCallback = {};
 	xbusCallback.allocateBuffer = allocateBuffer;
-	xbusCallback.handleMessage = mtDataHandler;
+	xbusCallback.handleMessage = mtMessageHandler;
 
 	xbusParser = XbusParser_create(&xbusCallback);
 	configureSerialPorts();