HyunGyu Kim / Mbed 2 deprecated MTi-1_example

kjj

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

main.cpp@53:3891f4259901, 2015-05-22 (annotated)

Committer:
Alex Young
Date:
Fri May 22 16:20:04 2015 +0200
Revision:
53:3891f4259901
Parent:
52:e2197b38c029
Child:
54:2e9bb1390c9c
Make PC serial baudrate a define

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