HyunGyu Kim / Mbed 2 deprecated MTi-1_example

kjj

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

main.cpp@26:665d3624f9ab, 2015-05-20 (annotated)

Committer:
Alex Young
Date:
Wed May 20 14:49:54 2015 +0200
Revision:
26:665d3624f9ab
Parent:
25:01356fb59467
Child:
29:d9310e7b58b5
Move PC command handling to a thread.

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 26:665d3624f9ab 7 #define RESPONSE_QUEUE_SIZE (1)
Alex Young 25:01356fb59467 8 #define MAX_XBUS_DATA_SIZE (128)
Alex Young 25:01356fb59467 9
Alex Young 4:98f063b2e6da 10 static Serial pc(PA_2, PA_3);
Alex Young 4:98f063b2e6da 11 static Serial mt(PB_9, PB_8);
Alex Young 4:98f063b2e6da 12 static XbusParser* xbusParser;
Alex Young 25:01356fb59467 13
Alex Young 25:01356fb59467 14 MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool;
Alex Young 25:01356fb59467 15 MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool;
Alex Young 26:665d3624f9ab 16 Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue;
Alex Young 4:98f063b2e6da 17
Alex Young 25:01356fb59467 18 static void* allocateMessageData(size_t bufSize)
Alex Young 4:98f063b2e6da 19 {
Alex Young 25:01356fb59467 20 return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL;
Alex Young 25:01356fb59467 21 }
Alex Young 25:01356fb59467 22
Alex Young 25:01356fb59467 23 static void deallocateMessageData(void const* buffer)
Alex Young 25:01356fb59467 24 {
Alex Young 25:01356fb59467 25 g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer);
Alex Young 4:98f063b2e6da 26 }
Alex Young 4:98f063b2e6da 27
Alex Young 4:98f063b2e6da 28 static void mtLowLevelHandler(void)
Alex Young 4:98f063b2e6da 29 {
Alex Young 4:98f063b2e6da 30 while (mt.readable())
Alex Young 4:98f063b2e6da 31 {
Alex Young 4:98f063b2e6da 32 XbusParser_parseByte(xbusParser, mt.getc());
Alex Young 4:98f063b2e6da 33 }
Alex Young 4:98f063b2e6da 34 }
Alex Young 4:98f063b2e6da 35
Alex Young 26:665d3624f9ab 36 XbusMessage const* doTransaction(XbusMessage* m)
Alex Young 11:8593ba137917 37 {
Alex Young 26:665d3624f9ab 38 uint8_t buf[64];
Alex Young 26:665d3624f9ab 39 size_t rawLength = XbusMessage_format(buf, m);
Alex Young 11:8593ba137917 40 for (size_t i = 0; i < rawLength; ++i)
Alex Young 11:8593ba137917 41 {
Alex Young 11:8593ba137917 42 mt.putc(buf[i]);
Alex Young 11:8593ba137917 43 }
Alex Young 26:665d3624f9ab 44
Alex Young 26:665d3624f9ab 45 osEvent ev = g_responseQueue.get(500);
Alex Young 26:665d3624f9ab 46 return ev.status == osEventMessage ? (XbusMessage*)ev.value.p : NULL;
Alex Young 26:665d3624f9ab 47 }
Alex Young 26:665d3624f9ab 48
Alex Young 26:665d3624f9ab 49 static void freeMessage(XbusMessage const* m)
Alex Young 26:665d3624f9ab 50 {
Alex Young 26:665d3624f9ab 51 if (m->data)
Alex Young 26:665d3624f9ab 52 deallocateMessageData(m->data);
Alex Young 26:665d3624f9ab 53 g_messagePool.free((XbusMessage*)m);
Alex Young 26:665d3624f9ab 54 }
Alex Young 26:665d3624f9ab 55
Alex Young 26:665d3624f9ab 56 static void sendCommand(XsMessageId cmdId)
Alex Young 26:665d3624f9ab 57 {
Alex Young 26:665d3624f9ab 58 XbusMessage m = {cmdId};
Alex Young 26:665d3624f9ab 59 XbusMessage const* response = doTransaction(&m);
Alex Young 26:665d3624f9ab 60
Alex Young 26:665d3624f9ab 61 if (response)
Alex Young 26:665d3624f9ab 62 {
Alex Young 26:665d3624f9ab 63 switch (response->mid)
Alex Young 26:665d3624f9ab 64 {
Alex Young 26:665d3624f9ab 65 case XMID_GotoConfigAck:
Alex Young 26:665d3624f9ab 66 pc.printf("Device went to config mode\n");
Alex Young 26:665d3624f9ab 67 break;
Alex Young 26:665d3624f9ab 68
Alex Young 26:665d3624f9ab 69 case XMID_DeviceId:
Alex Young 26:665d3624f9ab 70 pc.printf("Device ID: %08X\n", *(uint32_t*)response->data);
Alex Young 26:665d3624f9ab 71 break;
Alex Young 26:665d3624f9ab 72
Alex Young 26:665d3624f9ab 73 case XMID_OutputConfig:
Alex Young 26:665d3624f9ab 74 {
Alex Young 26:665d3624f9ab 75 pc.printf("Output configuration\n");
Alex Young 26:665d3624f9ab 76 OutputConfiguration* conf = (OutputConfiguration*)response->data;
Alex Young 26:665d3624f9ab 77 for (int i = 0; i < response->length; ++i)
Alex Young 26:665d3624f9ab 78 {
Alex Young 26:665d3624f9ab 79 pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 26:665d3624f9ab 80 ++conf;
Alex Young 26:665d3624f9ab 81 }
Alex Young 26:665d3624f9ab 82 }
Alex Young 26:665d3624f9ab 83 break;
Alex Young 26:665d3624f9ab 84
Alex Young 26:665d3624f9ab 85
Alex Young 26:665d3624f9ab 86 default:
Alex Young 26:665d3624f9ab 87 pc.printf("Received response MID=%X, length=%d\n", response->mid, response->length);
Alex Young 26:665d3624f9ab 88 break;
Alex Young 26:665d3624f9ab 89 }
Alex Young 26:665d3624f9ab 90 freeMessage(response);
Alex Young 26:665d3624f9ab 91 }
Alex Young 26:665d3624f9ab 92 else
Alex Young 26:665d3624f9ab 93 {
Alex Young 26:665d3624f9ab 94 pc.printf("Timeout waiting for response.\n");
Alex Young 26:665d3624f9ab 95 }
Alex Young 11:8593ba137917 96 }
Alex Young 11:8593ba137917 97
Alex Young 11:8593ba137917 98 static void handlePcCommand(char cmd)
Alex Young 11:8593ba137917 99 {
Alex Young 11:8593ba137917 100 switch (cmd)
Alex Young 11:8593ba137917 101 {
Alex Young 11:8593ba137917 102 case 'c':
Alex Young 11:8593ba137917 103 sendCommand(XMID_GotoConfig);
Alex Young 11:8593ba137917 104 break;
Alex Young 11:8593ba137917 105
Alex Young 11:8593ba137917 106 case 'm':
Alex Young 11:8593ba137917 107 sendCommand(XMID_GotoMeasurement);
Alex Young 11:8593ba137917 108 break;
Alex Young 20:38560fa3d2eb 109
Alex Young 20:38560fa3d2eb 110 case 'd':
Alex Young 20:38560fa3d2eb 111 sendCommand(XMID_ReqDid);
Alex Young 20:38560fa3d2eb 112 break;
Alex Young 22:3eab999c5076 113
Alex Young 22:3eab999c5076 114 case 'o':
Alex Young 22:3eab999c5076 115 sendCommand(XMID_ReqOutputConfig);
Alex Young 22:3eab999c5076 116 break;
Alex Young 11:8593ba137917 117 }
Alex Young 11:8593ba137917 118 }
Alex Young 11:8593ba137917 119
Alex Young 24:2cc49dc854e3 120 static void handleDataMessage(struct XbusMessage const* message)
Alex Young 24:2cc49dc854e3 121 {
Alex Young 24:2cc49dc854e3 122 pc.printf("MTData2:");
Alex Young 24:2cc49dc854e3 123 uint16_t counter;
Alex Young 24:2cc49dc854e3 124 if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
Alex Young 24:2cc49dc854e3 125 {
Alex Young 24:2cc49dc854e3 126 pc.printf(" Packet counter: %5d", counter);
Alex Young 24:2cc49dc854e3 127 }
Alex Young 24:2cc49dc854e3 128 float ori[4];
Alex Young 24:2cc49dc854e3 129 if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
Alex Young 24:2cc49dc854e3 130 {
Alex Young 24:2cc49dc854e3 131 pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1],
Alex Young 24:2cc49dc854e3 132 ori[2], ori[3]);
Alex Young 24:2cc49dc854e3 133 }
Alex Young 24:2cc49dc854e3 134 uint32_t status;
Alex Young 24:2cc49dc854e3 135 if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
Alex Young 24:2cc49dc854e3 136 {
Alex Young 24:2cc49dc854e3 137 pc.printf(" Status:%X", status);
Alex Young 24:2cc49dc854e3 138 }
Alex Young 24:2cc49dc854e3 139 pc.printf("\n");
Alex Young 26:665d3624f9ab 140 deallocateMessageData(message->data);
Alex Young 24:2cc49dc854e3 141 }
Alex Young 24:2cc49dc854e3 142
Alex Young 24:2cc49dc854e3 143 static void mtMessageHandler(struct XbusMessage const* message)
Alex Young 4:98f063b2e6da 144 {
Alex Young 15:558d279addd9 145 if (message->mid == XMID_MtData2)
Alex Young 7:c913a7cd5231 146 {
Alex Young 24:2cc49dc854e3 147 handleDataMessage(message);
Alex Young 7:c913a7cd5231 148 }
Alex Young 7:c913a7cd5231 149 else
Alex Young 7:c913a7cd5231 150 {
Alex Young 26:665d3624f9ab 151 XbusMessage* m = g_messagePool.alloc();
Alex Young 26:665d3624f9ab 152 memcpy(m, message, sizeof(XbusMessage));
Alex Young 26:665d3624f9ab 153 g_responseQueue.put(m);
Alex Young 25:01356fb59467 154 }
Alex Young 4:98f063b2e6da 155 }
Alex Young 4:98f063b2e6da 156
Alex Young 4:98f063b2e6da 157 static void configureSerialPorts(void)
Alex Young 4:98f063b2e6da 158 {
Alex Young 4:98f063b2e6da 159 pc.baud(921600);
Alex Young 4:98f063b2e6da 160 pc.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 161
Alex Young 4:98f063b2e6da 162 mt.baud(921600);
Alex Young 4:98f063b2e6da 163 mt.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 164 mt.attach(mtLowLevelHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 165 }
Alex Young 4:98f063b2e6da 166
Alex Young 2:b3e402dc11ca 167 int main(void)
Alex Young 2:b3e402dc11ca 168 {
Alex Young 4:98f063b2e6da 169 XbusParserCallback xbusCallback = {};
Alex Young 25:01356fb59467 170 xbusCallback.allocateBuffer = allocateMessageData;
Alex Young 25:01356fb59467 171 xbusCallback.deallocateBuffer = deallocateMessageData;
Alex Young 24:2cc49dc854e3 172 xbusCallback.handleMessage = mtMessageHandler;
Alex Young 4:98f063b2e6da 173
Alex Young 4:98f063b2e6da 174 xbusParser = XbusParser_create(&xbusCallback);
Alex Young 4:98f063b2e6da 175 configureSerialPorts();
Alex Young 5:abc52dd88be2 176
Alex Young 5:abc52dd88be2 177 for (;;)
Alex Young 5:abc52dd88be2 178 {
Alex Young 26:665d3624f9ab 179 while (pc.readable())
Alex Young 26:665d3624f9ab 180 {
Alex Young 26:665d3624f9ab 181 handlePcCommand(pc.getc());
Alex Young 26:665d3624f9ab 182 }
Alex Young 5:abc52dd88be2 183 }
Alex Young 4:98f063b2e6da 184 }