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