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@36:21198d933917, 2015-05-21 (annotated)

Committer:
Alex Young
Date:
Thu May 21 12:24:01 2015 +0200
Revision:
36:21198d933917
Parent:
35:7e519b88c610
Child:
37:3e87bf647c68
Add file header for main.cpp

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Alex Young 36:21198d933917 1 /*!
Alex Young 36:21198d933917 2 * \file
Alex Young 36:21198d933917 3 * \copyright
Alex Young 36:21198d933917 4 * Copyright (C) Xsens Technologies B.V., 2015. All rights reserved.
Alex Young 36:21198d933917 5 *
Alex Young 36:21198d933917 6 * This source code is intended for use only by Xsens Technologies BV and
Alex Young 36:21198d933917 7 * those that have explicit written permission to use it from
Alex Young 36:21198d933917 8 * Xsens Technologies BV.
Alex Young 36:21198d933917 9 *
Alex Young 36:21198d933917 10 * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
Alex Young 36:21198d933917 11 * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
Alex Young 36:21198d933917 12 * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
Alex Young 36:21198d933917 13 * PARTICULAR PURPOSE.
Alex Young 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 4:98f063b2e6da 20
Alex Young 25:01356fb59467 21 #define MEMORY_POOL_SIZE (4)
Alex Young 26:665d3624f9ab 22 #define RESPONSE_QUEUE_SIZE (1)
Alex Young 25:01356fb59467 23 #define MAX_XBUS_DATA_SIZE (128)
Alex Young 25:01356fb59467 24
Alex Young 4:98f063b2e6da 25 static Serial pc(PA_2, PA_3);
Alex Young 4:98f063b2e6da 26 static Serial mt(PB_9, PB_8);
Alex Young 35:7e519b88c610 27 /*!
Alex Young 35:7e519b88c610 28 * \brief MT reset line.
Alex Young 35:7e519b88c610 29 *
Alex Young 35:7e519b88c610 30 * MT is held in reset on startup.
Alex Young 35:7e519b88c610 31 */
Alex Young 35:7e519b88c610 32 static DigitalOut mtReset(PA_10, 0);
Alex Young 4:98f063b2e6da 33 static XbusParser* xbusParser;
Alex Young 25:01356fb59467 34
Alex Young 25:01356fb59467 35 MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool;
Alex Young 25:01356fb59467 36 MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool;
Alex Young 26:665d3624f9ab 37 Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue;
Alex Young 4:98f063b2e6da 38
Alex Young 25:01356fb59467 39 static void* allocateMessageData(size_t bufSize)
Alex Young 4:98f063b2e6da 40 {
Alex Young 25:01356fb59467 41 return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL;
Alex Young 25:01356fb59467 42 }
Alex Young 25:01356fb59467 43
Alex Young 25:01356fb59467 44 static void deallocateMessageData(void const* buffer)
Alex Young 25:01356fb59467 45 {
Alex Young 25:01356fb59467 46 g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer);
Alex Young 4:98f063b2e6da 47 }
Alex Young 4:98f063b2e6da 48
Alex Young 4:98f063b2e6da 49 static void mtLowLevelHandler(void)
Alex Young 4:98f063b2e6da 50 {
Alex Young 4:98f063b2e6da 51 while (mt.readable())
Alex Young 4:98f063b2e6da 52 {
Alex Young 4:98f063b2e6da 53 XbusParser_parseByte(xbusParser, mt.getc());
Alex Young 4:98f063b2e6da 54 }
Alex Young 4:98f063b2e6da 55 }
Alex Young 4:98f063b2e6da 56
Alex Young 34:3d7a6519a256 57 static void sendMessage(XbusMessage const* m)
Alex Young 11:8593ba137917 58 {
Alex Young 26:665d3624f9ab 59 uint8_t buf[64];
Alex Young 26:665d3624f9ab 60 size_t rawLength = XbusMessage_format(buf, m);
Alex Young 11:8593ba137917 61 for (size_t i = 0; i < rawLength; ++i)
Alex Young 11:8593ba137917 62 {
Alex Young 11:8593ba137917 63 mt.putc(buf[i]);
Alex Young 11:8593ba137917 64 }
Alex Young 34:3d7a6519a256 65 }
Alex Young 34:3d7a6519a256 66
Alex Young 34:3d7a6519a256 67 static XbusMessage const* doTransaction(XbusMessage const* m)
Alex Young 34:3d7a6519a256 68 {
Alex Young 34:3d7a6519a256 69 sendMessage(m);
Alex Young 26:665d3624f9ab 70
Alex Young 26:665d3624f9ab 71 osEvent ev = g_responseQueue.get(500);
Alex Young 26:665d3624f9ab 72 return ev.status == osEventMessage ? (XbusMessage*)ev.value.p : NULL;
Alex Young 26:665d3624f9ab 73 }
Alex Young 26:665d3624f9ab 74
Alex Young 31:ce1ea9ae861e 75 /*!
Alex Young 31:ce1ea9ae861e 76 * \brief RAII object to manage message memory deallocation.
Alex Young 31:ce1ea9ae861e 77 *
Alex Young 31:ce1ea9ae861e 78 * Will automatically free the memory used by a XbusMessage when going out
Alex Young 31:ce1ea9ae861e 79 * of scope.
Alex Young 31:ce1ea9ae861e 80 */
Alex Young 31:ce1ea9ae861e 81 class XbusMessageMemoryManager
Alex Young 26:665d3624f9ab 82 {
Alex Young 31:ce1ea9ae861e 83 public:
Alex Young 31:ce1ea9ae861e 84 XbusMessageMemoryManager(XbusMessage const* message)
Alex Young 31:ce1ea9ae861e 85 : m_message(message)
Alex Young 31:ce1ea9ae861e 86 {
Alex Young 31:ce1ea9ae861e 87 }
Alex Young 31:ce1ea9ae861e 88
Alex Young 31:ce1ea9ae861e 89 ~XbusMessageMemoryManager()
Alex Young 31:ce1ea9ae861e 90 {
Alex Young 31:ce1ea9ae861e 91 if (m_message)
Alex Young 31:ce1ea9ae861e 92 {
Alex Young 31:ce1ea9ae861e 93 if (m_message->data)
Alex Young 31:ce1ea9ae861e 94 deallocateMessageData(m_message->data);
Alex Young 31:ce1ea9ae861e 95 g_messagePool.free(const_cast<XbusMessage*>(m_message));
Alex Young 31:ce1ea9ae861e 96 }
Alex Young 31:ce1ea9ae861e 97 }
Alex Young 31:ce1ea9ae861e 98
Alex Young 31:ce1ea9ae861e 99 private:
Alex Young 31:ce1ea9ae861e 100 XbusMessage const* m_message;
Alex Young 31:ce1ea9ae861e 101 };
Alex Young 26:665d3624f9ab 102
Alex Young 29:d9310e7b58b5 103 static void dumpResponse(XbusMessage const* response)
Alex Young 29:d9310e7b58b5 104 {
Alex Young 29:d9310e7b58b5 105 switch (response->mid)
Alex Young 29:d9310e7b58b5 106 {
Alex Young 29:d9310e7b58b5 107 case XMID_GotoConfigAck:
Alex Young 29:d9310e7b58b5 108 pc.printf("Device went to config mode\n");
Alex Young 29:d9310e7b58b5 109 break;
Alex Young 29:d9310e7b58b5 110
Alex Young 29:d9310e7b58b5 111 case XMID_DeviceId:
Alex Young 29:d9310e7b58b5 112 pc.printf("Device ID: %08X\n", *(uint32_t*)response->data);
Alex Young 29:d9310e7b58b5 113 break;
Alex Young 29:d9310e7b58b5 114
Alex Young 29:d9310e7b58b5 115 case XMID_OutputConfig:
Alex Young 29:d9310e7b58b5 116 {
Alex Young 29:d9310e7b58b5 117 pc.printf("Output configuration\n");
Alex Young 29:d9310e7b58b5 118 OutputConfiguration* conf = (OutputConfiguration*)response->data;
Alex Young 29:d9310e7b58b5 119 for (int i = 0; i < response->length; ++i)
Alex Young 29:d9310e7b58b5 120 {
Alex Young 29:d9310e7b58b5 121 pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 29:d9310e7b58b5 122 ++conf;
Alex Young 29:d9310e7b58b5 123 }
Alex Young 29:d9310e7b58b5 124 }
Alex Young 29:d9310e7b58b5 125 break;
Alex Young 29:d9310e7b58b5 126
Alex Young 29:d9310e7b58b5 127 case XMID_Error:
Alex Young 29:d9310e7b58b5 128 pc.printf("Device error!");
Alex Young 29:d9310e7b58b5 129 break;
Alex Young 29:d9310e7b58b5 130
Alex Young 29:d9310e7b58b5 131 default:
Alex Young 29:d9310e7b58b5 132 pc.printf("Received response MID=%X, length=%d\n", response->mid, response->length);
Alex Young 29:d9310e7b58b5 133 break;
Alex Young 29:d9310e7b58b5 134 }
Alex Young 29:d9310e7b58b5 135 }
Alex Young 29:d9310e7b58b5 136
Alex Young 26:665d3624f9ab 137 static void sendCommand(XsMessageId cmdId)
Alex Young 26:665d3624f9ab 138 {
Alex Young 26:665d3624f9ab 139 XbusMessage m = {cmdId};
Alex Young 26:665d3624f9ab 140 XbusMessage const* response = doTransaction(&m);
Alex Young 31:ce1ea9ae861e 141 XbusMessageMemoryManager janitor(response);
Alex Young 26:665d3624f9ab 142
Alex Young 26:665d3624f9ab 143 if (response)
Alex Young 26:665d3624f9ab 144 {
Alex Young 29:d9310e7b58b5 145 dumpResponse(response);
Alex Young 26:665d3624f9ab 146 }
Alex Young 26:665d3624f9ab 147 else
Alex Young 26:665d3624f9ab 148 {
Alex Young 26:665d3624f9ab 149 pc.printf("Timeout waiting for response.\n");
Alex Young 26:665d3624f9ab 150 }
Alex Young 11:8593ba137917 151 }
Alex Young 11:8593ba137917 152
Alex Young 11:8593ba137917 153 static void handlePcCommand(char cmd)
Alex Young 11:8593ba137917 154 {
Alex Young 11:8593ba137917 155 switch (cmd)
Alex Young 11:8593ba137917 156 {
Alex Young 11:8593ba137917 157 case 'c':
Alex Young 11:8593ba137917 158 sendCommand(XMID_GotoConfig);
Alex Young 11:8593ba137917 159 break;
Alex Young 11:8593ba137917 160
Alex Young 11:8593ba137917 161 case 'm':
Alex Young 11:8593ba137917 162 sendCommand(XMID_GotoMeasurement);
Alex Young 11:8593ba137917 163 break;
Alex Young 20:38560fa3d2eb 164
Alex Young 20:38560fa3d2eb 165 case 'd':
Alex Young 20:38560fa3d2eb 166 sendCommand(XMID_ReqDid);
Alex Young 20:38560fa3d2eb 167 break;
Alex Young 22:3eab999c5076 168
Alex Young 22:3eab999c5076 169 case 'o':
Alex Young 22:3eab999c5076 170 sendCommand(XMID_ReqOutputConfig);
Alex Young 22:3eab999c5076 171 break;
Alex Young 11:8593ba137917 172 }
Alex Young 11:8593ba137917 173 }
Alex Young 11:8593ba137917 174
Alex Young 24:2cc49dc854e3 175 static void handleDataMessage(struct XbusMessage const* message)
Alex Young 24:2cc49dc854e3 176 {
Alex Young 24:2cc49dc854e3 177 pc.printf("MTData2:");
Alex Young 24:2cc49dc854e3 178 uint16_t counter;
Alex Young 24:2cc49dc854e3 179 if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
Alex Young 24:2cc49dc854e3 180 {
Alex Young 24:2cc49dc854e3 181 pc.printf(" Packet counter: %5d", counter);
Alex Young 24:2cc49dc854e3 182 }
Alex Young 24:2cc49dc854e3 183 float ori[4];
Alex Young 24:2cc49dc854e3 184 if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
Alex Young 24:2cc49dc854e3 185 {
Alex Young 24:2cc49dc854e3 186 pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1],
Alex Young 24:2cc49dc854e3 187 ori[2], ori[3]);
Alex Young 24:2cc49dc854e3 188 }
Alex Young 24:2cc49dc854e3 189 uint32_t status;
Alex Young 24:2cc49dc854e3 190 if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
Alex Young 24:2cc49dc854e3 191 {
Alex Young 24:2cc49dc854e3 192 pc.printf(" Status:%X", status);
Alex Young 24:2cc49dc854e3 193 }
Alex Young 24:2cc49dc854e3 194 pc.printf("\n");
Alex Young 26:665d3624f9ab 195 deallocateMessageData(message->data);
Alex Young 24:2cc49dc854e3 196 }
Alex Young 24:2cc49dc854e3 197
Alex Young 24:2cc49dc854e3 198 static void mtMessageHandler(struct XbusMessage const* message)
Alex Young 4:98f063b2e6da 199 {
Alex Young 15:558d279addd9 200 if (message->mid == XMID_MtData2)
Alex Young 7:c913a7cd5231 201 {
Alex Young 24:2cc49dc854e3 202 handleDataMessage(message);
Alex Young 7:c913a7cd5231 203 }
Alex Young 7:c913a7cd5231 204 else
Alex Young 7:c913a7cd5231 205 {
Alex Young 26:665d3624f9ab 206 XbusMessage* m = g_messagePool.alloc();
Alex Young 26:665d3624f9ab 207 memcpy(m, message, sizeof(XbusMessage));
Alex Young 26:665d3624f9ab 208 g_responseQueue.put(m);
Alex Young 25:01356fb59467 209 }
Alex Young 4:98f063b2e6da 210 }
Alex Young 4:98f063b2e6da 211
Alex Young 4:98f063b2e6da 212 static void configureSerialPorts(void)
Alex Young 4:98f063b2e6da 213 {
Alex Young 4:98f063b2e6da 214 pc.baud(921600);
Alex Young 4:98f063b2e6da 215 pc.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 216
Alex Young 4:98f063b2e6da 217 mt.baud(921600);
Alex Young 4:98f063b2e6da 218 mt.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 219 mt.attach(mtLowLevelHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 220 }
Alex Young 4:98f063b2e6da 221
Alex Young 29:d9310e7b58b5 222 static uint32_t readDeviceId(void)
Alex Young 29:d9310e7b58b5 223 {
Alex Young 29:d9310e7b58b5 224 XbusMessage reqDid = {XMID_ReqDid};
Alex Young 29:d9310e7b58b5 225 XbusMessage const* didRsp = doTransaction(&reqDid);
Alex Young 31:ce1ea9ae861e 226 XbusMessageMemoryManager janitor(didRsp);
Alex Young 29:d9310e7b58b5 227 uint32_t deviceId = 0;
Alex Young 29:d9310e7b58b5 228 if (didRsp)
Alex Young 29:d9310e7b58b5 229 {
Alex Young 29:d9310e7b58b5 230 if (didRsp->mid == XMID_DeviceId)
Alex Young 29:d9310e7b58b5 231 {
Alex Young 29:d9310e7b58b5 232 deviceId = *(uint32_t*)didRsp->data;
Alex Young 29:d9310e7b58b5 233 }
Alex Young 29:d9310e7b58b5 234 }
Alex Young 29:d9310e7b58b5 235 return deviceId;
Alex Young 29:d9310e7b58b5 236 }
Alex Young 29:d9310e7b58b5 237
Alex Young 32:fafe0f42d82b 238 static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements)
Alex Young 29:d9310e7b58b5 239 {
Alex Young 32:fafe0f42d82b 240 XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf};
Alex Young 32:fafe0f42d82b 241 XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg);
Alex Young 32:fafe0f42d82b 242 XbusMessageMemoryManager janitor(outputConfRsp);
Alex Young 32:fafe0f42d82b 243 if (outputConfRsp)
Alex Young 29:d9310e7b58b5 244 {
Alex Young 32:fafe0f42d82b 245 if (outputConfRsp->mid == XMID_OutputConfig)
Alex Young 29:d9310e7b58b5 246 {
Alex Young 32:fafe0f42d82b 247 pc.printf("Output configuration set to:\n");
Alex Young 32:fafe0f42d82b 248 OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data;
Alex Young 32:fafe0f42d82b 249 for (int i = 0; i < outputConfRsp->length; ++i)
Alex Young 32:fafe0f42d82b 250 {
Alex Young 32:fafe0f42d82b 251 pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 32:fafe0f42d82b 252 ++conf;
Alex Young 32:fafe0f42d82b 253 }
Alex Young 32:fafe0f42d82b 254 return true;
Alex Young 29:d9310e7b58b5 255 }
Alex Young 29:d9310e7b58b5 256 else
Alex Young 29:d9310e7b58b5 257 {
Alex Young 32:fafe0f42d82b 258 dumpResponse(outputConfRsp);
Alex Young 29:d9310e7b58b5 259 }
Alex Young 32:fafe0f42d82b 260 }
Alex Young 32:fafe0f42d82b 261 else
Alex Young 32:fafe0f42d82b 262 {
Alex Young 32:fafe0f42d82b 263 pc.printf("Failed to set output configuration.\n");
Alex Young 32:fafe0f42d82b 264 }
Alex Young 32:fafe0f42d82b 265 return false;
Alex Young 32:fafe0f42d82b 266 }
Alex Young 29:d9310e7b58b5 267
Alex Young 32:fafe0f42d82b 268 static bool configureMotionTracker(void)
Alex Young 32:fafe0f42d82b 269 {
Alex Young 32:fafe0f42d82b 270 uint32_t deviceId = readDeviceId();
Alex Young 32:fafe0f42d82b 271
Alex Young 32:fafe0f42d82b 272 if (deviceId)
Alex Young 32:fafe0f42d82b 273 {
Alex Young 32:fafe0f42d82b 274 uint8_t deviceType = (deviceId >> 24) & 0x0F;
Alex Young 32:fafe0f42d82b 275 pc.printf("Found MTi-%d\n", deviceType);
Alex Young 32:fafe0f42d82b 276
Alex Young 32:fafe0f42d82b 277 if (deviceType == 1)
Alex Young 29:d9310e7b58b5 278 {
Alex Young 32:fafe0f42d82b 279 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 280 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 281 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 282 {XDI_Acceleration, 100},
Alex Young 32:fafe0f42d82b 283 {XDI_RateOfTurn, 100},
Alex Young 32:fafe0f42d82b 284 {XDI_MagneticField, 100}
Alex Young 32:fafe0f42d82b 285 };
Alex Young 32:fafe0f42d82b 286 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 287 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 288 }
Alex Young 29:d9310e7b58b5 289 else
Alex Young 29:d9310e7b58b5 290 {
Alex Young 32:fafe0f42d82b 291 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 292 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 293 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 294 {XDI_Quaternion, 100},
Alex Young 32:fafe0f42d82b 295 {XDI_StatusWord, 65535}
Alex Young 32:fafe0f42d82b 296 };
Alex Young 32:fafe0f42d82b 297 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 298 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 299 }
Alex Young 29:d9310e7b58b5 300 }
Alex Young 32:fafe0f42d82b 301
Alex Young 32:fafe0f42d82b 302 return false;
Alex Young 29:d9310e7b58b5 303 }
Alex Young 29:d9310e7b58b5 304
Alex Young 35:7e519b88c610 305 /*!
Alex Young 35:7e519b88c610 306 * \brief Wait for a wakeup message from the MTi.
Alex Young 35:7e519b88c610 307 * \return true if wakeup received within 1 second, else false.
Alex Young 35:7e519b88c610 308 *
Alex Young 35:7e519b88c610 309 * The MTi sends a XMID_Wakeup message once it has completed its bootup
Alex Young 35:7e519b88c610 310 * procedure. If this is acknowledged by a XMID_WakeupAck message then the MTi
Alex Young 35:7e519b88c610 311 * will stay in configuration mode. Otherwise it will automatically enter
Alex Young 35:7e519b88c610 312 * measurement mode with the stored output configuration.
Alex Young 35:7e519b88c610 313 */
Alex Young 35:7e519b88c610 314 bool waitForWakeup(void)
Alex Young 35:7e519b88c610 315 {
Alex Young 35:7e519b88c610 316 osEvent ev = g_responseQueue.get(1000);
Alex Young 35:7e519b88c610 317 if (ev.status == osEventMessage)
Alex Young 35:7e519b88c610 318 {
Alex Young 35:7e519b88c610 319 XbusMessage const* m = (XbusMessage const*)ev.value.p;
Alex Young 35:7e519b88c610 320 XbusMessageMemoryManager janitor(m);
Alex Young 35:7e519b88c610 321 return m->mid == XMID_Wakeup;
Alex Young 35:7e519b88c610 322 }
Alex Young 35:7e519b88c610 323 return false;
Alex Young 35:7e519b88c610 324 }
Alex Young 35:7e519b88c610 325
Alex Young 35:7e519b88c610 326 /*!
Alex Young 35:7e519b88c610 327 * \brief Releases the MTi reset line and waits for a wakeup message.
Alex Young 35:7e519b88c610 328 */
Alex Young 35:7e519b88c610 329 static void wakeupMotionTracker(void)
Alex Young 35:7e519b88c610 330 {
Alex Young 35:7e519b88c610 331 mtReset.write(1); // Release MT from reset.
Alex Young 35:7e519b88c610 332 if (waitForWakeup())
Alex Young 35:7e519b88c610 333 {
Alex Young 35:7e519b88c610 334 XbusMessage ack = {XMID_WakeupAck};
Alex Young 35:7e519b88c610 335 sendMessage(&ack);
Alex Young 35:7e519b88c610 336 }
Alex Young 35:7e519b88c610 337 }
Alex Young 35:7e519b88c610 338
Alex Young 2:b3e402dc11ca 339 int main(void)
Alex Young 2:b3e402dc11ca 340 {
Alex Young 4:98f063b2e6da 341 XbusParserCallback xbusCallback = {};
Alex Young 25:01356fb59467 342 xbusCallback.allocateBuffer = allocateMessageData;
Alex Young 25:01356fb59467 343 xbusCallback.deallocateBuffer = deallocateMessageData;
Alex Young 24:2cc49dc854e3 344 xbusCallback.handleMessage = mtMessageHandler;
Alex Young 4:98f063b2e6da 345
Alex Young 4:98f063b2e6da 346 xbusParser = XbusParser_create(&xbusCallback);
Alex Young 4:98f063b2e6da 347 configureSerialPorts();
Alex Young 35:7e519b88c610 348 wakeupMotionTracker();
Alex Young 29:d9310e7b58b5 349 if (configureMotionTracker())
Alex Young 5:abc52dd88be2 350 {
Alex Young 29:d9310e7b58b5 351 for (;;)
Alex Young 26:665d3624f9ab 352 {
Alex Young 29:d9310e7b58b5 353 while (pc.readable())
Alex Young 29:d9310e7b58b5 354 {
Alex Young 29:d9310e7b58b5 355 handlePcCommand(pc.getc());
Alex Young 29:d9310e7b58b5 356 }
Alex Young 26:665d3624f9ab 357 }
Alex Young 5:abc52dd88be2 358 }
Alex Young 29:d9310e7b58b5 359 else
Alex Young 29:d9310e7b58b5 360 {
Alex Young 29:d9310e7b58b5 361 pc.printf("Failed to configure motion tracker.\n");
Alex Young 29:d9310e7b58b5 362 return -1;
Alex Young 29:d9310e7b58b5 363 }
Alex Young 4:98f063b2e6da 364 }