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:
xsens_mheskamp
Date:
Thu May 03 10:35:39 2018 +0200
Revision:
70:ff3afeaa31be
Parent:
68:6d6dbeefd196
Added some function documentation

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