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@42:e70756d2d8bc, 2015-05-21 (annotated)

Committer:
Alex Young
Date:
Thu May 21 15:21:02 2015 +0200
Revision:
42:e70756d2d8bc
Parent:
41:504b6821a96f
Child:
43:470c019246e4
Add support for printing acc/gyr/mag data

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 40:b77a8c10c76d 20 #include "xsdeviceid.h"
Alex Young 4:98f063b2e6da 21
Alex Young 25:01356fb59467 22 #define MEMORY_POOL_SIZE (4)
Alex Young 26:665d3624f9ab 23 #define RESPONSE_QUEUE_SIZE (1)
Alex Young 25:01356fb59467 24 #define MAX_XBUS_DATA_SIZE (128)
Alex Young 25:01356fb59467 25
Alex Young 4:98f063b2e6da 26 static Serial pc(PA_2, PA_3);
Alex Young 4:98f063b2e6da 27 static Serial mt(PB_9, PB_8);
Alex Young 35:7e519b88c610 28 /*!
Alex Young 35:7e519b88c610 29 * \brief MT reset line.
Alex Young 35:7e519b88c610 30 *
Alex Young 35:7e519b88c610 31 * MT is held in reset on startup.
Alex Young 35:7e519b88c610 32 */
Alex Young 35:7e519b88c610 33 static DigitalOut mtReset(PA_10, 0);
Alex Young 4:98f063b2e6da 34 static XbusParser* xbusParser;
Alex Young 25:01356fb59467 35
Alex Young 25:01356fb59467 36 MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool;
Alex Young 25:01356fb59467 37 MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool;
Alex Young 26:665d3624f9ab 38 Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue;
Alex Young 4:98f063b2e6da 39
Alex Young 25:01356fb59467 40 static void* allocateMessageData(size_t bufSize)
Alex Young 4:98f063b2e6da 41 {
Alex Young 25:01356fb59467 42 return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL;
Alex Young 25:01356fb59467 43 }
Alex Young 25:01356fb59467 44
Alex Young 25:01356fb59467 45 static void deallocateMessageData(void const* buffer)
Alex Young 25:01356fb59467 46 {
Alex Young 25:01356fb59467 47 g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer);
Alex Young 4:98f063b2e6da 48 }
Alex Young 4:98f063b2e6da 49
Alex Young 4:98f063b2e6da 50 static void mtLowLevelHandler(void)
Alex Young 4:98f063b2e6da 51 {
Alex Young 4:98f063b2e6da 52 while (mt.readable())
Alex Young 4:98f063b2e6da 53 {
Alex Young 4:98f063b2e6da 54 XbusParser_parseByte(xbusParser, mt.getc());
Alex Young 4:98f063b2e6da 55 }
Alex Young 4:98f063b2e6da 56 }
Alex Young 4:98f063b2e6da 57
Alex Young 34:3d7a6519a256 58 static void sendMessage(XbusMessage const* m)
Alex Young 11:8593ba137917 59 {
Alex Young 26:665d3624f9ab 60 uint8_t buf[64];
Alex Young 26:665d3624f9ab 61 size_t rawLength = XbusMessage_format(buf, m);
Alex Young 11:8593ba137917 62 for (size_t i = 0; i < rawLength; ++i)
Alex Young 11:8593ba137917 63 {
Alex Young 11:8593ba137917 64 mt.putc(buf[i]);
Alex Young 11:8593ba137917 65 }
Alex Young 34:3d7a6519a256 66 }
Alex Young 34:3d7a6519a256 67
Alex Young 34:3d7a6519a256 68 static XbusMessage const* doTransaction(XbusMessage const* m)
Alex Young 34:3d7a6519a256 69 {
Alex Young 34:3d7a6519a256 70 sendMessage(m);
Alex Young 26:665d3624f9ab 71
Alex Young 26:665d3624f9ab 72 osEvent ev = g_responseQueue.get(500);
Alex Young 26:665d3624f9ab 73 return ev.status == osEventMessage ? (XbusMessage*)ev.value.p : NULL;
Alex Young 26:665d3624f9ab 74 }
Alex Young 26:665d3624f9ab 75
Alex Young 31:ce1ea9ae861e 76 /*!
Alex Young 31:ce1ea9ae861e 77 * \brief RAII object to manage message memory deallocation.
Alex Young 31:ce1ea9ae861e 78 *
Alex Young 31:ce1ea9ae861e 79 * Will automatically free the memory used by a XbusMessage when going out
Alex Young 31:ce1ea9ae861e 80 * of scope.
Alex Young 31:ce1ea9ae861e 81 */
Alex Young 31:ce1ea9ae861e 82 class XbusMessageMemoryManager
Alex Young 26:665d3624f9ab 83 {
Alex Young 31:ce1ea9ae861e 84 public:
Alex Young 31:ce1ea9ae861e 85 XbusMessageMemoryManager(XbusMessage const* message)
Alex Young 31:ce1ea9ae861e 86 : m_message(message)
Alex Young 31:ce1ea9ae861e 87 {
Alex Young 31:ce1ea9ae861e 88 }
Alex Young 31:ce1ea9ae861e 89
Alex Young 31:ce1ea9ae861e 90 ~XbusMessageMemoryManager()
Alex Young 31:ce1ea9ae861e 91 {
Alex Young 31:ce1ea9ae861e 92 if (m_message)
Alex Young 31:ce1ea9ae861e 93 {
Alex Young 31:ce1ea9ae861e 94 if (m_message->data)
Alex Young 31:ce1ea9ae861e 95 deallocateMessageData(m_message->data);
Alex Young 31:ce1ea9ae861e 96 g_messagePool.free(const_cast<XbusMessage*>(m_message));
Alex Young 31:ce1ea9ae861e 97 }
Alex Young 31:ce1ea9ae861e 98 }
Alex Young 31:ce1ea9ae861e 99
Alex Young 31:ce1ea9ae861e 100 private:
Alex Young 31:ce1ea9ae861e 101 XbusMessage const* m_message;
Alex Young 31:ce1ea9ae861e 102 };
Alex Young 26:665d3624f9ab 103
Alex Young 29:d9310e7b58b5 104 static void dumpResponse(XbusMessage const* response)
Alex Young 29:d9310e7b58b5 105 {
Alex Young 29:d9310e7b58b5 106 switch (response->mid)
Alex Young 29:d9310e7b58b5 107 {
Alex Young 29:d9310e7b58b5 108 case XMID_GotoConfigAck:
Alex Young 39:9014c5236864 109 pc.printf("Device went to config mode.\n");
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 11:8593ba137917 149 }
Alex Young 11:8593ba137917 150 }
Alex Young 11:8593ba137917 151
Alex Young 24:2cc49dc854e3 152 static void handleDataMessage(struct XbusMessage const* message)
Alex Young 24:2cc49dc854e3 153 {
Alex Young 24:2cc49dc854e3 154 pc.printf("MTData2:");
Alex Young 24:2cc49dc854e3 155 uint16_t counter;
Alex Young 24:2cc49dc854e3 156 if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
Alex Young 24:2cc49dc854e3 157 {
Alex Young 24:2cc49dc854e3 158 pc.printf(" Packet counter: %5d", counter);
Alex Young 24:2cc49dc854e3 159 }
Alex Young 24:2cc49dc854e3 160 float ori[4];
Alex Young 24:2cc49dc854e3 161 if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
Alex Young 24:2cc49dc854e3 162 {
Alex Young 24:2cc49dc854e3 163 pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1],
Alex Young 24:2cc49dc854e3 164 ori[2], ori[3]);
Alex Young 24:2cc49dc854e3 165 }
Alex Young 42:e70756d2d8bc 166 float acc[3];
Alex Young 42:e70756d2d8bc 167 if (XbusMessage_getDataItem(acc, XDI_Acceleration, message))
Alex Young 42:e70756d2d8bc 168 {
Alex Young 42:e70756d2d8bc 169 pc.printf(" Acceleration: (% .3f, % .3f, % .3f)", acc[0], acc[1], acc[2]);
Alex Young 42:e70756d2d8bc 170 }
Alex Young 42:e70756d2d8bc 171 float gyr[3];
Alex Young 42:e70756d2d8bc 172 if (XbusMessage_getDataItem(gyr, XDI_RateOfTurn, message))
Alex Young 42:e70756d2d8bc 173 {
Alex Young 42:e70756d2d8bc 174 pc.printf(" Rate Of Turn: (% .3f, % .3f, % .3f)", gyr[0], gyr[1], gyr[2]);
Alex Young 42:e70756d2d8bc 175 }
Alex Young 42:e70756d2d8bc 176 float mag[3];
Alex Young 42:e70756d2d8bc 177 if (XbusMessage_getDataItem(mag, XDI_MagneticField, message))
Alex Young 42:e70756d2d8bc 178 {
Alex Young 42:e70756d2d8bc 179 pc.printf(" Magnetic Field: (% .3f, % .3f, % .3f)", mag[0], mag[1], mag[2]);
Alex Young 42:e70756d2d8bc 180 }
Alex Young 24:2cc49dc854e3 181 uint32_t status;
Alex Young 24:2cc49dc854e3 182 if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
Alex Young 24:2cc49dc854e3 183 {
Alex Young 24:2cc49dc854e3 184 pc.printf(" Status:%X", status);
Alex Young 24:2cc49dc854e3 185 }
Alex Young 24:2cc49dc854e3 186 pc.printf("\n");
Alex Young 26:665d3624f9ab 187 deallocateMessageData(message->data);
Alex Young 24:2cc49dc854e3 188 }
Alex Young 24:2cc49dc854e3 189
Alex Young 24:2cc49dc854e3 190 static void mtMessageHandler(struct XbusMessage const* message)
Alex Young 4:98f063b2e6da 191 {
Alex Young 15:558d279addd9 192 if (message->mid == XMID_MtData2)
Alex Young 7:c913a7cd5231 193 {
Alex Young 24:2cc49dc854e3 194 handleDataMessage(message);
Alex Young 7:c913a7cd5231 195 }
Alex Young 7:c913a7cd5231 196 else
Alex Young 7:c913a7cd5231 197 {
Alex Young 26:665d3624f9ab 198 XbusMessage* m = g_messagePool.alloc();
Alex Young 26:665d3624f9ab 199 memcpy(m, message, sizeof(XbusMessage));
Alex Young 26:665d3624f9ab 200 g_responseQueue.put(m);
Alex Young 25:01356fb59467 201 }
Alex Young 4:98f063b2e6da 202 }
Alex Young 4:98f063b2e6da 203
Alex Young 4:98f063b2e6da 204 static void configureSerialPorts(void)
Alex Young 4:98f063b2e6da 205 {
Alex Young 4:98f063b2e6da 206 pc.baud(921600);
Alex Young 4:98f063b2e6da 207 pc.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 208
Alex Young 37:3e87bf647c68 209 mt.baud(115200);
Alex Young 4:98f063b2e6da 210 mt.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 211 mt.attach(mtLowLevelHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 212 }
Alex Young 4:98f063b2e6da 213
Alex Young 29:d9310e7b58b5 214 static uint32_t readDeviceId(void)
Alex Young 29:d9310e7b58b5 215 {
Alex Young 29:d9310e7b58b5 216 XbusMessage reqDid = {XMID_ReqDid};
Alex Young 29:d9310e7b58b5 217 XbusMessage const* didRsp = doTransaction(&reqDid);
Alex Young 31:ce1ea9ae861e 218 XbusMessageMemoryManager janitor(didRsp);
Alex Young 29:d9310e7b58b5 219 uint32_t deviceId = 0;
Alex Young 29:d9310e7b58b5 220 if (didRsp)
Alex Young 29:d9310e7b58b5 221 {
Alex Young 29:d9310e7b58b5 222 if (didRsp->mid == XMID_DeviceId)
Alex Young 29:d9310e7b58b5 223 {
Alex Young 29:d9310e7b58b5 224 deviceId = *(uint32_t*)didRsp->data;
Alex Young 29:d9310e7b58b5 225 }
Alex Young 29:d9310e7b58b5 226 }
Alex Young 29:d9310e7b58b5 227 return deviceId;
Alex Young 29:d9310e7b58b5 228 }
Alex Young 29:d9310e7b58b5 229
Alex Young 32:fafe0f42d82b 230 static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements)
Alex Young 29:d9310e7b58b5 231 {
Alex Young 32:fafe0f42d82b 232 XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf};
Alex Young 32:fafe0f42d82b 233 XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg);
Alex Young 32:fafe0f42d82b 234 XbusMessageMemoryManager janitor(outputConfRsp);
Alex Young 32:fafe0f42d82b 235 if (outputConfRsp)
Alex Young 29:d9310e7b58b5 236 {
Alex Young 32:fafe0f42d82b 237 if (outputConfRsp->mid == XMID_OutputConfig)
Alex Young 29:d9310e7b58b5 238 {
Alex Young 32:fafe0f42d82b 239 pc.printf("Output configuration set to:\n");
Alex Young 32:fafe0f42d82b 240 OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data;
Alex Young 32:fafe0f42d82b 241 for (int i = 0; i < outputConfRsp->length; ++i)
Alex Young 32:fafe0f42d82b 242 {
Alex Young 32:fafe0f42d82b 243 pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 32:fafe0f42d82b 244 ++conf;
Alex Young 32:fafe0f42d82b 245 }
Alex Young 32:fafe0f42d82b 246 return true;
Alex Young 29:d9310e7b58b5 247 }
Alex Young 29:d9310e7b58b5 248 else
Alex Young 29:d9310e7b58b5 249 {
Alex Young 32:fafe0f42d82b 250 dumpResponse(outputConfRsp);
Alex Young 29:d9310e7b58b5 251 }
Alex Young 32:fafe0f42d82b 252 }
Alex Young 32:fafe0f42d82b 253 else
Alex Young 32:fafe0f42d82b 254 {
Alex Young 32:fafe0f42d82b 255 pc.printf("Failed to set output configuration.\n");
Alex Young 32:fafe0f42d82b 256 }
Alex Young 32:fafe0f42d82b 257 return false;
Alex Young 32:fafe0f42d82b 258 }
Alex Young 29:d9310e7b58b5 259
Alex Young 32:fafe0f42d82b 260 static bool configureMotionTracker(void)
Alex Young 32:fafe0f42d82b 261 {
Alex Young 32:fafe0f42d82b 262 uint32_t deviceId = readDeviceId();
Alex Young 32:fafe0f42d82b 263
Alex Young 32:fafe0f42d82b 264 if (deviceId)
Alex Young 32:fafe0f42d82b 265 {
Alex Young 40:b77a8c10c76d 266 pc.printf("Found device with ID: %08X.\n", deviceId);
Alex Young 40:b77a8c10c76d 267 if (!XsDeviceId_isMtMk4_X(deviceId))
Alex Young 40:b77a8c10c76d 268 {
Alex Young 40:b77a8c10c76d 269 pc.printf("Device is not an MTi-1 series.\n");
Alex Young 40:b77a8c10c76d 270 return false;
Alex Young 40:b77a8c10c76d 271 }
Alex Young 32:fafe0f42d82b 272
Alex Young 40:b77a8c10c76d 273 DeviceFunction function = XsDeviceId_getFunction(deviceId);
Alex Young 40:b77a8c10c76d 274 pc.printf("Device is an MTi-%d: %s.\n", function, XsDeviceId_functionDescription(function));
Alex Young 40:b77a8c10c76d 275
Alex Young 40:b77a8c10c76d 276 if (function == DF_IMU)
Alex Young 29:d9310e7b58b5 277 {
Alex Young 32:fafe0f42d82b 278 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 279 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 280 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 281 {XDI_Acceleration, 100},
Alex Young 32:fafe0f42d82b 282 {XDI_RateOfTurn, 100},
Alex Young 32:fafe0f42d82b 283 {XDI_MagneticField, 100}
Alex Young 32:fafe0f42d82b 284 };
Alex Young 32:fafe0f42d82b 285 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 286 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 287 }
Alex Young 29:d9310e7b58b5 288 else
Alex Young 29:d9310e7b58b5 289 {
Alex Young 32:fafe0f42d82b 290 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 291 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 292 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 293 {XDI_Quaternion, 100},
Alex Young 32:fafe0f42d82b 294 {XDI_StatusWord, 65535}
Alex Young 32:fafe0f42d82b 295 };
Alex Young 32:fafe0f42d82b 296 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 297 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 298 }
Alex Young 29:d9310e7b58b5 299 }
Alex Young 32:fafe0f42d82b 300
Alex Young 32:fafe0f42d82b 301 return false;
Alex Young 29:d9310e7b58b5 302 }
Alex Young 29:d9310e7b58b5 303
Alex Young 35:7e519b88c610 304 /*!
Alex Young 35:7e519b88c610 305 * \brief Wait for a wakeup message from the MTi.
Alex Young 37:3e87bf647c68 306 * \param timeout Time to wait to receive the wakeup message.
Alex Young 37:3e87bf647c68 307 * \return true if wakeup received within timeout, else false.
Alex Young 35:7e519b88c610 308 *
Alex Young 35:7e519b88c610 309 * The MTi sends a XMID_Wakeup message once it has completed its bootup
Alex Young 35:7e519b88c610 310 * procedure. If this is acknowledged by a XMID_WakeupAck message then the MTi
Alex Young 35:7e519b88c610 311 * will stay in configuration mode. Otherwise it will automatically enter
Alex Young 35:7e519b88c610 312 * measurement mode with the stored output configuration.
Alex Young 35:7e519b88c610 313 */
Alex Young 37:3e87bf647c68 314 bool waitForWakeup(uint32_t timeout)
Alex Young 35:7e519b88c610 315 {
Alex Young 37:3e87bf647c68 316 osEvent ev = g_responseQueue.get(timeout);
Alex Young 35:7e519b88c610 317 if (ev.status == osEventMessage)
Alex Young 35:7e519b88c610 318 {
Alex Young 35:7e519b88c610 319 XbusMessage const* m = (XbusMessage const*)ev.value.p;
Alex Young 35:7e519b88c610 320 XbusMessageMemoryManager janitor(m);
Alex Young 35:7e519b88c610 321 return m->mid == XMID_Wakeup;
Alex Young 35:7e519b88c610 322 }
Alex Young 35:7e519b88c610 323 return false;
Alex Young 35:7e519b88c610 324 }
Alex Young 35:7e519b88c610 325
Alex Young 35:7e519b88c610 326 /*!
Alex Young 37:3e87bf647c68 327 * \brief Send wakeup acknowledge message to MTi.
Alex Young 37:3e87bf647c68 328 *
Alex Young 37:3e87bf647c68 329 * Sending a wakeup acknowledge will cause the device to stay in configuration
Alex Young 37:3e87bf647c68 330 * mode instead of automatically transitioning to measurement mode with the
Alex Young 37:3e87bf647c68 331 * stored output configuration.
Alex Young 37:3e87bf647c68 332 */
Alex Young 37:3e87bf647c68 333 void sendWakeupAck(void)
Alex Young 37:3e87bf647c68 334 {
Alex Young 37:3e87bf647c68 335 XbusMessage ack = {XMID_WakeupAck};
Alex Young 37:3e87bf647c68 336 sendMessage(&ack);
Alex Young 37:3e87bf647c68 337 pc.printf("Device ready for operation.\n");
Alex Young 37:3e87bf647c68 338 }
Alex Young 37:3e87bf647c68 339
Alex Young 37:3e87bf647c68 340 /*!
Alex Young 37:3e87bf647c68 341 * \brief Restore communication with the MTi.
Alex Young 37:3e87bf647c68 342 *
Alex Young 37:3e87bf647c68 343 * On bootup the MTi will listen for a magic byte to signal that it should
Alex Young 37:3e87bf647c68 344 * return to default baudrate and output configuration. This can be used to
Alex Young 37:3e87bf647c68 345 * recover from a bad or unknown configuration.
Alex Young 37:3e87bf647c68 346 */
Alex Young 37:3e87bf647c68 347 void restoreCommunication(void)
Alex Young 37:3e87bf647c68 348 {
Alex Young 37:3e87bf647c68 349 pc.printf("Restoring communication with device... ");
Alex Young 37:3e87bf647c68 350 mtReset = 0;
Alex Young 37:3e87bf647c68 351 Thread::wait(1);
Alex Young 37:3e87bf647c68 352 mtReset = 1;
Alex Young 37:3e87bf647c68 353
Alex Young 37:3e87bf647c68 354 do
Alex Young 37:3e87bf647c68 355 {
Alex Young 37:3e87bf647c68 356 mt.putc(0xDE);
Alex Young 37:3e87bf647c68 357 }
Alex Young 37:3e87bf647c68 358 while (!waitForWakeup(1));
Alex Young 37:3e87bf647c68 359 pc.printf("done\n");
Alex Young 37:3e87bf647c68 360
Alex Young 37:3e87bf647c68 361 sendWakeupAck();
Alex Young 37:3e87bf647c68 362 }
Alex Young 37:3e87bf647c68 363
Alex Young 37:3e87bf647c68 364 /*!
Alex Young 35:7e519b88c610 365 * \brief Releases the MTi reset line and waits for a wakeup message.
Alex Young 37:3e87bf647c68 366 *
Alex Young 37:3e87bf647c68 367 * If no wakeup message is received within 1 second the restore communications
Alex Young 37:3e87bf647c68 368 * procedure is done to reset the MTi to default baudrate and output configuration.
Alex Young 35:7e519b88c610 369 */
Alex Young 35:7e519b88c610 370 static void wakeupMotionTracker(void)
Alex Young 35:7e519b88c610 371 {
Alex Young 35:7e519b88c610 372 mtReset.write(1); // Release MT from reset.
Alex Young 37:3e87bf647c68 373 if (waitForWakeup(1000))
Alex Young 35:7e519b88c610 374 {
Alex Young 37:3e87bf647c68 375 sendWakeupAck();
Alex Young 37:3e87bf647c68 376 }
Alex Young 37:3e87bf647c68 377 else
Alex Young 37:3e87bf647c68 378 {
Alex Young 37:3e87bf647c68 379 restoreCommunication();
Alex Young 35:7e519b88c610 380 }
Alex Young 35:7e519b88c610 381 }
Alex Young 35:7e519b88c610 382
Alex Young 38:d8d410d1662c 383 static void printIntroMessage(void)
Alex Young 38:d8d410d1662c 384 {
Alex Young 38:d8d410d1662c 385 pc.printf("\n\n\n\n\n");
Alex Young 38:d8d410d1662c 386 pc.printf("MTi-1 series embedded example firmware.\n");
Alex Young 38:d8d410d1662c 387 }
Alex Young 38:d8d410d1662c 388
Alex Young 38:d8d410d1662c 389 static void printUsageInstructions(void)
Alex Young 38:d8d410d1662c 390 {
Alex Young 38:d8d410d1662c 391 pc.printf("\n");
Alex Young 38:d8d410d1662c 392 pc.printf("Press 'm' to start measuring and 'c' to return to config mode.\n");
Alex Young 38:d8d410d1662c 393 }
Alex Young 38:d8d410d1662c 394
Alex Young 2:b3e402dc11ca 395 int main(void)
Alex Young 2:b3e402dc11ca 396 {
Alex Young 4:98f063b2e6da 397 XbusParserCallback xbusCallback = {};
Alex Young 25:01356fb59467 398 xbusCallback.allocateBuffer = allocateMessageData;
Alex Young 25:01356fb59467 399 xbusCallback.deallocateBuffer = deallocateMessageData;
Alex Young 24:2cc49dc854e3 400 xbusCallback.handleMessage = mtMessageHandler;
Alex Young 4:98f063b2e6da 401
Alex Young 4:98f063b2e6da 402 xbusParser = XbusParser_create(&xbusCallback);
Alex Young 4:98f063b2e6da 403 configureSerialPorts();
Alex Young 38:d8d410d1662c 404
Alex Young 38:d8d410d1662c 405 printIntroMessage();
Alex Young 35:7e519b88c610 406 wakeupMotionTracker();
Alex Young 29:d9310e7b58b5 407 if (configureMotionTracker())
Alex Young 5:abc52dd88be2 408 {
Alex Young 38:d8d410d1662c 409 printUsageInstructions();
Alex Young 29:d9310e7b58b5 410 for (;;)
Alex Young 26:665d3624f9ab 411 {
Alex Young 29:d9310e7b58b5 412 while (pc.readable())
Alex Young 29:d9310e7b58b5 413 {
Alex Young 29:d9310e7b58b5 414 handlePcCommand(pc.getc());
Alex Young 29:d9310e7b58b5 415 }
Alex Young 41:504b6821a96f 416 Thread::wait(10);
Alex Young 26:665d3624f9ab 417 }
Alex Young 5:abc52dd88be2 418 }
Alex Young 29:d9310e7b58b5 419 else
Alex Young 29:d9310e7b58b5 420 {
Alex Young 29:d9310e7b58b5 421 pc.printf("Failed to configure motion tracker.\n");
Alex Young 29:d9310e7b58b5 422 return -1;
Alex Young 29:d9310e7b58b5 423 }
Alex Young 4:98f063b2e6da 424 }