kjj
Dependencies: mbed-rtos mbed Xbus
Fork of MTi-1_example by
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();