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@39:9014c5236864, 2015-05-21 (annotated)

Committer:
Alex Young
Date:
Thu May 21 13:10:39 2015 +0200
Revision:
39:9014c5236864
Parent:
38:d8d410d1662c
Child:
40:b77a8c10c76d
Remove 'd' and 'o' PC commands.



These were only used for testing, and are now replaced by the firmware

startup procedure.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Alex Young 36:21198d933917 1 /*!
Alex Young 36:21198d933917 2 * \file
Alex Young 36:21198d933917 3 * \copyright
Alex Young 36:21198d933917 4 * Copyright (C) Xsens Technologies B.V., 2015. All rights reserved.
Alex Young 36:21198d933917 5 *
Alex Young 36:21198d933917 6 * This source code is intended for use only by Xsens Technologies BV and
Alex Young 36:21198d933917 7 * those that have explicit written permission to use it from
Alex Young 36:21198d933917 8 * Xsens Technologies BV.
Alex Young 36:21198d933917 9 *
Alex Young 36:21198d933917 10 * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
Alex Young 36:21198d933917 11 * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
Alex Young 36:21198d933917 12 * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
Alex Young 36:21198d933917 13 * PARTICULAR PURPOSE.
Alex Young 36:21198d933917 14 */
Alex Young 36:21198d933917 15
Alex Young 4:98f063b2e6da 16 #include "mbed.h"
Alex Young 25:01356fb59467 17 #include "rtos.h"
Alex Young 4:98f063b2e6da 18 #include "xbusparser.h"
Alex Young 11:8593ba137917 19 #include "xbusmessage.h"
Alex Young 4:98f063b2e6da 20
Alex Young 25:01356fb59467 21 #define MEMORY_POOL_SIZE (4)
Alex Young 26:665d3624f9ab 22 #define RESPONSE_QUEUE_SIZE (1)
Alex Young 25:01356fb59467 23 #define MAX_XBUS_DATA_SIZE (128)
Alex Young 25:01356fb59467 24
Alex Young 4:98f063b2e6da 25 static Serial pc(PA_2, PA_3);
Alex Young 4:98f063b2e6da 26 static Serial mt(PB_9, PB_8);
Alex Young 35:7e519b88c610 27 /*!
Alex Young 35:7e519b88c610 28 * \brief MT reset line.
Alex Young 35:7e519b88c610 29 *
Alex Young 35:7e519b88c610 30 * MT is held in reset on startup.
Alex Young 35:7e519b88c610 31 */
Alex Young 35:7e519b88c610 32 static DigitalOut mtReset(PA_10, 0);
Alex Young 4:98f063b2e6da 33 static XbusParser* xbusParser;
Alex Young 25:01356fb59467 34
Alex Young 25:01356fb59467 35 MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool;
Alex Young 25:01356fb59467 36 MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool;
Alex Young 26:665d3624f9ab 37 Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue;
Alex Young 4:98f063b2e6da 38
Alex Young 25:01356fb59467 39 static void* allocateMessageData(size_t bufSize)
Alex Young 4:98f063b2e6da 40 {
Alex Young 25:01356fb59467 41 return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL;
Alex Young 25:01356fb59467 42 }
Alex Young 25:01356fb59467 43
Alex Young 25:01356fb59467 44 static void deallocateMessageData(void const* buffer)
Alex Young 25:01356fb59467 45 {
Alex Young 25:01356fb59467 46 g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer);
Alex Young 4:98f063b2e6da 47 }
Alex Young 4:98f063b2e6da 48
Alex Young 4:98f063b2e6da 49 static void mtLowLevelHandler(void)
Alex Young 4:98f063b2e6da 50 {
Alex Young 4:98f063b2e6da 51 while (mt.readable())
Alex Young 4:98f063b2e6da 52 {
Alex Young 4:98f063b2e6da 53 XbusParser_parseByte(xbusParser, mt.getc());
Alex Young 4:98f063b2e6da 54 }
Alex Young 4:98f063b2e6da 55 }
Alex Young 4:98f063b2e6da 56
Alex Young 34:3d7a6519a256 57 static void sendMessage(XbusMessage const* m)
Alex Young 11:8593ba137917 58 {
Alex Young 26:665d3624f9ab 59 uint8_t buf[64];
Alex Young 26:665d3624f9ab 60 size_t rawLength = XbusMessage_format(buf, m);
Alex Young 11:8593ba137917 61 for (size_t i = 0; i < rawLength; ++i)
Alex Young 11:8593ba137917 62 {
Alex Young 11:8593ba137917 63 mt.putc(buf[i]);
Alex Young 11:8593ba137917 64 }
Alex Young 34:3d7a6519a256 65 }
Alex Young 34:3d7a6519a256 66
Alex Young 34:3d7a6519a256 67 static XbusMessage const* doTransaction(XbusMessage const* m)
Alex Young 34:3d7a6519a256 68 {
Alex Young 34:3d7a6519a256 69 sendMessage(m);
Alex Young 26:665d3624f9ab 70
Alex Young 26:665d3624f9ab 71 osEvent ev = g_responseQueue.get(500);
Alex Young 26:665d3624f9ab 72 return ev.status == osEventMessage ? (XbusMessage*)ev.value.p : NULL;
Alex Young 26:665d3624f9ab 73 }
Alex Young 26:665d3624f9ab 74
Alex Young 31:ce1ea9ae861e 75 /*!
Alex Young 31:ce1ea9ae861e 76 * \brief RAII object to manage message memory deallocation.
Alex Young 31:ce1ea9ae861e 77 *
Alex Young 31:ce1ea9ae861e 78 * Will automatically free the memory used by a XbusMessage when going out
Alex Young 31:ce1ea9ae861e 79 * of scope.
Alex Young 31:ce1ea9ae861e 80 */
Alex Young 31:ce1ea9ae861e 81 class XbusMessageMemoryManager
Alex Young 26:665d3624f9ab 82 {
Alex Young 31:ce1ea9ae861e 83 public:
Alex Young 31:ce1ea9ae861e 84 XbusMessageMemoryManager(XbusMessage const* message)
Alex Young 31:ce1ea9ae861e 85 : m_message(message)
Alex Young 31:ce1ea9ae861e 86 {
Alex Young 31:ce1ea9ae861e 87 }
Alex Young 31:ce1ea9ae861e 88
Alex Young 31:ce1ea9ae861e 89 ~XbusMessageMemoryManager()
Alex Young 31:ce1ea9ae861e 90 {
Alex Young 31:ce1ea9ae861e 91 if (m_message)
Alex Young 31:ce1ea9ae861e 92 {
Alex Young 31:ce1ea9ae861e 93 if (m_message->data)
Alex Young 31:ce1ea9ae861e 94 deallocateMessageData(m_message->data);
Alex Young 31:ce1ea9ae861e 95 g_messagePool.free(const_cast<XbusMessage*>(m_message));
Alex Young 31:ce1ea9ae861e 96 }
Alex Young 31:ce1ea9ae861e 97 }
Alex Young 31:ce1ea9ae861e 98
Alex Young 31:ce1ea9ae861e 99 private:
Alex Young 31:ce1ea9ae861e 100 XbusMessage const* m_message;
Alex Young 31:ce1ea9ae861e 101 };
Alex Young 26:665d3624f9ab 102
Alex Young 29:d9310e7b58b5 103 static void dumpResponse(XbusMessage const* response)
Alex Young 29:d9310e7b58b5 104 {
Alex Young 29:d9310e7b58b5 105 switch (response->mid)
Alex Young 29:d9310e7b58b5 106 {
Alex Young 29:d9310e7b58b5 107 case XMID_GotoConfigAck:
Alex Young 39:9014c5236864 108 pc.printf("Device went to config mode.\n");
Alex Young 29:d9310e7b58b5 109 break;
Alex Young 29:d9310e7b58b5 110
Alex Young 29:d9310e7b58b5 111 case XMID_Error:
Alex Young 29:d9310e7b58b5 112 pc.printf("Device error!");
Alex Young 29:d9310e7b58b5 113 break;
Alex Young 29:d9310e7b58b5 114
Alex Young 29:d9310e7b58b5 115 default:
Alex Young 29:d9310e7b58b5 116 pc.printf("Received response MID=%X, length=%d\n", response->mid, response->length);
Alex Young 29:d9310e7b58b5 117 break;
Alex Young 29:d9310e7b58b5 118 }
Alex Young 29:d9310e7b58b5 119 }
Alex Young 29:d9310e7b58b5 120
Alex Young 26:665d3624f9ab 121 static void sendCommand(XsMessageId cmdId)
Alex Young 26:665d3624f9ab 122 {
Alex Young 26:665d3624f9ab 123 XbusMessage m = {cmdId};
Alex Young 26:665d3624f9ab 124 XbusMessage const* response = doTransaction(&m);
Alex Young 31:ce1ea9ae861e 125 XbusMessageMemoryManager janitor(response);
Alex Young 26:665d3624f9ab 126
Alex Young 26:665d3624f9ab 127 if (response)
Alex Young 26:665d3624f9ab 128 {
Alex Young 29:d9310e7b58b5 129 dumpResponse(response);
Alex Young 26:665d3624f9ab 130 }
Alex Young 26:665d3624f9ab 131 else
Alex Young 26:665d3624f9ab 132 {
Alex Young 26:665d3624f9ab 133 pc.printf("Timeout waiting for response.\n");
Alex Young 26:665d3624f9ab 134 }
Alex Young 11:8593ba137917 135 }
Alex Young 11:8593ba137917 136
Alex Young 11:8593ba137917 137 static void handlePcCommand(char cmd)
Alex Young 11:8593ba137917 138 {
Alex Young 11:8593ba137917 139 switch (cmd)
Alex Young 11:8593ba137917 140 {
Alex Young 11:8593ba137917 141 case 'c':
Alex Young 11:8593ba137917 142 sendCommand(XMID_GotoConfig);
Alex Young 11:8593ba137917 143 break;
Alex Young 11:8593ba137917 144
Alex Young 11:8593ba137917 145 case 'm':
Alex Young 11:8593ba137917 146 sendCommand(XMID_GotoMeasurement);
Alex Young 11:8593ba137917 147 break;
Alex Young 11:8593ba137917 148 }
Alex Young 11:8593ba137917 149 }
Alex Young 11:8593ba137917 150
Alex Young 24:2cc49dc854e3 151 static void handleDataMessage(struct XbusMessage const* message)
Alex Young 24:2cc49dc854e3 152 {
Alex Young 24:2cc49dc854e3 153 pc.printf("MTData2:");
Alex Young 24:2cc49dc854e3 154 uint16_t counter;
Alex Young 24:2cc49dc854e3 155 if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
Alex Young 24:2cc49dc854e3 156 {
Alex Young 24:2cc49dc854e3 157 pc.printf(" Packet counter: %5d", counter);
Alex Young 24:2cc49dc854e3 158 }
Alex Young 24:2cc49dc854e3 159 float ori[4];
Alex Young 24:2cc49dc854e3 160 if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
Alex Young 24:2cc49dc854e3 161 {
Alex Young 24:2cc49dc854e3 162 pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1],
Alex Young 24:2cc49dc854e3 163 ori[2], ori[3]);
Alex Young 24:2cc49dc854e3 164 }
Alex Young 24:2cc49dc854e3 165 uint32_t status;
Alex Young 24:2cc49dc854e3 166 if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
Alex Young 24:2cc49dc854e3 167 {
Alex Young 24:2cc49dc854e3 168 pc.printf(" Status:%X", status);
Alex Young 24:2cc49dc854e3 169 }
Alex Young 24:2cc49dc854e3 170 pc.printf("\n");
Alex Young 26:665d3624f9ab 171 deallocateMessageData(message->data);
Alex Young 24:2cc49dc854e3 172 }
Alex Young 24:2cc49dc854e3 173
Alex Young 24:2cc49dc854e3 174 static void mtMessageHandler(struct XbusMessage const* message)
Alex Young 4:98f063b2e6da 175 {
Alex Young 15:558d279addd9 176 if (message->mid == XMID_MtData2)
Alex Young 7:c913a7cd5231 177 {
Alex Young 24:2cc49dc854e3 178 handleDataMessage(message);
Alex Young 7:c913a7cd5231 179 }
Alex Young 7:c913a7cd5231 180 else
Alex Young 7:c913a7cd5231 181 {
Alex Young 26:665d3624f9ab 182 XbusMessage* m = g_messagePool.alloc();
Alex Young 26:665d3624f9ab 183 memcpy(m, message, sizeof(XbusMessage));
Alex Young 26:665d3624f9ab 184 g_responseQueue.put(m);
Alex Young 25:01356fb59467 185 }
Alex Young 4:98f063b2e6da 186 }
Alex Young 4:98f063b2e6da 187
Alex Young 4:98f063b2e6da 188 static void configureSerialPorts(void)
Alex Young 4:98f063b2e6da 189 {
Alex Young 4:98f063b2e6da 190 pc.baud(921600);
Alex Young 4:98f063b2e6da 191 pc.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 192
Alex Young 37:3e87bf647c68 193 mt.baud(115200);
Alex Young 4:98f063b2e6da 194 mt.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 195 mt.attach(mtLowLevelHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 196 }
Alex Young 4:98f063b2e6da 197
Alex Young 29:d9310e7b58b5 198 static uint32_t readDeviceId(void)
Alex Young 29:d9310e7b58b5 199 {
Alex Young 29:d9310e7b58b5 200 XbusMessage reqDid = {XMID_ReqDid};
Alex Young 29:d9310e7b58b5 201 XbusMessage const* didRsp = doTransaction(&reqDid);
Alex Young 31:ce1ea9ae861e 202 XbusMessageMemoryManager janitor(didRsp);
Alex Young 29:d9310e7b58b5 203 uint32_t deviceId = 0;
Alex Young 29:d9310e7b58b5 204 if (didRsp)
Alex Young 29:d9310e7b58b5 205 {
Alex Young 29:d9310e7b58b5 206 if (didRsp->mid == XMID_DeviceId)
Alex Young 29:d9310e7b58b5 207 {
Alex Young 29:d9310e7b58b5 208 deviceId = *(uint32_t*)didRsp->data;
Alex Young 29:d9310e7b58b5 209 }
Alex Young 29:d9310e7b58b5 210 }
Alex Young 29:d9310e7b58b5 211 return deviceId;
Alex Young 29:d9310e7b58b5 212 }
Alex Young 29:d9310e7b58b5 213
Alex Young 32:fafe0f42d82b 214 static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements)
Alex Young 29:d9310e7b58b5 215 {
Alex Young 32:fafe0f42d82b 216 XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf};
Alex Young 32:fafe0f42d82b 217 XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg);
Alex Young 32:fafe0f42d82b 218 XbusMessageMemoryManager janitor(outputConfRsp);
Alex Young 32:fafe0f42d82b 219 if (outputConfRsp)
Alex Young 29:d9310e7b58b5 220 {
Alex Young 32:fafe0f42d82b 221 if (outputConfRsp->mid == XMID_OutputConfig)
Alex Young 29:d9310e7b58b5 222 {
Alex Young 32:fafe0f42d82b 223 pc.printf("Output configuration set to:\n");
Alex Young 32:fafe0f42d82b 224 OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data;
Alex Young 32:fafe0f42d82b 225 for (int i = 0; i < outputConfRsp->length; ++i)
Alex Young 32:fafe0f42d82b 226 {
Alex Young 32:fafe0f42d82b 227 pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 32:fafe0f42d82b 228 ++conf;
Alex Young 32:fafe0f42d82b 229 }
Alex Young 32:fafe0f42d82b 230 return true;
Alex Young 29:d9310e7b58b5 231 }
Alex Young 29:d9310e7b58b5 232 else
Alex Young 29:d9310e7b58b5 233 {
Alex Young 32:fafe0f42d82b 234 dumpResponse(outputConfRsp);
Alex Young 29:d9310e7b58b5 235 }
Alex Young 32:fafe0f42d82b 236 }
Alex Young 32:fafe0f42d82b 237 else
Alex Young 32:fafe0f42d82b 238 {
Alex Young 32:fafe0f42d82b 239 pc.printf("Failed to set output configuration.\n");
Alex Young 32:fafe0f42d82b 240 }
Alex Young 32:fafe0f42d82b 241 return false;
Alex Young 32:fafe0f42d82b 242 }
Alex Young 29:d9310e7b58b5 243
Alex Young 32:fafe0f42d82b 244 static bool configureMotionTracker(void)
Alex Young 32:fafe0f42d82b 245 {
Alex Young 32:fafe0f42d82b 246 uint32_t deviceId = readDeviceId();
Alex Young 32:fafe0f42d82b 247
Alex Young 32:fafe0f42d82b 248 if (deviceId)
Alex Young 32:fafe0f42d82b 249 {
Alex Young 32:fafe0f42d82b 250 uint8_t deviceType = (deviceId >> 24) & 0x0F;
Alex Young 32:fafe0f42d82b 251 pc.printf("Found MTi-%d\n", deviceType);
Alex Young 32:fafe0f42d82b 252
Alex Young 32:fafe0f42d82b 253 if (deviceType == 1)
Alex Young 29:d9310e7b58b5 254 {
Alex Young 32:fafe0f42d82b 255 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 256 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 257 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 258 {XDI_Acceleration, 100},
Alex Young 32:fafe0f42d82b 259 {XDI_RateOfTurn, 100},
Alex Young 32:fafe0f42d82b 260 {XDI_MagneticField, 100}
Alex Young 32:fafe0f42d82b 261 };
Alex Young 32:fafe0f42d82b 262 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 263 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 264 }
Alex Young 29:d9310e7b58b5 265 else
Alex Young 29:d9310e7b58b5 266 {
Alex Young 32:fafe0f42d82b 267 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 268 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 269 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 270 {XDI_Quaternion, 100},
Alex Young 32:fafe0f42d82b 271 {XDI_StatusWord, 65535}
Alex Young 32:fafe0f42d82b 272 };
Alex Young 32:fafe0f42d82b 273 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 274 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 275 }
Alex Young 29:d9310e7b58b5 276 }
Alex Young 32:fafe0f42d82b 277
Alex Young 32:fafe0f42d82b 278 return false;
Alex Young 29:d9310e7b58b5 279 }
Alex Young 29:d9310e7b58b5 280
Alex Young 35:7e519b88c610 281 /*!
Alex Young 35:7e519b88c610 282 * \brief Wait for a wakeup message from the MTi.
Alex Young 37:3e87bf647c68 283 * \param timeout Time to wait to receive the wakeup message.
Alex Young 37:3e87bf647c68 284 * \return true if wakeup received within timeout, else false.
Alex Young 35:7e519b88c610 285 *
Alex Young 35:7e519b88c610 286 * The MTi sends a XMID_Wakeup message once it has completed its bootup
Alex Young 35:7e519b88c610 287 * procedure. If this is acknowledged by a XMID_WakeupAck message then the MTi
Alex Young 35:7e519b88c610 288 * will stay in configuration mode. Otherwise it will automatically enter
Alex Young 35:7e519b88c610 289 * measurement mode with the stored output configuration.
Alex Young 35:7e519b88c610 290 */
Alex Young 37:3e87bf647c68 291 bool waitForWakeup(uint32_t timeout)
Alex Young 35:7e519b88c610 292 {
Alex Young 37:3e87bf647c68 293 osEvent ev = g_responseQueue.get(timeout);
Alex Young 35:7e519b88c610 294 if (ev.status == osEventMessage)
Alex Young 35:7e519b88c610 295 {
Alex Young 35:7e519b88c610 296 XbusMessage const* m = (XbusMessage const*)ev.value.p;
Alex Young 35:7e519b88c610 297 XbusMessageMemoryManager janitor(m);
Alex Young 35:7e519b88c610 298 return m->mid == XMID_Wakeup;
Alex Young 35:7e519b88c610 299 }
Alex Young 35:7e519b88c610 300 return false;
Alex Young 35:7e519b88c610 301 }
Alex Young 35:7e519b88c610 302
Alex Young 35:7e519b88c610 303 /*!
Alex Young 37:3e87bf647c68 304 * \brief Send wakeup acknowledge message to MTi.
Alex Young 37:3e87bf647c68 305 *
Alex Young 37:3e87bf647c68 306 * Sending a wakeup acknowledge will cause the device to stay in configuration
Alex Young 37:3e87bf647c68 307 * mode instead of automatically transitioning to measurement mode with the
Alex Young 37:3e87bf647c68 308 * stored output configuration.
Alex Young 37:3e87bf647c68 309 */
Alex Young 37:3e87bf647c68 310 void sendWakeupAck(void)
Alex Young 37:3e87bf647c68 311 {
Alex Young 37:3e87bf647c68 312 XbusMessage ack = {XMID_WakeupAck};
Alex Young 37:3e87bf647c68 313 sendMessage(&ack);
Alex Young 37:3e87bf647c68 314 pc.printf("Device ready for operation.\n");
Alex Young 37:3e87bf647c68 315 }
Alex Young 37:3e87bf647c68 316
Alex Young 37:3e87bf647c68 317 /*!
Alex Young 37:3e87bf647c68 318 * \brief Restore communication with the MTi.
Alex Young 37:3e87bf647c68 319 *
Alex Young 37:3e87bf647c68 320 * On bootup the MTi will listen for a magic byte to signal that it should
Alex Young 37:3e87bf647c68 321 * return to default baudrate and output configuration. This can be used to
Alex Young 37:3e87bf647c68 322 * recover from a bad or unknown configuration.
Alex Young 37:3e87bf647c68 323 */
Alex Young 37:3e87bf647c68 324 void restoreCommunication(void)
Alex Young 37:3e87bf647c68 325 {
Alex Young 37:3e87bf647c68 326 pc.printf("Restoring communication with device... ");
Alex Young 37:3e87bf647c68 327 mtReset = 0;
Alex Young 37:3e87bf647c68 328 Thread::wait(1);
Alex Young 37:3e87bf647c68 329 mtReset = 1;
Alex Young 37:3e87bf647c68 330
Alex Young 37:3e87bf647c68 331 do
Alex Young 37:3e87bf647c68 332 {
Alex Young 37:3e87bf647c68 333 mt.putc(0xDE);
Alex Young 37:3e87bf647c68 334 }
Alex Young 37:3e87bf647c68 335 while (!waitForWakeup(1));
Alex Young 37:3e87bf647c68 336 pc.printf("done\n");
Alex Young 37:3e87bf647c68 337
Alex Young 37:3e87bf647c68 338 sendWakeupAck();
Alex Young 37:3e87bf647c68 339 }
Alex Young 37:3e87bf647c68 340
Alex Young 37:3e87bf647c68 341 /*!
Alex Young 35:7e519b88c610 342 * \brief Releases the MTi reset line and waits for a wakeup message.
Alex Young 37:3e87bf647c68 343 *
Alex Young 37:3e87bf647c68 344 * If no wakeup message is received within 1 second the restore communications
Alex Young 37:3e87bf647c68 345 * procedure is done to reset the MTi to default baudrate and output configuration.
Alex Young 35:7e519b88c610 346 */
Alex Young 35:7e519b88c610 347 static void wakeupMotionTracker(void)
Alex Young 35:7e519b88c610 348 {
Alex Young 35:7e519b88c610 349 mtReset.write(1); // Release MT from reset.
Alex Young 37:3e87bf647c68 350 if (waitForWakeup(1000))
Alex Young 35:7e519b88c610 351 {
Alex Young 37:3e87bf647c68 352 sendWakeupAck();
Alex Young 37:3e87bf647c68 353 }
Alex Young 37:3e87bf647c68 354 else
Alex Young 37:3e87bf647c68 355 {
Alex Young 37:3e87bf647c68 356 restoreCommunication();
Alex Young 35:7e519b88c610 357 }
Alex Young 35:7e519b88c610 358 }
Alex Young 35:7e519b88c610 359
Alex Young 38:d8d410d1662c 360 static void printIntroMessage(void)
Alex Young 38:d8d410d1662c 361 {
Alex Young 38:d8d410d1662c 362 pc.printf("\n\n\n\n\n");
Alex Young 38:d8d410d1662c 363 pc.printf("MTi-1 series embedded example firmware.\n");
Alex Young 38:d8d410d1662c 364 }
Alex Young 38:d8d410d1662c 365
Alex Young 38:d8d410d1662c 366 static void printUsageInstructions(void)
Alex Young 38:d8d410d1662c 367 {
Alex Young 38:d8d410d1662c 368 pc.printf("\n");
Alex Young 38:d8d410d1662c 369 pc.printf("Press 'm' to start measuring and 'c' to return to config mode.\n");
Alex Young 38:d8d410d1662c 370 }
Alex Young 38:d8d410d1662c 371
Alex Young 2:b3e402dc11ca 372 int main(void)
Alex Young 2:b3e402dc11ca 373 {
Alex Young 4:98f063b2e6da 374 XbusParserCallback xbusCallback = {};
Alex Young 25:01356fb59467 375 xbusCallback.allocateBuffer = allocateMessageData;
Alex Young 25:01356fb59467 376 xbusCallback.deallocateBuffer = deallocateMessageData;
Alex Young 24:2cc49dc854e3 377 xbusCallback.handleMessage = mtMessageHandler;
Alex Young 4:98f063b2e6da 378
Alex Young 4:98f063b2e6da 379 xbusParser = XbusParser_create(&xbusCallback);
Alex Young 4:98f063b2e6da 380 configureSerialPorts();
Alex Young 38:d8d410d1662c 381
Alex Young 38:d8d410d1662c 382 printIntroMessage();
Alex Young 35:7e519b88c610 383 wakeupMotionTracker();
Alex Young 29:d9310e7b58b5 384 if (configureMotionTracker())
Alex Young 5:abc52dd88be2 385 {
Alex Young 38:d8d410d1662c 386 printUsageInstructions();
Alex Young 29:d9310e7b58b5 387 for (;;)
Alex Young 26:665d3624f9ab 388 {
Alex Young 29:d9310e7b58b5 389 while (pc.readable())
Alex Young 29:d9310e7b58b5 390 {
Alex Young 29:d9310e7b58b5 391 handlePcCommand(pc.getc());
Alex Young 29:d9310e7b58b5 392 }
Alex Young 26:665d3624f9ab 393 }
Alex Young 5:abc52dd88be2 394 }
Alex Young 29:d9310e7b58b5 395 else
Alex Young 29:d9310e7b58b5 396 {
Alex Young 29:d9310e7b58b5 397 pc.printf("Failed to configure motion tracker.\n");
Alex Young 29:d9310e7b58b5 398 return -1;
Alex Young 29:d9310e7b58b5 399 }
Alex Young 4:98f063b2e6da 400 }