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@34:3d7a6519a256, 2015-05-21 (annotated)

Committer:
Alex Young
Date:
Thu May 21 12:19:50 2015 +0200
Revision:
34:3d7a6519a256
Parent:
32:fafe0f42d82b
Child:
35:7e519b88c610
Split out sending message from a full transaction.



The wakeup ack message will not have a response from the MT.

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