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@53:3891f4259901, 2015-05-22 (annotated)

Committer:
Alex Young
Date:
Fri May 22 16:20:04 2015 +0200
Revision:
53:3891f4259901
Parent:
52:e2197b38c029
Child:
54:2e9bb1390c9c
Make PC serial baudrate a define

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