Xsens / Mbed 2 deprecated MTi-1_example

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Alex Young

main.cpp@19:46e88d37ecef, 2015-05-19 (annotated)

Committer:
Alex Young
Date:
Tue May 19 15:49:29 2015 +0200
Revision:
19:46e88d37ecef
Parent:
16:4bdcdac223d8
Child:
20:38560fa3d2eb
Add support for parsing individual xbus messages.



Most xbus messages, except from MTData2 messages have a fixed format. To

make life easy for the user we will parse these and return the parsed

data in the memory pointed to by the XbusMessage data pointer. This way

we can just define structs for the different messages and users can

switch on the mid to decide how to cast the data pointer.

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 }