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@42:e70756d2d8bc, 2015-05-21 (annotated)

Committer:
Alex Young
Date:
Thu May 21 15:21:02 2015 +0200
Revision:
42:e70756d2d8bc
Parent:
41:504b6821a96f
Child:
43:470c019246e4
Add support for printing acc/gyr/mag data

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