rik freije / Mbed 2 deprecated MTi-1_rikbeun

rikbeuncode

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

main.cpp@20:38560fa3d2eb, 2015-05-19 (annotated)

Committer:
Alex Young
Date:
Tue May 19 15:51:47 2015 +0200
Revision:
20:38560fa3d2eb
Parent:
16:4bdcdac223d8
Child:
22:3eab999c5076
Add simple command to read device Id

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 20:38560fa3d2eb 45
Alex Young 20:38560fa3d2eb 46 case 'd':
Alex Young 20:38560fa3d2eb 47 sendCommand(XMID_ReqDid);
Alex Young 20:38560fa3d2eb 48 break;
Alex Young 11:8593ba137917 49 }
Alex Young 11:8593ba137917 50 }
Alex Young 11:8593ba137917 51
Alex Young 11:8593ba137917 52 static void pcHandler(void)
Alex Young 11:8593ba137917 53 {
Alex Young 11:8593ba137917 54 while (pc.readable())
Alex Young 11:8593ba137917 55 {
Alex Young 11:8593ba137917 56 handlePcCommand(pc.getc());
Alex Young 11:8593ba137917 57 }
Alex Young 11:8593ba137917 58 }
Alex Young 11:8593ba137917 59
Alex Young 14:155f9a55ec51 60 static void mtDataHandler(struct XbusMessage const* message)
Alex Young 4:98f063b2e6da 61 {
Alex Young 15:558d279addd9 62 if (message->mid == XMID_MtData2)
Alex Young 7:c913a7cd5231 63 {
Alex Young 8:77cd45916596 64 pc.printf("MTData2:");
Alex Young 7:c913a7cd5231 65 uint16_t counter;
Alex Young 16:4bdcdac223d8 66 if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
Alex Young 7:c913a7cd5231 67 {
Alex Young 8:77cd45916596 68 pc.printf(" Packet counter: %5d", counter);
Alex Young 7:c913a7cd5231 69 }
Alex Young 7:c913a7cd5231 70 float ori[4];
Alex Young 16:4bdcdac223d8 71 if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
Alex Young 7:c913a7cd5231 72 {
Alex Young 8:77cd45916596 73 pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1],
Alex Young 7:c913a7cd5231 74 ori[2], ori[3]);
Alex Young 7:c913a7cd5231 75 }
Alex Young 7:c913a7cd5231 76 uint32_t status;
Alex Young 16:4bdcdac223d8 77 if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
Alex Young 7:c913a7cd5231 78 {
Alex Young 8:77cd45916596 79 pc.printf(" Status:%X", status);
Alex Young 7:c913a7cd5231 80 }
Alex Young 8:77cd45916596 81 pc.printf("\n");
Alex Young 7:c913a7cd5231 82 }
Alex Young 20:38560fa3d2eb 83 else if (message->mid == XMID_DeviceId)
Alex Young 20:38560fa3d2eb 84 {
Alex Young 20:38560fa3d2eb 85 pc.printf("Device ID: %8X\n", *(uint32_t*)message->data);
Alex Young 20:38560fa3d2eb 86 }
Alex Young 7:c913a7cd5231 87 else
Alex Young 7:c913a7cd5231 88 {
Alex Young 14:155f9a55ec51 89 pc.printf("Received Xbus message. MID=%X, length=%d\n", message->mid, message->length);
Alex Young 7:c913a7cd5231 90 }
Alex Young 4:98f063b2e6da 91 }
Alex Young 4:98f063b2e6da 92
Alex Young 4:98f063b2e6da 93 static void configureSerialPorts(void)
Alex Young 4:98f063b2e6da 94 {
Alex Young 4:98f063b2e6da 95 pc.baud(921600);
Alex Young 4:98f063b2e6da 96 pc.format(8, Serial::None, 2);
Alex Young 11:8593ba137917 97 pc.attach(pcHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 98
Alex Young 4:98f063b2e6da 99 mt.baud(921600);
Alex Young 4:98f063b2e6da 100 mt.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 101 mt.attach(mtLowLevelHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 102 }
Alex Young 4:98f063b2e6da 103
Alex Young 2:b3e402dc11ca 104 int main(void)
Alex Young 2:b3e402dc11ca 105 {
Alex Young 4:98f063b2e6da 106 XbusParserCallback xbusCallback = {};
Alex Young 4:98f063b2e6da 107 xbusCallback.allocateBuffer = allocateBuffer;
Alex Young 4:98f063b2e6da 108 xbusCallback.handleMessage = mtDataHandler;
Alex Young 4:98f063b2e6da 109
Alex Young 4:98f063b2e6da 110 xbusParser = XbusParser_create(&xbusCallback);
Alex Young 4:98f063b2e6da 111 configureSerialPorts();
Alex Young 5:abc52dd88be2 112
Alex Young 5:abc52dd88be2 113 for (;;)
Alex Young 5:abc52dd88be2 114 {
Alex Young 5:abc52dd88be2 115 sleep();
Alex Young 5:abc52dd88be2 116 }
Alex Young 4:98f063b2e6da 117 }