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@60:ab9dad3560d3, 2015-06-12 (annotated)
- Committer:
- Alex Young
- Date:
- Fri Jun 12 11:59:16 2015 +0200
- Revision:
- 60:ab9dad3560d3
- Parent:
- 59:f9166c19451f
- Child:
- 61:b9d3e7e5ba0c
Add support for EA LPC4088 QuickStart Board
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 |
54:2e9bb1390c9c | 14 | |
Alex Young |
54:2e9bb1390c9c | 15 | * \page Overview Firmware overview |
Alex Young |
54:2e9bb1390c9c | 16 | * |
Alex Young |
54:2e9bb1390c9c | 17 | * Example firmware for communicating with an Xsens MTi-1 series motion |
Alex Young |
54:2e9bb1390c9c | 18 | * tracker (MT). |
Alex Young |
54:2e9bb1390c9c | 19 | * |
Alex Young |
54:2e9bb1390c9c | 20 | * The firmware uses the mbed-rtos library to provide RTOS features such as |
Alex Young |
54:2e9bb1390c9c | 21 | * memory pools and queues. A single thread (main) is used with reception of |
Alex Young |
54:2e9bb1390c9c | 22 | * data from the motion tracker via a UART handled by interrupts. |
Alex Young |
54:2e9bb1390c9c | 23 | * |
Alex Young |
54:2e9bb1390c9c | 24 | * \section Hardware setup |
Alex Young |
54:2e9bb1390c9c | 25 | * The firmware has been tested with a ST Nucleo F302R8 development board. |
Alex Young |
54:2e9bb1390c9c | 26 | * The Nucleo board should be connected to the MTi1 development board using the |
Alex Young |
54:2e9bb1390c9c | 27 | * Arduino compatible headers on the Nucleo board as follows: |
Alex Young |
54:2e9bb1390c9c | 28 | * |
Alex Young |
54:2e9bb1390c9c | 29 | * | Nucleo pin | MTi1 func. | MTi1 dev. pin | |
Alex Young |
54:2e9bb1390c9c | 30 | * |------------|-------------|---------------| |
Alex Young |
54:2e9bb1390c9c | 31 | * | IORef | VDDIO_EXT | P301-3 | |
Alex Young |
54:2e9bb1390c9c | 32 | * | 5V | VDD_EXT | P301-1 | |
Alex Young |
54:2e9bb1390c9c | 33 | * | GND | GND | P301-2 | |
Alex Young |
54:2e9bb1390c9c | 34 | * | SCL/D15 | DEV_UART_TX | P301-9 | |
Alex Young |
54:2e9bb1390c9c | 35 | * | SDA/D14 | DEV_UART_RX | P301-11 | |
Alex Young |
54:2e9bb1390c9c | 36 | * | D2 | nRST | P301-7 | |
Alex Young |
54:2e9bb1390c9c | 37 | * |
Alex Young |
54:2e9bb1390c9c | 38 | * Communication with the host PC is achieved using the built-in USB serial |
Alex Young |
54:2e9bb1390c9c | 39 | * bridge of the Nucleo board. |
Alex Young |
54:2e9bb1390c9c | 40 | * |
Alex Young |
54:2e9bb1390c9c | 41 | * \subsection Porting |
Alex Young |
54:2e9bb1390c9c | 42 | * To port to a different mbed platform only the serial Rx/Tx lines and the |
Alex Young |
54:2e9bb1390c9c | 43 | * reset line pins should need to be updated. |
Alex Young |
54:2e9bb1390c9c | 44 | * |
Alex Young |
54:2e9bb1390c9c | 45 | * \section Firmware Operation |
Alex Young |
54:2e9bb1390c9c | 46 | * The firmware starts by initializing the serial ports used to communicate |
Alex Young |
54:2e9bb1390c9c | 47 | * with the host PC and with the MT. During the initialization the MT is held |
Alex Young |
54:2e9bb1390c9c | 48 | * in reset using the nRST input. |
Alex Young |
54:2e9bb1390c9c | 49 | * |
Alex Young |
54:2e9bb1390c9c | 50 | * Once the firmware is ready to communicate with the MT the reset line is |
Alex Young |
54:2e9bb1390c9c | 51 | * released and the firmware waits for a wakeup message from the MT. If this is |
Alex Young |
54:2e9bb1390c9c | 52 | * not received within 1 second the firmware will try to restore communication |
Alex Young |
54:2e9bb1390c9c | 53 | * with the MT using a special restore communication procedure. |
Alex Young |
54:2e9bb1390c9c | 54 | * |
Alex Young |
54:2e9bb1390c9c | 55 | * When the MT is ready for communication the firmware requests the device ID |
Alex Young |
54:2e9bb1390c9c | 56 | * of the MT, and based on this determines which type of MTi is connected. |
Alex Young |
54:2e9bb1390c9c | 57 | * If the MT is an MTi-1 then it will be configured to send inertial and |
tjerkhofmeijer | 56:041d3d9c300a | 58 | * magnetic measurement data. MTi-2 and MTi-3 devices have onboard orientation |
Alex Young |
54:2e9bb1390c9c | 59 | * estimation and will therefore be configured to provide quaternion output. |
Alex Young |
36:21198d933917 | 60 | */ |
Alex Young |
36:21198d933917 | 61 | |
Alex Young |
4:98f063b2e6da | 62 | #include "mbed.h" |
Alex Young |
25:01356fb59467 | 63 | #include "rtos.h" |
Alex Young |
4:98f063b2e6da | 64 | #include "xbusparser.h" |
Alex Young |
11:8593ba137917 | 65 | #include "xbusmessage.h" |
Alex Young |
40:b77a8c10c76d | 66 | #include "xsdeviceid.h" |
Alex Young |
4:98f063b2e6da | 67 | |
Alex Young |
59:f9166c19451f | 68 | #if defined(TARGET_NUCLEO_F302R8) |
Alex Young |
57:c3c85ebb7375 | 69 | #define PC_TX PA_2 |
Alex Young |
57:c3c85ebb7375 | 70 | #define PC_RX PA_3 |
Alex Young |
57:c3c85ebb7375 | 71 | #define MT_TX PB_9 |
Alex Young |
57:c3c85ebb7375 | 72 | #define MT_RX PB_8 |
Alex Young |
57:c3c85ebb7375 | 73 | #define MT_NRESET PA_10 |
Alex Young |
59:f9166c19451f | 74 | #elif defined(TARGET_KL46Z) |
Alex Young |
59:f9166c19451f | 75 | #define PC_TX USBTX |
Alex Young |
59:f9166c19451f | 76 | #define PC_RX USBRX |
Alex Young |
59:f9166c19451f | 77 | #define MT_TX PTE0 |
Alex Young |
59:f9166c19451f | 78 | #define MT_RX PTE1 |
Alex Young |
59:f9166c19451f | 79 | #define MT_NRESET PTD3 |
Alex Young |
60:ab9dad3560d3 | 80 | #elif defined(TARGET_LPC4088) |
Alex Young |
60:ab9dad3560d3 | 81 | #define PC_TX USBTX |
Alex Young |
60:ab9dad3560d3 | 82 | #define PC_RX USBRX |
Alex Young |
60:ab9dad3560d3 | 83 | #define MT_TX P0_25 |
Alex Young |
60:ab9dad3560d3 | 84 | #define MT_RX P0_26 |
Alex Young |
60:ab9dad3560d3 | 85 | #define MT_NRESET P1_30 |
Alex Young |
57:c3c85ebb7375 | 86 | #else |
Alex Young |
57:c3c85ebb7375 | 87 | #error "Support for selected mbed platform has not been added." |
Alex Young |
57:c3c85ebb7375 | 88 | #endif |
Alex Young |
57:c3c85ebb7375 | 89 | |
Alex Young |
57:c3c85ebb7375 | 90 | |
Alex Young |
44:b3980e8ac074 | 91 | /*! |
Alex Young |
53:3891f4259901 | 92 | * \brief Baudrate used to communicate with host PC. |
Alex Young |
53:3891f4259901 | 93 | */ |
Alex Young |
53:3891f4259901 | 94 | #define PC_UART_BAUDRATE (921600) |
Alex Young |
53:3891f4259901 | 95 | |
Alex Young |
53:3891f4259901 | 96 | /*! |
Alex Young |
44:b3980e8ac074 | 97 | * \brief The number of items to hold in the memory pools. |
Alex Young |
44:b3980e8ac074 | 98 | */ |
Alex Young |
25:01356fb59467 | 99 | #define MEMORY_POOL_SIZE (4) |
Alex Young |
44:b3980e8ac074 | 100 | /*! |
Alex Young |
44:b3980e8ac074 | 101 | * \brief The size of the queue used for device responses. |
Alex Young |
44:b3980e8ac074 | 102 | * This is set to one as in typical Xbus operation each command receives a |
Alex Young |
44:b3980e8ac074 | 103 | * response before the next command is sent. |
Alex Young |
44:b3980e8ac074 | 104 | */ |
Alex Young |
26:665d3624f9ab | 105 | #define RESPONSE_QUEUE_SIZE (1) |
Alex Young |
44:b3980e8ac074 | 106 | /*! |
Alex Young |
44:b3980e8ac074 | 107 | * \brief The size of the queue used for data messages. |
Alex Young |
44:b3980e8ac074 | 108 | * This is set to two to allow some overlap between printing received data to |
Alex Young |
44:b3980e8ac074 | 109 | * the PC serial port and the reception of the subsequent data packet. In |
Alex Young |
44:b3980e8ac074 | 110 | * more complex applications it might be necessary to increase this if |
Alex Young |
44:b3980e8ac074 | 111 | * message processing might occasionally require more time than normal. |
Alex Young |
44:b3980e8ac074 | 112 | */ |
Alex Young |
43:470c019246e4 | 113 | #define DATA_QUEUE_SIZE (2) |
Alex Young |
44:b3980e8ac074 | 114 | /*! |
Alex Young |
49:38ecfbff5391 | 115 | * \brief The maximum size of an xbus message supported by the application. |
Alex Young |
44:b3980e8ac074 | 116 | * This is the size of the message buffers in the message data memory pool. |
Alex Young |
44:b3980e8ac074 | 117 | */ |
Alex Young |
25:01356fb59467 | 118 | #define MAX_XBUS_DATA_SIZE (128) |
Alex Young |
25:01356fb59467 | 119 | |
Alex Young |
44:b3980e8ac074 | 120 | /*! \brief Serial port for communication with the host PC. */ |
Alex Young |
57:c3c85ebb7375 | 121 | static Serial pc(PC_TX, PC_RX); |
Alex Young |
58:db60ef0a0d16 | 122 | /*! |
Alex Young |
58:db60ef0a0d16 | 123 | * \brief Serial port for communication with the MT. |
Alex Young |
58:db60ef0a0d16 | 124 | * |
Alex Young |
58:db60ef0a0d16 | 125 | * We use a RawSerial port as the Stream inteface used by the regular |
Alex Young |
58:db60ef0a0d16 | 126 | * Serial class can have problems with the RTOS when using interrupts. |
Alex Young |
58:db60ef0a0d16 | 127 | */ |
Alex Young |
58:db60ef0a0d16 | 128 | static RawSerial mt(MT_TX, MT_RX); |
Alex Young |
35:7e519b88c610 | 129 | /*! |
Alex Young |
35:7e519b88c610 | 130 | * \brief MT reset line. |
Alex Young |
35:7e519b88c610 | 131 | * |
Alex Young |
35:7e519b88c610 | 132 | * MT is held in reset on startup. |
Alex Young |
35:7e519b88c610 | 133 | */ |
Alex Young |
57:c3c85ebb7375 | 134 | static DigitalOut mtReset(MT_NRESET, 0); |
Alex Young |
44:b3980e8ac074 | 135 | /*! \brief XbusParser used to parse incoming Xbus messages from the MT. */ |
Alex Young |
4:98f063b2e6da | 136 | static XbusParser* xbusParser; |
Alex Young |
25:01356fb59467 | 137 | |
Alex Young |
44:b3980e8ac074 | 138 | /*! |
Alex Young |
44:b3980e8ac074 | 139 | * \brief Memory pool used for storing Xbus messages when passing them |
Alex Young |
44:b3980e8ac074 | 140 | * to the main thread. |
Alex Young |
44:b3980e8ac074 | 141 | */ |
Alex Young |
25:01356fb59467 | 142 | MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool; |
Alex Young |
44:b3980e8ac074 | 143 | /*! |
Alex Young |
44:b3980e8ac074 | 144 | * \brief Memory pool used for storing the payload of Xbus messages. |
Alex Young |
44:b3980e8ac074 | 145 | */ |
Alex Young |
25:01356fb59467 | 146 | MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool; |
Alex Young |
44:b3980e8ac074 | 147 | /*! |
Alex Young |
44:b3980e8ac074 | 148 | * \brief Queue used to pass data messages to the main thread for processing. |
Alex Young |
44:b3980e8ac074 | 149 | */ |
Alex Young |
44:b3980e8ac074 | 150 | Queue<XbusMessage, DATA_QUEUE_SIZE> g_dataQueue; |
Alex Young |
44:b3980e8ac074 | 151 | /*! |
Alex Young |
44:b3980e8ac074 | 152 | * \brief Queue used for passing all other messages to the main thread for processing. |
Alex Young |
44:b3980e8ac074 | 153 | */ |
Alex Young |
26:665d3624f9ab | 154 | Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue; |
Alex Young |
4:98f063b2e6da | 155 | |
Alex Young |
44:b3980e8ac074 | 156 | /*! |
Alex Young |
44:b3980e8ac074 | 157 | * \brief Allocate message data buffer from the message data pool. |
Alex Young |
44:b3980e8ac074 | 158 | */ |
Alex Young |
25:01356fb59467 | 159 | static void* allocateMessageData(size_t bufSize) |
Alex Young |
4:98f063b2e6da | 160 | { |
Alex Young |
25:01356fb59467 | 161 | return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL; |
Alex Young |
25:01356fb59467 | 162 | } |
Alex Young |
25:01356fb59467 | 163 | |
Alex Young |
44:b3980e8ac074 | 164 | /*! |
Alex Young |
44:b3980e8ac074 | 165 | * \brief Deallocate message data previously allocated from the message |
Alex Young |
44:b3980e8ac074 | 166 | * data pool. |
Alex Young |
44:b3980e8ac074 | 167 | */ |
Alex Young |
25:01356fb59467 | 168 | static void deallocateMessageData(void const* buffer) |
Alex Young |
25:01356fb59467 | 169 | { |
Alex Young |
25:01356fb59467 | 170 | g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer); |
Alex Young |
4:98f063b2e6da | 171 | } |
Alex Young |
4:98f063b2e6da | 172 | |
Alex Young |
44:b3980e8ac074 | 173 | /*! |
Alex Young |
44:b3980e8ac074 | 174 | * \brief RX Interrupt handler for the MT serial port. |
Alex Young |
44:b3980e8ac074 | 175 | * |
Alex Young |
44:b3980e8ac074 | 176 | * Passes received data to an XbusParser to extract messages. |
Alex Young |
44:b3980e8ac074 | 177 | */ |
Alex Young |
4:98f063b2e6da | 178 | static void mtLowLevelHandler(void) |
Alex Young |
4:98f063b2e6da | 179 | { |
Alex Young |
4:98f063b2e6da | 180 | while (mt.readable()) |
Alex Young |
4:98f063b2e6da | 181 | { |
Alex Young |
4:98f063b2e6da | 182 | XbusParser_parseByte(xbusParser, mt.getc()); |
Alex Young |
4:98f063b2e6da | 183 | } |
Alex Young |
4:98f063b2e6da | 184 | } |
Alex Young |
4:98f063b2e6da | 185 | |
Alex Young |
44:b3980e8ac074 | 186 | /*! |
Alex Young |
44:b3980e8ac074 | 187 | * \brief Send a message to the MT |
Alex Young |
44:b3980e8ac074 | 188 | * |
Alex Young |
44:b3980e8ac074 | 189 | * This function formats the message data and writes this to the MT serial |
Alex Young |
44:b3980e8ac074 | 190 | * port. It does not wait for any response. |
Alex Young |
44:b3980e8ac074 | 191 | */ |
Alex Young |
34:3d7a6519a256 | 192 | static void sendMessage(XbusMessage const* m) |
Alex Young |
11:8593ba137917 | 193 | { |
Alex Young |
26:665d3624f9ab | 194 | uint8_t buf[64]; |
Alex Young |
26:665d3624f9ab | 195 | size_t rawLength = XbusMessage_format(buf, m); |
Alex Young |
11:8593ba137917 | 196 | for (size_t i = 0; i < rawLength; ++i) |
Alex Young |
11:8593ba137917 | 197 | { |
Alex Young |
11:8593ba137917 | 198 | mt.putc(buf[i]); |
Alex Young |
11:8593ba137917 | 199 | } |
Alex Young |
34:3d7a6519a256 | 200 | } |
Alex Young |
34:3d7a6519a256 | 201 | |
Alex Young |
44:b3980e8ac074 | 202 | /*! |
Alex Young |
44:b3980e8ac074 | 203 | * \brief Send a message to the MT and wait for a response. |
Alex Young |
44:b3980e8ac074 | 204 | * \returns Response message from the MT, or NULL is no response received |
Alex Young |
44:b3980e8ac074 | 205 | * within 500ms. |
Alex Young |
44:b3980e8ac074 | 206 | * |
Alex Young |
44:b3980e8ac074 | 207 | * Blocking behaviour is implemented by waiting for a response to be written |
Alex Young |
44:b3980e8ac074 | 208 | * to the response queue by the XbusParser. |
Alex Young |
44:b3980e8ac074 | 209 | */ |
Alex Young |
34:3d7a6519a256 | 210 | static XbusMessage const* doTransaction(XbusMessage const* m) |
Alex Young |
34:3d7a6519a256 | 211 | { |
Alex Young |
34:3d7a6519a256 | 212 | sendMessage(m); |
Alex Young |
26:665d3624f9ab | 213 | |
Alex Young |
26:665d3624f9ab | 214 | osEvent ev = g_responseQueue.get(500); |
Alex Young |
26:665d3624f9ab | 215 | return ev.status == osEventMessage ? (XbusMessage*)ev.value.p : NULL; |
Alex Young |
26:665d3624f9ab | 216 | } |
Alex Young |
26:665d3624f9ab | 217 | |
Alex Young |
31:ce1ea9ae861e | 218 | /*! |
Alex Young |
31:ce1ea9ae861e | 219 | * \brief RAII object to manage message memory deallocation. |
Alex Young |
31:ce1ea9ae861e | 220 | * |
Alex Young |
49:38ecfbff5391 | 221 | * Will automatically free the memory used by an XbusMessage when going out |
Alex Young |
31:ce1ea9ae861e | 222 | * of scope. |
Alex Young |
31:ce1ea9ae861e | 223 | */ |
Alex Young |
31:ce1ea9ae861e | 224 | class XbusMessageMemoryManager |
Alex Young |
26:665d3624f9ab | 225 | { |
Alex Young |
31:ce1ea9ae861e | 226 | public: |
Alex Young |
31:ce1ea9ae861e | 227 | XbusMessageMemoryManager(XbusMessage const* message) |
Alex Young |
31:ce1ea9ae861e | 228 | : m_message(message) |
Alex Young |
31:ce1ea9ae861e | 229 | { |
Alex Young |
31:ce1ea9ae861e | 230 | } |
Alex Young |
31:ce1ea9ae861e | 231 | |
Alex Young |
31:ce1ea9ae861e | 232 | ~XbusMessageMemoryManager() |
Alex Young |
31:ce1ea9ae861e | 233 | { |
Alex Young |
31:ce1ea9ae861e | 234 | if (m_message) |
Alex Young |
31:ce1ea9ae861e | 235 | { |
Alex Young |
31:ce1ea9ae861e | 236 | if (m_message->data) |
Alex Young |
31:ce1ea9ae861e | 237 | deallocateMessageData(m_message->data); |
Alex Young |
31:ce1ea9ae861e | 238 | g_messagePool.free(const_cast<XbusMessage*>(m_message)); |
Alex Young |
31:ce1ea9ae861e | 239 | } |
Alex Young |
31:ce1ea9ae861e | 240 | } |
Alex Young |
31:ce1ea9ae861e | 241 | |
Alex Young |
31:ce1ea9ae861e | 242 | private: |
Alex Young |
31:ce1ea9ae861e | 243 | XbusMessage const* m_message; |
Alex Young |
31:ce1ea9ae861e | 244 | }; |
Alex Young |
26:665d3624f9ab | 245 | |
Alex Young |
44:b3980e8ac074 | 246 | /*! |
Alex Young |
44:b3980e8ac074 | 247 | * \brief Dump information from a message to the PC serial port. |
Alex Young |
44:b3980e8ac074 | 248 | */ |
Alex Young |
29:d9310e7b58b5 | 249 | static void dumpResponse(XbusMessage const* response) |
Alex Young |
29:d9310e7b58b5 | 250 | { |
Alex Young |
29:d9310e7b58b5 | 251 | switch (response->mid) |
Alex Young |
29:d9310e7b58b5 | 252 | { |
Alex Young |
29:d9310e7b58b5 | 253 | case XMID_GotoConfigAck: |
Alex Young |
52:e2197b38c029 | 254 | pc.printf("Device went to config mode.\r\n"); |
Alex Young |
29:d9310e7b58b5 | 255 | break; |
Alex Young |
29:d9310e7b58b5 | 256 | |
Alex Young |
29:d9310e7b58b5 | 257 | case XMID_Error: |
Alex Young |
29:d9310e7b58b5 | 258 | pc.printf("Device error!"); |
Alex Young |
29:d9310e7b58b5 | 259 | break; |
Alex Young |
29:d9310e7b58b5 | 260 | |
Alex Young |
29:d9310e7b58b5 | 261 | default: |
Alex Young |
52:e2197b38c029 | 262 | pc.printf("Received response MID=%X, length=%d\r\n", response->mid, response->length); |
Alex Young |
29:d9310e7b58b5 | 263 | break; |
Alex Young |
29:d9310e7b58b5 | 264 | } |
Alex Young |
29:d9310e7b58b5 | 265 | } |
Alex Young |
29:d9310e7b58b5 | 266 | |
Alex Young |
44:b3980e8ac074 | 267 | /*! |
Alex Young |
44:b3980e8ac074 | 268 | * \brief Send a command to the MT and wait for a response. |
Alex Young |
44:b3980e8ac074 | 269 | * \param cmdId The XsMessageId of the command to send. |
Alex Young |
44:b3980e8ac074 | 270 | * |
Alex Young |
44:b3980e8ac074 | 271 | * Commands are simple messages without and payload data. |
Alex Young |
44:b3980e8ac074 | 272 | */ |
Alex Young |
26:665d3624f9ab | 273 | static void sendCommand(XsMessageId cmdId) |
Alex Young |
26:665d3624f9ab | 274 | { |
Alex Young |
26:665d3624f9ab | 275 | XbusMessage m = {cmdId}; |
Alex Young |
26:665d3624f9ab | 276 | XbusMessage const* response = doTransaction(&m); |
Alex Young |
31:ce1ea9ae861e | 277 | XbusMessageMemoryManager janitor(response); |
Alex Young |
26:665d3624f9ab | 278 | |
Alex Young |
26:665d3624f9ab | 279 | if (response) |
Alex Young |
26:665d3624f9ab | 280 | { |
Alex Young |
29:d9310e7b58b5 | 281 | dumpResponse(response); |
Alex Young |
26:665d3624f9ab | 282 | } |
Alex Young |
26:665d3624f9ab | 283 | else |
Alex Young |
26:665d3624f9ab | 284 | { |
Alex Young |
52:e2197b38c029 | 285 | pc.printf("Timeout waiting for response.\r\n"); |
Alex Young |
26:665d3624f9ab | 286 | } |
Alex Young |
11:8593ba137917 | 287 | } |
Alex Young |
11:8593ba137917 | 288 | |
Alex Young |
44:b3980e8ac074 | 289 | /*! |
Alex Young |
44:b3980e8ac074 | 290 | * \brief Handle a command from the PC |
Alex Young |
44:b3980e8ac074 | 291 | * |
Alex Young |
44:b3980e8ac074 | 292 | * The example application supports single character commands from the host |
Alex Young |
44:b3980e8ac074 | 293 | * PC to switch between configuration and measurement modes. |
Alex Young |
44:b3980e8ac074 | 294 | */ |
Alex Young |
11:8593ba137917 | 295 | static void handlePcCommand(char cmd) |
Alex Young |
11:8593ba137917 | 296 | { |
Alex Young |
11:8593ba137917 | 297 | switch (cmd) |
Alex Young |
11:8593ba137917 | 298 | { |
Alex Young |
11:8593ba137917 | 299 | case 'c': |
Alex Young |
11:8593ba137917 | 300 | sendCommand(XMID_GotoConfig); |
Alex Young |
11:8593ba137917 | 301 | break; |
Alex Young |
11:8593ba137917 | 302 | |
Alex Young |
11:8593ba137917 | 303 | case 'm': |
Alex Young |
11:8593ba137917 | 304 | sendCommand(XMID_GotoMeasurement); |
Alex Young |
11:8593ba137917 | 305 | break; |
Alex Young |
11:8593ba137917 | 306 | } |
Alex Young |
11:8593ba137917 | 307 | } |
Alex Young |
11:8593ba137917 | 308 | |
Alex Young |
44:b3980e8ac074 | 309 | /*! |
Alex Young |
44:b3980e8ac074 | 310 | * \brief XbusParser callback function to handle received messages. |
Alex Young |
44:b3980e8ac074 | 311 | * \param message Pointer to the last received message. |
Alex Young |
44:b3980e8ac074 | 312 | * |
Alex Young |
44:b3980e8ac074 | 313 | * In this example received messages are copied into one of two message |
Alex Young |
44:b3980e8ac074 | 314 | * queues for later handling by the main thread. Data messages are put |
Alex Young |
49:38ecfbff5391 | 315 | * in one queue, while all other responses are placed in the second queue. |
Alex Young |
44:b3980e8ac074 | 316 | * This is done so that data and other messages can be handled separately |
Alex Young |
44:b3980e8ac074 | 317 | * by the application code. |
Alex Young |
44:b3980e8ac074 | 318 | */ |
Alex Young |
24:2cc49dc854e3 | 319 | static void mtMessageHandler(struct XbusMessage const* message) |
Alex Young |
4:98f063b2e6da | 320 | { |
Alex Young |
43:470c019246e4 | 321 | XbusMessage* m = g_messagePool.alloc(); |
Alex Young |
43:470c019246e4 | 322 | if (m) |
Alex Young |
7:c913a7cd5231 | 323 | { |
Alex Young |
43:470c019246e4 | 324 | memcpy(m, message, sizeof(XbusMessage)); |
Alex Young |
43:470c019246e4 | 325 | if (message->mid == XMID_MtData2) |
Alex Young |
43:470c019246e4 | 326 | { |
Alex Young |
43:470c019246e4 | 327 | g_dataQueue.put(m); |
Alex Young |
43:470c019246e4 | 328 | } |
Alex Young |
43:470c019246e4 | 329 | else |
Alex Young |
43:470c019246e4 | 330 | { |
Alex Young |
43:470c019246e4 | 331 | g_responseQueue.put(m); |
Alex Young |
43:470c019246e4 | 332 | } |
Alex Young |
7:c913a7cd5231 | 333 | } |
Alex Young |
43:470c019246e4 | 334 | else if (message->data) |
Alex Young |
7:c913a7cd5231 | 335 | { |
Alex Young |
43:470c019246e4 | 336 | deallocateMessageData(message->data); |
Alex Young |
25:01356fb59467 | 337 | } |
Alex Young |
4:98f063b2e6da | 338 | } |
Alex Young |
4:98f063b2e6da | 339 | |
Alex Young |
44:b3980e8ac074 | 340 | /*! |
Alex Young |
44:b3980e8ac074 | 341 | * \brief Configure the serial ports used to communicate with the motion |
Alex Young |
44:b3980e8ac074 | 342 | * tracker and host PC. |
Alex Young |
44:b3980e8ac074 | 343 | */ |
Alex Young |
4:98f063b2e6da | 344 | static void configureSerialPorts(void) |
Alex Young |
4:98f063b2e6da | 345 | { |
Alex Young |
53:3891f4259901 | 346 | pc.baud(PC_UART_BAUDRATE); |
Alex Young |
55:9a2d6f947f0d | 347 | pc.format(8, Serial::None, 1); |
Alex Young |
4:98f063b2e6da | 348 | |
Alex Young |
37:3e87bf647c68 | 349 | mt.baud(115200); |
Alex Young |
55:9a2d6f947f0d | 350 | mt.format(8, Serial::None, 1); |
Alex Young |
4:98f063b2e6da | 351 | mt.attach(mtLowLevelHandler, Serial::RxIrq); |
Alex Young |
4:98f063b2e6da | 352 | } |
Alex Young |
4:98f063b2e6da | 353 | |
Alex Young |
44:b3980e8ac074 | 354 | /*! |
Alex Young |
44:b3980e8ac074 | 355 | * \brief Read the device ID of the motion tracker. |
Alex Young |
44:b3980e8ac074 | 356 | */ |
Alex Young |
29:d9310e7b58b5 | 357 | static uint32_t readDeviceId(void) |
Alex Young |
29:d9310e7b58b5 | 358 | { |
Alex Young |
29:d9310e7b58b5 | 359 | XbusMessage reqDid = {XMID_ReqDid}; |
Alex Young |
29:d9310e7b58b5 | 360 | XbusMessage const* didRsp = doTransaction(&reqDid); |
Alex Young |
31:ce1ea9ae861e | 361 | XbusMessageMemoryManager janitor(didRsp); |
Alex Young |
29:d9310e7b58b5 | 362 | uint32_t deviceId = 0; |
Alex Young |
29:d9310e7b58b5 | 363 | if (didRsp) |
Alex Young |
29:d9310e7b58b5 | 364 | { |
Alex Young |
29:d9310e7b58b5 | 365 | if (didRsp->mid == XMID_DeviceId) |
Alex Young |
29:d9310e7b58b5 | 366 | { |
Alex Young |
29:d9310e7b58b5 | 367 | deviceId = *(uint32_t*)didRsp->data; |
Alex Young |
29:d9310e7b58b5 | 368 | } |
Alex Young |
29:d9310e7b58b5 | 369 | } |
Alex Young |
29:d9310e7b58b5 | 370 | return deviceId; |
Alex Young |
29:d9310e7b58b5 | 371 | } |
Alex Young |
29:d9310e7b58b5 | 372 | |
Alex Young |
44:b3980e8ac074 | 373 | /*! |
Alex Young |
44:b3980e8ac074 | 374 | * \brief Sets MT output configuration. |
Alex Young |
44:b3980e8ac074 | 375 | * \param conf Pointer to an array of OutputConfiguration elements. |
Alex Young |
44:b3980e8ac074 | 376 | * \param elements The number of elements in the configuration array. |
Alex Young |
44:b3980e8ac074 | 377 | * |
Alex Young |
44:b3980e8ac074 | 378 | * The response from the device indicates the actual values that will |
Alex Young |
44:b3980e8ac074 | 379 | * be used by the motion tracker. These may differ from the requested |
Alex Young |
44:b3980e8ac074 | 380 | * parameters as the motion tracker validates the requested parameters |
Alex Young |
44:b3980e8ac074 | 381 | * before applying them. |
Alex Young |
44:b3980e8ac074 | 382 | */ |
Alex Young |
32:fafe0f42d82b | 383 | static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements) |
Alex Young |
29:d9310e7b58b5 | 384 | { |
Alex Young |
32:fafe0f42d82b | 385 | XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf}; |
Alex Young |
32:fafe0f42d82b | 386 | XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg); |
Alex Young |
32:fafe0f42d82b | 387 | XbusMessageMemoryManager janitor(outputConfRsp); |
Alex Young |
32:fafe0f42d82b | 388 | if (outputConfRsp) |
Alex Young |
29:d9310e7b58b5 | 389 | { |
Alex Young |
32:fafe0f42d82b | 390 | if (outputConfRsp->mid == XMID_OutputConfig) |
Alex Young |
29:d9310e7b58b5 | 391 | { |
Alex Young |
52:e2197b38c029 | 392 | pc.printf("Output configuration set to:\r\n"); |
Alex Young |
32:fafe0f42d82b | 393 | OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data; |
Alex Young |
32:fafe0f42d82b | 394 | for (int i = 0; i < outputConfRsp->length; ++i) |
Alex Young |
32:fafe0f42d82b | 395 | { |
Alex Young |
52:e2197b38c029 | 396 | pc.printf("\t%s: %d Hz\r\n", XbusMessage_dataDescription(conf->dtype), conf->freq); |
Alex Young |
32:fafe0f42d82b | 397 | ++conf; |
Alex Young |
32:fafe0f42d82b | 398 | } |
Alex Young |
32:fafe0f42d82b | 399 | return true; |
Alex Young |
29:d9310e7b58b5 | 400 | } |
Alex Young |
29:d9310e7b58b5 | 401 | else |
Alex Young |
29:d9310e7b58b5 | 402 | { |
Alex Young |
32:fafe0f42d82b | 403 | dumpResponse(outputConfRsp); |
Alex Young |
29:d9310e7b58b5 | 404 | } |
Alex Young |
32:fafe0f42d82b | 405 | } |
Alex Young |
32:fafe0f42d82b | 406 | else |
Alex Young |
32:fafe0f42d82b | 407 | { |
Alex Young |
52:e2197b38c029 | 408 | pc.printf("Failed to set output configuration.\r\n"); |
Alex Young |
32:fafe0f42d82b | 409 | } |
Alex Young |
32:fafe0f42d82b | 410 | return false; |
Alex Young |
32:fafe0f42d82b | 411 | } |
Alex Young |
29:d9310e7b58b5 | 412 | |
Alex Young |
44:b3980e8ac074 | 413 | /*! |
Alex Young |
44:b3980e8ac074 | 414 | * \brief Sets the motion tracker output configuration based on the function |
Alex Young |
44:b3980e8ac074 | 415 | * of the attached device. |
Alex Young |
44:b3980e8ac074 | 416 | * |
Alex Young |
44:b3980e8ac074 | 417 | * The output configuration depends on the type of MTi-1 device connected. |
Alex Young |
49:38ecfbff5391 | 418 | * An MTI-1 (IMU) device does not have an onboard orientation filter so |
Alex Young |
44:b3980e8ac074 | 419 | * cannot output quaternion data, only inertial and magnetic measurement |
Alex Young |
44:b3980e8ac074 | 420 | * data. |
Alex Young |
44:b3980e8ac074 | 421 | * MTi-2 and MTi-3 devices have an onboard filter so can send quaternions. |
Alex Young |
44:b3980e8ac074 | 422 | */ |
Alex Young |
32:fafe0f42d82b | 423 | static bool configureMotionTracker(void) |
Alex Young |
32:fafe0f42d82b | 424 | { |
Alex Young |
32:fafe0f42d82b | 425 | uint32_t deviceId = readDeviceId(); |
Alex Young |
32:fafe0f42d82b | 426 | |
Alex Young |
32:fafe0f42d82b | 427 | if (deviceId) |
Alex Young |
32:fafe0f42d82b | 428 | { |
Alex Young |
52:e2197b38c029 | 429 | pc.printf("Found device with ID: %08X.\r\n", deviceId); |
Alex Young |
40:b77a8c10c76d | 430 | if (!XsDeviceId_isMtMk4_X(deviceId)) |
Alex Young |
40:b77a8c10c76d | 431 | { |
Alex Young |
52:e2197b38c029 | 432 | pc.printf("Device is not an MTi-1 series.\r\n"); |
Alex Young |
40:b77a8c10c76d | 433 | return false; |
Alex Young |
40:b77a8c10c76d | 434 | } |
Alex Young |
32:fafe0f42d82b | 435 | |
Alex Young |
40:b77a8c10c76d | 436 | DeviceFunction function = XsDeviceId_getFunction(deviceId); |
Alex Young |
52:e2197b38c029 | 437 | pc.printf("Device is an MTi-%d: %s.\r\n", function, XsDeviceId_functionDescription(function)); |
Alex Young |
40:b77a8c10c76d | 438 | |
Alex Young |
40:b77a8c10c76d | 439 | if (function == DF_IMU) |
Alex Young |
29:d9310e7b58b5 | 440 | { |
Alex Young |
32:fafe0f42d82b | 441 | OutputConfiguration conf[] = { |
Alex Young |
32:fafe0f42d82b | 442 | {XDI_PacketCounter, 65535}, |
Alex Young |
32:fafe0f42d82b | 443 | {XDI_SampleTimeFine, 65535}, |
Alex Young |
32:fafe0f42d82b | 444 | {XDI_Acceleration, 100}, |
Alex Young |
32:fafe0f42d82b | 445 | {XDI_RateOfTurn, 100}, |
Alex Young |
32:fafe0f42d82b | 446 | {XDI_MagneticField, 100} |
Alex Young |
32:fafe0f42d82b | 447 | }; |
Alex Young |
32:fafe0f42d82b | 448 | return setOutputConfiguration(conf, |
Alex Young |
32:fafe0f42d82b | 449 | sizeof(conf) / sizeof(OutputConfiguration)); |
Alex Young |
29:d9310e7b58b5 | 450 | } |
Alex Young |
29:d9310e7b58b5 | 451 | else |
Alex Young |
29:d9310e7b58b5 | 452 | { |
Alex Young |
32:fafe0f42d82b | 453 | OutputConfiguration conf[] = { |
Alex Young |
32:fafe0f42d82b | 454 | {XDI_PacketCounter, 65535}, |
Alex Young |
32:fafe0f42d82b | 455 | {XDI_SampleTimeFine, 65535}, |
Alex Young |
32:fafe0f42d82b | 456 | {XDI_Quaternion, 100}, |
Alex Young |
32:fafe0f42d82b | 457 | {XDI_StatusWord, 65535} |
Alex Young |
32:fafe0f42d82b | 458 | }; |
Alex Young |
32:fafe0f42d82b | 459 | return setOutputConfiguration(conf, |
Alex Young |
32:fafe0f42d82b | 460 | sizeof(conf) / sizeof(OutputConfiguration)); |
Alex Young |
29:d9310e7b58b5 | 461 | } |
Alex Young |
29:d9310e7b58b5 | 462 | } |
Alex Young |
32:fafe0f42d82b | 463 | |
Alex Young |
32:fafe0f42d82b | 464 | return false; |
Alex Young |
29:d9310e7b58b5 | 465 | } |
Alex Young |
29:d9310e7b58b5 | 466 | |
Alex Young |
35:7e519b88c610 | 467 | /*! |
Alex Young |
35:7e519b88c610 | 468 | * \brief Wait for a wakeup message from the MTi. |
Alex Young |
37:3e87bf647c68 | 469 | * \param timeout Time to wait to receive the wakeup message. |
Alex Young |
37:3e87bf647c68 | 470 | * \return true if wakeup received within timeout, else false. |
Alex Young |
35:7e519b88c610 | 471 | * |
Alex Young |
49:38ecfbff5391 | 472 | * The MTi sends an XMID_Wakeup message once it has completed its bootup |
Alex Young |
49:38ecfbff5391 | 473 | * procedure. If this is acknowledged by an XMID_WakeupAck message then the MTi |
Alex Young |
35:7e519b88c610 | 474 | * will stay in configuration mode. Otherwise it will automatically enter |
Alex Young |
35:7e519b88c610 | 475 | * measurement mode with the stored output configuration. |
Alex Young |
35:7e519b88c610 | 476 | */ |
Alex Young |
37:3e87bf647c68 | 477 | bool waitForWakeup(uint32_t timeout) |
Alex Young |
35:7e519b88c610 | 478 | { |
Alex Young |
37:3e87bf647c68 | 479 | osEvent ev = g_responseQueue.get(timeout); |
Alex Young |
35:7e519b88c610 | 480 | if (ev.status == osEventMessage) |
Alex Young |
35:7e519b88c610 | 481 | { |
Alex Young |
35:7e519b88c610 | 482 | XbusMessage const* m = (XbusMessage const*)ev.value.p; |
Alex Young |
35:7e519b88c610 | 483 | XbusMessageMemoryManager janitor(m); |
Alex Young |
35:7e519b88c610 | 484 | return m->mid == XMID_Wakeup; |
Alex Young |
35:7e519b88c610 | 485 | } |
Alex Young |
35:7e519b88c610 | 486 | return false; |
Alex Young |
35:7e519b88c610 | 487 | } |
Alex Young |
35:7e519b88c610 | 488 | |
Alex Young |
35:7e519b88c610 | 489 | /*! |
Alex Young |
37:3e87bf647c68 | 490 | * \brief Send wakeup acknowledge message to MTi. |
Alex Young |
37:3e87bf647c68 | 491 | * |
Alex Young |
37:3e87bf647c68 | 492 | * Sending a wakeup acknowledge will cause the device to stay in configuration |
Alex Young |
37:3e87bf647c68 | 493 | * mode instead of automatically transitioning to measurement mode with the |
Alex Young |
37:3e87bf647c68 | 494 | * stored output configuration. |
Alex Young |
37:3e87bf647c68 | 495 | */ |
Alex Young |
37:3e87bf647c68 | 496 | void sendWakeupAck(void) |
Alex Young |
37:3e87bf647c68 | 497 | { |
Alex Young |
37:3e87bf647c68 | 498 | XbusMessage ack = {XMID_WakeupAck}; |
Alex Young |
37:3e87bf647c68 | 499 | sendMessage(&ack); |
Alex Young |
52:e2197b38c029 | 500 | pc.printf("Device ready for operation.\r\n"); |
Alex Young |
37:3e87bf647c68 | 501 | } |
Alex Young |
37:3e87bf647c68 | 502 | |
Alex Young |
37:3e87bf647c68 | 503 | /*! |
Alex Young |
37:3e87bf647c68 | 504 | * \brief Restore communication with the MTi. |
Alex Young |
37:3e87bf647c68 | 505 | * |
Alex Young |
37:3e87bf647c68 | 506 | * On bootup the MTi will listen for a magic byte to signal that it should |
Alex Young |
37:3e87bf647c68 | 507 | * return to default baudrate and output configuration. This can be used to |
Alex Young |
37:3e87bf647c68 | 508 | * recover from a bad or unknown configuration. |
Alex Young |
37:3e87bf647c68 | 509 | */ |
Alex Young |
37:3e87bf647c68 | 510 | void restoreCommunication(void) |
Alex Young |
37:3e87bf647c68 | 511 | { |
Alex Young |
37:3e87bf647c68 | 512 | pc.printf("Restoring communication with device... "); |
Alex Young |
37:3e87bf647c68 | 513 | mtReset = 0; |
Alex Young |
37:3e87bf647c68 | 514 | Thread::wait(1); |
Alex Young |
37:3e87bf647c68 | 515 | mtReset = 1; |
Alex Young |
37:3e87bf647c68 | 516 | |
Alex Young |
37:3e87bf647c68 | 517 | do |
Alex Young |
37:3e87bf647c68 | 518 | { |
Alex Young |
37:3e87bf647c68 | 519 | mt.putc(0xDE); |
Alex Young |
37:3e87bf647c68 | 520 | } |
Alex Young |
37:3e87bf647c68 | 521 | while (!waitForWakeup(1)); |
Alex Young |
52:e2197b38c029 | 522 | pc.printf("done\r\n"); |
Alex Young |
37:3e87bf647c68 | 523 | |
Alex Young |
37:3e87bf647c68 | 524 | sendWakeupAck(); |
Alex Young |
37:3e87bf647c68 | 525 | } |
Alex Young |
37:3e87bf647c68 | 526 | |
Alex Young |
37:3e87bf647c68 | 527 | /*! |
Alex Young |
35:7e519b88c610 | 528 | * \brief Releases the MTi reset line and waits for a wakeup message. |
Alex Young |
37:3e87bf647c68 | 529 | * |
Alex Young |
37:3e87bf647c68 | 530 | * If no wakeup message is received within 1 second the restore communications |
Alex Young |
37:3e87bf647c68 | 531 | * procedure is done to reset the MTi to default baudrate and output configuration. |
Alex Young |
35:7e519b88c610 | 532 | */ |
Alex Young |
35:7e519b88c610 | 533 | static void wakeupMotionTracker(void) |
Alex Young |
35:7e519b88c610 | 534 | { |
Alex Young |
35:7e519b88c610 | 535 | mtReset.write(1); // Release MT from reset. |
Alex Young |
37:3e87bf647c68 | 536 | if (waitForWakeup(1000)) |
Alex Young |
35:7e519b88c610 | 537 | { |
Alex Young |
37:3e87bf647c68 | 538 | sendWakeupAck(); |
Alex Young |
37:3e87bf647c68 | 539 | } |
Alex Young |
37:3e87bf647c68 | 540 | else |
Alex Young |
37:3e87bf647c68 | 541 | { |
Alex Young |
37:3e87bf647c68 | 542 | restoreCommunication(); |
Alex Young |
35:7e519b88c610 | 543 | } |
Alex Young |
35:7e519b88c610 | 544 | } |
Alex Young |
35:7e519b88c610 | 545 | |
Alex Young |
38:d8d410d1662c | 546 | static void printIntroMessage(void) |
Alex Young |
38:d8d410d1662c | 547 | { |
Alex Young |
52:e2197b38c029 | 548 | pc.printf("\r\n\r\n\r\n\r\n\r\n"); |
Alex Young |
52:e2197b38c029 | 549 | pc.printf("MTi-1 series embedded example firmware.\r\n"); |
Alex Young |
38:d8d410d1662c | 550 | } |
Alex Young |
38:d8d410d1662c | 551 | |
Alex Young |
38:d8d410d1662c | 552 | static void printUsageInstructions(void) |
Alex Young |
38:d8d410d1662c | 553 | { |
Alex Young |
52:e2197b38c029 | 554 | pc.printf("\r\n"); |
Alex Young |
52:e2197b38c029 | 555 | pc.printf("Press 'm' to start measuring and 'c' to return to config mode.\r\n"); |
Alex Young |
38:d8d410d1662c | 556 | } |
Alex Young |
38:d8d410d1662c | 557 | |
Alex Young |
44:b3980e8ac074 | 558 | /*! |
Alex Young |
44:b3980e8ac074 | 559 | * \brief Output the contents of a data message to the PC serial port. |
Alex Young |
44:b3980e8ac074 | 560 | */ |
Alex Young |
43:470c019246e4 | 561 | static void printMessageData(struct XbusMessage const* message) |
Alex Young |
43:470c019246e4 | 562 | { |
Alex Young |
43:470c019246e4 | 563 | if (!message) |
Alex Young |
43:470c019246e4 | 564 | return; |
Alex Young |
43:470c019246e4 | 565 | |
Alex Young |
43:470c019246e4 | 566 | pc.printf("MTData2:"); |
Alex Young |
43:470c019246e4 | 567 | uint16_t counter; |
Alex Young |
43:470c019246e4 | 568 | if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message)) |
Alex Young |
43:470c019246e4 | 569 | { |
Alex Young |
43:470c019246e4 | 570 | pc.printf(" Packet counter: %5d", counter); |
Alex Young |
43:470c019246e4 | 571 | } |
Alex Young |
43:470c019246e4 | 572 | float ori[4]; |
Alex Young |
43:470c019246e4 | 573 | if (XbusMessage_getDataItem(ori, XDI_Quaternion, message)) |
Alex Young |
43:470c019246e4 | 574 | { |
Alex Young |
43:470c019246e4 | 575 | pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1], |
Alex Young |
43:470c019246e4 | 576 | ori[2], ori[3]); |
Alex Young |
43:470c019246e4 | 577 | } |
Alex Young |
43:470c019246e4 | 578 | float acc[3]; |
Alex Young |
43:470c019246e4 | 579 | if (XbusMessage_getDataItem(acc, XDI_Acceleration, message)) |
Alex Young |
43:470c019246e4 | 580 | { |
Alex Young |
43:470c019246e4 | 581 | pc.printf(" Acceleration: (% .3f, % .3f, % .3f)", acc[0], acc[1], acc[2]); |
Alex Young |
43:470c019246e4 | 582 | } |
Alex Young |
43:470c019246e4 | 583 | float gyr[3]; |
Alex Young |
43:470c019246e4 | 584 | if (XbusMessage_getDataItem(gyr, XDI_RateOfTurn, message)) |
Alex Young |
43:470c019246e4 | 585 | { |
Alex Young |
43:470c019246e4 | 586 | pc.printf(" Rate Of Turn: (% .3f, % .3f, % .3f)", gyr[0], gyr[1], gyr[2]); |
Alex Young |
43:470c019246e4 | 587 | } |
Alex Young |
43:470c019246e4 | 588 | float mag[3]; |
Alex Young |
43:470c019246e4 | 589 | if (XbusMessage_getDataItem(mag, XDI_MagneticField, message)) |
Alex Young |
43:470c019246e4 | 590 | { |
Alex Young |
43:470c019246e4 | 591 | pc.printf(" Magnetic Field: (% .3f, % .3f, % .3f)", mag[0], mag[1], mag[2]); |
Alex Young |
43:470c019246e4 | 592 | } |
Alex Young |
43:470c019246e4 | 593 | uint32_t status; |
Alex Young |
43:470c019246e4 | 594 | if (XbusMessage_getDataItem(&status, XDI_StatusWord, message)) |
Alex Young |
43:470c019246e4 | 595 | { |
Alex Young |
43:470c019246e4 | 596 | pc.printf(" Status:%X", status); |
Alex Young |
43:470c019246e4 | 597 | } |
Alex Young |
52:e2197b38c029 | 598 | pc.printf("\r\n"); |
Alex Young |
43:470c019246e4 | 599 | } |
Alex Young |
43:470c019246e4 | 600 | |
Alex Young |
43:470c019246e4 | 601 | |
Alex Young |
2:b3e402dc11ca | 602 | int main(void) |
Alex Young |
2:b3e402dc11ca | 603 | { |
Alex Young |
4:98f063b2e6da | 604 | XbusParserCallback xbusCallback = {}; |
Alex Young |
25:01356fb59467 | 605 | xbusCallback.allocateBuffer = allocateMessageData; |
Alex Young |
25:01356fb59467 | 606 | xbusCallback.deallocateBuffer = deallocateMessageData; |
Alex Young |
24:2cc49dc854e3 | 607 | xbusCallback.handleMessage = mtMessageHandler; |
Alex Young |
4:98f063b2e6da | 608 | |
Alex Young |
4:98f063b2e6da | 609 | xbusParser = XbusParser_create(&xbusCallback); |
Alex Young |
4:98f063b2e6da | 610 | configureSerialPorts(); |
Alex Young |
38:d8d410d1662c | 611 | |
Alex Young |
38:d8d410d1662c | 612 | printIntroMessage(); |
Alex Young |
35:7e519b88c610 | 613 | wakeupMotionTracker(); |
Alex Young |
29:d9310e7b58b5 | 614 | if (configureMotionTracker()) |
Alex Young |
5:abc52dd88be2 | 615 | { |
Alex Young |
38:d8d410d1662c | 616 | printUsageInstructions(); |
Alex Young |
29:d9310e7b58b5 | 617 | for (;;) |
Alex Young |
26:665d3624f9ab | 618 | { |
Alex Young |
29:d9310e7b58b5 | 619 | while (pc.readable()) |
Alex Young |
29:d9310e7b58b5 | 620 | { |
Alex Young |
29:d9310e7b58b5 | 621 | handlePcCommand(pc.getc()); |
Alex Young |
29:d9310e7b58b5 | 622 | } |
Alex Young |
43:470c019246e4 | 623 | |
Alex Young |
43:470c019246e4 | 624 | osEvent ev = g_dataQueue.get(10); |
Alex Young |
43:470c019246e4 | 625 | if (ev.status == osEventMessage) |
Alex Young |
43:470c019246e4 | 626 | { |
Alex Young |
43:470c019246e4 | 627 | XbusMessage const* data = (XbusMessage const*)ev.value.p; |
Alex Young |
43:470c019246e4 | 628 | XbusMessageMemoryManager janitor(data); |
Alex Young |
43:470c019246e4 | 629 | printMessageData(data); |
Alex Young |
43:470c019246e4 | 630 | } |
Alex Young |
26:665d3624f9ab | 631 | } |
Alex Young |
5:abc52dd88be2 | 632 | } |
Alex Young |
29:d9310e7b58b5 | 633 | else |
Alex Young |
29:d9310e7b58b5 | 634 | { |
Alex Young |
52:e2197b38c029 | 635 | pc.printf("Failed to configure motion tracker.\r\n"); |
Alex Young |
29:d9310e7b58b5 | 636 | return -1; |
Alex Young |
29:d9310e7b58b5 | 637 | } |
Alex Young |
4:98f063b2e6da | 638 | } |