Add LPC1768

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

Committer:
Alex Young
Date:
Thu May 21 12:20:39 2015 +0200
Revision:
35:7e519b88c610
Parent:
34:3d7a6519a256
Child:
36:21198d933917
Reset MT on startup.

Use the reset line to hold the MT in reset until we are ready to start.
Then release the reset line and wait for the wakeup message.

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