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@41:504b6821a96f, 2015-05-21 (annotated)

Committer:
Alex Young
Date:
Thu May 21 14:41:44 2015 +0200
Revision:
41:504b6821a96f
Parent:
40:b77a8c10c76d
Child:
42:e70756d2d8bc
Add wait in main loop.



This seems to help with response to pc commands.

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 24:2cc49dc854e3 166 uint32_t status;
Alex Young 24:2cc49dc854e3 167 if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
Alex Young 24:2cc49dc854e3 168 {
Alex Young 24:2cc49dc854e3 169 pc.printf(" Status:%X", status);
Alex Young 24:2cc49dc854e3 170 }
Alex Young 24:2cc49dc854e3 171 pc.printf("\n");
Alex Young 26:665d3624f9ab 172 deallocateMessageData(message->data);
Alex Young 24:2cc49dc854e3 173 }
Alex Young 24:2cc49dc854e3 174
Alex Young 24:2cc49dc854e3 175 static void mtMessageHandler(struct XbusMessage const* message)
Alex Young 4:98f063b2e6da 176 {
Alex Young 15:558d279addd9 177 if (message->mid == XMID_MtData2)
Alex Young 7:c913a7cd5231 178 {
Alex Young 24:2cc49dc854e3 179 handleDataMessage(message);
Alex Young 7:c913a7cd5231 180 }
Alex Young 7:c913a7cd5231 181 else
Alex Young 7:c913a7cd5231 182 {
Alex Young 26:665d3624f9ab 183 XbusMessage* m = g_messagePool.alloc();
Alex Young 26:665d3624f9ab 184 memcpy(m, message, sizeof(XbusMessage));
Alex Young 26:665d3624f9ab 185 g_responseQueue.put(m);
Alex Young 25:01356fb59467 186 }
Alex Young 4:98f063b2e6da 187 }
Alex Young 4:98f063b2e6da 188
Alex Young 4:98f063b2e6da 189 static void configureSerialPorts(void)
Alex Young 4:98f063b2e6da 190 {
Alex Young 4:98f063b2e6da 191 pc.baud(921600);
Alex Young 4:98f063b2e6da 192 pc.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 193
Alex Young 37:3e87bf647c68 194 mt.baud(115200);
Alex Young 4:98f063b2e6da 195 mt.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 196 mt.attach(mtLowLevelHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 197 }
Alex Young 4:98f063b2e6da 198
Alex Young 29:d9310e7b58b5 199 static uint32_t readDeviceId(void)
Alex Young 29:d9310e7b58b5 200 {
Alex Young 29:d9310e7b58b5 201 XbusMessage reqDid = {XMID_ReqDid};
Alex Young 29:d9310e7b58b5 202 XbusMessage const* didRsp = doTransaction(&reqDid);
Alex Young 31:ce1ea9ae861e 203 XbusMessageMemoryManager janitor(didRsp);
Alex Young 29:d9310e7b58b5 204 uint32_t deviceId = 0;
Alex Young 29:d9310e7b58b5 205 if (didRsp)
Alex Young 29:d9310e7b58b5 206 {
Alex Young 29:d9310e7b58b5 207 if (didRsp->mid == XMID_DeviceId)
Alex Young 29:d9310e7b58b5 208 {
Alex Young 29:d9310e7b58b5 209 deviceId = *(uint32_t*)didRsp->data;
Alex Young 29:d9310e7b58b5 210 }
Alex Young 29:d9310e7b58b5 211 }
Alex Young 29:d9310e7b58b5 212 return deviceId;
Alex Young 29:d9310e7b58b5 213 }
Alex Young 29:d9310e7b58b5 214
Alex Young 32:fafe0f42d82b 215 static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements)
Alex Young 29:d9310e7b58b5 216 {
Alex Young 32:fafe0f42d82b 217 XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf};
Alex Young 32:fafe0f42d82b 218 XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg);
Alex Young 32:fafe0f42d82b 219 XbusMessageMemoryManager janitor(outputConfRsp);
Alex Young 32:fafe0f42d82b 220 if (outputConfRsp)
Alex Young 29:d9310e7b58b5 221 {
Alex Young 32:fafe0f42d82b 222 if (outputConfRsp->mid == XMID_OutputConfig)
Alex Young 29:d9310e7b58b5 223 {
Alex Young 32:fafe0f42d82b 224 pc.printf("Output configuration set to:\n");
Alex Young 32:fafe0f42d82b 225 OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data;
Alex Young 32:fafe0f42d82b 226 for (int i = 0; i < outputConfRsp->length; ++i)
Alex Young 32:fafe0f42d82b 227 {
Alex Young 32:fafe0f42d82b 228 pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 32:fafe0f42d82b 229 ++conf;
Alex Young 32:fafe0f42d82b 230 }
Alex Young 32:fafe0f42d82b 231 return true;
Alex Young 29:d9310e7b58b5 232 }
Alex Young 29:d9310e7b58b5 233 else
Alex Young 29:d9310e7b58b5 234 {
Alex Young 32:fafe0f42d82b 235 dumpResponse(outputConfRsp);
Alex Young 29:d9310e7b58b5 236 }
Alex Young 32:fafe0f42d82b 237 }
Alex Young 32:fafe0f42d82b 238 else
Alex Young 32:fafe0f42d82b 239 {
Alex Young 32:fafe0f42d82b 240 pc.printf("Failed to set output configuration.\n");
Alex Young 32:fafe0f42d82b 241 }
Alex Young 32:fafe0f42d82b 242 return false;
Alex Young 32:fafe0f42d82b 243 }
Alex Young 29:d9310e7b58b5 244
Alex Young 32:fafe0f42d82b 245 static bool configureMotionTracker(void)
Alex Young 32:fafe0f42d82b 246 {
Alex Young 32:fafe0f42d82b 247 uint32_t deviceId = readDeviceId();
Alex Young 32:fafe0f42d82b 248
Alex Young 32:fafe0f42d82b 249 if (deviceId)
Alex Young 32:fafe0f42d82b 250 {
Alex Young 40:b77a8c10c76d 251 pc.printf("Found device with ID: %08X.\n", deviceId);
Alex Young 40:b77a8c10c76d 252 if (!XsDeviceId_isMtMk4_X(deviceId))
Alex Young 40:b77a8c10c76d 253 {
Alex Young 40:b77a8c10c76d 254 pc.printf("Device is not an MTi-1 series.\n");
Alex Young 40:b77a8c10c76d 255 return false;
Alex Young 40:b77a8c10c76d 256 }
Alex Young 32:fafe0f42d82b 257
Alex Young 40:b77a8c10c76d 258 DeviceFunction function = XsDeviceId_getFunction(deviceId);
Alex Young 40:b77a8c10c76d 259 pc.printf("Device is an MTi-%d: %s.\n", function, XsDeviceId_functionDescription(function));
Alex Young 40:b77a8c10c76d 260
Alex Young 40:b77a8c10c76d 261 if (function == DF_IMU)
Alex Young 29:d9310e7b58b5 262 {
Alex Young 32:fafe0f42d82b 263 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 264 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 265 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 266 {XDI_Acceleration, 100},
Alex Young 32:fafe0f42d82b 267 {XDI_RateOfTurn, 100},
Alex Young 32:fafe0f42d82b 268 {XDI_MagneticField, 100}
Alex Young 32:fafe0f42d82b 269 };
Alex Young 32:fafe0f42d82b 270 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 271 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 272 }
Alex Young 29:d9310e7b58b5 273 else
Alex Young 29:d9310e7b58b5 274 {
Alex Young 32:fafe0f42d82b 275 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 276 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 277 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 278 {XDI_Quaternion, 100},
Alex Young 32:fafe0f42d82b 279 {XDI_StatusWord, 65535}
Alex Young 32:fafe0f42d82b 280 };
Alex Young 32:fafe0f42d82b 281 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 282 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 283 }
Alex Young 29:d9310e7b58b5 284 }
Alex Young 32:fafe0f42d82b 285
Alex Young 32:fafe0f42d82b 286 return false;
Alex Young 29:d9310e7b58b5 287 }
Alex Young 29:d9310e7b58b5 288
Alex Young 35:7e519b88c610 289 /*!
Alex Young 35:7e519b88c610 290 * \brief Wait for a wakeup message from the MTi.
Alex Young 37:3e87bf647c68 291 * \param timeout Time to wait to receive the wakeup message.
Alex Young 37:3e87bf647c68 292 * \return true if wakeup received within timeout, else false.
Alex Young 35:7e519b88c610 293 *
Alex Young 35:7e519b88c610 294 * The MTi sends a XMID_Wakeup message once it has completed its bootup
Alex Young 35:7e519b88c610 295 * procedure. If this is acknowledged by a XMID_WakeupAck message then the MTi
Alex Young 35:7e519b88c610 296 * will stay in configuration mode. Otherwise it will automatically enter
Alex Young 35:7e519b88c610 297 * measurement mode with the stored output configuration.
Alex Young 35:7e519b88c610 298 */
Alex Young 37:3e87bf647c68 299 bool waitForWakeup(uint32_t timeout)
Alex Young 35:7e519b88c610 300 {
Alex Young 37:3e87bf647c68 301 osEvent ev = g_responseQueue.get(timeout);
Alex Young 35:7e519b88c610 302 if (ev.status == osEventMessage)
Alex Young 35:7e519b88c610 303 {
Alex Young 35:7e519b88c610 304 XbusMessage const* m = (XbusMessage const*)ev.value.p;
Alex Young 35:7e519b88c610 305 XbusMessageMemoryManager janitor(m);
Alex Young 35:7e519b88c610 306 return m->mid == XMID_Wakeup;
Alex Young 35:7e519b88c610 307 }
Alex Young 35:7e519b88c610 308 return false;
Alex Young 35:7e519b88c610 309 }
Alex Young 35:7e519b88c610 310
Alex Young 35:7e519b88c610 311 /*!
Alex Young 37:3e87bf647c68 312 * \brief Send wakeup acknowledge message to MTi.
Alex Young 37:3e87bf647c68 313 *
Alex Young 37:3e87bf647c68 314 * Sending a wakeup acknowledge will cause the device to stay in configuration
Alex Young 37:3e87bf647c68 315 * mode instead of automatically transitioning to measurement mode with the
Alex Young 37:3e87bf647c68 316 * stored output configuration.
Alex Young 37:3e87bf647c68 317 */
Alex Young 37:3e87bf647c68 318 void sendWakeupAck(void)
Alex Young 37:3e87bf647c68 319 {
Alex Young 37:3e87bf647c68 320 XbusMessage ack = {XMID_WakeupAck};
Alex Young 37:3e87bf647c68 321 sendMessage(&ack);
Alex Young 37:3e87bf647c68 322 pc.printf("Device ready for operation.\n");
Alex Young 37:3e87bf647c68 323 }
Alex Young 37:3e87bf647c68 324
Alex Young 37:3e87bf647c68 325 /*!
Alex Young 37:3e87bf647c68 326 * \brief Restore communication with the MTi.
Alex Young 37:3e87bf647c68 327 *
Alex Young 37:3e87bf647c68 328 * On bootup the MTi will listen for a magic byte to signal that it should
Alex Young 37:3e87bf647c68 329 * return to default baudrate and output configuration. This can be used to
Alex Young 37:3e87bf647c68 330 * recover from a bad or unknown configuration.
Alex Young 37:3e87bf647c68 331 */
Alex Young 37:3e87bf647c68 332 void restoreCommunication(void)
Alex Young 37:3e87bf647c68 333 {
Alex Young 37:3e87bf647c68 334 pc.printf("Restoring communication with device... ");
Alex Young 37:3e87bf647c68 335 mtReset = 0;
Alex Young 37:3e87bf647c68 336 Thread::wait(1);
Alex Young 37:3e87bf647c68 337 mtReset = 1;
Alex Young 37:3e87bf647c68 338
Alex Young 37:3e87bf647c68 339 do
Alex Young 37:3e87bf647c68 340 {
Alex Young 37:3e87bf647c68 341 mt.putc(0xDE);
Alex Young 37:3e87bf647c68 342 }
Alex Young 37:3e87bf647c68 343 while (!waitForWakeup(1));
Alex Young 37:3e87bf647c68 344 pc.printf("done\n");
Alex Young 37:3e87bf647c68 345
Alex Young 37:3e87bf647c68 346 sendWakeupAck();
Alex Young 37:3e87bf647c68 347 }
Alex Young 37:3e87bf647c68 348
Alex Young 37:3e87bf647c68 349 /*!
Alex Young 35:7e519b88c610 350 * \brief Releases the MTi reset line and waits for a wakeup message.
Alex Young 37:3e87bf647c68 351 *
Alex Young 37:3e87bf647c68 352 * If no wakeup message is received within 1 second the restore communications
Alex Young 37:3e87bf647c68 353 * procedure is done to reset the MTi to default baudrate and output configuration.
Alex Young 35:7e519b88c610 354 */
Alex Young 35:7e519b88c610 355 static void wakeupMotionTracker(void)
Alex Young 35:7e519b88c610 356 {
Alex Young 35:7e519b88c610 357 mtReset.write(1); // Release MT from reset.
Alex Young 37:3e87bf647c68 358 if (waitForWakeup(1000))
Alex Young 35:7e519b88c610 359 {
Alex Young 37:3e87bf647c68 360 sendWakeupAck();
Alex Young 37:3e87bf647c68 361 }
Alex Young 37:3e87bf647c68 362 else
Alex Young 37:3e87bf647c68 363 {
Alex Young 37:3e87bf647c68 364 restoreCommunication();
Alex Young 35:7e519b88c610 365 }
Alex Young 35:7e519b88c610 366 }
Alex Young 35:7e519b88c610 367
Alex Young 38:d8d410d1662c 368 static void printIntroMessage(void)
Alex Young 38:d8d410d1662c 369 {
Alex Young 38:d8d410d1662c 370 pc.printf("\n\n\n\n\n");
Alex Young 38:d8d410d1662c 371 pc.printf("MTi-1 series embedded example firmware.\n");
Alex Young 38:d8d410d1662c 372 }
Alex Young 38:d8d410d1662c 373
Alex Young 38:d8d410d1662c 374 static void printUsageInstructions(void)
Alex Young 38:d8d410d1662c 375 {
Alex Young 38:d8d410d1662c 376 pc.printf("\n");
Alex Young 38:d8d410d1662c 377 pc.printf("Press 'm' to start measuring and 'c' to return to config mode.\n");
Alex Young 38:d8d410d1662c 378 }
Alex Young 38:d8d410d1662c 379
Alex Young 2:b3e402dc11ca 380 int main(void)
Alex Young 2:b3e402dc11ca 381 {
Alex Young 4:98f063b2e6da 382 XbusParserCallback xbusCallback = {};
Alex Young 25:01356fb59467 383 xbusCallback.allocateBuffer = allocateMessageData;
Alex Young 25:01356fb59467 384 xbusCallback.deallocateBuffer = deallocateMessageData;
Alex Young 24:2cc49dc854e3 385 xbusCallback.handleMessage = mtMessageHandler;
Alex Young 4:98f063b2e6da 386
Alex Young 4:98f063b2e6da 387 xbusParser = XbusParser_create(&xbusCallback);
Alex Young 4:98f063b2e6da 388 configureSerialPorts();
Alex Young 38:d8d410d1662c 389
Alex Young 38:d8d410d1662c 390 printIntroMessage();
Alex Young 35:7e519b88c610 391 wakeupMotionTracker();
Alex Young 29:d9310e7b58b5 392 if (configureMotionTracker())
Alex Young 5:abc52dd88be2 393 {
Alex Young 38:d8d410d1662c 394 printUsageInstructions();
Alex Young 29:d9310e7b58b5 395 for (;;)
Alex Young 26:665d3624f9ab 396 {
Alex Young 29:d9310e7b58b5 397 while (pc.readable())
Alex Young 29:d9310e7b58b5 398 {
Alex Young 29:d9310e7b58b5 399 handlePcCommand(pc.getc());
Alex Young 29:d9310e7b58b5 400 }
Alex Young 41:504b6821a96f 401 Thread::wait(10);
Alex Young 26:665d3624f9ab 402 }
Alex Young 5:abc52dd88be2 403 }
Alex Young 29:d9310e7b58b5 404 else
Alex Young 29:d9310e7b58b5 405 {
Alex Young 29:d9310e7b58b5 406 pc.printf("Failed to configure motion tracker.\n");
Alex Young 29:d9310e7b58b5 407 return -1;
Alex Young 29:d9310e7b58b5 408 }
Alex Young 4:98f063b2e6da 409 }