Diff: main.cpp

Revision:

43:470c019246e4

Parent:

42:e70756d2d8bc

Child:

44:b3980e8ac074

--- a/main.cpp	Thu May 21 15:21:02 2015 +0200
+++ b/main.cpp	Thu May 21 15:34:18 2015 +0200
@@ -21,6 +21,7 @@
 
 #define MEMORY_POOL_SIZE (4)
 #define RESPONSE_QUEUE_SIZE (1)
+#define DATA_QUEUE_SIZE (2)
 #define MAX_XBUS_DATA_SIZE (128)
 
 static Serial pc(PA_2, PA_3);
@@ -36,6 +37,7 @@
 MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool;
 MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool;
 Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue;
+Queue<XbusMessage, DATA_QUEUE_SIZE> g_dataQueue;
 
 static void* allocateMessageData(size_t bufSize)
 {
@@ -149,55 +151,24 @@
 	}
 }
 
-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]);
-	}
-	float acc[3];
-	if (XbusMessage_getDataItem(acc, XDI_Acceleration, message))
-	{
-		pc.printf(" Acceleration: (% .3f, % .3f, % .3f)", acc[0], acc[1], acc[2]);
-	}
-	float gyr[3];
-	if (XbusMessage_getDataItem(gyr, XDI_RateOfTurn, message))
-	{
-		pc.printf(" Rate Of Turn: (% .3f, % .3f, % .3f)", gyr[0], gyr[1], gyr[2]);
-	}
-	float mag[3];
-	if (XbusMessage_getDataItem(mag, XDI_MagneticField, message))
-	{
-		pc.printf(" Magnetic Field: (% .3f, % .3f, % .3f)", mag[0], mag[1], mag[2]);
-	}
-	uint32_t status;
-	if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
-	{
-		pc.printf(" Status:%X", status);
-	}
-	pc.printf("\n");
-	deallocateMessageData(message->data);
-}
-
 static void mtMessageHandler(struct XbusMessage const* message)
 {
-	if (message->mid == XMID_MtData2)
+	XbusMessage* m = g_messagePool.alloc();
+	if (m)
 	{
-		handleDataMessage(message);
+		memcpy(m, message, sizeof(XbusMessage));
+		if (message->mid == XMID_MtData2)
+		{
+			g_dataQueue.put(m);
+		}
+		else
+		{
+			g_responseQueue.put(m);
+		}
 	}
-	else
+	else if (message->data)
 	{
-		XbusMessage* m = g_messagePool.alloc();
-		memcpy(m, message, sizeof(XbusMessage));
-		g_responseQueue.put(m);
+		deallocateMessageData(message->data);
 	}
 }
 
@@ -392,6 +363,47 @@
 	pc.printf("Press 'm' to start measuring and 'c' to return to config mode.\n");
 }
 
+static void printMessageData(struct XbusMessage const* message)
+{
+	if (!message)
+		return;
+
+	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]);
+	}
+	float acc[3];
+	if (XbusMessage_getDataItem(acc, XDI_Acceleration, message))
+	{
+		pc.printf(" Acceleration: (% .3f, % .3f, % .3f)", acc[0], acc[1], acc[2]);
+	}
+	float gyr[3];
+	if (XbusMessage_getDataItem(gyr, XDI_RateOfTurn, message))
+	{
+		pc.printf(" Rate Of Turn: (% .3f, % .3f, % .3f)", gyr[0], gyr[1], gyr[2]);
+	}
+	float mag[3];
+	if (XbusMessage_getDataItem(mag, XDI_MagneticField, message))
+	{
+		pc.printf(" Magnetic Field: (% .3f, % .3f, % .3f)", mag[0], mag[1], mag[2]);
+	}
+	uint32_t status;
+	if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
+	{
+		pc.printf(" Status:%X", status);
+	}
+	pc.printf("\n");
+}
+
+
 int main(void)
 {
 	XbusParserCallback xbusCallback = {};
@@ -413,7 +425,14 @@
 			{
 				handlePcCommand(pc.getc());
 			}
-			Thread::wait(10);
+
+			osEvent ev = g_dataQueue.get(10);
+			if (ev.status == osEventMessage)
+			{
+				XbusMessage const* data = (XbusMessage const*)ev.value.p;
+				XbusMessageMemoryManager janitor(data);
+				printMessageData(data);
+			}
 		}
 	}
 	else