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@59:f9166c19451f, 2015-06-12 (annotated)

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