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@16:4bdcdac223d8, 2015-05-19 (annotated)

Committer:
Alex Young
Date:
Tue May 19 13:40:33 2015 +0200
Revision:
16:4bdcdac223d8
Parent:
15:558d279addd9
Child:
20:38560fa3d2eb
Move MtData2 parsing to xbus message module

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 11:8593ba137917 3 #include "xbusmessage.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 11:8593ba137917 23 static void sendCommand(XsMessageId cmdId)
Alex Young 11:8593ba137917 24 {
Alex Young 11:8593ba137917 25 uint8_t buf[8];
Alex Young 11:8593ba137917 26 XbusMessage m = {cmdId};
Alex Young 11:8593ba137917 27 size_t rawLength = XbusMessage_format(buf, &m);
Alex Young 11:8593ba137917 28 for (size_t i = 0; i < rawLength; ++i)
Alex Young 11:8593ba137917 29 {
Alex Young 11:8593ba137917 30 mt.putc(buf[i]);
Alex Young 11:8593ba137917 31 }
Alex Young 11:8593ba137917 32 }
Alex Young 11:8593ba137917 33
Alex Young 11:8593ba137917 34 static void handlePcCommand(char cmd)
Alex Young 11:8593ba137917 35 {
Alex Young 11:8593ba137917 36 switch (cmd)
Alex Young 11:8593ba137917 37 {
Alex Young 11:8593ba137917 38 case 'c':
Alex Young 11:8593ba137917 39 sendCommand(XMID_GotoConfig);
Alex Young 11:8593ba137917 40 break;
Alex Young 11:8593ba137917 41
Alex Young 11:8593ba137917 42 case 'm':
Alex Young 11:8593ba137917 43 sendCommand(XMID_GotoMeasurement);
Alex Young 11:8593ba137917 44 break;
Alex Young 11:8593ba137917 45 }
Alex Young 11:8593ba137917 46 }
Alex Young 11:8593ba137917 47
Alex Young 11:8593ba137917 48 static void pcHandler(void)
Alex Young 11:8593ba137917 49 {
Alex Young 11:8593ba137917 50 while (pc.readable())
Alex Young 11:8593ba137917 51 {
Alex Young 11:8593ba137917 52 handlePcCommand(pc.getc());
Alex Young 11:8593ba137917 53 }
Alex Young 11:8593ba137917 54 }
Alex Young 11:8593ba137917 55
Alex Young 14:155f9a55ec51 56 static void mtDataHandler(struct XbusMessage const* message)
Alex Young 4:98f063b2e6da 57 {
Alex Young 15:558d279addd9 58 if (message->mid == XMID_MtData2)
Alex Young 7:c913a7cd5231 59 {
Alex Young 8:77cd45916596 60 pc.printf("MTData2:");
Alex Young 7:c913a7cd5231 61 uint16_t counter;
Alex Young 16:4bdcdac223d8 62 if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
Alex Young 7:c913a7cd5231 63 {
Alex Young 8:77cd45916596 64 pc.printf(" Packet counter: %5d", counter);
Alex Young 7:c913a7cd5231 65 }
Alex Young 7:c913a7cd5231 66 float ori[4];
Alex Young 16:4bdcdac223d8 67 if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
Alex Young 7:c913a7cd5231 68 {
Alex Young 8:77cd45916596 69 pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1],
Alex Young 7:c913a7cd5231 70 ori[2], ori[3]);
Alex Young 7:c913a7cd5231 71 }
Alex Young 7:c913a7cd5231 72 uint32_t status;
Alex Young 16:4bdcdac223d8 73 if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
Alex Young 7:c913a7cd5231 74 {
Alex Young 8:77cd45916596 75 pc.printf(" Status:%X", status);
Alex Young 7:c913a7cd5231 76 }
Alex Young 8:77cd45916596 77 pc.printf("\n");
Alex Young 7:c913a7cd5231 78 }
Alex Young 7:c913a7cd5231 79 else
Alex Young 7:c913a7cd5231 80 {
Alex Young 14:155f9a55ec51 81 pc.printf("Received Xbus message. MID=%X, length=%d\n", message->mid, message->length);
Alex Young 7:c913a7cd5231 82 }
Alex Young 4:98f063b2e6da 83 }
Alex Young 4:98f063b2e6da 84
Alex Young 4:98f063b2e6da 85 static void configureSerialPorts(void)
Alex Young 4:98f063b2e6da 86 {
Alex Young 4:98f063b2e6da 87 pc.baud(921600);
Alex Young 4:98f063b2e6da 88 pc.format(8, Serial::None, 2);
Alex Young 11:8593ba137917 89 pc.attach(pcHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 90
Alex Young 4:98f063b2e6da 91 mt.baud(921600);
Alex Young 4:98f063b2e6da 92 mt.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 93 mt.attach(mtLowLevelHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 94 }
Alex Young 4:98f063b2e6da 95
Alex Young 2:b3e402dc11ca 96 int main(void)
Alex Young 2:b3e402dc11ca 97 {
Alex Young 4:98f063b2e6da 98 XbusParserCallback xbusCallback = {};
Alex Young 4:98f063b2e6da 99 xbusCallback.allocateBuffer = allocateBuffer;
Alex Young 4:98f063b2e6da 100 xbusCallback.handleMessage = mtDataHandler;
Alex Young 4:98f063b2e6da 101
Alex Young 4:98f063b2e6da 102 xbusParser = XbusParser_create(&xbusCallback);
Alex Young 4:98f063b2e6da 103 configureSerialPorts();
Alex Young 5:abc52dd88be2 104
Alex Young 5:abc52dd88be2 105 for (;;)
Alex Young 5:abc52dd88be2 106 {
Alex Young 5:abc52dd88be2 107 sleep();
Alex Young 5:abc52dd88be2 108 }
Alex Young 4:98f063b2e6da 109 }