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@34:3d7a6519a256, 2015-05-21 (annotated)
- Committer:
- Alex Young
- Date:
- Thu May 21 12:19:50 2015 +0200
- Revision:
- 34:3d7a6519a256
- Parent:
- 32:fafe0f42d82b
- Child:
- 35:7e519b88c610
Split out sending message from a full transaction.
The wakeup ack message will not have a response from the MT.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Alex Young |
4:98f063b2e6da | 1 | #include "mbed.h" |
Alex Young |
25:01356fb59467 | 2 | #include "rtos.h" |
Alex Young |
4:98f063b2e6da | 3 | #include "xbusparser.h" |
Alex Young |
11:8593ba137917 | 4 | #include "xbusmessage.h" |
Alex Young |
4:98f063b2e6da | 5 | |
Alex Young |
25:01356fb59467 | 6 | #define MEMORY_POOL_SIZE (4) |
Alex Young |
26:665d3624f9ab | 7 | #define RESPONSE_QUEUE_SIZE (1) |
Alex Young |
25:01356fb59467 | 8 | #define MAX_XBUS_DATA_SIZE (128) |
Alex Young |
25:01356fb59467 | 9 | |
Alex Young |
4:98f063b2e6da | 10 | static Serial pc(PA_2, PA_3); |
Alex Young |
4:98f063b2e6da | 11 | static Serial mt(PB_9, PB_8); |
Alex Young |
4:98f063b2e6da | 12 | static XbusParser* xbusParser; |
Alex Young |
25:01356fb59467 | 13 | |
Alex Young |
25:01356fb59467 | 14 | MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool; |
Alex Young |
25:01356fb59467 | 15 | MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool; |
Alex Young |
26:665d3624f9ab | 16 | Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue; |
Alex Young |
4:98f063b2e6da | 17 | |
Alex Young |
25:01356fb59467 | 18 | static void* allocateMessageData(size_t bufSize) |
Alex Young |
4:98f063b2e6da | 19 | { |
Alex Young |
25:01356fb59467 | 20 | return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL; |
Alex Young |
25:01356fb59467 | 21 | } |
Alex Young |
25:01356fb59467 | 22 | |
Alex Young |
25:01356fb59467 | 23 | static void deallocateMessageData(void const* buffer) |
Alex Young |
25:01356fb59467 | 24 | { |
Alex Young |
25:01356fb59467 | 25 | g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer); |
Alex Young |
4:98f063b2e6da | 26 | } |
Alex Young |
4:98f063b2e6da | 27 | |
Alex Young |
4:98f063b2e6da | 28 | static void mtLowLevelHandler(void) |
Alex Young |
4:98f063b2e6da | 29 | { |
Alex Young |
4:98f063b2e6da | 30 | while (mt.readable()) |
Alex Young |
4:98f063b2e6da | 31 | { |
Alex Young |
4:98f063b2e6da | 32 | XbusParser_parseByte(xbusParser, mt.getc()); |
Alex Young |
4:98f063b2e6da | 33 | } |
Alex Young |
4:98f063b2e6da | 34 | } |
Alex Young |
4:98f063b2e6da | 35 | |
Alex Young |
34:3d7a6519a256 | 36 | static void sendMessage(XbusMessage const* m) |
Alex Young |
11:8593ba137917 | 37 | { |
Alex Young |
26:665d3624f9ab | 38 | uint8_t buf[64]; |
Alex Young |
26:665d3624f9ab | 39 | size_t rawLength = XbusMessage_format(buf, m); |
Alex Young |
11:8593ba137917 | 40 | for (size_t i = 0; i < rawLength; ++i) |
Alex Young |
11:8593ba137917 | 41 | { |
Alex Young |
11:8593ba137917 | 42 | mt.putc(buf[i]); |
Alex Young |
11:8593ba137917 | 43 | } |
Alex Young |
34:3d7a6519a256 | 44 | } |
Alex Young |
34:3d7a6519a256 | 45 | |
Alex Young |
34:3d7a6519a256 | 46 | static XbusMessage const* doTransaction(XbusMessage const* m) |
Alex Young |
34:3d7a6519a256 | 47 | { |
Alex Young |
34:3d7a6519a256 | 48 | sendMessage(m); |
Alex Young |
26:665d3624f9ab | 49 | |
Alex Young |
26:665d3624f9ab | 50 | osEvent ev = g_responseQueue.get(500); |
Alex Young |
26:665d3624f9ab | 51 | return ev.status == osEventMessage ? (XbusMessage*)ev.value.p : NULL; |
Alex Young |
26:665d3624f9ab | 52 | } |
Alex Young |
26:665d3624f9ab | 53 | |
Alex Young |
31:ce1ea9ae861e | 54 | /*! |
Alex Young |
31:ce1ea9ae861e | 55 | * \brief RAII object to manage message memory deallocation. |
Alex Young |
31:ce1ea9ae861e | 56 | * |
Alex Young |
31:ce1ea9ae861e | 57 | * Will automatically free the memory used by a XbusMessage when going out |
Alex Young |
31:ce1ea9ae861e | 58 | * of scope. |
Alex Young |
31:ce1ea9ae861e | 59 | */ |
Alex Young |
31:ce1ea9ae861e | 60 | class XbusMessageMemoryManager |
Alex Young |
26:665d3624f9ab | 61 | { |
Alex Young |
31:ce1ea9ae861e | 62 | public: |
Alex Young |
31:ce1ea9ae861e | 63 | XbusMessageMemoryManager(XbusMessage const* message) |
Alex Young |
31:ce1ea9ae861e | 64 | : m_message(message) |
Alex Young |
31:ce1ea9ae861e | 65 | { |
Alex Young |
31:ce1ea9ae861e | 66 | } |
Alex Young |
31:ce1ea9ae861e | 67 | |
Alex Young |
31:ce1ea9ae861e | 68 | ~XbusMessageMemoryManager() |
Alex Young |
31:ce1ea9ae861e | 69 | { |
Alex Young |
31:ce1ea9ae861e | 70 | if (m_message) |
Alex Young |
31:ce1ea9ae861e | 71 | { |
Alex Young |
31:ce1ea9ae861e | 72 | if (m_message->data) |
Alex Young |
31:ce1ea9ae861e | 73 | deallocateMessageData(m_message->data); |
Alex Young |
31:ce1ea9ae861e | 74 | g_messagePool.free(const_cast<XbusMessage*>(m_message)); |
Alex Young |
31:ce1ea9ae861e | 75 | } |
Alex Young |
31:ce1ea9ae861e | 76 | } |
Alex Young |
31:ce1ea9ae861e | 77 | |
Alex Young |
31:ce1ea9ae861e | 78 | private: |
Alex Young |
31:ce1ea9ae861e | 79 | XbusMessage const* m_message; |
Alex Young |
31:ce1ea9ae861e | 80 | }; |
Alex Young |
26:665d3624f9ab | 81 | |
Alex Young |
29:d9310e7b58b5 | 82 | static void dumpResponse(XbusMessage const* response) |
Alex Young |
29:d9310e7b58b5 | 83 | { |
Alex Young |
29:d9310e7b58b5 | 84 | switch (response->mid) |
Alex Young |
29:d9310e7b58b5 | 85 | { |
Alex Young |
29:d9310e7b58b5 | 86 | case XMID_GotoConfigAck: |
Alex Young |
29:d9310e7b58b5 | 87 | pc.printf("Device went to config mode\n"); |
Alex Young |
29:d9310e7b58b5 | 88 | break; |
Alex Young |
29:d9310e7b58b5 | 89 | |
Alex Young |
29:d9310e7b58b5 | 90 | case XMID_DeviceId: |
Alex Young |
29:d9310e7b58b5 | 91 | pc.printf("Device ID: %08X\n", *(uint32_t*)response->data); |
Alex Young |
29:d9310e7b58b5 | 92 | break; |
Alex Young |
29:d9310e7b58b5 | 93 | |
Alex Young |
29:d9310e7b58b5 | 94 | case XMID_OutputConfig: |
Alex Young |
29:d9310e7b58b5 | 95 | { |
Alex Young |
29:d9310e7b58b5 | 96 | pc.printf("Output configuration\n"); |
Alex Young |
29:d9310e7b58b5 | 97 | OutputConfiguration* conf = (OutputConfiguration*)response->data; |
Alex Young |
29:d9310e7b58b5 | 98 | for (int i = 0; i < response->length; ++i) |
Alex Young |
29:d9310e7b58b5 | 99 | { |
Alex Young |
29:d9310e7b58b5 | 100 | pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq); |
Alex Young |
29:d9310e7b58b5 | 101 | ++conf; |
Alex Young |
29:d9310e7b58b5 | 102 | } |
Alex Young |
29:d9310e7b58b5 | 103 | } |
Alex Young |
29:d9310e7b58b5 | 104 | break; |
Alex Young |
29:d9310e7b58b5 | 105 | |
Alex Young |
29:d9310e7b58b5 | 106 | case XMID_Error: |
Alex Young |
29:d9310e7b58b5 | 107 | pc.printf("Device error!"); |
Alex Young |
29:d9310e7b58b5 | 108 | break; |
Alex Young |
29:d9310e7b58b5 | 109 | |
Alex Young |
29:d9310e7b58b5 | 110 | default: |
Alex Young |
29:d9310e7b58b5 | 111 | pc.printf("Received response MID=%X, length=%d\n", response->mid, response->length); |
Alex Young |
29:d9310e7b58b5 | 112 | break; |
Alex Young |
29:d9310e7b58b5 | 113 | } |
Alex Young |
29:d9310e7b58b5 | 114 | } |
Alex Young |
29:d9310e7b58b5 | 115 | |
Alex Young |
26:665d3624f9ab | 116 | static void sendCommand(XsMessageId cmdId) |
Alex Young |
26:665d3624f9ab | 117 | { |
Alex Young |
26:665d3624f9ab | 118 | XbusMessage m = {cmdId}; |
Alex Young |
26:665d3624f9ab | 119 | XbusMessage const* response = doTransaction(&m); |
Alex Young |
31:ce1ea9ae861e | 120 | XbusMessageMemoryManager janitor(response); |
Alex Young |
26:665d3624f9ab | 121 | |
Alex Young |
26:665d3624f9ab | 122 | if (response) |
Alex Young |
26:665d3624f9ab | 123 | { |
Alex Young |
29:d9310e7b58b5 | 124 | dumpResponse(response); |
Alex Young |
26:665d3624f9ab | 125 | } |
Alex Young |
26:665d3624f9ab | 126 | else |
Alex Young |
26:665d3624f9ab | 127 | { |
Alex Young |
26:665d3624f9ab | 128 | pc.printf("Timeout waiting for response.\n"); |
Alex Young |
26:665d3624f9ab | 129 | } |
Alex Young |
11:8593ba137917 | 130 | } |
Alex Young |
11:8593ba137917 | 131 | |
Alex Young |
11:8593ba137917 | 132 | static void handlePcCommand(char cmd) |
Alex Young |
11:8593ba137917 | 133 | { |
Alex Young |
11:8593ba137917 | 134 | switch (cmd) |
Alex Young |
11:8593ba137917 | 135 | { |
Alex Young |
11:8593ba137917 | 136 | case 'c': |
Alex Young |
11:8593ba137917 | 137 | sendCommand(XMID_GotoConfig); |
Alex Young |
11:8593ba137917 | 138 | break; |
Alex Young |
11:8593ba137917 | 139 | |
Alex Young |
11:8593ba137917 | 140 | case 'm': |
Alex Young |
11:8593ba137917 | 141 | sendCommand(XMID_GotoMeasurement); |
Alex Young |
11:8593ba137917 | 142 | break; |
Alex Young |
20:38560fa3d2eb | 143 | |
Alex Young |
20:38560fa3d2eb | 144 | case 'd': |
Alex Young |
20:38560fa3d2eb | 145 | sendCommand(XMID_ReqDid); |
Alex Young |
20:38560fa3d2eb | 146 | break; |
Alex Young |
22:3eab999c5076 | 147 | |
Alex Young |
22:3eab999c5076 | 148 | case 'o': |
Alex Young |
22:3eab999c5076 | 149 | sendCommand(XMID_ReqOutputConfig); |
Alex Young |
22:3eab999c5076 | 150 | break; |
Alex Young |
11:8593ba137917 | 151 | } |
Alex Young |
11:8593ba137917 | 152 | } |
Alex Young |
11:8593ba137917 | 153 | |
Alex Young |
24:2cc49dc854e3 | 154 | static void handleDataMessage(struct XbusMessage const* message) |
Alex Young |
24:2cc49dc854e3 | 155 | { |
Alex Young |
24:2cc49dc854e3 | 156 | pc.printf("MTData2:"); |
Alex Young |
24:2cc49dc854e3 | 157 | uint16_t counter; |
Alex Young |
24:2cc49dc854e3 | 158 | if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message)) |
Alex Young |
24:2cc49dc854e3 | 159 | { |
Alex Young |
24:2cc49dc854e3 | 160 | pc.printf(" Packet counter: %5d", counter); |
Alex Young |
24:2cc49dc854e3 | 161 | } |
Alex Young |
24:2cc49dc854e3 | 162 | float ori[4]; |
Alex Young |
24:2cc49dc854e3 | 163 | if (XbusMessage_getDataItem(ori, XDI_Quaternion, message)) |
Alex Young |
24:2cc49dc854e3 | 164 | { |
Alex Young |
24:2cc49dc854e3 | 165 | pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1], |
Alex Young |
24:2cc49dc854e3 | 166 | ori[2], ori[3]); |
Alex Young |
24:2cc49dc854e3 | 167 | } |
Alex Young |
24:2cc49dc854e3 | 168 | uint32_t status; |
Alex Young |
24:2cc49dc854e3 | 169 | if (XbusMessage_getDataItem(&status, XDI_StatusWord, message)) |
Alex Young |
24:2cc49dc854e3 | 170 | { |
Alex Young |
24:2cc49dc854e3 | 171 | pc.printf(" Status:%X", status); |
Alex Young |
24:2cc49dc854e3 | 172 | } |
Alex Young |
24:2cc49dc854e3 | 173 | pc.printf("\n"); |
Alex Young |
26:665d3624f9ab | 174 | deallocateMessageData(message->data); |
Alex Young |
24:2cc49dc854e3 | 175 | } |
Alex Young |
24:2cc49dc854e3 | 176 | |
Alex Young |
24:2cc49dc854e3 | 177 | static void mtMessageHandler(struct XbusMessage const* message) |
Alex Young |
4:98f063b2e6da | 178 | { |
Alex Young |
15:558d279addd9 | 179 | if (message->mid == XMID_MtData2) |
Alex Young |
7:c913a7cd5231 | 180 | { |
Alex Young |
24:2cc49dc854e3 | 181 | handleDataMessage(message); |
Alex Young |
7:c913a7cd5231 | 182 | } |
Alex Young |
7:c913a7cd5231 | 183 | else |
Alex Young |
7:c913a7cd5231 | 184 | { |
Alex Young |
26:665d3624f9ab | 185 | XbusMessage* m = g_messagePool.alloc(); |
Alex Young |
26:665d3624f9ab | 186 | memcpy(m, message, sizeof(XbusMessage)); |
Alex Young |
26:665d3624f9ab | 187 | g_responseQueue.put(m); |
Alex Young |
25:01356fb59467 | 188 | } |
Alex Young |
4:98f063b2e6da | 189 | } |
Alex Young |
4:98f063b2e6da | 190 | |
Alex Young |
4:98f063b2e6da | 191 | static void configureSerialPorts(void) |
Alex Young |
4:98f063b2e6da | 192 | { |
Alex Young |
4:98f063b2e6da | 193 | pc.baud(921600); |
Alex Young |
4:98f063b2e6da | 194 | pc.format(8, Serial::None, 2); |
Alex Young |
4:98f063b2e6da | 195 | |
Alex Young |
4:98f063b2e6da | 196 | mt.baud(921600); |
Alex Young |
4:98f063b2e6da | 197 | mt.format(8, Serial::None, 2); |
Alex Young |
4:98f063b2e6da | 198 | mt.attach(mtLowLevelHandler, Serial::RxIrq); |
Alex Young |
4:98f063b2e6da | 199 | } |
Alex Young |
4:98f063b2e6da | 200 | |
Alex Young |
29:d9310e7b58b5 | 201 | static uint32_t readDeviceId(void) |
Alex Young |
29:d9310e7b58b5 | 202 | { |
Alex Young |
29:d9310e7b58b5 | 203 | XbusMessage reqDid = {XMID_ReqDid}; |
Alex Young |
29:d9310e7b58b5 | 204 | XbusMessage const* didRsp = doTransaction(&reqDid); |
Alex Young |
31:ce1ea9ae861e | 205 | XbusMessageMemoryManager janitor(didRsp); |
Alex Young |
29:d9310e7b58b5 | 206 | uint32_t deviceId = 0; |
Alex Young |
29:d9310e7b58b5 | 207 | if (didRsp) |
Alex Young |
29:d9310e7b58b5 | 208 | { |
Alex Young |
29:d9310e7b58b5 | 209 | if (didRsp->mid == XMID_DeviceId) |
Alex Young |
29:d9310e7b58b5 | 210 | { |
Alex Young |
29:d9310e7b58b5 | 211 | deviceId = *(uint32_t*)didRsp->data; |
Alex Young |
29:d9310e7b58b5 | 212 | } |
Alex Young |
29:d9310e7b58b5 | 213 | } |
Alex Young |
29:d9310e7b58b5 | 214 | return deviceId; |
Alex Young |
29:d9310e7b58b5 | 215 | } |
Alex Young |
29:d9310e7b58b5 | 216 | |
Alex Young |
32:fafe0f42d82b | 217 | static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements) |
Alex Young |
29:d9310e7b58b5 | 218 | { |
Alex Young |
32:fafe0f42d82b | 219 | XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf}; |
Alex Young |
32:fafe0f42d82b | 220 | XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg); |
Alex Young |
32:fafe0f42d82b | 221 | XbusMessageMemoryManager janitor(outputConfRsp); |
Alex Young |
32:fafe0f42d82b | 222 | if (outputConfRsp) |
Alex Young |
29:d9310e7b58b5 | 223 | { |
Alex Young |
32:fafe0f42d82b | 224 | if (outputConfRsp->mid == XMID_OutputConfig) |
Alex Young |
29:d9310e7b58b5 | 225 | { |
Alex Young |
32:fafe0f42d82b | 226 | pc.printf("Output configuration set to:\n"); |
Alex Young |
32:fafe0f42d82b | 227 | OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data; |
Alex Young |
32:fafe0f42d82b | 228 | for (int i = 0; i < outputConfRsp->length; ++i) |
Alex Young |
32:fafe0f42d82b | 229 | { |
Alex Young |
32:fafe0f42d82b | 230 | pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq); |
Alex Young |
32:fafe0f42d82b | 231 | ++conf; |
Alex Young |
32:fafe0f42d82b | 232 | } |
Alex Young |
32:fafe0f42d82b | 233 | return true; |
Alex Young |
29:d9310e7b58b5 | 234 | } |
Alex Young |
29:d9310e7b58b5 | 235 | else |
Alex Young |
29:d9310e7b58b5 | 236 | { |
Alex Young |
32:fafe0f42d82b | 237 | dumpResponse(outputConfRsp); |
Alex Young |
29:d9310e7b58b5 | 238 | } |
Alex Young |
32:fafe0f42d82b | 239 | } |
Alex Young |
32:fafe0f42d82b | 240 | else |
Alex Young |
32:fafe0f42d82b | 241 | { |
Alex Young |
32:fafe0f42d82b | 242 | pc.printf("Failed to set output configuration.\n"); |
Alex Young |
32:fafe0f42d82b | 243 | } |
Alex Young |
32:fafe0f42d82b | 244 | return false; |
Alex Young |
32:fafe0f42d82b | 245 | } |
Alex Young |
29:d9310e7b58b5 | 246 | |
Alex Young |
32:fafe0f42d82b | 247 | static bool configureMotionTracker(void) |
Alex Young |
32:fafe0f42d82b | 248 | { |
Alex Young |
32:fafe0f42d82b | 249 | uint32_t deviceId = readDeviceId(); |
Alex Young |
32:fafe0f42d82b | 250 | |
Alex Young |
32:fafe0f42d82b | 251 | if (deviceId) |
Alex Young |
32:fafe0f42d82b | 252 | { |
Alex Young |
32:fafe0f42d82b | 253 | uint8_t deviceType = (deviceId >> 24) & 0x0F; |
Alex Young |
32:fafe0f42d82b | 254 | pc.printf("Found MTi-%d\n", deviceType); |
Alex Young |
32:fafe0f42d82b | 255 | |
Alex Young |
32:fafe0f42d82b | 256 | if (deviceType == 1) |
Alex Young |
29:d9310e7b58b5 | 257 | { |
Alex Young |
32:fafe0f42d82b | 258 | OutputConfiguration conf[] = { |
Alex Young |
32:fafe0f42d82b | 259 | {XDI_PacketCounter, 65535}, |
Alex Young |
32:fafe0f42d82b | 260 | {XDI_SampleTimeFine, 65535}, |
Alex Young |
32:fafe0f42d82b | 261 | {XDI_Acceleration, 100}, |
Alex Young |
32:fafe0f42d82b | 262 | {XDI_RateOfTurn, 100}, |
Alex Young |
32:fafe0f42d82b | 263 | {XDI_MagneticField, 100} |
Alex Young |
32:fafe0f42d82b | 264 | }; |
Alex Young |
32:fafe0f42d82b | 265 | return setOutputConfiguration(conf, |
Alex Young |
32:fafe0f42d82b | 266 | sizeof(conf) / sizeof(OutputConfiguration)); |
Alex Young |
29:d9310e7b58b5 | 267 | } |
Alex Young |
29:d9310e7b58b5 | 268 | else |
Alex Young |
29:d9310e7b58b5 | 269 | { |
Alex Young |
32:fafe0f42d82b | 270 | OutputConfiguration conf[] = { |
Alex Young |
32:fafe0f42d82b | 271 | {XDI_PacketCounter, 65535}, |
Alex Young |
32:fafe0f42d82b | 272 | {XDI_SampleTimeFine, 65535}, |
Alex Young |
32:fafe0f42d82b | 273 | {XDI_Quaternion, 100}, |
Alex Young |
32:fafe0f42d82b | 274 | {XDI_StatusWord, 65535} |
Alex Young |
32:fafe0f42d82b | 275 | }; |
Alex Young |
32:fafe0f42d82b | 276 | return setOutputConfiguration(conf, |
Alex Young |
32:fafe0f42d82b | 277 | sizeof(conf) / sizeof(OutputConfiguration)); |
Alex Young |
29:d9310e7b58b5 | 278 | } |
Alex Young |
29:d9310e7b58b5 | 279 | } |
Alex Young |
32:fafe0f42d82b | 280 | |
Alex Young |
32:fafe0f42d82b | 281 | return false; |
Alex Young |
29:d9310e7b58b5 | 282 | } |
Alex Young |
29:d9310e7b58b5 | 283 | |
Alex Young |
2:b3e402dc11ca | 284 | int main(void) |
Alex Young |
2:b3e402dc11ca | 285 | { |
Alex Young |
4:98f063b2e6da | 286 | XbusParserCallback xbusCallback = {}; |
Alex Young |
25:01356fb59467 | 287 | xbusCallback.allocateBuffer = allocateMessageData; |
Alex Young |
25:01356fb59467 | 288 | xbusCallback.deallocateBuffer = deallocateMessageData; |
Alex Young |
24:2cc49dc854e3 | 289 | xbusCallback.handleMessage = mtMessageHandler; |
Alex Young |
4:98f063b2e6da | 290 | |
Alex Young |
4:98f063b2e6da | 291 | xbusParser = XbusParser_create(&xbusCallback); |
Alex Young |
4:98f063b2e6da | 292 | configureSerialPorts(); |
Alex Young |
29:d9310e7b58b5 | 293 | if (configureMotionTracker()) |
Alex Young |
5:abc52dd88be2 | 294 | { |
Alex Young |
29:d9310e7b58b5 | 295 | for (;;) |
Alex Young |
26:665d3624f9ab | 296 | { |
Alex Young |
29:d9310e7b58b5 | 297 | while (pc.readable()) |
Alex Young |
29:d9310e7b58b5 | 298 | { |
Alex Young |
29:d9310e7b58b5 | 299 | handlePcCommand(pc.getc()); |
Alex Young |
29:d9310e7b58b5 | 300 | } |
Alex Young |
26:665d3624f9ab | 301 | } |
Alex Young |
5:abc52dd88be2 | 302 | } |
Alex Young |
29:d9310e7b58b5 | 303 | else |
Alex Young |
29:d9310e7b58b5 | 304 | { |
Alex Young |
29:d9310e7b58b5 | 305 | pc.printf("Failed to configure motion tracker.\n"); |
Alex Young |
29:d9310e7b58b5 | 306 | return -1; |
Alex Young |
29:d9310e7b58b5 | 307 | } |
Alex Young |
4:98f063b2e6da | 308 | } |