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@57:c3c85ebb7375, 2015-06-11 (annotated)

Committer:
Alex Young
Date:
Thu Jun 11 12:35:57 2015 +0200
Revision:
57:c3c85ebb7375
Parent:
56:041d3d9c300a
Child:
58:db60ef0a0d16
Use platform defines to setup GPIO/UARTs

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