rik freije / Mbed 2 deprecated MTi-1_rikbeun

rikbeuncode

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

main.cpp@25:01356fb59467, 2015-05-20 (annotated)

Committer:
Alex Young
Date:
Wed May 20 13:58:34 2015 +0200
Revision:
25:01356fb59467
Parent:
24:2cc49dc854e3
Child:
26:665d3624f9ab
Use rtos memory pool for message reception

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Alex Young 4:98f063b2e6da 1 #include "mbed.h"
Alex Young 25:01356fb59467 2 #include "rtos.h"
Alex Young 4:98f063b2e6da 3 #include "xbusparser.h"
Alex Young 11:8593ba137917 4 #include "xbusmessage.h"
Alex Young 4:98f063b2e6da 5
Alex Young 25:01356fb59467 6 #define MEMORY_POOL_SIZE (4)
Alex Young 25:01356fb59467 7 #define MAX_XBUS_DATA_SIZE (128)
Alex Young 25:01356fb59467 8
Alex Young 4:98f063b2e6da 9 static Serial pc(PA_2, PA_3);
Alex Young 4:98f063b2e6da 10 static Serial mt(PB_9, PB_8);
Alex Young 4:98f063b2e6da 11 static XbusParser* xbusParser;
Alex Young 25:01356fb59467 12
Alex Young 25:01356fb59467 13 MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool;
Alex Young 25:01356fb59467 14 MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool;
Alex Young 4:98f063b2e6da 15
Alex Young 25:01356fb59467 16 static void* allocateMessageData(size_t bufSize)
Alex Young 4:98f063b2e6da 17 {
Alex Young 25:01356fb59467 18 return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL;
Alex Young 25:01356fb59467 19 }
Alex Young 25:01356fb59467 20
Alex Young 25:01356fb59467 21 static void deallocateMessageData(void const* buffer)
Alex Young 25:01356fb59467 22 {
Alex Young 25:01356fb59467 23 g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer);
Alex Young 4:98f063b2e6da 24 }
Alex Young 4:98f063b2e6da 25
Alex Young 4:98f063b2e6da 26 static void mtLowLevelHandler(void)
Alex Young 4:98f063b2e6da 27 {
Alex Young 4:98f063b2e6da 28 while (mt.readable())
Alex Young 4:98f063b2e6da 29 {
Alex Young 4:98f063b2e6da 30 XbusParser_parseByte(xbusParser, mt.getc());
Alex Young 4:98f063b2e6da 31 }
Alex Young 4:98f063b2e6da 32 }
Alex Young 4:98f063b2e6da 33
Alex Young 11:8593ba137917 34 static void sendCommand(XsMessageId cmdId)
Alex Young 11:8593ba137917 35 {
Alex Young 11:8593ba137917 36 uint8_t buf[8];
Alex Young 11:8593ba137917 37 XbusMessage m = {cmdId};
Alex Young 11:8593ba137917 38 size_t rawLength = XbusMessage_format(buf, &m);
Alex Young 11:8593ba137917 39 for (size_t i = 0; i < rawLength; ++i)
Alex Young 11:8593ba137917 40 {
Alex Young 11:8593ba137917 41 mt.putc(buf[i]);
Alex Young 11:8593ba137917 42 }
Alex Young 11:8593ba137917 43 }
Alex Young 11:8593ba137917 44
Alex Young 11:8593ba137917 45 static void handlePcCommand(char cmd)
Alex Young 11:8593ba137917 46 {
Alex Young 11:8593ba137917 47 switch (cmd)
Alex Young 11:8593ba137917 48 {
Alex Young 11:8593ba137917 49 case 'c':
Alex Young 11:8593ba137917 50 sendCommand(XMID_GotoConfig);
Alex Young 11:8593ba137917 51 break;
Alex Young 11:8593ba137917 52
Alex Young 11:8593ba137917 53 case 'm':
Alex Young 11:8593ba137917 54 sendCommand(XMID_GotoMeasurement);
Alex Young 11:8593ba137917 55 break;
Alex Young 20:38560fa3d2eb 56
Alex Young 20:38560fa3d2eb 57 case 'd':
Alex Young 20:38560fa3d2eb 58 sendCommand(XMID_ReqDid);
Alex Young 20:38560fa3d2eb 59 break;
Alex Young 22:3eab999c5076 60
Alex Young 22:3eab999c5076 61 case 'o':
Alex Young 22:3eab999c5076 62 sendCommand(XMID_ReqOutputConfig);
Alex Young 22:3eab999c5076 63 break;
Alex Young 11:8593ba137917 64 }
Alex Young 11:8593ba137917 65 }
Alex Young 11:8593ba137917 66
Alex Young 11:8593ba137917 67 static void pcHandler(void)
Alex Young 11:8593ba137917 68 {
Alex Young 11:8593ba137917 69 while (pc.readable())
Alex Young 11:8593ba137917 70 {
Alex Young 11:8593ba137917 71 handlePcCommand(pc.getc());
Alex Young 11:8593ba137917 72 }
Alex Young 11:8593ba137917 73 }
Alex Young 11:8593ba137917 74
Alex Young 24:2cc49dc854e3 75 static void handleDataMessage(struct XbusMessage const* message)
Alex Young 24:2cc49dc854e3 76 {
Alex Young 24:2cc49dc854e3 77 pc.printf("MTData2:");
Alex Young 24:2cc49dc854e3 78 uint16_t counter;
Alex Young 24:2cc49dc854e3 79 if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
Alex Young 24:2cc49dc854e3 80 {
Alex Young 24:2cc49dc854e3 81 pc.printf(" Packet counter: %5d", counter);
Alex Young 24:2cc49dc854e3 82 }
Alex Young 24:2cc49dc854e3 83 float ori[4];
Alex Young 24:2cc49dc854e3 84 if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
Alex Young 24:2cc49dc854e3 85 {
Alex Young 24:2cc49dc854e3 86 pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1],
Alex Young 24:2cc49dc854e3 87 ori[2], ori[3]);
Alex Young 24:2cc49dc854e3 88 }
Alex Young 24:2cc49dc854e3 89 uint32_t status;
Alex Young 24:2cc49dc854e3 90 if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
Alex Young 24:2cc49dc854e3 91 {
Alex Young 24:2cc49dc854e3 92 pc.printf(" Status:%X", status);
Alex Young 24:2cc49dc854e3 93 }
Alex Young 24:2cc49dc854e3 94 pc.printf("\n");
Alex Young 24:2cc49dc854e3 95 }
Alex Young 24:2cc49dc854e3 96
Alex Young 24:2cc49dc854e3 97 static void mtMessageHandler(struct XbusMessage const* message)
Alex Young 4:98f063b2e6da 98 {
Alex Young 15:558d279addd9 99 if (message->mid == XMID_MtData2)
Alex Young 7:c913a7cd5231 100 {
Alex Young 24:2cc49dc854e3 101 handleDataMessage(message);
Alex Young 7:c913a7cd5231 102 }
Alex Young 20:38560fa3d2eb 103 else if (message->mid == XMID_DeviceId)
Alex Young 20:38560fa3d2eb 104 {
Alex Young 20:38560fa3d2eb 105 pc.printf("Device ID: %8X\n", *(uint32_t*)message->data);
Alex Young 20:38560fa3d2eb 106 }
Alex Young 22:3eab999c5076 107 else if (message->mid == XMID_OutputConfig)
Alex Young 22:3eab999c5076 108 {
Alex Young 22:3eab999c5076 109 pc.printf("Output configuration:\n");
Alex Young 22:3eab999c5076 110 struct OutputConfiguration* conf = (struct OutputConfiguration*)message->data;
Alex Young 22:3eab999c5076 111 for (int i = 0; i < message->length; ++i)
Alex Young 22:3eab999c5076 112 {
Alex Young 22:3eab999c5076 113 pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 22:3eab999c5076 114 ++conf;
Alex Young 22:3eab999c5076 115 }
Alex Young 22:3eab999c5076 116 }
Alex Young 7:c913a7cd5231 117 else
Alex Young 7:c913a7cd5231 118 {
Alex Young 14:155f9a55ec51 119 pc.printf("Received Xbus message. MID=%X, length=%d\n", message->mid, message->length);
Alex Young 7:c913a7cd5231 120 }
Alex Young 25:01356fb59467 121 if (message->data)
Alex Young 25:01356fb59467 122 {
Alex Young 25:01356fb59467 123 deallocateMessageData(message->data);
Alex Young 25:01356fb59467 124 }
Alex Young 4:98f063b2e6da 125 }
Alex Young 4:98f063b2e6da 126
Alex Young 4:98f063b2e6da 127 static void configureSerialPorts(void)
Alex Young 4:98f063b2e6da 128 {
Alex Young 4:98f063b2e6da 129 pc.baud(921600);
Alex Young 4:98f063b2e6da 130 pc.format(8, Serial::None, 2);
Alex Young 11:8593ba137917 131 pc.attach(pcHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 132
Alex Young 4:98f063b2e6da 133 mt.baud(921600);
Alex Young 4:98f063b2e6da 134 mt.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 135 mt.attach(mtLowLevelHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 136 }
Alex Young 4:98f063b2e6da 137
Alex Young 2:b3e402dc11ca 138 int main(void)
Alex Young 2:b3e402dc11ca 139 {
Alex Young 4:98f063b2e6da 140 XbusParserCallback xbusCallback = {};
Alex Young 25:01356fb59467 141 xbusCallback.allocateBuffer = allocateMessageData;
Alex Young 25:01356fb59467 142 xbusCallback.deallocateBuffer = deallocateMessageData;
Alex Young 24:2cc49dc854e3 143 xbusCallback.handleMessage = mtMessageHandler;
Alex Young 4:98f063b2e6da 144
Alex Young 4:98f063b2e6da 145 xbusParser = XbusParser_create(&xbusCallback);
Alex Young 4:98f063b2e6da 146 configureSerialPorts();
Alex Young 5:abc52dd88be2 147
Alex Young 5:abc52dd88be2 148 for (;;)
Alex Young 5:abc52dd88be2 149 {
Alex Young 5:abc52dd88be2 150 sleep();
Alex Young 5:abc52dd88be2 151 }
Alex Young 4:98f063b2e6da 152 }