David Khosravi / Mbed 2 deprecated MTi-1_example_LPC1768

Add LPC1768

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

main.cpp@29:d9310e7b58b5, 2015-05-20 (annotated)

Committer:
Alex Young
Date:
Wed May 20 16:57:39 2015 +0200
Revision:
29:d9310e7b58b5
Parent:
26:665d3624f9ab
Child:
31:ce1ea9ae861e
Set output config based on device type.

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 29:d9310e7b58b5 56 static void dumpResponse(XbusMessage const* response)
Alex Young 29:d9310e7b58b5 57 {
Alex Young 29:d9310e7b58b5 58 switch (response->mid)
Alex Young 29:d9310e7b58b5 59 {
Alex Young 29:d9310e7b58b5 60 case XMID_GotoConfigAck:
Alex Young 29:d9310e7b58b5 61 pc.printf("Device went to config mode\n");
Alex Young 29:d9310e7b58b5 62 break;
Alex Young 29:d9310e7b58b5 63
Alex Young 29:d9310e7b58b5 64 case XMID_DeviceId:
Alex Young 29:d9310e7b58b5 65 pc.printf("Device ID: %08X\n", *(uint32_t*)response->data);
Alex Young 29:d9310e7b58b5 66 break;
Alex Young 29:d9310e7b58b5 67
Alex Young 29:d9310e7b58b5 68 case XMID_OutputConfig:
Alex Young 29:d9310e7b58b5 69 {
Alex Young 29:d9310e7b58b5 70 pc.printf("Output configuration\n");
Alex Young 29:d9310e7b58b5 71 OutputConfiguration* conf = (OutputConfiguration*)response->data;
Alex Young 29:d9310e7b58b5 72 for (int i = 0; i < response->length; ++i)
Alex Young 29:d9310e7b58b5 73 {
Alex Young 29:d9310e7b58b5 74 pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 29:d9310e7b58b5 75 ++conf;
Alex Young 29:d9310e7b58b5 76 }
Alex Young 29:d9310e7b58b5 77 }
Alex Young 29:d9310e7b58b5 78 break;
Alex Young 29:d9310e7b58b5 79
Alex Young 29:d9310e7b58b5 80 case XMID_Error:
Alex Young 29:d9310e7b58b5 81 pc.printf("Device error!");
Alex Young 29:d9310e7b58b5 82 break;
Alex Young 29:d9310e7b58b5 83
Alex Young 29:d9310e7b58b5 84 default:
Alex Young 29:d9310e7b58b5 85 pc.printf("Received response MID=%X, length=%d\n", response->mid, response->length);
Alex Young 29:d9310e7b58b5 86 break;
Alex Young 29:d9310e7b58b5 87 }
Alex Young 29:d9310e7b58b5 88 }
Alex Young 29:d9310e7b58b5 89
Alex Young 26:665d3624f9ab 90 static void sendCommand(XsMessageId cmdId)
Alex Young 26:665d3624f9ab 91 {
Alex Young 26:665d3624f9ab 92 XbusMessage m = {cmdId};
Alex Young 26:665d3624f9ab 93 XbusMessage const* response = doTransaction(&m);
Alex Young 26:665d3624f9ab 94
Alex Young 26:665d3624f9ab 95 if (response)
Alex Young 26:665d3624f9ab 96 {
Alex Young 29:d9310e7b58b5 97 dumpResponse(response);
Alex Young 26:665d3624f9ab 98 freeMessage(response);
Alex Young 26:665d3624f9ab 99 }
Alex Young 26:665d3624f9ab 100 else
Alex Young 26:665d3624f9ab 101 {
Alex Young 26:665d3624f9ab 102 pc.printf("Timeout waiting for response.\n");
Alex Young 26:665d3624f9ab 103 }
Alex Young 11:8593ba137917 104 }
Alex Young 11:8593ba137917 105
Alex Young 11:8593ba137917 106 static void handlePcCommand(char cmd)
Alex Young 11:8593ba137917 107 {
Alex Young 11:8593ba137917 108 switch (cmd)
Alex Young 11:8593ba137917 109 {
Alex Young 11:8593ba137917 110 case 'c':
Alex Young 11:8593ba137917 111 sendCommand(XMID_GotoConfig);
Alex Young 11:8593ba137917 112 break;
Alex Young 11:8593ba137917 113
Alex Young 11:8593ba137917 114 case 'm':
Alex Young 11:8593ba137917 115 sendCommand(XMID_GotoMeasurement);
Alex Young 11:8593ba137917 116 break;
Alex Young 20:38560fa3d2eb 117
Alex Young 20:38560fa3d2eb 118 case 'd':
Alex Young 20:38560fa3d2eb 119 sendCommand(XMID_ReqDid);
Alex Young 20:38560fa3d2eb 120 break;
Alex Young 22:3eab999c5076 121
Alex Young 22:3eab999c5076 122 case 'o':
Alex Young 22:3eab999c5076 123 sendCommand(XMID_ReqOutputConfig);
Alex Young 22:3eab999c5076 124 break;
Alex Young 11:8593ba137917 125 }
Alex Young 11:8593ba137917 126 }
Alex Young 11:8593ba137917 127
Alex Young 24:2cc49dc854e3 128 static void handleDataMessage(struct XbusMessage const* message)
Alex Young 24:2cc49dc854e3 129 {
Alex Young 24:2cc49dc854e3 130 pc.printf("MTData2:");
Alex Young 24:2cc49dc854e3 131 uint16_t counter;
Alex Young 24:2cc49dc854e3 132 if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
Alex Young 24:2cc49dc854e3 133 {
Alex Young 24:2cc49dc854e3 134 pc.printf(" Packet counter: %5d", counter);
Alex Young 24:2cc49dc854e3 135 }
Alex Young 24:2cc49dc854e3 136 float ori[4];
Alex Young 24:2cc49dc854e3 137 if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
Alex Young 24:2cc49dc854e3 138 {
Alex Young 24:2cc49dc854e3 139 pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1],
Alex Young 24:2cc49dc854e3 140 ori[2], ori[3]);
Alex Young 24:2cc49dc854e3 141 }
Alex Young 24:2cc49dc854e3 142 uint32_t status;
Alex Young 24:2cc49dc854e3 143 if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
Alex Young 24:2cc49dc854e3 144 {
Alex Young 24:2cc49dc854e3 145 pc.printf(" Status:%X", status);
Alex Young 24:2cc49dc854e3 146 }
Alex Young 24:2cc49dc854e3 147 pc.printf("\n");
Alex Young 26:665d3624f9ab 148 deallocateMessageData(message->data);
Alex Young 24:2cc49dc854e3 149 }
Alex Young 24:2cc49dc854e3 150
Alex Young 24:2cc49dc854e3 151 static void mtMessageHandler(struct XbusMessage const* message)
Alex Young 4:98f063b2e6da 152 {
Alex Young 15:558d279addd9 153 if (message->mid == XMID_MtData2)
Alex Young 7:c913a7cd5231 154 {
Alex Young 24:2cc49dc854e3 155 handleDataMessage(message);
Alex Young 7:c913a7cd5231 156 }
Alex Young 7:c913a7cd5231 157 else
Alex Young 7:c913a7cd5231 158 {
Alex Young 26:665d3624f9ab 159 XbusMessage* m = g_messagePool.alloc();
Alex Young 26:665d3624f9ab 160 memcpy(m, message, sizeof(XbusMessage));
Alex Young 26:665d3624f9ab 161 g_responseQueue.put(m);
Alex Young 25:01356fb59467 162 }
Alex Young 4:98f063b2e6da 163 }
Alex Young 4:98f063b2e6da 164
Alex Young 4:98f063b2e6da 165 static void configureSerialPorts(void)
Alex Young 4:98f063b2e6da 166 {
Alex Young 4:98f063b2e6da 167 pc.baud(921600);
Alex Young 4:98f063b2e6da 168 pc.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 169
Alex Young 4:98f063b2e6da 170 mt.baud(921600);
Alex Young 4:98f063b2e6da 171 mt.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 172 mt.attach(mtLowLevelHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 173 }
Alex Young 4:98f063b2e6da 174
Alex Young 29:d9310e7b58b5 175 static uint32_t readDeviceId(void)
Alex Young 29:d9310e7b58b5 176 {
Alex Young 29:d9310e7b58b5 177 XbusMessage reqDid = {XMID_ReqDid};
Alex Young 29:d9310e7b58b5 178 XbusMessage const* didRsp = doTransaction(&reqDid);
Alex Young 29:d9310e7b58b5 179 uint32_t deviceId = 0;
Alex Young 29:d9310e7b58b5 180 if (didRsp)
Alex Young 29:d9310e7b58b5 181 {
Alex Young 29:d9310e7b58b5 182 if (didRsp->mid == XMID_DeviceId)
Alex Young 29:d9310e7b58b5 183 {
Alex Young 29:d9310e7b58b5 184 deviceId = *(uint32_t*)didRsp->data;
Alex Young 29:d9310e7b58b5 185 }
Alex Young 29:d9310e7b58b5 186 freeMessage(didRsp);
Alex Young 29:d9310e7b58b5 187 }
Alex Young 29:d9310e7b58b5 188 return deviceId;
Alex Young 29:d9310e7b58b5 189 }
Alex Young 29:d9310e7b58b5 190
Alex Young 29:d9310e7b58b5 191 static bool configureMotionTracker(void)
Alex Young 29:d9310e7b58b5 192 {
Alex Young 29:d9310e7b58b5 193 uint32_t deviceId = readDeviceId();
Alex Young 29:d9310e7b58b5 194 uint8_t deviceType = (deviceId >> 24) & 0x0F;
Alex Young 29:d9310e7b58b5 195
Alex Young 29:d9310e7b58b5 196 if (deviceId)
Alex Young 29:d9310e7b58b5 197 {
Alex Young 29:d9310e7b58b5 198 pc.printf("Found MTi-%d\n", deviceType);
Alex Young 29:d9310e7b58b5 199
Alex Young 29:d9310e7b58b5 200 OutputConfiguration conf[5] = {
Alex Young 29:d9310e7b58b5 201 {XDI_PacketCounter, 65535},
Alex Young 29:d9310e7b58b5 202 {XDI_SampleTimeFine, 65535}
Alex Young 29:d9310e7b58b5 203 };
Alex Young 29:d9310e7b58b5 204 if (deviceType == 1)
Alex Young 29:d9310e7b58b5 205 {
Alex Young 29:d9310e7b58b5 206 conf[2].dtype = XDI_Acceleration;
Alex Young 29:d9310e7b58b5 207 conf[2].freq = 100;
Alex Young 29:d9310e7b58b5 208 conf[3].dtype = XDI_RateOfTurn;
Alex Young 29:d9310e7b58b5 209 conf[3].freq = 100;
Alex Young 29:d9310e7b58b5 210 conf[4].dtype = XDI_MagneticField;
Alex Young 29:d9310e7b58b5 211 conf[4].freq = 100;
Alex Young 29:d9310e7b58b5 212 }
Alex Young 29:d9310e7b58b5 213 else
Alex Young 29:d9310e7b58b5 214 {
Alex Young 29:d9310e7b58b5 215 conf[2].dtype = XDI_Quaternion;
Alex Young 29:d9310e7b58b5 216 conf[2].freq = 100;
Alex Young 29:d9310e7b58b5 217 conf[3].dtype = XDI_StatusWord;
Alex Young 29:d9310e7b58b5 218 conf[3].freq = 65535;
Alex Young 29:d9310e7b58b5 219 }
Alex Young 29:d9310e7b58b5 220
Alex Young 29:d9310e7b58b5 221 XbusMessage outputConfMsg = {XMID_SetOutputConfig, 5, &conf};
Alex Young 29:d9310e7b58b5 222 XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg);
Alex Young 29:d9310e7b58b5 223 if (outputConfRsp)
Alex Young 29:d9310e7b58b5 224 {
Alex Young 29:d9310e7b58b5 225 if (outputConfRsp->mid == XMID_OutputConfig)
Alex Young 29:d9310e7b58b5 226 {
Alex Young 29:d9310e7b58b5 227 pc.printf("Output configuration set to:\n");
Alex Young 29:d9310e7b58b5 228 OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data;
Alex Young 29:d9310e7b58b5 229 for (int i = 0; i < outputConfRsp->length; ++i)
Alex Young 29:d9310e7b58b5 230 {
Alex Young 29:d9310e7b58b5 231 pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 29:d9310e7b58b5 232 ++conf;
Alex Young 29:d9310e7b58b5 233 }
Alex Young 29:d9310e7b58b5 234 }
Alex Young 29:d9310e7b58b5 235 else
Alex Young 29:d9310e7b58b5 236 {
Alex Young 29:d9310e7b58b5 237 dumpResponse(outputConfRsp);
Alex Young 29:d9310e7b58b5 238 }
Alex Young 29:d9310e7b58b5 239 freeMessage(outputConfRsp);
Alex Young 29:d9310e7b58b5 240 }
Alex Young 29:d9310e7b58b5 241 else
Alex Young 29:d9310e7b58b5 242 {
Alex Young 29:d9310e7b58b5 243 pc.printf("Failed to set output configuration.\n");
Alex Young 29:d9310e7b58b5 244 }
Alex Young 29:d9310e7b58b5 245
Alex Young 29:d9310e7b58b5 246 return true;
Alex Young 29:d9310e7b58b5 247 }
Alex Young 29:d9310e7b58b5 248 else
Alex Young 29:d9310e7b58b5 249 {
Alex Young 29:d9310e7b58b5 250 return false;
Alex Young 29:d9310e7b58b5 251 }
Alex Young 29:d9310e7b58b5 252 }
Alex Young 29:d9310e7b58b5 253
Alex Young 2:b3e402dc11ca 254 int main(void)
Alex Young 2:b3e402dc11ca 255 {
Alex Young 4:98f063b2e6da 256 XbusParserCallback xbusCallback = {};
Alex Young 25:01356fb59467 257 xbusCallback.allocateBuffer = allocateMessageData;
Alex Young 25:01356fb59467 258 xbusCallback.deallocateBuffer = deallocateMessageData;
Alex Young 24:2cc49dc854e3 259 xbusCallback.handleMessage = mtMessageHandler;
Alex Young 4:98f063b2e6da 260
Alex Young 4:98f063b2e6da 261 xbusParser = XbusParser_create(&xbusCallback);
Alex Young 4:98f063b2e6da 262 configureSerialPorts();
Alex Young 29:d9310e7b58b5 263 if (configureMotionTracker())
Alex Young 5:abc52dd88be2 264 {
Alex Young 29:d9310e7b58b5 265 for (;;)
Alex Young 26:665d3624f9ab 266 {
Alex Young 29:d9310e7b58b5 267 while (pc.readable())
Alex Young 29:d9310e7b58b5 268 {
Alex Young 29:d9310e7b58b5 269 handlePcCommand(pc.getc());
Alex Young 29:d9310e7b58b5 270 }
Alex Young 26:665d3624f9ab 271 }
Alex Young 5:abc52dd88be2 272 }
Alex Young 29:d9310e7b58b5 273 else
Alex Young 29:d9310e7b58b5 274 {
Alex Young 29:d9310e7b58b5 275 pc.printf("Failed to configure motion tracker.\n");
Alex Young 29:d9310e7b58b5 276 return -1;
Alex Young 29:d9310e7b58b5 277 }
Alex Young 4:98f063b2e6da 278 }