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