HyunGyu Kim / Mbed 2 deprecated MTi-1_example

kjj

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

main.cpp@22:3eab999c5076, 2015-05-19 (annotated)

Committer:
Alex Young
Date:
Tue May 19 16:59:53 2015 +0200
Revision:
22:3eab999c5076
Parent:
20:38560fa3d2eb
Child:
24:2cc49dc854e3
Add support for reading back output configuration.

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