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

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Alex Young

Important Information

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

Overview

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

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

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

Supported Platforms

The program has been tested on the following mbed platforms:

Using the Example

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

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

Information

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

Information

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

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

Press 'm' to start measuring and 'c' to return to config mode.
Committer:
Alex Young
Date:
Thu May 21 15:34:18 2015 +0200
Revision:
43:470c019246e4
Parent:
42:e70756d2d8bc
Child:
44:b3980e8ac074
Move printing of message data to main thread.

Printing data may take some time, so should not really be done in an
interrupt context. Instead use a queue to pass the message data to the
main thread for processing.

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 25:01356fb59467 22 #define MEMORY_POOL_SIZE (4)
Alex Young 26:665d3624f9ab 23 #define RESPONSE_QUEUE_SIZE (1)
Alex Young 43:470c019246e4 24 #define DATA_QUEUE_SIZE (2)
Alex Young 25:01356fb59467 25 #define MAX_XBUS_DATA_SIZE (128)
Alex Young 25:01356fb59467 26
Alex Young 4:98f063b2e6da 27 static Serial pc(PA_2, PA_3);
Alex Young 4:98f063b2e6da 28 static Serial mt(PB_9, PB_8);
Alex Young 35:7e519b88c610 29 /*!
Alex Young 35:7e519b88c610 30 * \brief MT reset line.
Alex Young 35:7e519b88c610 31 *
Alex Young 35:7e519b88c610 32 * MT is held in reset on startup.
Alex Young 35:7e519b88c610 33 */
Alex Young 35:7e519b88c610 34 static DigitalOut mtReset(PA_10, 0);
Alex Young 4:98f063b2e6da 35 static XbusParser* xbusParser;
Alex Young 25:01356fb59467 36
Alex Young 25:01356fb59467 37 MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool;
Alex Young 25:01356fb59467 38 MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool;
Alex Young 26:665d3624f9ab 39 Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue;
Alex Young 43:470c019246e4 40 Queue<XbusMessage, DATA_QUEUE_SIZE> g_dataQueue;
Alex Young 4:98f063b2e6da 41
Alex Young 25:01356fb59467 42 static void* allocateMessageData(size_t bufSize)
Alex Young 4:98f063b2e6da 43 {
Alex Young 25:01356fb59467 44 return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL;
Alex Young 25:01356fb59467 45 }
Alex Young 25:01356fb59467 46
Alex Young 25:01356fb59467 47 static void deallocateMessageData(void const* buffer)
Alex Young 25:01356fb59467 48 {
Alex Young 25:01356fb59467 49 g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer);
Alex Young 4:98f063b2e6da 50 }
Alex Young 4:98f063b2e6da 51
Alex Young 4:98f063b2e6da 52 static void mtLowLevelHandler(void)
Alex Young 4:98f063b2e6da 53 {
Alex Young 4:98f063b2e6da 54 while (mt.readable())
Alex Young 4:98f063b2e6da 55 {
Alex Young 4:98f063b2e6da 56 XbusParser_parseByte(xbusParser, mt.getc());
Alex Young 4:98f063b2e6da 57 }
Alex Young 4:98f063b2e6da 58 }
Alex Young 4:98f063b2e6da 59
Alex Young 34:3d7a6519a256 60 static void sendMessage(XbusMessage const* m)
Alex Young 11:8593ba137917 61 {
Alex Young 26:665d3624f9ab 62 uint8_t buf[64];
Alex Young 26:665d3624f9ab 63 size_t rawLength = XbusMessage_format(buf, m);
Alex Young 11:8593ba137917 64 for (size_t i = 0; i < rawLength; ++i)
Alex Young 11:8593ba137917 65 {
Alex Young 11:8593ba137917 66 mt.putc(buf[i]);
Alex Young 11:8593ba137917 67 }
Alex Young 34:3d7a6519a256 68 }
Alex Young 34:3d7a6519a256 69
Alex Young 34:3d7a6519a256 70 static XbusMessage const* doTransaction(XbusMessage const* m)
Alex Young 34:3d7a6519a256 71 {
Alex Young 34:3d7a6519a256 72 sendMessage(m);
Alex Young 26:665d3624f9ab 73
Alex Young 26:665d3624f9ab 74 osEvent ev = g_responseQueue.get(500);
Alex Young 26:665d3624f9ab 75 return ev.status == osEventMessage ? (XbusMessage*)ev.value.p : NULL;
Alex Young 26:665d3624f9ab 76 }
Alex Young 26:665d3624f9ab 77
Alex Young 31:ce1ea9ae861e 78 /*!
Alex Young 31:ce1ea9ae861e 79 * \brief RAII object to manage message memory deallocation.
Alex Young 31:ce1ea9ae861e 80 *
Alex Young 31:ce1ea9ae861e 81 * Will automatically free the memory used by a XbusMessage when going out
Alex Young 31:ce1ea9ae861e 82 * of scope.
Alex Young 31:ce1ea9ae861e 83 */
Alex Young 31:ce1ea9ae861e 84 class XbusMessageMemoryManager
Alex Young 26:665d3624f9ab 85 {
Alex Young 31:ce1ea9ae861e 86 public:
Alex Young 31:ce1ea9ae861e 87 XbusMessageMemoryManager(XbusMessage const* message)
Alex Young 31:ce1ea9ae861e 88 : m_message(message)
Alex Young 31:ce1ea9ae861e 89 {
Alex Young 31:ce1ea9ae861e 90 }
Alex Young 31:ce1ea9ae861e 91
Alex Young 31:ce1ea9ae861e 92 ~XbusMessageMemoryManager()
Alex Young 31:ce1ea9ae861e 93 {
Alex Young 31:ce1ea9ae861e 94 if (m_message)
Alex Young 31:ce1ea9ae861e 95 {
Alex Young 31:ce1ea9ae861e 96 if (m_message->data)
Alex Young 31:ce1ea9ae861e 97 deallocateMessageData(m_message->data);
Alex Young 31:ce1ea9ae861e 98 g_messagePool.free(const_cast<XbusMessage*>(m_message));
Alex Young 31:ce1ea9ae861e 99 }
Alex Young 31:ce1ea9ae861e 100 }
Alex Young 31:ce1ea9ae861e 101
Alex Young 31:ce1ea9ae861e 102 private:
Alex Young 31:ce1ea9ae861e 103 XbusMessage const* m_message;
Alex Young 31:ce1ea9ae861e 104 };
Alex Young 26:665d3624f9ab 105
Alex Young 29:d9310e7b58b5 106 static void dumpResponse(XbusMessage const* response)
Alex Young 29:d9310e7b58b5 107 {
Alex Young 29:d9310e7b58b5 108 switch (response->mid)
Alex Young 29:d9310e7b58b5 109 {
Alex Young 29:d9310e7b58b5 110 case XMID_GotoConfigAck:
Alex Young 39:9014c5236864 111 pc.printf("Device went to config mode.\n");
Alex Young 29:d9310e7b58b5 112 break;
Alex Young 29:d9310e7b58b5 113
Alex Young 29:d9310e7b58b5 114 case XMID_Error:
Alex Young 29:d9310e7b58b5 115 pc.printf("Device error!");
Alex Young 29:d9310e7b58b5 116 break;
Alex Young 29:d9310e7b58b5 117
Alex Young 29:d9310e7b58b5 118 default:
Alex Young 29:d9310e7b58b5 119 pc.printf("Received response MID=%X, length=%d\n", response->mid, response->length);
Alex Young 29:d9310e7b58b5 120 break;
Alex Young 29:d9310e7b58b5 121 }
Alex Young 29:d9310e7b58b5 122 }
Alex Young 29:d9310e7b58b5 123
Alex Young 26:665d3624f9ab 124 static void sendCommand(XsMessageId cmdId)
Alex Young 26:665d3624f9ab 125 {
Alex Young 26:665d3624f9ab 126 XbusMessage m = {cmdId};
Alex Young 26:665d3624f9ab 127 XbusMessage const* response = doTransaction(&m);
Alex Young 31:ce1ea9ae861e 128 XbusMessageMemoryManager janitor(response);
Alex Young 26:665d3624f9ab 129
Alex Young 26:665d3624f9ab 130 if (response)
Alex Young 26:665d3624f9ab 131 {
Alex Young 29:d9310e7b58b5 132 dumpResponse(response);
Alex Young 26:665d3624f9ab 133 }
Alex Young 26:665d3624f9ab 134 else
Alex Young 26:665d3624f9ab 135 {
Alex Young 26:665d3624f9ab 136 pc.printf("Timeout waiting for response.\n");
Alex Young 26:665d3624f9ab 137 }
Alex Young 11:8593ba137917 138 }
Alex Young 11:8593ba137917 139
Alex Young 11:8593ba137917 140 static void handlePcCommand(char cmd)
Alex Young 11:8593ba137917 141 {
Alex Young 11:8593ba137917 142 switch (cmd)
Alex Young 11:8593ba137917 143 {
Alex Young 11:8593ba137917 144 case 'c':
Alex Young 11:8593ba137917 145 sendCommand(XMID_GotoConfig);
Alex Young 11:8593ba137917 146 break;
Alex Young 11:8593ba137917 147
Alex Young 11:8593ba137917 148 case 'm':
Alex Young 11:8593ba137917 149 sendCommand(XMID_GotoMeasurement);
Alex Young 11:8593ba137917 150 break;
Alex Young 11:8593ba137917 151 }
Alex Young 11:8593ba137917 152 }
Alex Young 11:8593ba137917 153
Alex Young 24:2cc49dc854e3 154 static void mtMessageHandler(struct XbusMessage const* message)
Alex Young 4:98f063b2e6da 155 {
Alex Young 43:470c019246e4 156 XbusMessage* m = g_messagePool.alloc();
Alex Young 43:470c019246e4 157 if (m)
Alex Young 7:c913a7cd5231 158 {
Alex Young 43:470c019246e4 159 memcpy(m, message, sizeof(XbusMessage));
Alex Young 43:470c019246e4 160 if (message->mid == XMID_MtData2)
Alex Young 43:470c019246e4 161 {
Alex Young 43:470c019246e4 162 g_dataQueue.put(m);
Alex Young 43:470c019246e4 163 }
Alex Young 43:470c019246e4 164 else
Alex Young 43:470c019246e4 165 {
Alex Young 43:470c019246e4 166 g_responseQueue.put(m);
Alex Young 43:470c019246e4 167 }
Alex Young 7:c913a7cd5231 168 }
Alex Young 43:470c019246e4 169 else if (message->data)
Alex Young 7:c913a7cd5231 170 {
Alex Young 43:470c019246e4 171 deallocateMessageData(message->data);
Alex Young 25:01356fb59467 172 }
Alex Young 4:98f063b2e6da 173 }
Alex Young 4:98f063b2e6da 174
Alex Young 4:98f063b2e6da 175 static void configureSerialPorts(void)
Alex Young 4:98f063b2e6da 176 {
Alex Young 4:98f063b2e6da 177 pc.baud(921600);
Alex Young 4:98f063b2e6da 178 pc.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 179
Alex Young 37:3e87bf647c68 180 mt.baud(115200);
Alex Young 4:98f063b2e6da 181 mt.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 182 mt.attach(mtLowLevelHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 183 }
Alex Young 4:98f063b2e6da 184
Alex Young 29:d9310e7b58b5 185 static uint32_t readDeviceId(void)
Alex Young 29:d9310e7b58b5 186 {
Alex Young 29:d9310e7b58b5 187 XbusMessage reqDid = {XMID_ReqDid};
Alex Young 29:d9310e7b58b5 188 XbusMessage const* didRsp = doTransaction(&reqDid);
Alex Young 31:ce1ea9ae861e 189 XbusMessageMemoryManager janitor(didRsp);
Alex Young 29:d9310e7b58b5 190 uint32_t deviceId = 0;
Alex Young 29:d9310e7b58b5 191 if (didRsp)
Alex Young 29:d9310e7b58b5 192 {
Alex Young 29:d9310e7b58b5 193 if (didRsp->mid == XMID_DeviceId)
Alex Young 29:d9310e7b58b5 194 {
Alex Young 29:d9310e7b58b5 195 deviceId = *(uint32_t*)didRsp->data;
Alex Young 29:d9310e7b58b5 196 }
Alex Young 29:d9310e7b58b5 197 }
Alex Young 29:d9310e7b58b5 198 return deviceId;
Alex Young 29:d9310e7b58b5 199 }
Alex Young 29:d9310e7b58b5 200
Alex Young 32:fafe0f42d82b 201 static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements)
Alex Young 29:d9310e7b58b5 202 {
Alex Young 32:fafe0f42d82b 203 XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf};
Alex Young 32:fafe0f42d82b 204 XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg);
Alex Young 32:fafe0f42d82b 205 XbusMessageMemoryManager janitor(outputConfRsp);
Alex Young 32:fafe0f42d82b 206 if (outputConfRsp)
Alex Young 29:d9310e7b58b5 207 {
Alex Young 32:fafe0f42d82b 208 if (outputConfRsp->mid == XMID_OutputConfig)
Alex Young 29:d9310e7b58b5 209 {
Alex Young 32:fafe0f42d82b 210 pc.printf("Output configuration set to:\n");
Alex Young 32:fafe0f42d82b 211 OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data;
Alex Young 32:fafe0f42d82b 212 for (int i = 0; i < outputConfRsp->length; ++i)
Alex Young 32:fafe0f42d82b 213 {
Alex Young 32:fafe0f42d82b 214 pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 32:fafe0f42d82b 215 ++conf;
Alex Young 32:fafe0f42d82b 216 }
Alex Young 32:fafe0f42d82b 217 return true;
Alex Young 29:d9310e7b58b5 218 }
Alex Young 29:d9310e7b58b5 219 else
Alex Young 29:d9310e7b58b5 220 {
Alex Young 32:fafe0f42d82b 221 dumpResponse(outputConfRsp);
Alex Young 29:d9310e7b58b5 222 }
Alex Young 32:fafe0f42d82b 223 }
Alex Young 32:fafe0f42d82b 224 else
Alex Young 32:fafe0f42d82b 225 {
Alex Young 32:fafe0f42d82b 226 pc.printf("Failed to set output configuration.\n");
Alex Young 32:fafe0f42d82b 227 }
Alex Young 32:fafe0f42d82b 228 return false;
Alex Young 32:fafe0f42d82b 229 }
Alex Young 29:d9310e7b58b5 230
Alex Young 32:fafe0f42d82b 231 static bool configureMotionTracker(void)
Alex Young 32:fafe0f42d82b 232 {
Alex Young 32:fafe0f42d82b 233 uint32_t deviceId = readDeviceId();
Alex Young 32:fafe0f42d82b 234
Alex Young 32:fafe0f42d82b 235 if (deviceId)
Alex Young 32:fafe0f42d82b 236 {
Alex Young 40:b77a8c10c76d 237 pc.printf("Found device with ID: %08X.\n", deviceId);
Alex Young 40:b77a8c10c76d 238 if (!XsDeviceId_isMtMk4_X(deviceId))
Alex Young 40:b77a8c10c76d 239 {
Alex Young 40:b77a8c10c76d 240 pc.printf("Device is not an MTi-1 series.\n");
Alex Young 40:b77a8c10c76d 241 return false;
Alex Young 40:b77a8c10c76d 242 }
Alex Young 32:fafe0f42d82b 243
Alex Young 40:b77a8c10c76d 244 DeviceFunction function = XsDeviceId_getFunction(deviceId);
Alex Young 40:b77a8c10c76d 245 pc.printf("Device is an MTi-%d: %s.\n", function, XsDeviceId_functionDescription(function));
Alex Young 40:b77a8c10c76d 246
Alex Young 40:b77a8c10c76d 247 if (function == DF_IMU)
Alex Young 29:d9310e7b58b5 248 {
Alex Young 32:fafe0f42d82b 249 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 250 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 251 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 252 {XDI_Acceleration, 100},
Alex Young 32:fafe0f42d82b 253 {XDI_RateOfTurn, 100},
Alex Young 32:fafe0f42d82b 254 {XDI_MagneticField, 100}
Alex Young 32:fafe0f42d82b 255 };
Alex Young 32:fafe0f42d82b 256 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 257 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 258 }
Alex Young 29:d9310e7b58b5 259 else
Alex Young 29:d9310e7b58b5 260 {
Alex Young 32:fafe0f42d82b 261 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 262 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 263 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 264 {XDI_Quaternion, 100},
Alex Young 32:fafe0f42d82b 265 {XDI_StatusWord, 65535}
Alex Young 32:fafe0f42d82b 266 };
Alex Young 32:fafe0f42d82b 267 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 268 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 269 }
Alex Young 29:d9310e7b58b5 270 }
Alex Young 32:fafe0f42d82b 271
Alex Young 32:fafe0f42d82b 272 return false;
Alex Young 29:d9310e7b58b5 273 }
Alex Young 29:d9310e7b58b5 274
Alex Young 35:7e519b88c610 275 /*!
Alex Young 35:7e519b88c610 276 * \brief Wait for a wakeup message from the MTi.
Alex Young 37:3e87bf647c68 277 * \param timeout Time to wait to receive the wakeup message.
Alex Young 37:3e87bf647c68 278 * \return true if wakeup received within timeout, else false.
Alex Young 35:7e519b88c610 279 *
Alex Young 35:7e519b88c610 280 * The MTi sends a XMID_Wakeup message once it has completed its bootup
Alex Young 35:7e519b88c610 281 * procedure. If this is acknowledged by a XMID_WakeupAck message then the MTi
Alex Young 35:7e519b88c610 282 * will stay in configuration mode. Otherwise it will automatically enter
Alex Young 35:7e519b88c610 283 * measurement mode with the stored output configuration.
Alex Young 35:7e519b88c610 284 */
Alex Young 37:3e87bf647c68 285 bool waitForWakeup(uint32_t timeout)
Alex Young 35:7e519b88c610 286 {
Alex Young 37:3e87bf647c68 287 osEvent ev = g_responseQueue.get(timeout);
Alex Young 35:7e519b88c610 288 if (ev.status == osEventMessage)
Alex Young 35:7e519b88c610 289 {
Alex Young 35:7e519b88c610 290 XbusMessage const* m = (XbusMessage const*)ev.value.p;
Alex Young 35:7e519b88c610 291 XbusMessageMemoryManager janitor(m);
Alex Young 35:7e519b88c610 292 return m->mid == XMID_Wakeup;
Alex Young 35:7e519b88c610 293 }
Alex Young 35:7e519b88c610 294 return false;
Alex Young 35:7e519b88c610 295 }
Alex Young 35:7e519b88c610 296
Alex Young 35:7e519b88c610 297 /*!
Alex Young 37:3e87bf647c68 298 * \brief Send wakeup acknowledge message to MTi.
Alex Young 37:3e87bf647c68 299 *
Alex Young 37:3e87bf647c68 300 * Sending a wakeup acknowledge will cause the device to stay in configuration
Alex Young 37:3e87bf647c68 301 * mode instead of automatically transitioning to measurement mode with the
Alex Young 37:3e87bf647c68 302 * stored output configuration.
Alex Young 37:3e87bf647c68 303 */
Alex Young 37:3e87bf647c68 304 void sendWakeupAck(void)
Alex Young 37:3e87bf647c68 305 {
Alex Young 37:3e87bf647c68 306 XbusMessage ack = {XMID_WakeupAck};
Alex Young 37:3e87bf647c68 307 sendMessage(&ack);
Alex Young 37:3e87bf647c68 308 pc.printf("Device ready for operation.\n");
Alex Young 37:3e87bf647c68 309 }
Alex Young 37:3e87bf647c68 310
Alex Young 37:3e87bf647c68 311 /*!
Alex Young 37:3e87bf647c68 312 * \brief Restore communication with the MTi.
Alex Young 37:3e87bf647c68 313 *
Alex Young 37:3e87bf647c68 314 * On bootup the MTi will listen for a magic byte to signal that it should
Alex Young 37:3e87bf647c68 315 * return to default baudrate and output configuration. This can be used to
Alex Young 37:3e87bf647c68 316 * recover from a bad or unknown configuration.
Alex Young 37:3e87bf647c68 317 */
Alex Young 37:3e87bf647c68 318 void restoreCommunication(void)
Alex Young 37:3e87bf647c68 319 {
Alex Young 37:3e87bf647c68 320 pc.printf("Restoring communication with device... ");
Alex Young 37:3e87bf647c68 321 mtReset = 0;
Alex Young 37:3e87bf647c68 322 Thread::wait(1);
Alex Young 37:3e87bf647c68 323 mtReset = 1;
Alex Young 37:3e87bf647c68 324
Alex Young 37:3e87bf647c68 325 do
Alex Young 37:3e87bf647c68 326 {
Alex Young 37:3e87bf647c68 327 mt.putc(0xDE);
Alex Young 37:3e87bf647c68 328 }
Alex Young 37:3e87bf647c68 329 while (!waitForWakeup(1));
Alex Young 37:3e87bf647c68 330 pc.printf("done\n");
Alex Young 37:3e87bf647c68 331
Alex Young 37:3e87bf647c68 332 sendWakeupAck();
Alex Young 37:3e87bf647c68 333 }
Alex Young 37:3e87bf647c68 334
Alex Young 37:3e87bf647c68 335 /*!
Alex Young 35:7e519b88c610 336 * \brief Releases the MTi reset line and waits for a wakeup message.
Alex Young 37:3e87bf647c68 337 *
Alex Young 37:3e87bf647c68 338 * If no wakeup message is received within 1 second the restore communications
Alex Young 37:3e87bf647c68 339 * procedure is done to reset the MTi to default baudrate and output configuration.
Alex Young 35:7e519b88c610 340 */
Alex Young 35:7e519b88c610 341 static void wakeupMotionTracker(void)
Alex Young 35:7e519b88c610 342 {
Alex Young 35:7e519b88c610 343 mtReset.write(1); // Release MT from reset.
Alex Young 37:3e87bf647c68 344 if (waitForWakeup(1000))
Alex Young 35:7e519b88c610 345 {
Alex Young 37:3e87bf647c68 346 sendWakeupAck();
Alex Young 37:3e87bf647c68 347 }
Alex Young 37:3e87bf647c68 348 else
Alex Young 37:3e87bf647c68 349 {
Alex Young 37:3e87bf647c68 350 restoreCommunication();
Alex Young 35:7e519b88c610 351 }
Alex Young 35:7e519b88c610 352 }
Alex Young 35:7e519b88c610 353
Alex Young 38:d8d410d1662c 354 static void printIntroMessage(void)
Alex Young 38:d8d410d1662c 355 {
Alex Young 38:d8d410d1662c 356 pc.printf("\n\n\n\n\n");
Alex Young 38:d8d410d1662c 357 pc.printf("MTi-1 series embedded example firmware.\n");
Alex Young 38:d8d410d1662c 358 }
Alex Young 38:d8d410d1662c 359
Alex Young 38:d8d410d1662c 360 static void printUsageInstructions(void)
Alex Young 38:d8d410d1662c 361 {
Alex Young 38:d8d410d1662c 362 pc.printf("\n");
Alex Young 38:d8d410d1662c 363 pc.printf("Press 'm' to start measuring and 'c' to return to config mode.\n");
Alex Young 38:d8d410d1662c 364 }
Alex Young 38:d8d410d1662c 365
Alex Young 43:470c019246e4 366 static void printMessageData(struct XbusMessage const* message)
Alex Young 43:470c019246e4 367 {
Alex Young 43:470c019246e4 368 if (!message)
Alex Young 43:470c019246e4 369 return;
Alex Young 43:470c019246e4 370
Alex Young 43:470c019246e4 371 pc.printf("MTData2:");
Alex Young 43:470c019246e4 372 uint16_t counter;
Alex Young 43:470c019246e4 373 if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
Alex Young 43:470c019246e4 374 {
Alex Young 43:470c019246e4 375 pc.printf(" Packet counter: %5d", counter);
Alex Young 43:470c019246e4 376 }
Alex Young 43:470c019246e4 377 float ori[4];
Alex Young 43:470c019246e4 378 if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
Alex Young 43:470c019246e4 379 {
Alex Young 43:470c019246e4 380 pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1],
Alex Young 43:470c019246e4 381 ori[2], ori[3]);
Alex Young 43:470c019246e4 382 }
Alex Young 43:470c019246e4 383 float acc[3];
Alex Young 43:470c019246e4 384 if (XbusMessage_getDataItem(acc, XDI_Acceleration, message))
Alex Young 43:470c019246e4 385 {
Alex Young 43:470c019246e4 386 pc.printf(" Acceleration: (% .3f, % .3f, % .3f)", acc[0], acc[1], acc[2]);
Alex Young 43:470c019246e4 387 }
Alex Young 43:470c019246e4 388 float gyr[3];
Alex Young 43:470c019246e4 389 if (XbusMessage_getDataItem(gyr, XDI_RateOfTurn, message))
Alex Young 43:470c019246e4 390 {
Alex Young 43:470c019246e4 391 pc.printf(" Rate Of Turn: (% .3f, % .3f, % .3f)", gyr[0], gyr[1], gyr[2]);
Alex Young 43:470c019246e4 392 }
Alex Young 43:470c019246e4 393 float mag[3];
Alex Young 43:470c019246e4 394 if (XbusMessage_getDataItem(mag, XDI_MagneticField, message))
Alex Young 43:470c019246e4 395 {
Alex Young 43:470c019246e4 396 pc.printf(" Magnetic Field: (% .3f, % .3f, % .3f)", mag[0], mag[1], mag[2]);
Alex Young 43:470c019246e4 397 }
Alex Young 43:470c019246e4 398 uint32_t status;
Alex Young 43:470c019246e4 399 if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
Alex Young 43:470c019246e4 400 {
Alex Young 43:470c019246e4 401 pc.printf(" Status:%X", status);
Alex Young 43:470c019246e4 402 }
Alex Young 43:470c019246e4 403 pc.printf("\n");
Alex Young 43:470c019246e4 404 }
Alex Young 43:470c019246e4 405
Alex Young 43:470c019246e4 406
Alex Young 2:b3e402dc11ca 407 int main(void)
Alex Young 2:b3e402dc11ca 408 {
Alex Young 4:98f063b2e6da 409 XbusParserCallback xbusCallback = {};
Alex Young 25:01356fb59467 410 xbusCallback.allocateBuffer = allocateMessageData;
Alex Young 25:01356fb59467 411 xbusCallback.deallocateBuffer = deallocateMessageData;
Alex Young 24:2cc49dc854e3 412 xbusCallback.handleMessage = mtMessageHandler;
Alex Young 4:98f063b2e6da 413
Alex Young 4:98f063b2e6da 414 xbusParser = XbusParser_create(&xbusCallback);
Alex Young 4:98f063b2e6da 415 configureSerialPorts();
Alex Young 38:d8d410d1662c 416
Alex Young 38:d8d410d1662c 417 printIntroMessage();
Alex Young 35:7e519b88c610 418 wakeupMotionTracker();
Alex Young 29:d9310e7b58b5 419 if (configureMotionTracker())
Alex Young 5:abc52dd88be2 420 {
Alex Young 38:d8d410d1662c 421 printUsageInstructions();
Alex Young 29:d9310e7b58b5 422 for (;;)
Alex Young 26:665d3624f9ab 423 {
Alex Young 29:d9310e7b58b5 424 while (pc.readable())
Alex Young 29:d9310e7b58b5 425 {
Alex Young 29:d9310e7b58b5 426 handlePcCommand(pc.getc());
Alex Young 29:d9310e7b58b5 427 }
Alex Young 43:470c019246e4 428
Alex Young 43:470c019246e4 429 osEvent ev = g_dataQueue.get(10);
Alex Young 43:470c019246e4 430 if (ev.status == osEventMessage)
Alex Young 43:470c019246e4 431 {
Alex Young 43:470c019246e4 432 XbusMessage const* data = (XbusMessage const*)ev.value.p;
Alex Young 43:470c019246e4 433 XbusMessageMemoryManager janitor(data);
Alex Young 43:470c019246e4 434 printMessageData(data);
Alex Young 43:470c019246e4 435 }
Alex Young 26:665d3624f9ab 436 }
Alex Young 5:abc52dd88be2 437 }
Alex Young 29:d9310e7b58b5 438 else
Alex Young 29:d9310e7b58b5 439 {
Alex Young 29:d9310e7b58b5 440 pc.printf("Failed to configure motion tracker.\n");
Alex Young 29:d9310e7b58b5 441 return -1;
Alex Young 29:d9310e7b58b5 442 }
Alex Young 4:98f063b2e6da 443 }