David Khosravi / Mbed 2 deprecated MTi-1_example_LPC1768

Add LPC1768

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

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?

UserRevisionLine numberNew 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 }