Xsens / Mbed 2 deprecated MTi-1_example

Example of using Xbus library to communicate with an MTi-1 series device using a full-duplex UART connection.

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Alex Young

Important Information

This example is deprecated and no longer maintained. There are new embedded examples available in the MT SDK folder of the MT Software Suite. For more information please visit: https://xsenstechnologies.force.com/knowledgebase/s/article/Introduction-to-the-MT-SDK-programming-examples-for-MTi-devices

Overview

The example program demonstrates connecting to an MTi-1 series device, restoring communications settings to default if necessary, and configuring the MTi to send data. For an MTi-1 the device is configured to send inertial sensor data, while MTi-2 and MTi-3 devices are configured to output orientation data using the onboard XKF3i filter.

Communication with the MTi-1 series device is implemented using a either a full-duplex UART, I2C or SPI bus. A reset line is used to reset the MTi during initialization. Data is output to a host PC terminal using a second UART.

For more information on the MTi-1 series communication protocol please refer to the datasheet: https://www.xsens.com/download/pdf/documentation/mti-1/mti-1-series_datasheet.pdf

Supported Platforms

The program has been tested on the following mbed platforms:

Using the Example

  1. To use the example program connect one of the supported mbed boards to the host PC and download the application from the mbed online compiler to the target device.
  2. With the mbed board unpowered (USB disconnected) wire the mbed board to the MTi-1 development board. The following connections are required:
    • In all cases:
      • 5V (or 3V3) main supply to VDD (P300-1)
      • MCU IO voltage (IORef) to VDDIO (P300-2)
      • GND to GND (P300-3)
      • MT_NRESET to nRST (P300-5)
    • For I2C communication:
      • MT_SCL to I2C_SCL (P300-9)
      • MT_SDA to I2C_SDA (P300-11)
      • MT_DRDY to DRDY (P300-15)
      • MT_ADD0 to ADD0 (P300-17)
      • MT_ADD1 to ADD1 (P300-19)
      • MT_ADD2 to ADD2 (P300-21)
    • For SPI communication:
      • MT_DRDY to DRDY (P300-15)
      • MT_SCLK to SPI_SCK (P300-17)
      • MT_MISO to SPI_MISO (P300-19)
      • MT_MOSI to SPI_MOSI (P300-21)
      • MT_nCS to SPI_nCS (P300-23)
    • For UART communication:
      • MT_RX to UART_TX (P300-9)
      • MT_TX to UART_RX (P300-11)

For more information on the MTi-1 development board please refer to the MTi-1 series user manual: https://www.xsens.com/download/pdf/documentation/mti-1/mti-1-series_dk_user_manual.pdf

Information

Check the defines at the top of main.cpp to determine which IO pins are used for the MT_xxx connections on each mbed platform.

Information

The active peripheral (I2C, SPI or UART) is selected on the MTi-1 development board through the PSEL0 and PSEL1 switches. Look on the bottom of the development board for the correct settings.

  1. Connect to the target using a serial terminal. The application is configured for:
    • Baudrate = 921600
    • Stop bits = 1
    • No parity bits
    • No flow control
  2. Reset the mbed board.
  3. You should be presented with a simple user interface as shown below: 
MTi-1 series embedded example firmware.
Device ready for operation.
Found device with ID: 03880011.
Device is an MTi-3: Attitude Heading Reference System.
Output configuration set to:
        Packet counter: 65535 Hz
        Sample time fine: 65535 Hz
        Quaternion: 100 Hz
        Status word: 65535 Hz

Press 'm' to start measuring and 'c' to return to config mode.

main.cpp@60:ab9dad3560d3, 2015-06-12 (annotated)

Committer:
Alex Young
Date:
Fri Jun 12 11:59:16 2015 +0200
Revision:
60:ab9dad3560d3
Parent:
59:f9166c19451f
Child:
61:b9d3e7e5ba0c
Add support for EA LPC4088 QuickStart Board

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