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@52:e2197b38c029, 2015-05-22 (annotated)

Committer:
Alex Young
Date:
Fri May 22 15:45:18 2015 +0200
Revision:
52:e2197b38c029
Parent:
49:38ecfbff5391
Child:
53:3891f4259901
Replace \n with \r\n

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