kjj
Dependencies: mbed-rtos mbed Xbus
Fork of MTi-1_example by
main.cpp@7:c913a7cd5231, 2015-05-13 (annotated)
- Committer:
- Alex Young
- Date:
- Wed May 13 14:53:07 2015 +0200
- Revision:
- 7:c913a7cd5231
- Parent:
- 5:abc52dd88be2
- Child:
- 8:77cd45916596
Decode data packets and write to PC
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 |
7:c913a7cd5231 | 27 | pc.printf("Data packet received\n"); |
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 |
7:c913a7cd5231 | 31 | pc.printf("\tPacket counter:\t%d\n", 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 |
7:c913a7cd5231 | 36 | pc.printf("\tOrientation\t%.3f, %.3f, %.3f, %.3f\n", 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 |
7:c913a7cd5231 | 42 | pc.printf("\tFilter status:\t%d\n", status); |
Alex Young |
7:c913a7cd5231 | 43 | } |
Alex Young |
7:c913a7cd5231 | 44 | pc.printf("\n\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 | |
Alex Young |
4:98f063b2e6da | 53 | static void configureSerialPorts(void) |
Alex Young |
4:98f063b2e6da | 54 | { |
Alex Young |
4:98f063b2e6da | 55 | pc.baud(921600); |
Alex Young |
4:98f063b2e6da | 56 | pc.format(8, Serial::None, 2); |
Alex Young |
4:98f063b2e6da | 57 | |
Alex Young |
4:98f063b2e6da | 58 | mt.baud(921600); |
Alex Young |
4:98f063b2e6da | 59 | mt.format(8, Serial::None, 2); |
Alex Young |
4:98f063b2e6da | 60 | mt.attach(mtLowLevelHandler, Serial::RxIrq); |
Alex Young |
4:98f063b2e6da | 61 | } |
Alex Young |
4:98f063b2e6da | 62 | |
Alex Young |
2:b3e402dc11ca | 63 | int main(void) |
Alex Young |
2:b3e402dc11ca | 64 | { |
Alex Young |
4:98f063b2e6da | 65 | XbusParserCallback xbusCallback = {}; |
Alex Young |
4:98f063b2e6da | 66 | xbusCallback.allocateBuffer = allocateBuffer; |
Alex Young |
4:98f063b2e6da | 67 | xbusCallback.handleMessage = mtDataHandler; |
Alex Young |
4:98f063b2e6da | 68 | |
Alex Young |
4:98f063b2e6da | 69 | xbusParser = XbusParser_create(&xbusCallback); |
Alex Young |
4:98f063b2e6da | 70 | configureSerialPorts(); |
Alex Young |
5:abc52dd88be2 | 71 | |
Alex Young |
5:abc52dd88be2 | 72 | for (;;) |
Alex Young |
5:abc52dd88be2 | 73 | { |
Alex Young |
5:abc52dd88be2 | 74 | sleep(); |
Alex Young |
5:abc52dd88be2 | 75 | } |
Alex Young |
4:98f063b2e6da | 76 | } |