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@13:dc2aa4678ec7, 2015-05-19 (annotated)

Committer:
Alex Young
Date:
Tue May 19 13:11:23 2015 +0200
Revision:
13:dc2aa4678ec7
Parent:
11:8593ba137917
Child:
14:155f9a55ec51
Ignore local mbed-rtos files



These are automatically added when exported from mbed.org.

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