HyunGyu Kim / Mbed 2 deprecated MTi-1_example

kjj

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

main.cpp@37:3e87bf647c68, 2015-05-21 (annotated)

Committer:
Alex Young
Date:
Thu May 21 12:54:35 2015 +0200
Revision:
37:3e87bf647c68
Parent:
36:21198d933917
Child:
38:d8d410d1662c
Implement support for restoring communication with the MTi

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 29:d9310e7b58b5 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_DeviceId:
Alex Young 29:d9310e7b58b5 112 pc.printf("Device ID: %08X\n", *(uint32_t*)response->data);
Alex Young 29:d9310e7b58b5 113 break;
Alex Young 29:d9310e7b58b5 114
Alex Young 29:d9310e7b58b5 115 case XMID_OutputConfig:
Alex Young 29:d9310e7b58b5 116 {
Alex Young 29:d9310e7b58b5 117 pc.printf("Output configuration\n");
Alex Young 29:d9310e7b58b5 118 OutputConfiguration* conf = (OutputConfiguration*)response->data;
Alex Young 29:d9310e7b58b5 119 for (int i = 0; i < response->length; ++i)
Alex Young 29:d9310e7b58b5 120 {
Alex Young 29:d9310e7b58b5 121 pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 29:d9310e7b58b5 122 ++conf;
Alex Young 29:d9310e7b58b5 123 }
Alex Young 29:d9310e7b58b5 124 }
Alex Young 29:d9310e7b58b5 125 break;
Alex Young 29:d9310e7b58b5 126
Alex Young 29:d9310e7b58b5 127 case XMID_Error:
Alex Young 29:d9310e7b58b5 128 pc.printf("Device error!");
Alex Young 29:d9310e7b58b5 129 break;
Alex Young 29:d9310e7b58b5 130
Alex Young 29:d9310e7b58b5 131 default:
Alex Young 29:d9310e7b58b5 132 pc.printf("Received response MID=%X, length=%d\n", response->mid, response->length);
Alex Young 29:d9310e7b58b5 133 break;
Alex Young 29:d9310e7b58b5 134 }
Alex Young 29:d9310e7b58b5 135 }
Alex Young 29:d9310e7b58b5 136
Alex Young 26:665d3624f9ab 137 static void sendCommand(XsMessageId cmdId)
Alex Young 26:665d3624f9ab 138 {
Alex Young 26:665d3624f9ab 139 XbusMessage m = {cmdId};
Alex Young 26:665d3624f9ab 140 XbusMessage const* response = doTransaction(&m);
Alex Young 31:ce1ea9ae861e 141 XbusMessageMemoryManager janitor(response);
Alex Young 26:665d3624f9ab 142
Alex Young 26:665d3624f9ab 143 if (response)
Alex Young 26:665d3624f9ab 144 {
Alex Young 29:d9310e7b58b5 145 dumpResponse(response);
Alex Young 26:665d3624f9ab 146 }
Alex Young 26:665d3624f9ab 147 else
Alex Young 26:665d3624f9ab 148 {
Alex Young 26:665d3624f9ab 149 pc.printf("Timeout waiting for response.\n");
Alex Young 26:665d3624f9ab 150 }
Alex Young 11:8593ba137917 151 }
Alex Young 11:8593ba137917 152
Alex Young 11:8593ba137917 153 static void handlePcCommand(char cmd)
Alex Young 11:8593ba137917 154 {
Alex Young 11:8593ba137917 155 switch (cmd)
Alex Young 11:8593ba137917 156 {
Alex Young 11:8593ba137917 157 case 'c':
Alex Young 11:8593ba137917 158 sendCommand(XMID_GotoConfig);
Alex Young 11:8593ba137917 159 break;
Alex Young 11:8593ba137917 160
Alex Young 11:8593ba137917 161 case 'm':
Alex Young 11:8593ba137917 162 sendCommand(XMID_GotoMeasurement);
Alex Young 11:8593ba137917 163 break;
Alex Young 20:38560fa3d2eb 164
Alex Young 20:38560fa3d2eb 165 case 'd':
Alex Young 20:38560fa3d2eb 166 sendCommand(XMID_ReqDid);
Alex Young 20:38560fa3d2eb 167 break;
Alex Young 22:3eab999c5076 168
Alex Young 22:3eab999c5076 169 case 'o':
Alex Young 22:3eab999c5076 170 sendCommand(XMID_ReqOutputConfig);
Alex Young 22:3eab999c5076 171 break;
Alex Young 11:8593ba137917 172 }
Alex Young 11:8593ba137917 173 }
Alex Young 11:8593ba137917 174
Alex Young 24:2cc49dc854e3 175 static void handleDataMessage(struct XbusMessage const* message)
Alex Young 24:2cc49dc854e3 176 {
Alex Young 24:2cc49dc854e3 177 pc.printf("MTData2:");
Alex Young 24:2cc49dc854e3 178 uint16_t counter;
Alex Young 24:2cc49dc854e3 179 if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
Alex Young 24:2cc49dc854e3 180 {
Alex Young 24:2cc49dc854e3 181 pc.printf(" Packet counter: %5d", counter);
Alex Young 24:2cc49dc854e3 182 }
Alex Young 24:2cc49dc854e3 183 float ori[4];
Alex Young 24:2cc49dc854e3 184 if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
Alex Young 24:2cc49dc854e3 185 {
Alex Young 24:2cc49dc854e3 186 pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1],
Alex Young 24:2cc49dc854e3 187 ori[2], ori[3]);
Alex Young 24:2cc49dc854e3 188 }
Alex Young 24:2cc49dc854e3 189 uint32_t status;
Alex Young 24:2cc49dc854e3 190 if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
Alex Young 24:2cc49dc854e3 191 {
Alex Young 24:2cc49dc854e3 192 pc.printf(" Status:%X", status);
Alex Young 24:2cc49dc854e3 193 }
Alex Young 24:2cc49dc854e3 194 pc.printf("\n");
Alex Young 26:665d3624f9ab 195 deallocateMessageData(message->data);
Alex Young 24:2cc49dc854e3 196 }
Alex Young 24:2cc49dc854e3 197
Alex Young 24:2cc49dc854e3 198 static void mtMessageHandler(struct XbusMessage const* message)
Alex Young 4:98f063b2e6da 199 {
Alex Young 15:558d279addd9 200 if (message->mid == XMID_MtData2)
Alex Young 7:c913a7cd5231 201 {
Alex Young 24:2cc49dc854e3 202 handleDataMessage(message);
Alex Young 7:c913a7cd5231 203 }
Alex Young 7:c913a7cd5231 204 else
Alex Young 7:c913a7cd5231 205 {
Alex Young 26:665d3624f9ab 206 XbusMessage* m = g_messagePool.alloc();
Alex Young 26:665d3624f9ab 207 memcpy(m, message, sizeof(XbusMessage));
Alex Young 26:665d3624f9ab 208 g_responseQueue.put(m);
Alex Young 25:01356fb59467 209 }
Alex Young 4:98f063b2e6da 210 }
Alex Young 4:98f063b2e6da 211
Alex Young 4:98f063b2e6da 212 static void configureSerialPorts(void)
Alex Young 4:98f063b2e6da 213 {
Alex Young 4:98f063b2e6da 214 pc.baud(921600);
Alex Young 4:98f063b2e6da 215 pc.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 216
Alex Young 37:3e87bf647c68 217 mt.baud(115200);
Alex Young 4:98f063b2e6da 218 mt.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 219 mt.attach(mtLowLevelHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 220 }
Alex Young 4:98f063b2e6da 221
Alex Young 29:d9310e7b58b5 222 static uint32_t readDeviceId(void)
Alex Young 29:d9310e7b58b5 223 {
Alex Young 29:d9310e7b58b5 224 XbusMessage reqDid = {XMID_ReqDid};
Alex Young 29:d9310e7b58b5 225 XbusMessage const* didRsp = doTransaction(&reqDid);
Alex Young 31:ce1ea9ae861e 226 XbusMessageMemoryManager janitor(didRsp);
Alex Young 29:d9310e7b58b5 227 uint32_t deviceId = 0;
Alex Young 29:d9310e7b58b5 228 if (didRsp)
Alex Young 29:d9310e7b58b5 229 {
Alex Young 29:d9310e7b58b5 230 if (didRsp->mid == XMID_DeviceId)
Alex Young 29:d9310e7b58b5 231 {
Alex Young 29:d9310e7b58b5 232 deviceId = *(uint32_t*)didRsp->data;
Alex Young 29:d9310e7b58b5 233 }
Alex Young 29:d9310e7b58b5 234 }
Alex Young 29:d9310e7b58b5 235 return deviceId;
Alex Young 29:d9310e7b58b5 236 }
Alex Young 29:d9310e7b58b5 237
Alex Young 32:fafe0f42d82b 238 static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements)
Alex Young 29:d9310e7b58b5 239 {
Alex Young 32:fafe0f42d82b 240 XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf};
Alex Young 32:fafe0f42d82b 241 XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg);
Alex Young 32:fafe0f42d82b 242 XbusMessageMemoryManager janitor(outputConfRsp);
Alex Young 32:fafe0f42d82b 243 if (outputConfRsp)
Alex Young 29:d9310e7b58b5 244 {
Alex Young 32:fafe0f42d82b 245 if (outputConfRsp->mid == XMID_OutputConfig)
Alex Young 29:d9310e7b58b5 246 {
Alex Young 32:fafe0f42d82b 247 pc.printf("Output configuration set to:\n");
Alex Young 32:fafe0f42d82b 248 OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data;
Alex Young 32:fafe0f42d82b 249 for (int i = 0; i < outputConfRsp->length; ++i)
Alex Young 32:fafe0f42d82b 250 {
Alex Young 32:fafe0f42d82b 251 pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 32:fafe0f42d82b 252 ++conf;
Alex Young 32:fafe0f42d82b 253 }
Alex Young 32:fafe0f42d82b 254 return true;
Alex Young 29:d9310e7b58b5 255 }
Alex Young 29:d9310e7b58b5 256 else
Alex Young 29:d9310e7b58b5 257 {
Alex Young 32:fafe0f42d82b 258 dumpResponse(outputConfRsp);
Alex Young 29:d9310e7b58b5 259 }
Alex Young 32:fafe0f42d82b 260 }
Alex Young 32:fafe0f42d82b 261 else
Alex Young 32:fafe0f42d82b 262 {
Alex Young 32:fafe0f42d82b 263 pc.printf("Failed to set output configuration.\n");
Alex Young 32:fafe0f42d82b 264 }
Alex Young 32:fafe0f42d82b 265 return false;
Alex Young 32:fafe0f42d82b 266 }
Alex Young 29:d9310e7b58b5 267
Alex Young 32:fafe0f42d82b 268 static bool configureMotionTracker(void)
Alex Young 32:fafe0f42d82b 269 {
Alex Young 32:fafe0f42d82b 270 uint32_t deviceId = readDeviceId();
Alex Young 32:fafe0f42d82b 271
Alex Young 32:fafe0f42d82b 272 if (deviceId)
Alex Young 32:fafe0f42d82b 273 {
Alex Young 32:fafe0f42d82b 274 uint8_t deviceType = (deviceId >> 24) & 0x0F;
Alex Young 32:fafe0f42d82b 275 pc.printf("Found MTi-%d\n", deviceType);
Alex Young 32:fafe0f42d82b 276
Alex Young 32:fafe0f42d82b 277 if (deviceType == 1)
Alex Young 29:d9310e7b58b5 278 {
Alex Young 32:fafe0f42d82b 279 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 280 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 281 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 282 {XDI_Acceleration, 100},
Alex Young 32:fafe0f42d82b 283 {XDI_RateOfTurn, 100},
Alex Young 32:fafe0f42d82b 284 {XDI_MagneticField, 100}
Alex Young 32:fafe0f42d82b 285 };
Alex Young 32:fafe0f42d82b 286 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 287 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 288 }
Alex Young 29:d9310e7b58b5 289 else
Alex Young 29:d9310e7b58b5 290 {
Alex Young 32:fafe0f42d82b 291 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 292 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 293 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 294 {XDI_Quaternion, 100},
Alex Young 32:fafe0f42d82b 295 {XDI_StatusWord, 65535}
Alex Young 32:fafe0f42d82b 296 };
Alex Young 32:fafe0f42d82b 297 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 298 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 299 }
Alex Young 29:d9310e7b58b5 300 }
Alex Young 32:fafe0f42d82b 301
Alex Young 32:fafe0f42d82b 302 return false;
Alex Young 29:d9310e7b58b5 303 }
Alex Young 29:d9310e7b58b5 304
Alex Young 35:7e519b88c610 305 /*!
Alex Young 35:7e519b88c610 306 * \brief Wait for a wakeup message from the MTi.
Alex Young 37:3e87bf647c68 307 * \param timeout Time to wait to receive the wakeup message.
Alex Young 37:3e87bf647c68 308 * \return true if wakeup received within timeout, else false.
Alex Young 35:7e519b88c610 309 *
Alex Young 35:7e519b88c610 310 * The MTi sends a XMID_Wakeup message once it has completed its bootup
Alex Young 35:7e519b88c610 311 * procedure. If this is acknowledged by a XMID_WakeupAck message then the MTi
Alex Young 35:7e519b88c610 312 * will stay in configuration mode. Otherwise it will automatically enter
Alex Young 35:7e519b88c610 313 * measurement mode with the stored output configuration.
Alex Young 35:7e519b88c610 314 */
Alex Young 37:3e87bf647c68 315 bool waitForWakeup(uint32_t timeout)
Alex Young 35:7e519b88c610 316 {
Alex Young 37:3e87bf647c68 317 osEvent ev = g_responseQueue.get(timeout);
Alex Young 35:7e519b88c610 318 if (ev.status == osEventMessage)
Alex Young 35:7e519b88c610 319 {
Alex Young 35:7e519b88c610 320 XbusMessage const* m = (XbusMessage const*)ev.value.p;
Alex Young 35:7e519b88c610 321 XbusMessageMemoryManager janitor(m);
Alex Young 35:7e519b88c610 322 return m->mid == XMID_Wakeup;
Alex Young 35:7e519b88c610 323 }
Alex Young 35:7e519b88c610 324 return false;
Alex Young 35:7e519b88c610 325 }
Alex Young 35:7e519b88c610 326
Alex Young 35:7e519b88c610 327 /*!
Alex Young 37:3e87bf647c68 328 * \brief Send wakeup acknowledge message to MTi.
Alex Young 37:3e87bf647c68 329 *
Alex Young 37:3e87bf647c68 330 * Sending a wakeup acknowledge will cause the device to stay in configuration
Alex Young 37:3e87bf647c68 331 * mode instead of automatically transitioning to measurement mode with the
Alex Young 37:3e87bf647c68 332 * stored output configuration.
Alex Young 37:3e87bf647c68 333 */
Alex Young 37:3e87bf647c68 334 void sendWakeupAck(void)
Alex Young 37:3e87bf647c68 335 {
Alex Young 37:3e87bf647c68 336 XbusMessage ack = {XMID_WakeupAck};
Alex Young 37:3e87bf647c68 337 sendMessage(&ack);
Alex Young 37:3e87bf647c68 338 pc.printf("Device ready for operation.\n");
Alex Young 37:3e87bf647c68 339 }
Alex Young 37:3e87bf647c68 340
Alex Young 37:3e87bf647c68 341 /*!
Alex Young 37:3e87bf647c68 342 * \brief Restore communication with the MTi.
Alex Young 37:3e87bf647c68 343 *
Alex Young 37:3e87bf647c68 344 * On bootup the MTi will listen for a magic byte to signal that it should
Alex Young 37:3e87bf647c68 345 * return to default baudrate and output configuration. This can be used to
Alex Young 37:3e87bf647c68 346 * recover from a bad or unknown configuration.
Alex Young 37:3e87bf647c68 347 */
Alex Young 37:3e87bf647c68 348 void restoreCommunication(void)
Alex Young 37:3e87bf647c68 349 {
Alex Young 37:3e87bf647c68 350 pc.printf("Restoring communication with device... ");
Alex Young 37:3e87bf647c68 351 mtReset = 0;
Alex Young 37:3e87bf647c68 352 Thread::wait(1);
Alex Young 37:3e87bf647c68 353 mtReset = 1;
Alex Young 37:3e87bf647c68 354
Alex Young 37:3e87bf647c68 355 do
Alex Young 37:3e87bf647c68 356 {
Alex Young 37:3e87bf647c68 357 mt.putc(0xDE);
Alex Young 37:3e87bf647c68 358 }
Alex Young 37:3e87bf647c68 359 while (!waitForWakeup(1));
Alex Young 37:3e87bf647c68 360 pc.printf("done\n");
Alex Young 37:3e87bf647c68 361
Alex Young 37:3e87bf647c68 362 sendWakeupAck();
Alex Young 37:3e87bf647c68 363 }
Alex Young 37:3e87bf647c68 364
Alex Young 37:3e87bf647c68 365 /*!
Alex Young 35:7e519b88c610 366 * \brief Releases the MTi reset line and waits for a wakeup message.
Alex Young 37:3e87bf647c68 367 *
Alex Young 37:3e87bf647c68 368 * If no wakeup message is received within 1 second the restore communications
Alex Young 37:3e87bf647c68 369 * procedure is done to reset the MTi to default baudrate and output configuration.
Alex Young 35:7e519b88c610 370 */
Alex Young 35:7e519b88c610 371 static void wakeupMotionTracker(void)
Alex Young 35:7e519b88c610 372 {
Alex Young 35:7e519b88c610 373 mtReset.write(1); // Release MT from reset.
Alex Young 37:3e87bf647c68 374 if (waitForWakeup(1000))
Alex Young 35:7e519b88c610 375 {
Alex Young 37:3e87bf647c68 376 sendWakeupAck();
Alex Young 37:3e87bf647c68 377 }
Alex Young 37:3e87bf647c68 378 else
Alex Young 37:3e87bf647c68 379 {
Alex Young 37:3e87bf647c68 380 restoreCommunication();
Alex Young 35:7e519b88c610 381 }
Alex Young 35:7e519b88c610 382 }
Alex Young 35:7e519b88c610 383
Alex Young 2:b3e402dc11ca 384 int main(void)
Alex Young 2:b3e402dc11ca 385 {
Alex Young 4:98f063b2e6da 386 XbusParserCallback xbusCallback = {};
Alex Young 25:01356fb59467 387 xbusCallback.allocateBuffer = allocateMessageData;
Alex Young 25:01356fb59467 388 xbusCallback.deallocateBuffer = deallocateMessageData;
Alex Young 24:2cc49dc854e3 389 xbusCallback.handleMessage = mtMessageHandler;
Alex Young 4:98f063b2e6da 390
Alex Young 4:98f063b2e6da 391 xbusParser = XbusParser_create(&xbusCallback);
Alex Young 4:98f063b2e6da 392 configureSerialPorts();
Alex Young 35:7e519b88c610 393 wakeupMotionTracker();
Alex Young 29:d9310e7b58b5 394 if (configureMotionTracker())
Alex Young 5:abc52dd88be2 395 {
Alex Young 29:d9310e7b58b5 396 for (;;)
Alex Young 26:665d3624f9ab 397 {
Alex Young 29:d9310e7b58b5 398 while (pc.readable())
Alex Young 29:d9310e7b58b5 399 {
Alex Young 29:d9310e7b58b5 400 handlePcCommand(pc.getc());
Alex Young 29:d9310e7b58b5 401 }
Alex Young 26:665d3624f9ab 402 }
Alex Young 5:abc52dd88be2 403 }
Alex Young 29:d9310e7b58b5 404 else
Alex Young 29:d9310e7b58b5 405 {
Alex Young 29:d9310e7b58b5 406 pc.printf("Failed to configure motion tracker.\n");
Alex Young 29:d9310e7b58b5 407 return -1;
Alex Young 29:d9310e7b58b5 408 }
Alex Young 4:98f063b2e6da 409 }