Xsens / Mbed 2 deprecated MTi-1_example

Example of using Xbus library to communicate with an MTi-1 series device using a full-duplex UART connection.

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Alex Young

Important Information

This example is deprecated and no longer maintained. There are new embedded examples available in the MT SDK folder of the MT Software Suite. For more information please visit: https://xsenstechnologies.force.com/knowledgebase/s/article/Introduction-to-the-MT-SDK-programming-examples-for-MTi-devices

Overview

The example program demonstrates connecting to an MTi-1 series device, restoring communications settings to default if necessary, and configuring the MTi to send data. For an MTi-1 the device is configured to send inertial sensor data, while MTi-2 and MTi-3 devices are configured to output orientation data using the onboard XKF3i filter.

Communication with the MTi-1 series device is implemented using a either a full-duplex UART, I2C or SPI bus. A reset line is used to reset the MTi during initialization. Data is output to a host PC terminal using a second UART.

For more information on the MTi-1 series communication protocol please refer to the datasheet: https://www.xsens.com/download/pdf/documentation/mti-1/mti-1-series_datasheet.pdf

Supported Platforms

The program has been tested on the following mbed platforms:

Using the Example

  1. To use the example program connect one of the supported mbed boards to the host PC and download the application from the mbed online compiler to the target device.
  2. With the mbed board unpowered (USB disconnected) wire the mbed board to the MTi-1 development board. The following connections are required:
    • In all cases:
      • 5V (or 3V3) main supply to VDD (P300-1)
      • MCU IO voltage (IORef) to VDDIO (P300-2)
      • GND to GND (P300-3)
      • MT_NRESET to nRST (P300-5)
    • For I2C communication:
      • MT_SCL to I2C_SCL (P300-9)
      • MT_SDA to I2C_SDA (P300-11)
      • MT_DRDY to DRDY (P300-15)
      • MT_ADD0 to ADD0 (P300-17)
      • MT_ADD1 to ADD1 (P300-19)
      • MT_ADD2 to ADD2 (P300-21)
    • For SPI communication:
      • MT_DRDY to DRDY (P300-15)
      • MT_SCLK to SPI_SCK (P300-17)
      • MT_MISO to SPI_MISO (P300-19)
      • MT_MOSI to SPI_MOSI (P300-21)
      • MT_nCS to SPI_nCS (P300-23)
    • For UART communication:
      • MT_RX to UART_TX (P300-9)
      • MT_TX to UART_RX (P300-11)

For more information on the MTi-1 development board please refer to the MTi-1 series user manual: https://www.xsens.com/download/pdf/documentation/mti-1/mti-1-series_dk_user_manual.pdf

Information

Check the defines at the top of main.cpp to determine which IO pins are used for the MT_xxx connections on each mbed platform.

Information

The active peripheral (I2C, SPI or UART) is selected on the MTi-1 development board through the PSEL0 and PSEL1 switches. Look on the bottom of the development board for the correct settings.

  1. Connect to the target using a serial terminal. The application is configured for:
    • Baudrate = 921600
    • Stop bits = 1
    • No parity bits
    • No flow control
  2. Reset the mbed board.
  3. You should be presented with a simple user interface as shown below: 
MTi-1 series embedded example firmware.
Device ready for operation.
Found device with ID: 03880011.
Device is an MTi-3: Attitude Heading Reference System.
Output configuration set to:
        Packet counter: 65535 Hz
        Sample time fine: 65535 Hz
        Quaternion: 100 Hz
        Status word: 65535 Hz

Press 'm' to start measuring and 'c' to return to config mode.

main.cpp@54:2e9bb1390c9c, 2015-05-22 (annotated)

Committer:
Alex Young
Date:
Fri May 22 16:20:29 2015 +0200
Revision:
54:2e9bb1390c9c
Parent:
53:3891f4259901
Child:
55:9a2d6f947f0d
Add some introduction text to main.cpp

Who changed what in which revision?

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