rik freije / Mbed 2 deprecated MTi-1_rikbeun

rikbeuncode

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

main.cpp@24:2cc49dc854e3, 2015-05-20 (annotated)

Committer:
Alex Young
Date:
Wed May 20 12:47:50 2015 +0200
Revision:
24:2cc49dc854e3
Parent:
22:3eab999c5076
Child:
25:01356fb59467
Extract data handling from other xbus messages.

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