FRDM-KL25Zand Xsens MTi-3
Dependencies: mbed mbed-rtos Xbus
main.cpp@8:77cd45916596, 2015-05-13 (annotated)
- Committer:
- Alex Young
- Date:
- Wed May 13 15:32:21 2015 +0200
- Revision:
- 8:77cd45916596
- Parent:
- 7:c913a7cd5231
- Child:
- 11:8593ba137917
Clean up formatting of output data.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Alex Young |
4:98f063b2e6da | 1 | #include "mbed.h" |
Alex Young |
4:98f063b2e6da | 2 | #include "xbusparser.h" |
Alex Young |
7:c913a7cd5231 | 3 | #include "mtdata2.h" |
Alex Young |
4:98f063b2e6da | 4 | |
Alex Young |
4:98f063b2e6da | 5 | static Serial pc(PA_2, PA_3); |
Alex Young |
4:98f063b2e6da | 6 | static Serial mt(PB_9, PB_8); |
Alex Young |
4:98f063b2e6da | 7 | static XbusParser* xbusParser; |
Alex Young |
4:98f063b2e6da | 8 | static uint8_t rxBuffer[256]; |
Alex Young |
4:98f063b2e6da | 9 | |
Alex Young |
4:98f063b2e6da | 10 | static void* allocateBuffer(size_t bufSize) |
Alex Young |
4:98f063b2e6da | 11 | { |
Alex Young |
4:98f063b2e6da | 12 | return bufSize < sizeof(rxBuffer) ? rxBuffer : NULL; |
Alex Young |
4:98f063b2e6da | 13 | } |
Alex Young |
4:98f063b2e6da | 14 | |
Alex Young |
4:98f063b2e6da | 15 | static void mtLowLevelHandler(void) |
Alex Young |
4:98f063b2e6da | 16 | { |
Alex Young |
4:98f063b2e6da | 17 | while (mt.readable()) |
Alex Young |
4:98f063b2e6da | 18 | { |
Alex Young |
4:98f063b2e6da | 19 | XbusParser_parseByte(xbusParser, mt.getc()); |
Alex Young |
4:98f063b2e6da | 20 | } |
Alex Young |
4:98f063b2e6da | 21 | } |
Alex Young |
4:98f063b2e6da | 22 | |
Alex Young |
4:98f063b2e6da | 23 | static void mtDataHandler(uint8_t mid, uint16_t dataLength, uint8_t const* data) |
Alex Young |
4:98f063b2e6da | 24 | { |
Alex Young |
7:c913a7cd5231 | 25 | if (mid == MTDATA2_MESSAGE_ID) |
Alex Young |
7:c913a7cd5231 | 26 | { |
Alex Young |
8:77cd45916596 | 27 | pc.printf("MTData2:"); |
Alex Young |
7:c913a7cd5231 | 28 | uint16_t counter; |
Alex Young |
7:c913a7cd5231 | 29 | if (MtData2_getItem(&counter, XDI_PacketCounter, data, dataLength)) |
Alex Young |
7:c913a7cd5231 | 30 | { |
Alex Young |
8:77cd45916596 | 31 | pc.printf(" Packet counter: %5d", counter); |
Alex Young |
7:c913a7cd5231 | 32 | } |
Alex Young |
7:c913a7cd5231 | 33 | float ori[4]; |
Alex Young |
7:c913a7cd5231 | 34 | if (MtData2_getItem(ori, XDI_Quaternion, data, dataLength)) |
Alex Young |
7:c913a7cd5231 | 35 | { |
Alex Young |
8:77cd45916596 | 36 | pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1], |
Alex Young |
7:c913a7cd5231 | 37 | ori[2], ori[3]); |
Alex Young |
7:c913a7cd5231 | 38 | } |
Alex Young |
7:c913a7cd5231 | 39 | uint32_t status; |
Alex Young |
7:c913a7cd5231 | 40 | if (MtData2_getItem(&status, XDI_StatusWord, data, dataLength)) |
Alex Young |
7:c913a7cd5231 | 41 | { |
Alex Young |
8:77cd45916596 | 42 | pc.printf(" Status:%X", status); |
Alex Young |
7:c913a7cd5231 | 43 | } |
Alex Young |
8:77cd45916596 | 44 | pc.printf("\n"); |
Alex Young |
7:c913a7cd5231 | 45 | } |
Alex Young |
7:c913a7cd5231 | 46 | else |
Alex Young |
7:c913a7cd5231 | 47 | { |
Alex Young |
7:c913a7cd5231 | 48 | pc.printf("Received Xbus message. MID=%X, length=%d\n", mid, dataLength); |
Alex Young |
7:c913a7cd5231 | 49 | } |
Alex Young |
4:98f063b2e6da | 50 | } |
Alex Young |
4:98f063b2e6da | 51 | |
Alex Young |
4:98f063b2e6da | 52 | static void configureSerialPorts(void) |
Alex Young |
4:98f063b2e6da | 53 | { |
Alex Young |
4:98f063b2e6da | 54 | pc.baud(921600); |
Alex Young |
4:98f063b2e6da | 55 | pc.format(8, Serial::None, 2); |
Alex Young |
4:98f063b2e6da | 56 | |
Alex Young |
4:98f063b2e6da | 57 | mt.baud(921600); |
Alex Young |
4:98f063b2e6da | 58 | mt.format(8, Serial::None, 2); |
Alex Young |
4:98f063b2e6da | 59 | mt.attach(mtLowLevelHandler, Serial::RxIrq); |
Alex Young |
4:98f063b2e6da | 60 | } |
Alex Young |
4:98f063b2e6da | 61 | |
Alex Young |
2:b3e402dc11ca | 62 | int main(void) |
Alex Young |
2:b3e402dc11ca | 63 | { |
Alex Young |
4:98f063b2e6da | 64 | XbusParserCallback xbusCallback = {}; |
Alex Young |
4:98f063b2e6da | 65 | xbusCallback.allocateBuffer = allocateBuffer; |
Alex Young |
4:98f063b2e6da | 66 | xbusCallback.handleMessage = mtDataHandler; |
Alex Young |
4:98f063b2e6da | 67 | |
Alex Young |
4:98f063b2e6da | 68 | xbusParser = XbusParser_create(&xbusCallback); |
Alex Young |
4:98f063b2e6da | 69 | configureSerialPorts(); |
Alex Young |
5:abc52dd88be2 | 70 | |
Alex Young |
5:abc52dd88be2 | 71 | for (;;) |
Alex Young |
5:abc52dd88be2 | 72 | { |
Alex Young |
5:abc52dd88be2 | 73 | sleep(); |
Alex Young |
5:abc52dd88be2 | 74 | } |
Alex Young |
4:98f063b2e6da | 75 | } |