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.
Committer:
tjerkhofmeijer
Date:
Tue Oct 06 10:29:07 2015 +0200
Revision:
66:f12dec1c0c3d
Parent:
65:38c908d1b515
Child:
68:6d6dbeefd196
Added support for I2C/SPI on EA-LP4088 platform.

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
tjerkhofmeijer 64:8a0f00a064bb 24 * data from the motion tracker.
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 *
tjerkhofmeijer 64:8a0f00a064bb 31 * | Nucleo pin | MTi1 func. | MTi1 dev. pin | Used for PSEL |
tjerkhofmeijer 64:8a0f00a064bb 32 * |------------|-------------|---------------|---------------|
tjerkhofmeijer 64:8a0f00a064bb 33 * | 5V | VDD | P300-1 | Any |
tjerkhofmeijer 64:8a0f00a064bb 34 * | IORef | VDDIO | P300-2 | Any |
tjerkhofmeijer 64:8a0f00a064bb 35 * | GND | GND | P300-3 | Any |
tjerkhofmeijer 64:8a0f00a064bb 36 * | D2 | nRST | P300-5 | Any |
tjerkhofmeijer 64:8a0f00a064bb 37 * | SCL/D15 | UART_TX/SCL | P300-9 | UART / I2C |
tjerkhofmeijer 64:8a0f00a064bb 38 * | SDA/D14 | UART_RX/SDA | P300-11 | UART / I2C |
tjerkhofmeijer 64:8a0f00a064bb 39 * | D3 | DRDY | P300-15 | SPI / I2C |
tjerkhofmeijer 64:8a0f00a064bb 40 * | SCK/D13 | SCK/ADD0 | P300-17 | SPI / I2C |
tjerkhofmeijer 64:8a0f00a064bb 41 * | MISO/D12 | MISO/ADD1 | P300-19 | SPI / I2C |
tjerkhofmeijer 64:8a0f00a064bb 42 * | MOSI/D11 | MOSI/ADD2 | P300-21 | SPI / I2C |
tjerkhofmeijer 64:8a0f00a064bb 43 * | CS/D10 | nCS | P300-23 | SPI |
Alex Young 54:2e9bb1390c9c 44 *
Alex Young 54:2e9bb1390c9c 45 * Communication with the host PC is achieved using the built-in USB serial
tjerkhofmeijer 64:8a0f00a064bb 46 * bridge of the Nucleo board. Communication with the MT is achieved through
tjerkhofmeijer 64:8a0f00a064bb 47 * either the UART, I2C or SPI interface. The active interface is chosen
tjerkhofmeijer 64:8a0f00a064bb 48 * on the MT's side by use of the PSEL0 and PSEL1 switch on the MTi1
tjerkhofmeijer 64:8a0f00a064bb 49 * development board. This example needs to be built with the matching
tjerkhofmeijer 64:8a0f00a064bb 50 * MTI_USES_xxxx_INTERFACE define set (see below)
Alex Young 54:2e9bb1390c9c 51 *
Alex Young 54:2e9bb1390c9c 52 * \subsection Porting
tjerkhofmeijer 64:8a0f00a064bb 53 * To port to a different mbed platform the following pin definitions need
tjerkhofmeijer 64:8a0f00a064bb 54 * to be updated.
tjerkhofmeijer 64:8a0f00a064bb 55 * In all cases: the reset line pin
tjerkhofmeijer 64:8a0f00a064bb 56 * For UART: the serial Rx/Tx lines UART_TX and UART_RX
tjerkhofmeijer 64:8a0f00a064bb 57 * For I2C: the SCL,SDA,DRDY and address lines
tjerkhofmeijer 64:8a0f00a064bb 58 * For SPI: The SCK,MISO,MOSI,nCS and DRDY lines
Alex Young 54:2e9bb1390c9c 59 *
Alex Young 54:2e9bb1390c9c 60 * \section Firmware Operation
Alex Young 54:2e9bb1390c9c 61 * The firmware starts by initializing the serial ports used to communicate
Alex Young 54:2e9bb1390c9c 62 * with the host PC and with the MT. During the initialization the MT is held
Alex Young 54:2e9bb1390c9c 63 * in reset using the nRST input.
Alex Young 54:2e9bb1390c9c 64 *
Alex Young 54:2e9bb1390c9c 65 * Once the firmware is ready to communicate with the MT the reset line is
Alex Young 54:2e9bb1390c9c 66 * released and the firmware waits for a wakeup message from the MT. If this is
Alex Young 54:2e9bb1390c9c 67 * not received within 1 second the firmware will try to restore communication
Alex Young 54:2e9bb1390c9c 68 * with the MT using a special restore communication procedure.
Alex Young 54:2e9bb1390c9c 69 *
Alex Young 54:2e9bb1390c9c 70 * When the MT is ready for communication the firmware requests the device ID
Alex Young 54:2e9bb1390c9c 71 * of the MT, and based on this determines which type of MTi is connected.
Alex Young 54:2e9bb1390c9c 72 * If the MT is an MTi-1 then it will be configured to send inertial and
tjerkhofmeijer 56:041d3d9c300a 73 * magnetic measurement data. MTi-2 and MTi-3 devices have onboard orientation
Alex Young 54:2e9bb1390c9c 74 * estimation and will therefore be configured to provide quaternion output.
Alex Young 36:21198d933917 75 */
Alex Young 36:21198d933917 76
Alex Young 4:98f063b2e6da 77 #include "mbed.h"
Alex Young 25:01356fb59467 78 #include "rtos.h"
Alex Young 4:98f063b2e6da 79 #include "xbusparser.h"
Alex Young 11:8593ba137917 80 #include "xbusmessage.h"
Alex Young 40:b77a8c10c76d 81 #include "xsdeviceid.h"
tjerkhofmeijer 64:8a0f00a064bb 82 #include "xbusdef.h"
tjerkhofmeijer 64:8a0f00a064bb 83
tjerkhofmeijer 64:8a0f00a064bb 84 // Select communication interface to use for MTi
tjerkhofmeijer 64:8a0f00a064bb 85 #define MTI_USES_I2C_INTERFACE
tjerkhofmeijer 64:8a0f00a064bb 86
tjerkhofmeijer 64:8a0f00a064bb 87 #if !(defined(MTI_USES_I2C_INTERFACE) || defined(MTI_USES_SPI_INTERFACE) || defined(MTI_USES_UART_INTERFACE))
tjerkhofmeijer 64:8a0f00a064bb 88 #error "Must select communication interface by defining one of: MTI_USES_I2C_INTERFACE, MTI_USES_SPI_INTERFACE or MTI_USES_UART_INTERFACE"
tjerkhofmeijer 64:8a0f00a064bb 89 #endif
Alex Young 4:98f063b2e6da 90
Alex Young 59:f9166c19451f 91 #if defined(TARGET_NUCLEO_F302R8)
tjerkhofmeijer 64:8a0f00a064bb 92
tjerkhofmeijer 66:f12dec1c0c3d 93 #define PC_TX PA_2
tjerkhofmeijer 66:f12dec1c0c3d 94 #define PC_RX PA_3
tjerkhofmeijer 66:f12dec1c0c3d 95 #define MT_TX PB_9
tjerkhofmeijer 66:f12dec1c0c3d 96 #define MT_RX PB_8
tjerkhofmeijer 66:f12dec1c0c3d 97 #define MT_SDA PB_9
tjerkhofmeijer 66:f12dec1c0c3d 98 #define MT_SCL PB_8
tjerkhofmeijer 66:f12dec1c0c3d 99 #define MT_ADD0 PB_13
tjerkhofmeijer 66:f12dec1c0c3d 100 #define MT_ADD1 PB_14
tjerkhofmeijer 66:f12dec1c0c3d 101 #define MT_ADD2 PB_15
tjerkhofmeijer 66:f12dec1c0c3d 102 #define MT_MOSI PB_15
tjerkhofmeijer 66:f12dec1c0c3d 103 #define MT_MISO PB_14
tjerkhofmeijer 66:f12dec1c0c3d 104 #define MT_SCLK PB_13
tjerkhofmeijer 66:f12dec1c0c3d 105 #define MT_nCS PB_6
tjerkhofmeijer 66:f12dec1c0c3d 106 #define MT_NRESET PA_10
tjerkhofmeijer 66:f12dec1c0c3d 107 #define MT_DRDY PB_3
tjerkhofmeijer 64:8a0f00a064bb 108
Alex Young 59:f9166c19451f 109 #elif defined(TARGET_KL46Z)
tjerkhofmeijer 64:8a0f00a064bb 110
tjerkhofmeijer 66:f12dec1c0c3d 111 #define PC_TX USBTX
tjerkhofmeijer 66:f12dec1c0c3d 112 #define PC_RX USBRX
tjerkhofmeijer 66:f12dec1c0c3d 113 #define MT_TX PTE0
tjerkhofmeijer 66:f12dec1c0c3d 114 #define MT_RX PTE1
tjerkhofmeijer 66:f12dec1c0c3d 115 #define MT_SDA PTE0
tjerkhofmeijer 66:f12dec1c0c3d 116 #define MT_SCL PTE1
tjerkhofmeijer 66:f12dec1c0c3d 117 #define MT_ADD0 PTD5
tjerkhofmeijer 66:f12dec1c0c3d 118 #define MT_ADD1 PTD7
tjerkhofmeijer 66:f12dec1c0c3d 119 #define MT_ADD2 PTD6
tjerkhofmeijer 66:f12dec1c0c3d 120 #define MT_MOSI PTD6
tjerkhofmeijer 66:f12dec1c0c3d 121 #define MT_MISO PTD7
tjerkhofmeijer 66:f12dec1c0c3d 122 #define MT_SCLK PTD5
tjerkhofmeijer 66:f12dec1c0c3d 123 #define MT_nCS PTD4
tjerkhofmeijer 66:f12dec1c0c3d 124 #define MT_NRESET PTD3
tjerkhofmeijer 66:f12dec1c0c3d 125 #define MT_DRDY PTD2
tjerkhofmeijer 64:8a0f00a064bb 126
Alex Young 60:ab9dad3560d3 127 #elif defined(TARGET_LPC4088)
tjerkhofmeijer 64:8a0f00a064bb 128
tjerkhofmeijer 66:f12dec1c0c3d 129 #define PC_TX USBTX
tjerkhofmeijer 66:f12dec1c0c3d 130 #define PC_RX USBRX
tjerkhofmeijer 66:f12dec1c0c3d 131 #define MT_TX p9
tjerkhofmeijer 66:f12dec1c0c3d 132 #define MT_RX p10
tjerkhofmeijer 66:f12dec1c0c3d 133 #define MT_SDA p9
tjerkhofmeijer 66:f12dec1c0c3d 134 #define MT_SCL p10
tjerkhofmeijer 66:f12dec1c0c3d 135 #define MT_ADD0 p13
tjerkhofmeijer 66:f12dec1c0c3d 136 #define MT_ADD1 p12
tjerkhofmeijer 66:f12dec1c0c3d 137 #define MT_ADD2 p11
tjerkhofmeijer 66:f12dec1c0c3d 138 #define MT_MOSI p11
tjerkhofmeijer 66:f12dec1c0c3d 139 #define MT_MISO p12
tjerkhofmeijer 66:f12dec1c0c3d 140 #define MT_SCLK p13
tjerkhofmeijer 66:f12dec1c0c3d 141 #define MT_nCS p14
tjerkhofmeijer 66:f12dec1c0c3d 142 #define MT_NRESET p8
tjerkhofmeijer 66:f12dec1c0c3d 143 #define MT_DRDY p15
tjerkhofmeijer 64:8a0f00a064bb 144
Alex Young 57:c3c85ebb7375 145 #else
tjerkhofmeijer 66:f12dec1c0c3d 146
Alex Young 57:c3c85ebb7375 147 #error "Support for selected mbed platform has not been added."
tjerkhofmeijer 66:f12dec1c0c3d 148
Alex Young 57:c3c85ebb7375 149 #endif
Alex Young 57:c3c85ebb7375 150
Alex Young 57:c3c85ebb7375 151
Alex Young 44:b3980e8ac074 152 /*!
Alex Young 53:3891f4259901 153 * \brief Baudrate used to communicate with host PC.
Alex Young 53:3891f4259901 154 */
Alex Young 53:3891f4259901 155 #define PC_UART_BAUDRATE (921600)
Alex Young 53:3891f4259901 156
Alex Young 53:3891f4259901 157 /*!
Alex Young 44:b3980e8ac074 158 * \brief The number of items to hold in the memory pools.
Alex Young 44:b3980e8ac074 159 */
Alex Young 25:01356fb59467 160 #define MEMORY_POOL_SIZE (4)
Alex Young 44:b3980e8ac074 161 /*!
Alex Young 44:b3980e8ac074 162 * \brief The size of the queue used for device responses.
Alex Young 44:b3980e8ac074 163 * This is set to one as in typical Xbus operation each command receives a
Alex Young 44:b3980e8ac074 164 * response before the next command is sent.
Alex Young 44:b3980e8ac074 165 */
Alex Young 26:665d3624f9ab 166 #define RESPONSE_QUEUE_SIZE (1)
Alex Young 44:b3980e8ac074 167 /*!
Alex Young 44:b3980e8ac074 168 * \brief The size of the queue used for data messages.
Alex Young 44:b3980e8ac074 169 * This is set to two to allow some overlap between printing received data to
Alex Young 44:b3980e8ac074 170 * the PC serial port and the reception of the subsequent data packet. In
Alex Young 44:b3980e8ac074 171 * more complex applications it might be necessary to increase this if
Alex Young 44:b3980e8ac074 172 * message processing might occasionally require more time than normal.
Alex Young 44:b3980e8ac074 173 */
Alex Young 43:470c019246e4 174 #define DATA_QUEUE_SIZE (2)
Alex Young 44:b3980e8ac074 175 /*!
Alex Young 49:38ecfbff5391 176 * \brief The maximum size of an xbus message supported by the application.
Alex Young 44:b3980e8ac074 177 * This is the size of the message buffers in the message data memory pool.
Alex Young 44:b3980e8ac074 178 */
Alex Young 25:01356fb59467 179 #define MAX_XBUS_DATA_SIZE (128)
Alex Young 25:01356fb59467 180
Alex Young 44:b3980e8ac074 181 /*! \brief Serial port for communication with the host PC. */
Alex Young 57:c3c85ebb7375 182 static Serial pc(PC_TX, PC_RX);
tjerkhofmeijer 64:8a0f00a064bb 183
tjerkhofmeijer 64:8a0f00a064bb 184 #if defined(MTI_USES_I2C_INTERFACE)
tjerkhofmeijer 64:8a0f00a064bb 185 /*!
tjerkhofmeijer 64:8a0f00a064bb 186 * \brief I2C master used for communication with the MT.
tjerkhofmeijer 64:8a0f00a064bb 187 */
tjerkhofmeijer 64:8a0f00a064bb 188 static I2C mt(MT_SDA, MT_SCL);
tjerkhofmeijer 64:8a0f00a064bb 189 static DigitalOut add0(MT_ADD0);
tjerkhofmeijer 64:8a0f00a064bb 190 static DigitalOut add1(MT_ADD1);
tjerkhofmeijer 64:8a0f00a064bb 191 static DigitalOut add2(MT_ADD2);
tjerkhofmeijer 64:8a0f00a064bb 192
tjerkhofmeijer 64:8a0f00a064bb 193 #elif defined(MTI_USES_SPI_INTERFACE)
tjerkhofmeijer 64:8a0f00a064bb 194 /*! \brief SPI master used for communication with the MT. */
tjerkhofmeijer 64:8a0f00a064bb 195 static SPI mt(MT_MOSI, MT_MISO, MT_SCLK);
tjerkhofmeijer 64:8a0f00a064bb 196
tjerkhofmeijer 64:8a0f00a064bb 197 /*! \brief Chip select line for the MT. */
tjerkhofmeijer 64:8a0f00a064bb 198 static DigitalOut cs(MT_nCS, 1);
tjerkhofmeijer 64:8a0f00a064bb 199
tjerkhofmeijer 64:8a0f00a064bb 200 #elif defined(MTI_USES_UART_INTERFACE)
Alex Young 58:db60ef0a0d16 201 /*!
Alex Young 58:db60ef0a0d16 202 * \brief Serial port for communication with the MT.
Alex Young 58:db60ef0a0d16 203 *
Alex Young 58:db60ef0a0d16 204 * We use a RawSerial port as the Stream inteface used by the regular
Alex Young 58:db60ef0a0d16 205 * Serial class can have problems with the RTOS when using interrupts.
Alex Young 58:db60ef0a0d16 206 */
Alex Young 58:db60ef0a0d16 207 static RawSerial mt(MT_TX, MT_RX);
tjerkhofmeijer 64:8a0f00a064bb 208 #endif
tjerkhofmeijer 64:8a0f00a064bb 209
tjerkhofmeijer 64:8a0f00a064bb 210 #if defined(MTI_USES_I2C_INTERFACE) || defined(MTI_USES_SPI_INTERFACE)
tjerkhofmeijer 64:8a0f00a064bb 211 /*!
tjerkhofmeijer 64:8a0f00a064bb 212 * \brief Interrput line used by MT to signal that data is available.
tjerkhofmeijer 64:8a0f00a064bb 213 */
tjerkhofmeijer 64:8a0f00a064bb 214 static InterruptIn drdy(MT_DRDY);
tjerkhofmeijer 64:8a0f00a064bb 215 #endif
tjerkhofmeijer 64:8a0f00a064bb 216
Alex Young 35:7e519b88c610 217 /*!
Alex Young 35:7e519b88c610 218 * \brief MT reset line.
Alex Young 35:7e519b88c610 219 *
Alex Young 35:7e519b88c610 220 * MT is held in reset on startup.
Alex Young 35:7e519b88c610 221 */
Alex Young 57:c3c85ebb7375 222 static DigitalOut mtReset(MT_NRESET, 0);
Alex Young 44:b3980e8ac074 223 /*! \brief XbusParser used to parse incoming Xbus messages from the MT. */
Alex Young 4:98f063b2e6da 224 static XbusParser* xbusParser;
Alex Young 25:01356fb59467 225
Alex Young 44:b3980e8ac074 226 /*!
Alex Young 44:b3980e8ac074 227 * \brief Memory pool used for storing Xbus messages when passing them
Alex Young 44:b3980e8ac074 228 * to the main thread.
Alex Young 44:b3980e8ac074 229 */
Alex Young 25:01356fb59467 230 MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool;
Alex Young 44:b3980e8ac074 231 /*!
Alex Young 44:b3980e8ac074 232 * \brief Memory pool used for storing the payload of Xbus messages.
Alex Young 44:b3980e8ac074 233 */
Alex Young 25:01356fb59467 234 MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool;
Alex Young 44:b3980e8ac074 235 /*!
Alex Young 44:b3980e8ac074 236 * \brief Queue used to pass data messages to the main thread for processing.
Alex Young 44:b3980e8ac074 237 */
Alex Young 44:b3980e8ac074 238 Queue<XbusMessage, DATA_QUEUE_SIZE> g_dataQueue;
Alex Young 44:b3980e8ac074 239 /*!
Alex Young 44:b3980e8ac074 240 * \brief Queue used for passing all other messages to the main thread for processing.
Alex Young 44:b3980e8ac074 241 */
Alex Young 26:665d3624f9ab 242 Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue;
Alex Young 4:98f063b2e6da 243
Alex Young 44:b3980e8ac074 244 /*!
Alex Young 44:b3980e8ac074 245 * \brief Allocate message data buffer from the message data pool.
Alex Young 44:b3980e8ac074 246 */
Alex Young 25:01356fb59467 247 static void* allocateMessageData(size_t bufSize)
Alex Young 4:98f063b2e6da 248 {
Alex Young 25:01356fb59467 249 return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL;
Alex Young 25:01356fb59467 250 }
Alex Young 25:01356fb59467 251
Alex Young 44:b3980e8ac074 252 /*!
Alex Young 44:b3980e8ac074 253 * \brief Deallocate message data previously allocated from the message
Alex Young 44:b3980e8ac074 254 * data pool.
Alex Young 44:b3980e8ac074 255 */
Alex Young 25:01356fb59467 256 static void deallocateMessageData(void const* buffer)
Alex Young 25:01356fb59467 257 {
Alex Young 25:01356fb59467 258 g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer);
Alex Young 4:98f063b2e6da 259 }
Alex Young 4:98f063b2e6da 260
tjerkhofmeijer 64:8a0f00a064bb 261 #if defined(MTI_USES_I2C_INTERFACE)
tjerkhofmeijer 64:8a0f00a064bb 262 #define MTI_I2C_ADDRESS (0x1D << 1)
tjerkhofmeijer 64:8a0f00a064bb 263 static void readData(uint8_t pipe, uint16_t dataLength)
tjerkhofmeijer 64:8a0f00a064bb 264 {
tjerkhofmeijer 64:8a0f00a064bb 265 const int preambleLength = 2;
tjerkhofmeijer 64:8a0f00a064bb 266 uint8_t* buf = (uint8_t*)allocateMessageData(dataLength+preambleLength);
tjerkhofmeijer 64:8a0f00a064bb 267 if (buf)
tjerkhofmeijer 64:8a0f00a064bb 268 {
tjerkhofmeijer 64:8a0f00a064bb 269 buf[0] = XBUS_PREAMBLE;
tjerkhofmeijer 64:8a0f00a064bb 270 buf[1] = XBUS_MASTERDEVICE;
tjerkhofmeijer 64:8a0f00a064bb 271 mt.write(MTI_I2C_ADDRESS, (char*)&pipe, sizeof(pipe), true);
tjerkhofmeijer 64:8a0f00a064bb 272 mt.read(MTI_I2C_ADDRESS, (char*)buf+preambleLength, dataLength);
tjerkhofmeijer 64:8a0f00a064bb 273 XbusParser_parseBuffer(xbusParser, buf, dataLength+preambleLength);
tjerkhofmeijer 64:8a0f00a064bb 274 deallocateMessageData(buf);
tjerkhofmeijer 64:8a0f00a064bb 275 }
tjerkhofmeijer 64:8a0f00a064bb 276 }
tjerkhofmeijer 64:8a0f00a064bb 277 static void mtInterruptHandler(void)
tjerkhofmeijer 64:8a0f00a064bb 278 {
tjerkhofmeijer 64:8a0f00a064bb 279 while (true)
tjerkhofmeijer 64:8a0f00a064bb 280 {
tjerkhofmeijer 64:8a0f00a064bb 281 uint8_t opcode = XBUS_PIPE_STATUS;
tjerkhofmeijer 64:8a0f00a064bb 282 uint8_t status[4];
tjerkhofmeijer 64:8a0f00a064bb 283 mt.write(MTI_I2C_ADDRESS, (char*)&opcode, sizeof(opcode), true);
tjerkhofmeijer 64:8a0f00a064bb 284 mt.read(MTI_I2C_ADDRESS, (char*)status, sizeof(status));
tjerkhofmeijer 64:8a0f00a064bb 285
tjerkhofmeijer 64:8a0f00a064bb 286 uint16_t notificationSize = status[0] | (status[1] << 8);
tjerkhofmeijer 64:8a0f00a064bb 287 uint16_t measurementSize = status[2] | (status[3] <<8);
tjerkhofmeijer 64:8a0f00a064bb 288
tjerkhofmeijer 64:8a0f00a064bb 289 if (notificationSize)
tjerkhofmeijer 64:8a0f00a064bb 290 {
tjerkhofmeijer 64:8a0f00a064bb 291 readData(XBUS_NOTIFICATION_PIPE, notificationSize);
tjerkhofmeijer 64:8a0f00a064bb 292 }
tjerkhofmeijer 64:8a0f00a064bb 293 else if (measurementSize)
tjerkhofmeijer 64:8a0f00a064bb 294 {
tjerkhofmeijer 64:8a0f00a064bb 295 readData(XBUS_MEASUREMENT_PIPE, measurementSize);
tjerkhofmeijer 64:8a0f00a064bb 296 }
tjerkhofmeijer 64:8a0f00a064bb 297 else
tjerkhofmeijer 64:8a0f00a064bb 298 break; // No more data available to read.
tjerkhofmeijer 64:8a0f00a064bb 299 }
tjerkhofmeijer 64:8a0f00a064bb 300 }
tjerkhofmeijer 64:8a0f00a064bb 301
tjerkhofmeijer 64:8a0f00a064bb 302 static void configureMtCommunicationInterface(void)
tjerkhofmeijer 64:8a0f00a064bb 303 {
tjerkhofmeijer 64:8a0f00a064bb 304 mt.frequency(400000);
tjerkhofmeijer 64:8a0f00a064bb 305 //Use the addX pins to configure I2C address 0x1D
tjerkhofmeijer 64:8a0f00a064bb 306 add0.write(0);
tjerkhofmeijer 64:8a0f00a064bb 307 add1.write(0);
tjerkhofmeijer 64:8a0f00a064bb 308 add2.write(0);
tjerkhofmeijer 64:8a0f00a064bb 309 drdy.rise(&mtInterruptHandler);
tjerkhofmeijer 64:8a0f00a064bb 310 }
tjerkhofmeijer 64:8a0f00a064bb 311
tjerkhofmeijer 64:8a0f00a064bb 312 /*!
tjerkhofmeijer 64:8a0f00a064bb 313 * \brief Send a message to the MT
tjerkhofmeijer 64:8a0f00a064bb 314 *
tjerkhofmeijer 64:8a0f00a064bb 315 * This function formats the message data and writes this to the MT I2C
tjerkhofmeijer 64:8a0f00a064bb 316 * interface. It does not wait for any response.
tjerkhofmeijer 64:8a0f00a064bb 317 */
tjerkhofmeijer 64:8a0f00a064bb 318 static void sendMessage(XbusMessage const* m)
tjerkhofmeijer 64:8a0f00a064bb 319 {
tjerkhofmeijer 64:8a0f00a064bb 320 uint8_t buf[64];
tjerkhofmeijer 64:8a0f00a064bb 321 size_t rawLength = XbusMessage_format(buf, m, XLLF_I2c);
tjerkhofmeijer 64:8a0f00a064bb 322 mt.write(MTI_I2C_ADDRESS, (char*)buf, rawLength);
tjerkhofmeijer 64:8a0f00a064bb 323 }
tjerkhofmeijer 64:8a0f00a064bb 324 #elif defined(MTI_USES_SPI_INTERFACE)
tjerkhofmeijer 64:8a0f00a064bb 325 static void sendOpcode(uint8_t opcode)
tjerkhofmeijer 64:8a0f00a064bb 326 {
tjerkhofmeijer 64:8a0f00a064bb 327 mt.write(opcode);
tjerkhofmeijer 64:8a0f00a064bb 328 for (int filler = 0; filler < 3; ++filler)
tjerkhofmeijer 64:8a0f00a064bb 329 {
tjerkhofmeijer 64:8a0f00a064bb 330 mt.write(filler);
tjerkhofmeijer 64:8a0f00a064bb 331 }
tjerkhofmeijer 64:8a0f00a064bb 332 }
tjerkhofmeijer 64:8a0f00a064bb 333
tjerkhofmeijer 64:8a0f00a064bb 334 static void readData(uint8_t pipe, uint16_t dataLength)
tjerkhofmeijer 64:8a0f00a064bb 335 {
tjerkhofmeijer 64:8a0f00a064bb 336 const int preambleLength = 2;
tjerkhofmeijer 64:8a0f00a064bb 337 uint8_t* buf = (uint8_t*)allocateMessageData(dataLength+preambleLength);
tjerkhofmeijer 64:8a0f00a064bb 338 if (buf)
tjerkhofmeijer 64:8a0f00a064bb 339 {
tjerkhofmeijer 64:8a0f00a064bb 340 uint8_t* dptr = buf;
tjerkhofmeijer 64:8a0f00a064bb 341 *dptr++ = XBUS_PREAMBLE;
tjerkhofmeijer 64:8a0f00a064bb 342 *dptr++ = XBUS_MASTERDEVICE;
tjerkhofmeijer 64:8a0f00a064bb 343 cs = 0;
tjerkhofmeijer 64:8a0f00a064bb 344 sendOpcode(pipe);
tjerkhofmeijer 64:8a0f00a064bb 345 for (int i = 0; i < dataLength; ++i)
tjerkhofmeijer 64:8a0f00a064bb 346 {
tjerkhofmeijer 64:8a0f00a064bb 347 *dptr++ = mt.write(0);
tjerkhofmeijer 64:8a0f00a064bb 348 }
tjerkhofmeijer 64:8a0f00a064bb 349 cs = 1;
tjerkhofmeijer 64:8a0f00a064bb 350 XbusParser_parseBuffer(xbusParser, buf, dptr - buf);
tjerkhofmeijer 64:8a0f00a064bb 351 deallocateMessageData(buf);
tjerkhofmeijer 64:8a0f00a064bb 352 }
tjerkhofmeijer 64:8a0f00a064bb 353 }
tjerkhofmeijer 64:8a0f00a064bb 354 static void mtInterruptHandler(void)
tjerkhofmeijer 64:8a0f00a064bb 355 {
tjerkhofmeijer 64:8a0f00a064bb 356 while (true)
tjerkhofmeijer 64:8a0f00a064bb 357 {
tjerkhofmeijer 64:8a0f00a064bb 358 cs = 0;
tjerkhofmeijer 64:8a0f00a064bb 359 sendOpcode(XBUS_PIPE_STATUS);
tjerkhofmeijer 64:8a0f00a064bb 360 uint8_t status[4];
tjerkhofmeijer 64:8a0f00a064bb 361 for (int i = 0; i < sizeof(status); ++i)
tjerkhofmeijer 64:8a0f00a064bb 362 {
tjerkhofmeijer 64:8a0f00a064bb 363 status[i] = mt.write(0);
tjerkhofmeijer 64:8a0f00a064bb 364 }
tjerkhofmeijer 64:8a0f00a064bb 365 cs = 1;
tjerkhofmeijer 64:8a0f00a064bb 366
tjerkhofmeijer 64:8a0f00a064bb 367 uint16_t notificationSize = status[0] | (status[1] << 8);
tjerkhofmeijer 64:8a0f00a064bb 368 uint16_t measurementSize = status[2] | (status[3] <<8);
tjerkhofmeijer 64:8a0f00a064bb 369
tjerkhofmeijer 64:8a0f00a064bb 370 if (notificationSize)
tjerkhofmeijer 64:8a0f00a064bb 371 {
tjerkhofmeijer 64:8a0f00a064bb 372 readData(XBUS_NOTIFICATION_PIPE, notificationSize);
tjerkhofmeijer 64:8a0f00a064bb 373 }
tjerkhofmeijer 64:8a0f00a064bb 374 else if (measurementSize)
tjerkhofmeijer 64:8a0f00a064bb 375 {
tjerkhofmeijer 64:8a0f00a064bb 376 readData(XBUS_MEASUREMENT_PIPE, measurementSize);
tjerkhofmeijer 64:8a0f00a064bb 377 }
tjerkhofmeijer 64:8a0f00a064bb 378 else
tjerkhofmeijer 64:8a0f00a064bb 379 break; // No more data available to read.
tjerkhofmeijer 64:8a0f00a064bb 380 }
tjerkhofmeijer 64:8a0f00a064bb 381 }
tjerkhofmeijer 64:8a0f00a064bb 382
tjerkhofmeijer 64:8a0f00a064bb 383 static void configureMtCommunicationInterface(void)
tjerkhofmeijer 64:8a0f00a064bb 384 {
tjerkhofmeijer 64:8a0f00a064bb 385 mt.frequency(1000000);
tjerkhofmeijer 64:8a0f00a064bb 386 mt.format(8, 3);
tjerkhofmeijer 64:8a0f00a064bb 387 drdy.rise(&mtInterruptHandler);
tjerkhofmeijer 64:8a0f00a064bb 388 }
tjerkhofmeijer 64:8a0f00a064bb 389
tjerkhofmeijer 64:8a0f00a064bb 390 /*!
tjerkhofmeijer 64:8a0f00a064bb 391 * \brief Send a message to the MT
tjerkhofmeijer 64:8a0f00a064bb 392 *
tjerkhofmeijer 64:8a0f00a064bb 393 * This function formats the message data and writes this to the MT SPI
tjerkhofmeijer 64:8a0f00a064bb 394 * interface. It does not wait for any response.
tjerkhofmeijer 64:8a0f00a064bb 395 */
tjerkhofmeijer 64:8a0f00a064bb 396 static void sendMessage(XbusMessage const* m)
tjerkhofmeijer 64:8a0f00a064bb 397 {
tjerkhofmeijer 64:8a0f00a064bb 398 uint8_t buf[64];
tjerkhofmeijer 64:8a0f00a064bb 399 size_t rawLength = XbusMessage_format(buf, m, XLLF_Spi);
tjerkhofmeijer 64:8a0f00a064bb 400 cs = 0;
tjerkhofmeijer 64:8a0f00a064bb 401 for (int i = 0; i < rawLength; ++i)
tjerkhofmeijer 64:8a0f00a064bb 402 {
tjerkhofmeijer 64:8a0f00a064bb 403 mt.write(buf[i]);
tjerkhofmeijer 64:8a0f00a064bb 404 }
tjerkhofmeijer 64:8a0f00a064bb 405 cs = 1;
tjerkhofmeijer 64:8a0f00a064bb 406 }
tjerkhofmeijer 64:8a0f00a064bb 407 #elif defined(MTI_USES_UART_INTERFACE)
Alex Young 44:b3980e8ac074 408 /*!
Alex Young 44:b3980e8ac074 409 * \brief RX Interrupt handler for the MT serial port.
Alex Young 44:b3980e8ac074 410 *
Alex Young 44:b3980e8ac074 411 * Passes received data to an XbusParser to extract messages.
Alex Young 44:b3980e8ac074 412 */
Alex Young 4:98f063b2e6da 413 static void mtLowLevelHandler(void)
Alex Young 4:98f063b2e6da 414 {
Alex Young 4:98f063b2e6da 415 while (mt.readable())
Alex Young 4:98f063b2e6da 416 {
Alex Young 4:98f063b2e6da 417 XbusParser_parseByte(xbusParser, mt.getc());
Alex Young 4:98f063b2e6da 418 }
Alex Young 4:98f063b2e6da 419 }
Alex Young 4:98f063b2e6da 420
Alex Young 44:b3980e8ac074 421 /*!
tjerkhofmeijer 64:8a0f00a064bb 422 * \brief Configure the serial port used for communication with the
tjerkhofmeijer 64:8a0f00a064bb 423 * motion tracker.
tjerkhofmeijer 64:8a0f00a064bb 424 */
tjerkhofmeijer 64:8a0f00a064bb 425 static void configureMtCommunicationInterface(void)
tjerkhofmeijer 64:8a0f00a064bb 426 {
tjerkhofmeijer 64:8a0f00a064bb 427 mt.baud(115200);
tjerkhofmeijer 64:8a0f00a064bb 428 mt.format(8, Serial::None, 1);
tjerkhofmeijer 64:8a0f00a064bb 429 mt.attach(mtLowLevelHandler, Serial::RxIrq);
tjerkhofmeijer 64:8a0f00a064bb 430 }
tjerkhofmeijer 64:8a0f00a064bb 431
tjerkhofmeijer 64:8a0f00a064bb 432 /*!
Alex Young 44:b3980e8ac074 433 * \brief Send a message to the MT
Alex Young 44:b3980e8ac074 434 *
Alex Young 44:b3980e8ac074 435 * This function formats the message data and writes this to the MT serial
Alex Young 44:b3980e8ac074 436 * port. It does not wait for any response.
Alex Young 44:b3980e8ac074 437 */
Alex Young 34:3d7a6519a256 438 static void sendMessage(XbusMessage const* m)
Alex Young 11:8593ba137917 439 {
Alex Young 26:665d3624f9ab 440 uint8_t buf[64];
tjerkhofmeijer 64:8a0f00a064bb 441 size_t rawLength = XbusMessage_format(buf, m, XLLF_Uart);
Alex Young 11:8593ba137917 442 for (size_t i = 0; i < rawLength; ++i)
Alex Young 11:8593ba137917 443 {
Alex Young 11:8593ba137917 444 mt.putc(buf[i]);
Alex Young 11:8593ba137917 445 }
Alex Young 34:3d7a6519a256 446 }
tjerkhofmeijer 64:8a0f00a064bb 447 #endif
tjerkhofmeijer 64:8a0f00a064bb 448
Alex Young 34:3d7a6519a256 449
Alex Young 44:b3980e8ac074 450 /*!
Alex Young 44:b3980e8ac074 451 * \brief Send a message to the MT and wait for a response.
Alex Young 44:b3980e8ac074 452 * \returns Response message from the MT, or NULL is no response received
Alex Young 44:b3980e8ac074 453 * within 500ms.
Alex Young 44:b3980e8ac074 454 *
Alex Young 44:b3980e8ac074 455 * Blocking behaviour is implemented by waiting for a response to be written
Alex Young 44:b3980e8ac074 456 * to the response queue by the XbusParser.
Alex Young 44:b3980e8ac074 457 */
Alex Young 34:3d7a6519a256 458 static XbusMessage const* doTransaction(XbusMessage const* m)
Alex Young 34:3d7a6519a256 459 {
Alex Young 34:3d7a6519a256 460 sendMessage(m);
Alex Young 26:665d3624f9ab 461
Alex Young 26:665d3624f9ab 462 osEvent ev = g_responseQueue.get(500);
Alex Young 26:665d3624f9ab 463 return ev.status == osEventMessage ? (XbusMessage*)ev.value.p : NULL;
Alex Young 26:665d3624f9ab 464 }
Alex Young 26:665d3624f9ab 465
Alex Young 31:ce1ea9ae861e 466 /*!
Alex Young 31:ce1ea9ae861e 467 * \brief RAII object to manage message memory deallocation.
Alex Young 31:ce1ea9ae861e 468 *
Alex Young 49:38ecfbff5391 469 * Will automatically free the memory used by an XbusMessage when going out
Alex Young 31:ce1ea9ae861e 470 * of scope.
Alex Young 31:ce1ea9ae861e 471 */
Alex Young 31:ce1ea9ae861e 472 class XbusMessageMemoryManager
Alex Young 26:665d3624f9ab 473 {
Alex Young 31:ce1ea9ae861e 474 public:
Alex Young 31:ce1ea9ae861e 475 XbusMessageMemoryManager(XbusMessage const* message)
Alex Young 31:ce1ea9ae861e 476 : m_message(message)
Alex Young 31:ce1ea9ae861e 477 {
Alex Young 31:ce1ea9ae861e 478 }
Alex Young 31:ce1ea9ae861e 479
Alex Young 31:ce1ea9ae861e 480 ~XbusMessageMemoryManager()
Alex Young 31:ce1ea9ae861e 481 {
Alex Young 31:ce1ea9ae861e 482 if (m_message)
Alex Young 31:ce1ea9ae861e 483 {
Alex Young 31:ce1ea9ae861e 484 if (m_message->data)
Alex Young 31:ce1ea9ae861e 485 deallocateMessageData(m_message->data);
Alex Young 31:ce1ea9ae861e 486 g_messagePool.free(const_cast<XbusMessage*>(m_message));
Alex Young 31:ce1ea9ae861e 487 }
Alex Young 31:ce1ea9ae861e 488 }
Alex Young 31:ce1ea9ae861e 489
Alex Young 31:ce1ea9ae861e 490 private:
Alex Young 31:ce1ea9ae861e 491 XbusMessage const* m_message;
Alex Young 31:ce1ea9ae861e 492 };
Alex Young 26:665d3624f9ab 493
Alex Young 44:b3980e8ac074 494 /*!
Alex Young 44:b3980e8ac074 495 * \brief Dump information from a message to the PC serial port.
Alex Young 44:b3980e8ac074 496 */
Alex Young 29:d9310e7b58b5 497 static void dumpResponse(XbusMessage const* response)
Alex Young 29:d9310e7b58b5 498 {
Alex Young 29:d9310e7b58b5 499 switch (response->mid)
Alex Young 29:d9310e7b58b5 500 {
Alex Young 29:d9310e7b58b5 501 case XMID_GotoConfigAck:
Alex Young 52:e2197b38c029 502 pc.printf("Device went to config mode.\r\n");
Alex Young 29:d9310e7b58b5 503 break;
Alex Young 29:d9310e7b58b5 504
Alex Young 29:d9310e7b58b5 505 case XMID_Error:
Alex Young 29:d9310e7b58b5 506 pc.printf("Device error!");
Alex Young 29:d9310e7b58b5 507 break;
Alex Young 29:d9310e7b58b5 508
Alex Young 29:d9310e7b58b5 509 default:
Alex Young 52:e2197b38c029 510 pc.printf("Received response MID=%X, length=%d\r\n", response->mid, response->length);
Alex Young 29:d9310e7b58b5 511 break;
Alex Young 29:d9310e7b58b5 512 }
Alex Young 29:d9310e7b58b5 513 }
Alex Young 29:d9310e7b58b5 514
Alex Young 44:b3980e8ac074 515 /*!
Alex Young 44:b3980e8ac074 516 * \brief Send a command to the MT and wait for a response.
Alex Young 44:b3980e8ac074 517 * \param cmdId The XsMessageId of the command to send.
Alex Young 44:b3980e8ac074 518 *
Alex Young 44:b3980e8ac074 519 * Commands are simple messages without and payload data.
Alex Young 44:b3980e8ac074 520 */
Alex Young 26:665d3624f9ab 521 static void sendCommand(XsMessageId cmdId)
Alex Young 26:665d3624f9ab 522 {
Alex Young 26:665d3624f9ab 523 XbusMessage m = {cmdId};
Alex Young 26:665d3624f9ab 524 XbusMessage const* response = doTransaction(&m);
Alex Young 31:ce1ea9ae861e 525 XbusMessageMemoryManager janitor(response);
Alex Young 26:665d3624f9ab 526
Alex Young 26:665d3624f9ab 527 if (response)
Alex Young 26:665d3624f9ab 528 {
Alex Young 29:d9310e7b58b5 529 dumpResponse(response);
Alex Young 26:665d3624f9ab 530 }
Alex Young 26:665d3624f9ab 531 else
Alex Young 26:665d3624f9ab 532 {
Alex Young 52:e2197b38c029 533 pc.printf("Timeout waiting for response.\r\n");
Alex Young 26:665d3624f9ab 534 }
Alex Young 11:8593ba137917 535 }
Alex Young 11:8593ba137917 536
Alex Young 44:b3980e8ac074 537 /*!
Alex Young 44:b3980e8ac074 538 * \brief Handle a command from the PC
Alex Young 44:b3980e8ac074 539 *
Alex Young 44:b3980e8ac074 540 * The example application supports single character commands from the host
Alex Young 44:b3980e8ac074 541 * PC to switch between configuration and measurement modes.
Alex Young 44:b3980e8ac074 542 */
Alex Young 11:8593ba137917 543 static void handlePcCommand(char cmd)
Alex Young 11:8593ba137917 544 {
Alex Young 11:8593ba137917 545 switch (cmd)
Alex Young 11:8593ba137917 546 {
Alex Young 11:8593ba137917 547 case 'c':
Alex Young 11:8593ba137917 548 sendCommand(XMID_GotoConfig);
Alex Young 11:8593ba137917 549 break;
Alex Young 11:8593ba137917 550
Alex Young 11:8593ba137917 551 case 'm':
Alex Young 11:8593ba137917 552 sendCommand(XMID_GotoMeasurement);
Alex Young 11:8593ba137917 553 break;
Alex Young 11:8593ba137917 554 }
Alex Young 11:8593ba137917 555 }
Alex Young 11:8593ba137917 556
Alex Young 44:b3980e8ac074 557 /*!
Alex Young 44:b3980e8ac074 558 * \brief XbusParser callback function to handle received messages.
Alex Young 44:b3980e8ac074 559 * \param message Pointer to the last received message.
Alex Young 44:b3980e8ac074 560 *
Alex Young 44:b3980e8ac074 561 * In this example received messages are copied into one of two message
Alex Young 44:b3980e8ac074 562 * queues for later handling by the main thread. Data messages are put
Alex Young 49:38ecfbff5391 563 * in one queue, while all other responses are placed in the second queue.
Alex Young 44:b3980e8ac074 564 * This is done so that data and other messages can be handled separately
Alex Young 44:b3980e8ac074 565 * by the application code.
Alex Young 44:b3980e8ac074 566 */
Alex Young 24:2cc49dc854e3 567 static void mtMessageHandler(struct XbusMessage const* message)
Alex Young 4:98f063b2e6da 568 {
Alex Young 43:470c019246e4 569 XbusMessage* m = g_messagePool.alloc();
Alex Young 43:470c019246e4 570 if (m)
Alex Young 7:c913a7cd5231 571 {
Alex Young 43:470c019246e4 572 memcpy(m, message, sizeof(XbusMessage));
Alex Young 43:470c019246e4 573 if (message->mid == XMID_MtData2)
Alex Young 43:470c019246e4 574 {
Alex Young 43:470c019246e4 575 g_dataQueue.put(m);
Alex Young 43:470c019246e4 576 }
Alex Young 43:470c019246e4 577 else
Alex Young 43:470c019246e4 578 {
Alex Young 43:470c019246e4 579 g_responseQueue.put(m);
Alex Young 43:470c019246e4 580 }
Alex Young 7:c913a7cd5231 581 }
Alex Young 43:470c019246e4 582 else if (message->data)
Alex Young 7:c913a7cd5231 583 {
Alex Young 43:470c019246e4 584 deallocateMessageData(message->data);
Alex Young 25:01356fb59467 585 }
Alex Young 4:98f063b2e6da 586 }
Alex Young 4:98f063b2e6da 587
Alex Young 44:b3980e8ac074 588 /*!
tjerkhofmeijer 64:8a0f00a064bb 589 * \brief Configure the serial port used to communicate with the host PC.
Alex Young 44:b3980e8ac074 590 */
tjerkhofmeijer 64:8a0f00a064bb 591 static void configurePcInterface(void)
Alex Young 4:98f063b2e6da 592 {
Alex Young 53:3891f4259901 593 pc.baud(PC_UART_BAUDRATE);
Alex Young 55:9a2d6f947f0d 594 pc.format(8, Serial::None, 1);
Alex Young 4:98f063b2e6da 595 }
Alex Young 4:98f063b2e6da 596
Alex Young 44:b3980e8ac074 597 /*!
Alex Young 44:b3980e8ac074 598 * \brief Read the device ID of the motion tracker.
Alex Young 44:b3980e8ac074 599 */
Alex Young 29:d9310e7b58b5 600 static uint32_t readDeviceId(void)
Alex Young 29:d9310e7b58b5 601 {
Alex Young 29:d9310e7b58b5 602 XbusMessage reqDid = {XMID_ReqDid};
Alex Young 29:d9310e7b58b5 603 XbusMessage const* didRsp = doTransaction(&reqDid);
Alex Young 31:ce1ea9ae861e 604 XbusMessageMemoryManager janitor(didRsp);
Alex Young 29:d9310e7b58b5 605 uint32_t deviceId = 0;
Alex Young 29:d9310e7b58b5 606 if (didRsp)
Alex Young 29:d9310e7b58b5 607 {
Alex Young 29:d9310e7b58b5 608 if (didRsp->mid == XMID_DeviceId)
Alex Young 29:d9310e7b58b5 609 {
Alex Young 29:d9310e7b58b5 610 deviceId = *(uint32_t*)didRsp->data;
Alex Young 29:d9310e7b58b5 611 }
Alex Young 29:d9310e7b58b5 612 }
Alex Young 29:d9310e7b58b5 613 return deviceId;
Alex Young 29:d9310e7b58b5 614 }
Alex Young 29:d9310e7b58b5 615
Alex Young 44:b3980e8ac074 616 /*!
Alex Young 44:b3980e8ac074 617 * \brief Sets MT output configuration.
Alex Young 44:b3980e8ac074 618 * \param conf Pointer to an array of OutputConfiguration elements.
Alex Young 44:b3980e8ac074 619 * \param elements The number of elements in the configuration array.
Alex Young 44:b3980e8ac074 620 *
Alex Young 44:b3980e8ac074 621 * The response from the device indicates the actual values that will
Alex Young 44:b3980e8ac074 622 * be used by the motion tracker. These may differ from the requested
Alex Young 44:b3980e8ac074 623 * parameters as the motion tracker validates the requested parameters
Alex Young 44:b3980e8ac074 624 * before applying them.
Alex Young 44:b3980e8ac074 625 */
Alex Young 32:fafe0f42d82b 626 static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements)
Alex Young 29:d9310e7b58b5 627 {
Alex Young 32:fafe0f42d82b 628 XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf};
Alex Young 32:fafe0f42d82b 629 XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg);
Alex Young 32:fafe0f42d82b 630 XbusMessageMemoryManager janitor(outputConfRsp);
Alex Young 32:fafe0f42d82b 631 if (outputConfRsp)
Alex Young 29:d9310e7b58b5 632 {
Alex Young 32:fafe0f42d82b 633 if (outputConfRsp->mid == XMID_OutputConfig)
Alex Young 29:d9310e7b58b5 634 {
Alex Young 52:e2197b38c029 635 pc.printf("Output configuration set to:\r\n");
Alex Young 32:fafe0f42d82b 636 OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data;
Alex Young 32:fafe0f42d82b 637 for (int i = 0; i < outputConfRsp->length; ++i)
Alex Young 32:fafe0f42d82b 638 {
Alex Young 52:e2197b38c029 639 pc.printf("\t%s: %d Hz\r\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 32:fafe0f42d82b 640 ++conf;
Alex Young 32:fafe0f42d82b 641 }
Alex Young 32:fafe0f42d82b 642 return true;
Alex Young 29:d9310e7b58b5 643 }
Alex Young 29:d9310e7b58b5 644 else
Alex Young 29:d9310e7b58b5 645 {
Alex Young 32:fafe0f42d82b 646 dumpResponse(outputConfRsp);
Alex Young 29:d9310e7b58b5 647 }
Alex Young 32:fafe0f42d82b 648 }
Alex Young 32:fafe0f42d82b 649 else
Alex Young 32:fafe0f42d82b 650 {
Alex Young 52:e2197b38c029 651 pc.printf("Failed to set output configuration.\r\n");
Alex Young 32:fafe0f42d82b 652 }
Alex Young 32:fafe0f42d82b 653 return false;
Alex Young 32:fafe0f42d82b 654 }
Alex Young 29:d9310e7b58b5 655
Alex Young 44:b3980e8ac074 656 /*!
Alex Young 44:b3980e8ac074 657 * \brief Sets the motion tracker output configuration based on the function
Alex Young 44:b3980e8ac074 658 * of the attached device.
Alex Young 44:b3980e8ac074 659 *
Alex Young 44:b3980e8ac074 660 * The output configuration depends on the type of MTi-1 device connected.
Alex Young 49:38ecfbff5391 661 * An MTI-1 (IMU) device does not have an onboard orientation filter so
Alex Young 44:b3980e8ac074 662 * cannot output quaternion data, only inertial and magnetic measurement
Alex Young 44:b3980e8ac074 663 * data.
Alex Young 44:b3980e8ac074 664 * MTi-2 and MTi-3 devices have an onboard filter so can send quaternions.
Alex Young 44:b3980e8ac074 665 */
Alex Young 32:fafe0f42d82b 666 static bool configureMotionTracker(void)
Alex Young 32:fafe0f42d82b 667 {
Alex Young 32:fafe0f42d82b 668 uint32_t deviceId = readDeviceId();
Alex Young 32:fafe0f42d82b 669
Alex Young 32:fafe0f42d82b 670 if (deviceId)
Alex Young 32:fafe0f42d82b 671 {
Alex Young 52:e2197b38c029 672 pc.printf("Found device with ID: %08X.\r\n", deviceId);
Alex Young 40:b77a8c10c76d 673 if (!XsDeviceId_isMtMk4_X(deviceId))
Alex Young 40:b77a8c10c76d 674 {
Alex Young 52:e2197b38c029 675 pc.printf("Device is not an MTi-1 series.\r\n");
Alex Young 40:b77a8c10c76d 676 return false;
Alex Young 40:b77a8c10c76d 677 }
Alex Young 32:fafe0f42d82b 678
Alex Young 40:b77a8c10c76d 679 DeviceFunction function = XsDeviceId_getFunction(deviceId);
Alex Young 52:e2197b38c029 680 pc.printf("Device is an MTi-%d: %s.\r\n", function, XsDeviceId_functionDescription(function));
Alex Young 40:b77a8c10c76d 681
Alex Young 40:b77a8c10c76d 682 if (function == DF_IMU)
Alex Young 29:d9310e7b58b5 683 {
Alex Young 32:fafe0f42d82b 684 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 685 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 686 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 687 {XDI_Acceleration, 100},
Alex Young 32:fafe0f42d82b 688 {XDI_RateOfTurn, 100},
Alex Young 32:fafe0f42d82b 689 {XDI_MagneticField, 100}
Alex Young 32:fafe0f42d82b 690 };
Alex Young 32:fafe0f42d82b 691 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 692 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 693 }
Alex Young 29:d9310e7b58b5 694 else
Alex Young 29:d9310e7b58b5 695 {
Alex Young 32:fafe0f42d82b 696 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 697 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 698 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 699 {XDI_Quaternion, 100},
Alex Young 32:fafe0f42d82b 700 {XDI_StatusWord, 65535}
Alex Young 32:fafe0f42d82b 701 };
Alex Young 32:fafe0f42d82b 702 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 703 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 704 }
Alex Young 29:d9310e7b58b5 705 }
Alex Young 32:fafe0f42d82b 706
Alex Young 32:fafe0f42d82b 707 return false;
Alex Young 29:d9310e7b58b5 708 }
Alex Young 29:d9310e7b58b5 709
Alex Young 35:7e519b88c610 710 /*!
Alex Young 35:7e519b88c610 711 * \brief Wait for a wakeup message from the MTi.
Alex Young 37:3e87bf647c68 712 * \param timeout Time to wait to receive the wakeup message.
Alex Young 37:3e87bf647c68 713 * \return true if wakeup received within timeout, else false.
Alex Young 35:7e519b88c610 714 *
Alex Young 49:38ecfbff5391 715 * The MTi sends an XMID_Wakeup message once it has completed its bootup
Alex Young 49:38ecfbff5391 716 * procedure. If this is acknowledged by an XMID_WakeupAck message then the MTi
Alex Young 35:7e519b88c610 717 * will stay in configuration mode. Otherwise it will automatically enter
Alex Young 35:7e519b88c610 718 * measurement mode with the stored output configuration.
Alex Young 35:7e519b88c610 719 */
Alex Young 37:3e87bf647c68 720 bool waitForWakeup(uint32_t timeout)
Alex Young 35:7e519b88c610 721 {
Alex Young 37:3e87bf647c68 722 osEvent ev = g_responseQueue.get(timeout);
Alex Young 35:7e519b88c610 723 if (ev.status == osEventMessage)
Alex Young 35:7e519b88c610 724 {
Alex Young 35:7e519b88c610 725 XbusMessage const* m = (XbusMessage const*)ev.value.p;
Alex Young 35:7e519b88c610 726 XbusMessageMemoryManager janitor(m);
Alex Young 35:7e519b88c610 727 return m->mid == XMID_Wakeup;
Alex Young 35:7e519b88c610 728 }
Alex Young 35:7e519b88c610 729 return false;
Alex Young 35:7e519b88c610 730 }
Alex Young 35:7e519b88c610 731
Alex Young 35:7e519b88c610 732 /*!
Alex Young 37:3e87bf647c68 733 * \brief Send wakeup acknowledge message to MTi.
Alex Young 37:3e87bf647c68 734 *
Alex Young 37:3e87bf647c68 735 * Sending a wakeup acknowledge will cause the device to stay in configuration
Alex Young 37:3e87bf647c68 736 * mode instead of automatically transitioning to measurement mode with the
Alex Young 37:3e87bf647c68 737 * stored output configuration.
Alex Young 37:3e87bf647c68 738 */
Alex Young 37:3e87bf647c68 739 void sendWakeupAck(void)
Alex Young 37:3e87bf647c68 740 {
Alex Young 37:3e87bf647c68 741 XbusMessage ack = {XMID_WakeupAck};
Alex Young 37:3e87bf647c68 742 sendMessage(&ack);
Alex Young 52:e2197b38c029 743 pc.printf("Device ready for operation.\r\n");
Alex Young 37:3e87bf647c68 744 }
Alex Young 37:3e87bf647c68 745
tjerkhofmeijer 64:8a0f00a064bb 746 #ifdef MTI_USES_UART_INTERFACE
Alex Young 37:3e87bf647c68 747 /*!
Alex Young 37:3e87bf647c68 748 * \brief Restore communication with the MTi.
Alex Young 37:3e87bf647c68 749 *
Alex Young 37:3e87bf647c68 750 * On bootup the MTi will listen for a magic byte to signal that it should
Alex Young 37:3e87bf647c68 751 * return to default baudrate and output configuration. This can be used to
Alex Young 37:3e87bf647c68 752 * recover from a bad or unknown configuration.
Alex Young 37:3e87bf647c68 753 */
Alex Young 37:3e87bf647c68 754 void restoreCommunication(void)
Alex Young 37:3e87bf647c68 755 {
Alex Young 37:3e87bf647c68 756 pc.printf("Restoring communication with device... ");
Alex Young 37:3e87bf647c68 757 mtReset = 0;
Alex Young 37:3e87bf647c68 758 Thread::wait(1);
Alex Young 37:3e87bf647c68 759 mtReset = 1;
Alex Young 37:3e87bf647c68 760
Alex Young 37:3e87bf647c68 761 do
Alex Young 37:3e87bf647c68 762 {
Alex Young 37:3e87bf647c68 763 mt.putc(0xDE);
Alex Young 37:3e87bf647c68 764 }
Alex Young 37:3e87bf647c68 765 while (!waitForWakeup(1));
Alex Young 52:e2197b38c029 766 pc.printf("done\r\n");
Alex Young 37:3e87bf647c68 767
Alex Young 37:3e87bf647c68 768 sendWakeupAck();
Alex Young 37:3e87bf647c68 769 }
tjerkhofmeijer 64:8a0f00a064bb 770 #endif
Alex Young 37:3e87bf647c68 771
Alex Young 37:3e87bf647c68 772 /*!
Alex Young 35:7e519b88c610 773 * \brief Releases the MTi reset line and waits for a wakeup message.
Alex Young 37:3e87bf647c68 774 *
Alex Young 37:3e87bf647c68 775 * If no wakeup message is received within 1 second the restore communications
Alex Young 37:3e87bf647c68 776 * procedure is done to reset the MTi to default baudrate and output configuration.
Alex Young 35:7e519b88c610 777 */
tjerkhofmeijer 64:8a0f00a064bb 778 static bool wakeupMotionTracker(void)
Alex Young 35:7e519b88c610 779 {
Alex Young 35:7e519b88c610 780 mtReset.write(1); // Release MT from reset.
Alex Young 37:3e87bf647c68 781 if (waitForWakeup(1000))
Alex Young 35:7e519b88c610 782 {
Alex Young 37:3e87bf647c68 783 sendWakeupAck();
Alex Young 37:3e87bf647c68 784 }
Alex Young 37:3e87bf647c68 785 else
Alex Young 37:3e87bf647c68 786 {
tjerkhofmeijer 64:8a0f00a064bb 787 #ifdef MTI_USES_UART_INTERFACE
Alex Young 37:3e87bf647c68 788 restoreCommunication();
tjerkhofmeijer 64:8a0f00a064bb 789 #else
tjerkhofmeijer 64:8a0f00a064bb 790 pc.printf("Failed to communicate with MTi device\r\n");
tjerkhofmeijer 64:8a0f00a064bb 791 return true;
tjerkhofmeijer 64:8a0f00a064bb 792 #endif
Alex Young 35:7e519b88c610 793 }
tjerkhofmeijer 64:8a0f00a064bb 794 return true;
Alex Young 35:7e519b88c610 795 }
Alex Young 35:7e519b88c610 796
Alex Young 38:d8d410d1662c 797 static void printIntroMessage(void)
Alex Young 38:d8d410d1662c 798 {
Alex Young 52:e2197b38c029 799 pc.printf("\r\n\r\n\r\n\r\n\r\n");
Alex Young 52:e2197b38c029 800 pc.printf("MTi-1 series embedded example firmware.\r\n");
Alex Young 38:d8d410d1662c 801 }
Alex Young 38:d8d410d1662c 802
Alex Young 38:d8d410d1662c 803 static void printUsageInstructions(void)
Alex Young 38:d8d410d1662c 804 {
Alex Young 52:e2197b38c029 805 pc.printf("\r\n");
Alex Young 52:e2197b38c029 806 pc.printf("Press 'm' to start measuring and 'c' to return to config mode.\r\n");
Alex Young 38:d8d410d1662c 807 }
Alex Young 38:d8d410d1662c 808
Alex Young 44:b3980e8ac074 809 /*!
Alex Young 44:b3980e8ac074 810 * \brief Output the contents of a data message to the PC serial port.
Alex Young 44:b3980e8ac074 811 */
Alex Young 43:470c019246e4 812 static void printMessageData(struct XbusMessage const* message)
Alex Young 43:470c019246e4 813 {
Alex Young 43:470c019246e4 814 if (!message)
Alex Young 43:470c019246e4 815 return;
Alex Young 43:470c019246e4 816
Alex Young 43:470c019246e4 817 pc.printf("MTData2:");
Alex Young 43:470c019246e4 818 uint16_t counter;
Alex Young 43:470c019246e4 819 if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
Alex Young 43:470c019246e4 820 {
Alex Young 43:470c019246e4 821 pc.printf(" Packet counter: %5d", counter);
Alex Young 43:470c019246e4 822 }
Alex Young 43:470c019246e4 823 float ori[4];
Alex Young 43:470c019246e4 824 if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
Alex Young 43:470c019246e4 825 {
Alex Young 43:470c019246e4 826 pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1],
Alex Young 43:470c019246e4 827 ori[2], ori[3]);
Alex Young 43:470c019246e4 828 }
Alex Young 43:470c019246e4 829 float acc[3];
Alex Young 43:470c019246e4 830 if (XbusMessage_getDataItem(acc, XDI_Acceleration, message))
Alex Young 43:470c019246e4 831 {
Alex Young 43:470c019246e4 832 pc.printf(" Acceleration: (% .3f, % .3f, % .3f)", acc[0], acc[1], acc[2]);
Alex Young 43:470c019246e4 833 }
Alex Young 43:470c019246e4 834 float gyr[3];
Alex Young 43:470c019246e4 835 if (XbusMessage_getDataItem(gyr, XDI_RateOfTurn, message))
Alex Young 43:470c019246e4 836 {
Alex Young 43:470c019246e4 837 pc.printf(" Rate Of Turn: (% .3f, % .3f, % .3f)", gyr[0], gyr[1], gyr[2]);
Alex Young 43:470c019246e4 838 }
Alex Young 43:470c019246e4 839 float mag[3];
Alex Young 43:470c019246e4 840 if (XbusMessage_getDataItem(mag, XDI_MagneticField, message))
Alex Young 43:470c019246e4 841 {
Alex Young 43:470c019246e4 842 pc.printf(" Magnetic Field: (% .3f, % .3f, % .3f)", mag[0], mag[1], mag[2]);
Alex Young 43:470c019246e4 843 }
Alex Young 43:470c019246e4 844 uint32_t status;
Alex Young 43:470c019246e4 845 if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
Alex Young 43:470c019246e4 846 {
Alex Young 43:470c019246e4 847 pc.printf(" Status:%X", status);
Alex Young 43:470c019246e4 848 }
Alex Young 52:e2197b38c029 849 pc.printf("\r\n");
Alex Young 43:470c019246e4 850 }
Alex Young 43:470c019246e4 851
Alex Young 43:470c019246e4 852
Alex Young 2:b3e402dc11ca 853 int main(void)
Alex Young 2:b3e402dc11ca 854 {
Alex Young 4:98f063b2e6da 855 XbusParserCallback xbusCallback = {};
Alex Young 25:01356fb59467 856 xbusCallback.allocateBuffer = allocateMessageData;
Alex Young 25:01356fb59467 857 xbusCallback.deallocateBuffer = deallocateMessageData;
Alex Young 24:2cc49dc854e3 858 xbusCallback.handleMessage = mtMessageHandler;
Alex Young 4:98f063b2e6da 859
Alex Young 4:98f063b2e6da 860 xbusParser = XbusParser_create(&xbusCallback);
tjerkhofmeijer 64:8a0f00a064bb 861 configurePcInterface();
tjerkhofmeijer 64:8a0f00a064bb 862 configureMtCommunicationInterface();
Alex Young 38:d8d410d1662c 863
Alex Young 38:d8d410d1662c 864 printIntroMessage();
tjerkhofmeijer 64:8a0f00a064bb 865 if (wakeupMotionTracker())
Alex Young 5:abc52dd88be2 866 {
tjerkhofmeijer 64:8a0f00a064bb 867 if (configureMotionTracker())
Alex Young 26:665d3624f9ab 868 {
tjerkhofmeijer 64:8a0f00a064bb 869 printUsageInstructions();
tjerkhofmeijer 64:8a0f00a064bb 870 for (;;)
Alex Young 29:d9310e7b58b5 871 {
tjerkhofmeijer 64:8a0f00a064bb 872 while (pc.readable())
tjerkhofmeijer 64:8a0f00a064bb 873 {
tjerkhofmeijer 64:8a0f00a064bb 874 handlePcCommand(pc.getc());
tjerkhofmeijer 64:8a0f00a064bb 875 }
Alex Young 43:470c019246e4 876
tjerkhofmeijer 64:8a0f00a064bb 877 osEvent ev = g_dataQueue.get(10);
tjerkhofmeijer 64:8a0f00a064bb 878 if (ev.status == osEventMessage)
tjerkhofmeijer 64:8a0f00a064bb 879 {
tjerkhofmeijer 64:8a0f00a064bb 880 XbusMessage const* data = (XbusMessage const*)ev.value.p;
tjerkhofmeijer 64:8a0f00a064bb 881 XbusMessageMemoryManager janitor(data);
tjerkhofmeijer 64:8a0f00a064bb 882 printMessageData(data);
tjerkhofmeijer 64:8a0f00a064bb 883 }
Alex Young 43:470c019246e4 884 }
Alex Young 26:665d3624f9ab 885 }
tjerkhofmeijer 64:8a0f00a064bb 886 else
tjerkhofmeijer 64:8a0f00a064bb 887 {
tjerkhofmeijer 64:8a0f00a064bb 888 pc.printf("Failed to configure motion tracker.\r\n");
tjerkhofmeijer 64:8a0f00a064bb 889 return -1;
tjerkhofmeijer 64:8a0f00a064bb 890 }
Alex Young 29:d9310e7b58b5 891 }
Alex Young 4:98f063b2e6da 892 }