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
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
- 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.
- 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)
- In all cases:
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.
- Connect to the target using a serial terminal. The application is configured for:
- Baudrate = 921600
- Stop bits = 1
- No parity bits
- No flow control
- Reset the mbed board.
- 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@40:b77a8c10c76d, 2015-05-21 (annotated)
- Committer:
- Alex Young
- Date:
- Thu May 21 14:36:52 2015 +0200
- Revision:
- 40:b77a8c10c76d
- Parent:
- 39:9014c5236864
- Child:
- 41:504b6821a96f
Add utility module for processing device ids
Who changed what in which revision?
User | Revision | Line number | New 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 |
26:665d3624f9ab | 401 | } |
Alex Young |
5:abc52dd88be2 | 402 | } |
Alex Young |
29:d9310e7b58b5 | 403 | else |
Alex Young |
29:d9310e7b58b5 | 404 | { |
Alex Young |
29:d9310e7b58b5 | 405 | pc.printf("Failed to configure motion tracker.\n"); |
Alex Young |
29:d9310e7b58b5 | 406 | return -1; |
Alex Young |
29:d9310e7b58b5 | 407 | } |
Alex Young |
4:98f063b2e6da | 408 | } |