FRDM-KL25Zand Xsens MTi-3
Dependencies: mbed mbed-rtos Xbus
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