HyunGyu Kim / Mbed 2 deprecated MTi-1_example

kjj

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

main.cpp@44:b3980e8ac074, 2015-05-21 (annotated)

Committer:
Alex Young
Date:
Thu May 21 16:01:20 2015 +0200
Revision:
44:b3980e8ac074
Parent:
43:470c019246e4
Child:
49:38ecfbff5391
Add doxygen comments for main.cpp

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 44:b3980e8ac074 22 /*!
Alex Young 44:b3980e8ac074 23 * \brief The number of items to hold in the memory pools.
Alex Young 44:b3980e8ac074 24 */
Alex Young 25:01356fb59467 25 #define MEMORY_POOL_SIZE (4)
Alex Young 44:b3980e8ac074 26 /*!
Alex Young 44:b3980e8ac074 27 * \brief The size of the queue used for device responses.
Alex Young 44:b3980e8ac074 28 * This is set to one as in typical Xbus operation each command receives a
Alex Young 44:b3980e8ac074 29 * response before the next command is sent.
Alex Young 44:b3980e8ac074 30 */
Alex Young 26:665d3624f9ab 31 #define RESPONSE_QUEUE_SIZE (1)
Alex Young 44:b3980e8ac074 32 /*!
Alex Young 44:b3980e8ac074 33 * \brief The size of the queue used for data messages.
Alex Young 44:b3980e8ac074 34 * This is set to two to allow some overlap between printing received data to
Alex Young 44:b3980e8ac074 35 * the PC serial port and the reception of the subsequent data packet. In
Alex Young 44:b3980e8ac074 36 * more complex applications it might be necessary to increase this if
Alex Young 44:b3980e8ac074 37 * message processing might occasionally require more time than normal.
Alex Young 44:b3980e8ac074 38 */
Alex Young 43:470c019246e4 39 #define DATA_QUEUE_SIZE (2)
Alex Young 44:b3980e8ac074 40 /*!
Alex Young 44:b3980e8ac074 41 * \brief The maximum size of a xbus message supported by the application.
Alex Young 44:b3980e8ac074 42 * This is the size of the message buffers in the message data memory pool.
Alex Young 44:b3980e8ac074 43 */
Alex Young 25:01356fb59467 44 #define MAX_XBUS_DATA_SIZE (128)
Alex Young 25:01356fb59467 45
Alex Young 44:b3980e8ac074 46 /*! \brief Serial port for communication with the host PC. */
Alex Young 4:98f063b2e6da 47 static Serial pc(PA_2, PA_3);
Alex Young 44:b3980e8ac074 48 /*! \brief Serial port for communication with the MT. */
Alex Young 4:98f063b2e6da 49 static Serial mt(PB_9, PB_8);
Alex Young 35:7e519b88c610 50 /*!
Alex Young 35:7e519b88c610 51 * \brief MT reset line.
Alex Young 35:7e519b88c610 52 *
Alex Young 35:7e519b88c610 53 * MT is held in reset on startup.
Alex Young 35:7e519b88c610 54 */
Alex Young 35:7e519b88c610 55 static DigitalOut mtReset(PA_10, 0);
Alex Young 44:b3980e8ac074 56 /*! \brief XbusParser used to parse incoming Xbus messages from the MT. */
Alex Young 4:98f063b2e6da 57 static XbusParser* xbusParser;
Alex Young 25:01356fb59467 58
Alex Young 44:b3980e8ac074 59 /*!
Alex Young 44:b3980e8ac074 60 * \brief Memory pool used for storing Xbus messages when passing them
Alex Young 44:b3980e8ac074 61 * to the main thread.
Alex Young 44:b3980e8ac074 62 */
Alex Young 25:01356fb59467 63 MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool;
Alex Young 44:b3980e8ac074 64 /*!
Alex Young 44:b3980e8ac074 65 * \brief Memory pool used for storing the payload of Xbus messages.
Alex Young 44:b3980e8ac074 66 */
Alex Young 25:01356fb59467 67 MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool;
Alex Young 44:b3980e8ac074 68 /*!
Alex Young 44:b3980e8ac074 69 * \brief Queue used to pass data messages to the main thread for processing.
Alex Young 44:b3980e8ac074 70 */
Alex Young 44:b3980e8ac074 71 Queue<XbusMessage, DATA_QUEUE_SIZE> g_dataQueue;
Alex Young 44:b3980e8ac074 72 /*!
Alex Young 44:b3980e8ac074 73 * \brief Queue used for passing all other messages to the main thread for processing.
Alex Young 44:b3980e8ac074 74 */
Alex Young 26:665d3624f9ab 75 Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue;
Alex Young 4:98f063b2e6da 76
Alex Young 44:b3980e8ac074 77 /*!
Alex Young 44:b3980e8ac074 78 * \brief Allocate message data buffer from the message data pool.
Alex Young 44:b3980e8ac074 79 */
Alex Young 25:01356fb59467 80 static void* allocateMessageData(size_t bufSize)
Alex Young 4:98f063b2e6da 81 {
Alex Young 25:01356fb59467 82 return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL;
Alex Young 25:01356fb59467 83 }
Alex Young 25:01356fb59467 84
Alex Young 44:b3980e8ac074 85 /*!
Alex Young 44:b3980e8ac074 86 * \brief Deallocate message data previously allocated from the message
Alex Young 44:b3980e8ac074 87 * data pool.
Alex Young 44:b3980e8ac074 88 */
Alex Young 25:01356fb59467 89 static void deallocateMessageData(void const* buffer)
Alex Young 25:01356fb59467 90 {
Alex Young 25:01356fb59467 91 g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer);
Alex Young 4:98f063b2e6da 92 }
Alex Young 4:98f063b2e6da 93
Alex Young 44:b3980e8ac074 94 /*!
Alex Young 44:b3980e8ac074 95 * \brief RX Interrupt handler for the MT serial port.
Alex Young 44:b3980e8ac074 96 *
Alex Young 44:b3980e8ac074 97 * Passes received data to an XbusParser to extract messages.
Alex Young 44:b3980e8ac074 98 */
Alex Young 4:98f063b2e6da 99 static void mtLowLevelHandler(void)
Alex Young 4:98f063b2e6da 100 {
Alex Young 4:98f063b2e6da 101 while (mt.readable())
Alex Young 4:98f063b2e6da 102 {
Alex Young 4:98f063b2e6da 103 XbusParser_parseByte(xbusParser, mt.getc());
Alex Young 4:98f063b2e6da 104 }
Alex Young 4:98f063b2e6da 105 }
Alex Young 4:98f063b2e6da 106
Alex Young 44:b3980e8ac074 107 /*!
Alex Young 44:b3980e8ac074 108 * \brief Send a message to the MT
Alex Young 44:b3980e8ac074 109 *
Alex Young 44:b3980e8ac074 110 * This function formats the message data and writes this to the MT serial
Alex Young 44:b3980e8ac074 111 * port. It does not wait for any response.
Alex Young 44:b3980e8ac074 112 */
Alex Young 34:3d7a6519a256 113 static void sendMessage(XbusMessage const* m)
Alex Young 11:8593ba137917 114 {
Alex Young 26:665d3624f9ab 115 uint8_t buf[64];
Alex Young 26:665d3624f9ab 116 size_t rawLength = XbusMessage_format(buf, m);
Alex Young 11:8593ba137917 117 for (size_t i = 0; i < rawLength; ++i)
Alex Young 11:8593ba137917 118 {
Alex Young 11:8593ba137917 119 mt.putc(buf[i]);
Alex Young 11:8593ba137917 120 }
Alex Young 34:3d7a6519a256 121 }
Alex Young 34:3d7a6519a256 122
Alex Young 44:b3980e8ac074 123 /*!
Alex Young 44:b3980e8ac074 124 * \brief Send a message to the MT and wait for a response.
Alex Young 44:b3980e8ac074 125 * \returns Response message from the MT, or NULL is no response received
Alex Young 44:b3980e8ac074 126 * within 500ms.
Alex Young 44:b3980e8ac074 127 *
Alex Young 44:b3980e8ac074 128 * Blocking behaviour is implemented by waiting for a response to be written
Alex Young 44:b3980e8ac074 129 * to the response queue by the XbusParser.
Alex Young 44:b3980e8ac074 130 */
Alex Young 34:3d7a6519a256 131 static XbusMessage const* doTransaction(XbusMessage const* m)
Alex Young 34:3d7a6519a256 132 {
Alex Young 34:3d7a6519a256 133 sendMessage(m);
Alex Young 26:665d3624f9ab 134
Alex Young 26:665d3624f9ab 135 osEvent ev = g_responseQueue.get(500);
Alex Young 26:665d3624f9ab 136 return ev.status == osEventMessage ? (XbusMessage*)ev.value.p : NULL;
Alex Young 26:665d3624f9ab 137 }
Alex Young 26:665d3624f9ab 138
Alex Young 31:ce1ea9ae861e 139 /*!
Alex Young 31:ce1ea9ae861e 140 * \brief RAII object to manage message memory deallocation.
Alex Young 31:ce1ea9ae861e 141 *
Alex Young 31:ce1ea9ae861e 142 * Will automatically free the memory used by a XbusMessage when going out
Alex Young 31:ce1ea9ae861e 143 * of scope.
Alex Young 31:ce1ea9ae861e 144 */
Alex Young 31:ce1ea9ae861e 145 class XbusMessageMemoryManager
Alex Young 26:665d3624f9ab 146 {
Alex Young 31:ce1ea9ae861e 147 public:
Alex Young 31:ce1ea9ae861e 148 XbusMessageMemoryManager(XbusMessage const* message)
Alex Young 31:ce1ea9ae861e 149 : m_message(message)
Alex Young 31:ce1ea9ae861e 150 {
Alex Young 31:ce1ea9ae861e 151 }
Alex Young 31:ce1ea9ae861e 152
Alex Young 31:ce1ea9ae861e 153 ~XbusMessageMemoryManager()
Alex Young 31:ce1ea9ae861e 154 {
Alex Young 31:ce1ea9ae861e 155 if (m_message)
Alex Young 31:ce1ea9ae861e 156 {
Alex Young 31:ce1ea9ae861e 157 if (m_message->data)
Alex Young 31:ce1ea9ae861e 158 deallocateMessageData(m_message->data);
Alex Young 31:ce1ea9ae861e 159 g_messagePool.free(const_cast<XbusMessage*>(m_message));
Alex Young 31:ce1ea9ae861e 160 }
Alex Young 31:ce1ea9ae861e 161 }
Alex Young 31:ce1ea9ae861e 162
Alex Young 31:ce1ea9ae861e 163 private:
Alex Young 31:ce1ea9ae861e 164 XbusMessage const* m_message;
Alex Young 31:ce1ea9ae861e 165 };
Alex Young 26:665d3624f9ab 166
Alex Young 44:b3980e8ac074 167 /*!
Alex Young 44:b3980e8ac074 168 * \brief Dump information from a message to the PC serial port.
Alex Young 44:b3980e8ac074 169 */
Alex Young 29:d9310e7b58b5 170 static void dumpResponse(XbusMessage const* response)
Alex Young 29:d9310e7b58b5 171 {
Alex Young 29:d9310e7b58b5 172 switch (response->mid)
Alex Young 29:d9310e7b58b5 173 {
Alex Young 29:d9310e7b58b5 174 case XMID_GotoConfigAck:
Alex Young 39:9014c5236864 175 pc.printf("Device went to config mode.\n");
Alex Young 29:d9310e7b58b5 176 break;
Alex Young 29:d9310e7b58b5 177
Alex Young 29:d9310e7b58b5 178 case XMID_Error:
Alex Young 29:d9310e7b58b5 179 pc.printf("Device error!");
Alex Young 29:d9310e7b58b5 180 break;
Alex Young 29:d9310e7b58b5 181
Alex Young 29:d9310e7b58b5 182 default:
Alex Young 29:d9310e7b58b5 183 pc.printf("Received response MID=%X, length=%d\n", response->mid, response->length);
Alex Young 29:d9310e7b58b5 184 break;
Alex Young 29:d9310e7b58b5 185 }
Alex Young 29:d9310e7b58b5 186 }
Alex Young 29:d9310e7b58b5 187
Alex Young 44:b3980e8ac074 188 /*!
Alex Young 44:b3980e8ac074 189 * \brief Send a command to the MT and wait for a response.
Alex Young 44:b3980e8ac074 190 * \param cmdId The XsMessageId of the command to send.
Alex Young 44:b3980e8ac074 191 *
Alex Young 44:b3980e8ac074 192 * Commands are simple messages without and payload data.
Alex Young 44:b3980e8ac074 193 */
Alex Young 26:665d3624f9ab 194 static void sendCommand(XsMessageId cmdId)
Alex Young 26:665d3624f9ab 195 {
Alex Young 26:665d3624f9ab 196 XbusMessage m = {cmdId};
Alex Young 26:665d3624f9ab 197 XbusMessage const* response = doTransaction(&m);
Alex Young 31:ce1ea9ae861e 198 XbusMessageMemoryManager janitor(response);
Alex Young 26:665d3624f9ab 199
Alex Young 26:665d3624f9ab 200 if (response)
Alex Young 26:665d3624f9ab 201 {
Alex Young 29:d9310e7b58b5 202 dumpResponse(response);
Alex Young 26:665d3624f9ab 203 }
Alex Young 26:665d3624f9ab 204 else
Alex Young 26:665d3624f9ab 205 {
Alex Young 26:665d3624f9ab 206 pc.printf("Timeout waiting for response.\n");
Alex Young 26:665d3624f9ab 207 }
Alex Young 11:8593ba137917 208 }
Alex Young 11:8593ba137917 209
Alex Young 44:b3980e8ac074 210 /*!
Alex Young 44:b3980e8ac074 211 * \brief Handle a command from the PC
Alex Young 44:b3980e8ac074 212 *
Alex Young 44:b3980e8ac074 213 * The example application supports single character commands from the host
Alex Young 44:b3980e8ac074 214 * PC to switch between configuration and measurement modes.
Alex Young 44:b3980e8ac074 215 */
Alex Young 11:8593ba137917 216 static void handlePcCommand(char cmd)
Alex Young 11:8593ba137917 217 {
Alex Young 11:8593ba137917 218 switch (cmd)
Alex Young 11:8593ba137917 219 {
Alex Young 11:8593ba137917 220 case 'c':
Alex Young 11:8593ba137917 221 sendCommand(XMID_GotoConfig);
Alex Young 11:8593ba137917 222 break;
Alex Young 11:8593ba137917 223
Alex Young 11:8593ba137917 224 case 'm':
Alex Young 11:8593ba137917 225 sendCommand(XMID_GotoMeasurement);
Alex Young 11:8593ba137917 226 break;
Alex Young 11:8593ba137917 227 }
Alex Young 11:8593ba137917 228 }
Alex Young 11:8593ba137917 229
Alex Young 44:b3980e8ac074 230 /*!
Alex Young 44:b3980e8ac074 231 * \brief XbusParser callback function to handle received messages.
Alex Young 44:b3980e8ac074 232 * \param message Pointer to the last received message.
Alex Young 44:b3980e8ac074 233 *
Alex Young 44:b3980e8ac074 234 * In this example received messages are copied into one of two message
Alex Young 44:b3980e8ac074 235 * queues for later handling by the main thread. Data messages are put
Alex Young 44:b3980e8ac074 236 * in one queue, while all other responses are place in the second queue.
Alex Young 44:b3980e8ac074 237 * This is done so that data and other messages can be handled separately
Alex Young 44:b3980e8ac074 238 * by the application code.
Alex Young 44:b3980e8ac074 239 */
Alex Young 24:2cc49dc854e3 240 static void mtMessageHandler(struct XbusMessage const* message)
Alex Young 4:98f063b2e6da 241 {
Alex Young 43:470c019246e4 242 XbusMessage* m = g_messagePool.alloc();
Alex Young 43:470c019246e4 243 if (m)
Alex Young 7:c913a7cd5231 244 {
Alex Young 43:470c019246e4 245 memcpy(m, message, sizeof(XbusMessage));
Alex Young 43:470c019246e4 246 if (message->mid == XMID_MtData2)
Alex Young 43:470c019246e4 247 {
Alex Young 43:470c019246e4 248 g_dataQueue.put(m);
Alex Young 43:470c019246e4 249 }
Alex Young 43:470c019246e4 250 else
Alex Young 43:470c019246e4 251 {
Alex Young 43:470c019246e4 252 g_responseQueue.put(m);
Alex Young 43:470c019246e4 253 }
Alex Young 7:c913a7cd5231 254 }
Alex Young 43:470c019246e4 255 else if (message->data)
Alex Young 7:c913a7cd5231 256 {
Alex Young 43:470c019246e4 257 deallocateMessageData(message->data);
Alex Young 25:01356fb59467 258 }
Alex Young 4:98f063b2e6da 259 }
Alex Young 4:98f063b2e6da 260
Alex Young 44:b3980e8ac074 261 /*!
Alex Young 44:b3980e8ac074 262 * \brief Configure the serial ports used to communicate with the motion
Alex Young 44:b3980e8ac074 263 * tracker and host PC.
Alex Young 44:b3980e8ac074 264 */
Alex Young 4:98f063b2e6da 265 static void configureSerialPorts(void)
Alex Young 4:98f063b2e6da 266 {
Alex Young 4:98f063b2e6da 267 pc.baud(921600);
Alex Young 4:98f063b2e6da 268 pc.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 269
Alex Young 37:3e87bf647c68 270 mt.baud(115200);
Alex Young 4:98f063b2e6da 271 mt.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 272 mt.attach(mtLowLevelHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 273 }
Alex Young 4:98f063b2e6da 274
Alex Young 44:b3980e8ac074 275 /*!
Alex Young 44:b3980e8ac074 276 * \brief Read the device ID of the motion tracker.
Alex Young 44:b3980e8ac074 277 */
Alex Young 29:d9310e7b58b5 278 static uint32_t readDeviceId(void)
Alex Young 29:d9310e7b58b5 279 {
Alex Young 29:d9310e7b58b5 280 XbusMessage reqDid = {XMID_ReqDid};
Alex Young 29:d9310e7b58b5 281 XbusMessage const* didRsp = doTransaction(&reqDid);
Alex Young 31:ce1ea9ae861e 282 XbusMessageMemoryManager janitor(didRsp);
Alex Young 29:d9310e7b58b5 283 uint32_t deviceId = 0;
Alex Young 29:d9310e7b58b5 284 if (didRsp)
Alex Young 29:d9310e7b58b5 285 {
Alex Young 29:d9310e7b58b5 286 if (didRsp->mid == XMID_DeviceId)
Alex Young 29:d9310e7b58b5 287 {
Alex Young 29:d9310e7b58b5 288 deviceId = *(uint32_t*)didRsp->data;
Alex Young 29:d9310e7b58b5 289 }
Alex Young 29:d9310e7b58b5 290 }
Alex Young 29:d9310e7b58b5 291 return deviceId;
Alex Young 29:d9310e7b58b5 292 }
Alex Young 29:d9310e7b58b5 293
Alex Young 44:b3980e8ac074 294 /*!
Alex Young 44:b3980e8ac074 295 * \brief Sets MT output configuration.
Alex Young 44:b3980e8ac074 296 * \param conf Pointer to an array of OutputConfiguration elements.
Alex Young 44:b3980e8ac074 297 * \param elements The number of elements in the configuration array.
Alex Young 44:b3980e8ac074 298 *
Alex Young 44:b3980e8ac074 299 * The response from the device indicates the actual values that will
Alex Young 44:b3980e8ac074 300 * be used by the motion tracker. These may differ from the requested
Alex Young 44:b3980e8ac074 301 * parameters as the motion tracker validates the requested parameters
Alex Young 44:b3980e8ac074 302 * before applying them.
Alex Young 44:b3980e8ac074 303 */
Alex Young 32:fafe0f42d82b 304 static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements)
Alex Young 29:d9310e7b58b5 305 {
Alex Young 32:fafe0f42d82b 306 XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf};
Alex Young 32:fafe0f42d82b 307 XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg);
Alex Young 32:fafe0f42d82b 308 XbusMessageMemoryManager janitor(outputConfRsp);
Alex Young 32:fafe0f42d82b 309 if (outputConfRsp)
Alex Young 29:d9310e7b58b5 310 {
Alex Young 32:fafe0f42d82b 311 if (outputConfRsp->mid == XMID_OutputConfig)
Alex Young 29:d9310e7b58b5 312 {
Alex Young 32:fafe0f42d82b 313 pc.printf("Output configuration set to:\n");
Alex Young 32:fafe0f42d82b 314 OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data;
Alex Young 32:fafe0f42d82b 315 for (int i = 0; i < outputConfRsp->length; ++i)
Alex Young 32:fafe0f42d82b 316 {
Alex Young 32:fafe0f42d82b 317 pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 32:fafe0f42d82b 318 ++conf;
Alex Young 32:fafe0f42d82b 319 }
Alex Young 32:fafe0f42d82b 320 return true;
Alex Young 29:d9310e7b58b5 321 }
Alex Young 29:d9310e7b58b5 322 else
Alex Young 29:d9310e7b58b5 323 {
Alex Young 32:fafe0f42d82b 324 dumpResponse(outputConfRsp);
Alex Young 29:d9310e7b58b5 325 }
Alex Young 32:fafe0f42d82b 326 }
Alex Young 32:fafe0f42d82b 327 else
Alex Young 32:fafe0f42d82b 328 {
Alex Young 32:fafe0f42d82b 329 pc.printf("Failed to set output configuration.\n");
Alex Young 32:fafe0f42d82b 330 }
Alex Young 32:fafe0f42d82b 331 return false;
Alex Young 32:fafe0f42d82b 332 }
Alex Young 29:d9310e7b58b5 333
Alex Young 44:b3980e8ac074 334 /*!
Alex Young 44:b3980e8ac074 335 * \brief Sets the motion tracker output configuration based on the function
Alex Young 44:b3980e8ac074 336 * of the attached device.
Alex Young 44:b3980e8ac074 337 *
Alex Young 44:b3980e8ac074 338 * The output configuration depends on the type of MTi-1 device connected.
Alex Young 44:b3980e8ac074 339 * A MTI-1 (IMU) device does not have an onboard orientation filter so
Alex Young 44:b3980e8ac074 340 * cannot output quaternion data, only inertial and magnetic measurement
Alex Young 44:b3980e8ac074 341 * data.
Alex Young 44:b3980e8ac074 342 * MTi-2 and MTi-3 devices have an onboard filter so can send quaternions.
Alex Young 44:b3980e8ac074 343 */
Alex Young 32:fafe0f42d82b 344 static bool configureMotionTracker(void)
Alex Young 32:fafe0f42d82b 345 {
Alex Young 32:fafe0f42d82b 346 uint32_t deviceId = readDeviceId();
Alex Young 32:fafe0f42d82b 347
Alex Young 32:fafe0f42d82b 348 if (deviceId)
Alex Young 32:fafe0f42d82b 349 {
Alex Young 40:b77a8c10c76d 350 pc.printf("Found device with ID: %08X.\n", deviceId);
Alex Young 40:b77a8c10c76d 351 if (!XsDeviceId_isMtMk4_X(deviceId))
Alex Young 40:b77a8c10c76d 352 {
Alex Young 40:b77a8c10c76d 353 pc.printf("Device is not an MTi-1 series.\n");
Alex Young 40:b77a8c10c76d 354 return false;
Alex Young 40:b77a8c10c76d 355 }
Alex Young 32:fafe0f42d82b 356
Alex Young 40:b77a8c10c76d 357 DeviceFunction function = XsDeviceId_getFunction(deviceId);
Alex Young 40:b77a8c10c76d 358 pc.printf("Device is an MTi-%d: %s.\n", function, XsDeviceId_functionDescription(function));
Alex Young 40:b77a8c10c76d 359
Alex Young 40:b77a8c10c76d 360 if (function == DF_IMU)
Alex Young 29:d9310e7b58b5 361 {
Alex Young 32:fafe0f42d82b 362 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 363 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 364 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 365 {XDI_Acceleration, 100},
Alex Young 32:fafe0f42d82b 366 {XDI_RateOfTurn, 100},
Alex Young 32:fafe0f42d82b 367 {XDI_MagneticField, 100}
Alex Young 32:fafe0f42d82b 368 };
Alex Young 32:fafe0f42d82b 369 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 370 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 371 }
Alex Young 29:d9310e7b58b5 372 else
Alex Young 29:d9310e7b58b5 373 {
Alex Young 32:fafe0f42d82b 374 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 375 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 376 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 377 {XDI_Quaternion, 100},
Alex Young 32:fafe0f42d82b 378 {XDI_StatusWord, 65535}
Alex Young 32:fafe0f42d82b 379 };
Alex Young 32:fafe0f42d82b 380 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 381 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 382 }
Alex Young 29:d9310e7b58b5 383 }
Alex Young 32:fafe0f42d82b 384
Alex Young 32:fafe0f42d82b 385 return false;
Alex Young 29:d9310e7b58b5 386 }
Alex Young 29:d9310e7b58b5 387
Alex Young 35:7e519b88c610 388 /*!
Alex Young 35:7e519b88c610 389 * \brief Wait for a wakeup message from the MTi.
Alex Young 37:3e87bf647c68 390 * \param timeout Time to wait to receive the wakeup message.
Alex Young 37:3e87bf647c68 391 * \return true if wakeup received within timeout, else false.
Alex Young 35:7e519b88c610 392 *
Alex Young 35:7e519b88c610 393 * The MTi sends a XMID_Wakeup message once it has completed its bootup
Alex Young 35:7e519b88c610 394 * procedure. If this is acknowledged by a XMID_WakeupAck message then the MTi
Alex Young 35:7e519b88c610 395 * will stay in configuration mode. Otherwise it will automatically enter
Alex Young 35:7e519b88c610 396 * measurement mode with the stored output configuration.
Alex Young 35:7e519b88c610 397 */
Alex Young 37:3e87bf647c68 398 bool waitForWakeup(uint32_t timeout)
Alex Young 35:7e519b88c610 399 {
Alex Young 37:3e87bf647c68 400 osEvent ev = g_responseQueue.get(timeout);
Alex Young 35:7e519b88c610 401 if (ev.status == osEventMessage)
Alex Young 35:7e519b88c610 402 {
Alex Young 35:7e519b88c610 403 XbusMessage const* m = (XbusMessage const*)ev.value.p;
Alex Young 35:7e519b88c610 404 XbusMessageMemoryManager janitor(m);
Alex Young 35:7e519b88c610 405 return m->mid == XMID_Wakeup;
Alex Young 35:7e519b88c610 406 }
Alex Young 35:7e519b88c610 407 return false;
Alex Young 35:7e519b88c610 408 }
Alex Young 35:7e519b88c610 409
Alex Young 35:7e519b88c610 410 /*!
Alex Young 37:3e87bf647c68 411 * \brief Send wakeup acknowledge message to MTi.
Alex Young 37:3e87bf647c68 412 *
Alex Young 37:3e87bf647c68 413 * Sending a wakeup acknowledge will cause the device to stay in configuration
Alex Young 37:3e87bf647c68 414 * mode instead of automatically transitioning to measurement mode with the
Alex Young 37:3e87bf647c68 415 * stored output configuration.
Alex Young 37:3e87bf647c68 416 */
Alex Young 37:3e87bf647c68 417 void sendWakeupAck(void)
Alex Young 37:3e87bf647c68 418 {
Alex Young 37:3e87bf647c68 419 XbusMessage ack = {XMID_WakeupAck};
Alex Young 37:3e87bf647c68 420 sendMessage(&ack);
Alex Young 37:3e87bf647c68 421 pc.printf("Device ready for operation.\n");
Alex Young 37:3e87bf647c68 422 }
Alex Young 37:3e87bf647c68 423
Alex Young 37:3e87bf647c68 424 /*!
Alex Young 37:3e87bf647c68 425 * \brief Restore communication with the MTi.
Alex Young 37:3e87bf647c68 426 *
Alex Young 37:3e87bf647c68 427 * On bootup the MTi will listen for a magic byte to signal that it should
Alex Young 37:3e87bf647c68 428 * return to default baudrate and output configuration. This can be used to
Alex Young 37:3e87bf647c68 429 * recover from a bad or unknown configuration.
Alex Young 37:3e87bf647c68 430 */
Alex Young 37:3e87bf647c68 431 void restoreCommunication(void)
Alex Young 37:3e87bf647c68 432 {
Alex Young 37:3e87bf647c68 433 pc.printf("Restoring communication with device... ");
Alex Young 37:3e87bf647c68 434 mtReset = 0;
Alex Young 37:3e87bf647c68 435 Thread::wait(1);
Alex Young 37:3e87bf647c68 436 mtReset = 1;
Alex Young 37:3e87bf647c68 437
Alex Young 37:3e87bf647c68 438 do
Alex Young 37:3e87bf647c68 439 {
Alex Young 37:3e87bf647c68 440 mt.putc(0xDE);
Alex Young 37:3e87bf647c68 441 }
Alex Young 37:3e87bf647c68 442 while (!waitForWakeup(1));
Alex Young 37:3e87bf647c68 443 pc.printf("done\n");
Alex Young 37:3e87bf647c68 444
Alex Young 37:3e87bf647c68 445 sendWakeupAck();
Alex Young 37:3e87bf647c68 446 }
Alex Young 37:3e87bf647c68 447
Alex Young 37:3e87bf647c68 448 /*!
Alex Young 35:7e519b88c610 449 * \brief Releases the MTi reset line and waits for a wakeup message.
Alex Young 37:3e87bf647c68 450 *
Alex Young 37:3e87bf647c68 451 * If no wakeup message is received within 1 second the restore communications
Alex Young 37:3e87bf647c68 452 * procedure is done to reset the MTi to default baudrate and output configuration.
Alex Young 35:7e519b88c610 453 */
Alex Young 35:7e519b88c610 454 static void wakeupMotionTracker(void)
Alex Young 35:7e519b88c610 455 {
Alex Young 35:7e519b88c610 456 mtReset.write(1); // Release MT from reset.
Alex Young 37:3e87bf647c68 457 if (waitForWakeup(1000))
Alex Young 35:7e519b88c610 458 {
Alex Young 37:3e87bf647c68 459 sendWakeupAck();
Alex Young 37:3e87bf647c68 460 }
Alex Young 37:3e87bf647c68 461 else
Alex Young 37:3e87bf647c68 462 {
Alex Young 37:3e87bf647c68 463 restoreCommunication();
Alex Young 35:7e519b88c610 464 }
Alex Young 35:7e519b88c610 465 }
Alex Young 35:7e519b88c610 466
Alex Young 38:d8d410d1662c 467 static void printIntroMessage(void)
Alex Young 38:d8d410d1662c 468 {
Alex Young 38:d8d410d1662c 469 pc.printf("\n\n\n\n\n");
Alex Young 38:d8d410d1662c 470 pc.printf("MTi-1 series embedded example firmware.\n");
Alex Young 38:d8d410d1662c 471 }
Alex Young 38:d8d410d1662c 472
Alex Young 38:d8d410d1662c 473 static void printUsageInstructions(void)
Alex Young 38:d8d410d1662c 474 {
Alex Young 38:d8d410d1662c 475 pc.printf("\n");
Alex Young 38:d8d410d1662c 476 pc.printf("Press 'm' to start measuring and 'c' to return to config mode.\n");
Alex Young 38:d8d410d1662c 477 }
Alex Young 38:d8d410d1662c 478
Alex Young 44:b3980e8ac074 479 /*!
Alex Young 44:b3980e8ac074 480 * \brief Output the contents of a data message to the PC serial port.
Alex Young 44:b3980e8ac074 481 */
Alex Young 43:470c019246e4 482 static void printMessageData(struct XbusMessage const* message)
Alex Young 43:470c019246e4 483 {
Alex Young 43:470c019246e4 484 if (!message)
Alex Young 43:470c019246e4 485 return;
Alex Young 43:470c019246e4 486
Alex Young 43:470c019246e4 487 pc.printf("MTData2:");
Alex Young 43:470c019246e4 488 uint16_t counter;
Alex Young 43:470c019246e4 489 if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
Alex Young 43:470c019246e4 490 {
Alex Young 43:470c019246e4 491 pc.printf(" Packet counter: %5d", counter);
Alex Young 43:470c019246e4 492 }
Alex Young 43:470c019246e4 493 float ori[4];
Alex Young 43:470c019246e4 494 if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
Alex Young 43:470c019246e4 495 {
Alex Young 43:470c019246e4 496 pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1],
Alex Young 43:470c019246e4 497 ori[2], ori[3]);
Alex Young 43:470c019246e4 498 }
Alex Young 43:470c019246e4 499 float acc[3];
Alex Young 43:470c019246e4 500 if (XbusMessage_getDataItem(acc, XDI_Acceleration, message))
Alex Young 43:470c019246e4 501 {
Alex Young 43:470c019246e4 502 pc.printf(" Acceleration: (% .3f, % .3f, % .3f)", acc[0], acc[1], acc[2]);
Alex Young 43:470c019246e4 503 }
Alex Young 43:470c019246e4 504 float gyr[3];
Alex Young 43:470c019246e4 505 if (XbusMessage_getDataItem(gyr, XDI_RateOfTurn, message))
Alex Young 43:470c019246e4 506 {
Alex Young 43:470c019246e4 507 pc.printf(" Rate Of Turn: (% .3f, % .3f, % .3f)", gyr[0], gyr[1], gyr[2]);
Alex Young 43:470c019246e4 508 }
Alex Young 43:470c019246e4 509 float mag[3];
Alex Young 43:470c019246e4 510 if (XbusMessage_getDataItem(mag, XDI_MagneticField, message))
Alex Young 43:470c019246e4 511 {
Alex Young 43:470c019246e4 512 pc.printf(" Magnetic Field: (% .3f, % .3f, % .3f)", mag[0], mag[1], mag[2]);
Alex Young 43:470c019246e4 513 }
Alex Young 43:470c019246e4 514 uint32_t status;
Alex Young 43:470c019246e4 515 if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
Alex Young 43:470c019246e4 516 {
Alex Young 43:470c019246e4 517 pc.printf(" Status:%X", status);
Alex Young 43:470c019246e4 518 }
Alex Young 43:470c019246e4 519 pc.printf("\n");
Alex Young 43:470c019246e4 520 }
Alex Young 43:470c019246e4 521
Alex Young 43:470c019246e4 522
Alex Young 2:b3e402dc11ca 523 int main(void)
Alex Young 2:b3e402dc11ca 524 {
Alex Young 4:98f063b2e6da 525 XbusParserCallback xbusCallback = {};
Alex Young 25:01356fb59467 526 xbusCallback.allocateBuffer = allocateMessageData;
Alex Young 25:01356fb59467 527 xbusCallback.deallocateBuffer = deallocateMessageData;
Alex Young 24:2cc49dc854e3 528 xbusCallback.handleMessage = mtMessageHandler;
Alex Young 4:98f063b2e6da 529
Alex Young 4:98f063b2e6da 530 xbusParser = XbusParser_create(&xbusCallback);
Alex Young 4:98f063b2e6da 531 configureSerialPorts();
Alex Young 38:d8d410d1662c 532
Alex Young 38:d8d410d1662c 533 printIntroMessage();
Alex Young 35:7e519b88c610 534 wakeupMotionTracker();
Alex Young 29:d9310e7b58b5 535 if (configureMotionTracker())
Alex Young 5:abc52dd88be2 536 {
Alex Young 38:d8d410d1662c 537 printUsageInstructions();
Alex Young 29:d9310e7b58b5 538 for (;;)
Alex Young 26:665d3624f9ab 539 {
Alex Young 29:d9310e7b58b5 540 while (pc.readable())
Alex Young 29:d9310e7b58b5 541 {
Alex Young 29:d9310e7b58b5 542 handlePcCommand(pc.getc());
Alex Young 29:d9310e7b58b5 543 }
Alex Young 43:470c019246e4 544
Alex Young 43:470c019246e4 545 osEvent ev = g_dataQueue.get(10);
Alex Young 43:470c019246e4 546 if (ev.status == osEventMessage)
Alex Young 43:470c019246e4 547 {
Alex Young 43:470c019246e4 548 XbusMessage const* data = (XbusMessage const*)ev.value.p;
Alex Young 43:470c019246e4 549 XbusMessageMemoryManager janitor(data);
Alex Young 43:470c019246e4 550 printMessageData(data);
Alex Young 43:470c019246e4 551 }
Alex Young 26:665d3624f9ab 552 }
Alex Young 5:abc52dd88be2 553 }
Alex Young 29:d9310e7b58b5 554 else
Alex Young 29:d9310e7b58b5 555 {
Alex Young 29:d9310e7b58b5 556 pc.printf("Failed to configure motion tracker.\n");
Alex Young 29:d9310e7b58b5 557 return -1;
Alex Young 29:d9310e7b58b5 558 }
Alex Young 4:98f063b2e6da 559 }