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:
Fri Oct 02 16:24:06 2015 +0200
Revision:
64:8a0f00a064bb
Parent:
63:138c196f0b88
Child:
65:38c908d1b515
MTi 1-series example is updated with support for I2C and SPI interfaces

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