HyunGyu Kim / Mbed 2 deprecated MTi-1_example

kjj

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

main.cpp@43:470c019246e4, 2015-05-21 (annotated)

Committer:
Alex Young
Date:
Thu May 21 15:34:18 2015 +0200
Revision:
43:470c019246e4
Parent:
42:e70756d2d8bc
Child:
44:b3980e8ac074
Move printing of message data to main thread.



Printing data may take some time, so should not really be done in an

interrupt context. Instead use a queue to pass the message data to the

main thread for processing.

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