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

Committer:
Alex Young
Date:
Fri Jun 12 13:23:26 2015 +0200
Revision:
61:b9d3e7e5ba0c
Parent:
60:ab9dad3560d3
Child:
63:138c196f0b88
Change to Apache license

Who changed what in which revision?

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