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@70:ff3afeaa31be, 2018-05-03 (annotated)
- Committer:
- xsens_mheskamp
- Date:
- Thu May 03 10:35:39 2018 +0200
- Revision:
- 70:ff3afeaa31be
- Parent:
- 68:6d6dbeefd196
Added some function documentation
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 |
61:b9d3e7e5ba0c | 3 | * \copyright Copyright (C) Xsens Technologies B.V., 2015. |
Alex Young |
61:b9d3e7e5ba0c | 4 | * |
Alex Young |
61:b9d3e7e5ba0c | 5 | * Licensed under the Apache License, Version 2.0 (the "License"); you may not |
Alex Young |
61:b9d3e7e5ba0c | 6 | * use this file except in compliance with the License. You may obtain a copy |
Alex Young |
61:b9d3e7e5ba0c | 7 | * of the License at |
Alex Young |
36:21198d933917 | 8 | * |
Alex Young |
61:b9d3e7e5ba0c | 9 | * http://www.apache.org/licenses/LICENSE-2.0 |
Alex Young |
36:21198d933917 | 10 | * |
Alex Young |
61:b9d3e7e5ba0c | 11 | * Unless required by applicable law or agreed to in writing, software |
Alex Young |
61:b9d3e7e5ba0c | 12 | * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT |
Alex Young |
61:b9d3e7e5ba0c | 13 | * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the |
Alex Young |
61:b9d3e7e5ba0c | 14 | * License for the specific language governing permissions and limitations |
Alex Young |
61:b9d3e7e5ba0c | 15 | * under the License. |
Alex Young |
61:b9d3e7e5ba0c | 16 | * |
Alex Young |
54:2e9bb1390c9c | 17 | * \page Overview Firmware overview |
Alex Young |
54:2e9bb1390c9c | 18 | * |
Alex Young |
54:2e9bb1390c9c | 19 | * Example firmware for communicating with an Xsens MTi-1 series motion |
Alex Young |
54:2e9bb1390c9c | 20 | * tracker (MT). |
Alex Young |
54:2e9bb1390c9c | 21 | * |
Alex Young |
54:2e9bb1390c9c | 22 | * The firmware uses the mbed-rtos library to provide RTOS features such as |
Alex Young |
54:2e9bb1390c9c | 23 | * memory pools and queues. A single thread (main) is used with reception of |
tjerkhofmeijer | 64:8a0f00a064bb | 24 | * data from the motion tracker. |
Alex Young |
54:2e9bb1390c9c | 25 | * |
Alex Young |
54:2e9bb1390c9c | 26 | * \section Hardware setup |
Alex Young |
54:2e9bb1390c9c | 27 | * The firmware has been tested with a ST Nucleo F302R8 development board. |
Alex Young |
54:2e9bb1390c9c | 28 | * The Nucleo board should be connected to the MTi1 development board using the |
Alex Young |
54:2e9bb1390c9c | 29 | * Arduino compatible headers on the Nucleo board as follows: |
Alex Young |
54:2e9bb1390c9c | 30 | * |
tjerkhofmeijer | 64:8a0f00a064bb | 31 | * | Nucleo pin | MTi1 func. | MTi1 dev. pin | Used for PSEL | |
tjerkhofmeijer | 64:8a0f00a064bb | 32 | * |------------|-------------|---------------|---------------| |
tjerkhofmeijer | 64:8a0f00a064bb | 33 | * | 5V | VDD | P300-1 | Any | |
tjerkhofmeijer | 64:8a0f00a064bb | 34 | * | IORef | VDDIO | P300-2 | Any | |
tjerkhofmeijer | 64:8a0f00a064bb | 35 | * | GND | GND | P300-3 | Any | |
tjerkhofmeijer | 64:8a0f00a064bb | 36 | * | D2 | nRST | P300-5 | Any | |
tjerkhofmeijer | 64:8a0f00a064bb | 37 | * | SCL/D15 | UART_TX/SCL | P300-9 | UART / I2C | |
tjerkhofmeijer | 64:8a0f00a064bb | 38 | * | SDA/D14 | UART_RX/SDA | P300-11 | UART / I2C | |
tjerkhofmeijer | 64:8a0f00a064bb | 39 | * | D3 | DRDY | P300-15 | SPI / I2C | |
tjerkhofmeijer | 64:8a0f00a064bb | 40 | * | SCK/D13 | SCK/ADD0 | P300-17 | SPI / I2C | |
tjerkhofmeijer | 64:8a0f00a064bb | 41 | * | MISO/D12 | MISO/ADD1 | P300-19 | SPI / I2C | |
tjerkhofmeijer | 64:8a0f00a064bb | 42 | * | MOSI/D11 | MOSI/ADD2 | P300-21 | SPI / I2C | |
tjerkhofmeijer | 64:8a0f00a064bb | 43 | * | CS/D10 | nCS | P300-23 | SPI | |
Alex Young |
54:2e9bb1390c9c | 44 | * |
Alex Young |
54:2e9bb1390c9c | 45 | * Communication with the host PC is achieved using the built-in USB serial |
tjerkhofmeijer | 64:8a0f00a064bb | 46 | * bridge of the Nucleo board. Communication with the MT is achieved through |
tjerkhofmeijer | 64:8a0f00a064bb | 47 | * either the UART, I2C or SPI interface. The active interface is chosen |
tjerkhofmeijer | 64:8a0f00a064bb | 48 | * on the MT's side by use of the PSEL0 and PSEL1 switch on the MTi1 |
tjerkhofmeijer | 64:8a0f00a064bb | 49 | * development board. This example needs to be built with the matching |
tjerkhofmeijer | 64:8a0f00a064bb | 50 | * MTI_USES_xxxx_INTERFACE define set (see below) |
Alex Young |
54:2e9bb1390c9c | 51 | * |
Alex Young |
54:2e9bb1390c9c | 52 | * \subsection Porting |
tjerkhofmeijer | 64:8a0f00a064bb | 53 | * To port to a different mbed platform the following pin definitions need |
tjerkhofmeijer | 64:8a0f00a064bb | 54 | * to be updated. |
tjerkhofmeijer | 64:8a0f00a064bb | 55 | * In all cases: the reset line pin |
tjerkhofmeijer | 64:8a0f00a064bb | 56 | * For UART: the serial Rx/Tx lines UART_TX and UART_RX |
tjerkhofmeijer | 64:8a0f00a064bb | 57 | * For I2C: the SCL,SDA,DRDY and address lines |
tjerkhofmeijer | 64:8a0f00a064bb | 58 | * For SPI: The SCK,MISO,MOSI,nCS and DRDY lines |
Alex Young |
54:2e9bb1390c9c | 59 | * |
Alex Young |
54:2e9bb1390c9c | 60 | * \section Firmware Operation |
Alex Young |
54:2e9bb1390c9c | 61 | * The firmware starts by initializing the serial ports used to communicate |
Alex Young |
54:2e9bb1390c9c | 62 | * with the host PC and with the MT. During the initialization the MT is held |
Alex Young |
54:2e9bb1390c9c | 63 | * in reset using the nRST input. |
Alex Young |
54:2e9bb1390c9c | 64 | * |
Alex Young |
54:2e9bb1390c9c | 65 | * Once the firmware is ready to communicate with the MT the reset line is |
Alex Young |
54:2e9bb1390c9c | 66 | * released and the firmware waits for a wakeup message from the MT. If this is |
Alex Young |
54:2e9bb1390c9c | 67 | * not received within 1 second the firmware will try to restore communication |
Alex Young |
54:2e9bb1390c9c | 68 | * with the MT using a special restore communication procedure. |
Alex Young |
54:2e9bb1390c9c | 69 | * |
Alex Young |
54:2e9bb1390c9c | 70 | * When the MT is ready for communication the firmware requests the device ID |
Alex Young |
54:2e9bb1390c9c | 71 | * of the MT, and based on this determines which type of MTi is connected. |
Alex Young |
54:2e9bb1390c9c | 72 | * If the MT is an MTi-1 then it will be configured to send inertial and |
tjerkhofmeijer | 56:041d3d9c300a | 73 | * magnetic measurement data. MTi-2 and MTi-3 devices have onboard orientation |
Alex Young |
54:2e9bb1390c9c | 74 | * estimation and will therefore be configured to provide quaternion output. |
Alex Young |
36:21198d933917 | 75 | */ |
Alex Young |
36:21198d933917 | 76 | |
Alex Young |
4:98f063b2e6da | 77 | #include "mbed.h" |
Alex Young |
25:01356fb59467 | 78 | #include "rtos.h" |
Alex Young |
4:98f063b2e6da | 79 | #include "xbusparser.h" |
Alex Young |
11:8593ba137917 | 80 | #include "xbusmessage.h" |
Alex Young |
40:b77a8c10c76d | 81 | #include "xsdeviceid.h" |
tjerkhofmeijer | 64:8a0f00a064bb | 82 | #include "xbusdef.h" |
tjerkhofmeijer | 64:8a0f00a064bb | 83 | |
tjerkhofmeijer | 64:8a0f00a064bb | 84 | // Select communication interface to use for MTi |
tjerkhofmeijer | 64:8a0f00a064bb | 85 | #define MTI_USES_I2C_INTERFACE |
tjerkhofmeijer | 64:8a0f00a064bb | 86 | |
tjerkhofmeijer | 64:8a0f00a064bb | 87 | #if !(defined(MTI_USES_I2C_INTERFACE) || defined(MTI_USES_SPI_INTERFACE) || defined(MTI_USES_UART_INTERFACE)) |
tjerkhofmeijer | 64:8a0f00a064bb | 88 | #error "Must select communication interface by defining one of: MTI_USES_I2C_INTERFACE, MTI_USES_SPI_INTERFACE or MTI_USES_UART_INTERFACE" |
tjerkhofmeijer | 64:8a0f00a064bb | 89 | #endif |
Alex Young |
4:98f063b2e6da | 90 | |
Alex Young |
59:f9166c19451f | 91 | #if defined(TARGET_NUCLEO_F302R8) |
tjerkhofmeijer | 64:8a0f00a064bb | 92 | |
tjerkhofmeijer | 66:f12dec1c0c3d | 93 | #define PC_TX PA_2 |
tjerkhofmeijer | 66:f12dec1c0c3d | 94 | #define PC_RX PA_3 |
tjerkhofmeijer | 66:f12dec1c0c3d | 95 | #define MT_TX PB_9 |
tjerkhofmeijer | 66:f12dec1c0c3d | 96 | #define MT_RX PB_8 |
tjerkhofmeijer | 66:f12dec1c0c3d | 97 | #define MT_SDA PB_9 |
tjerkhofmeijer | 66:f12dec1c0c3d | 98 | #define MT_SCL PB_8 |
tjerkhofmeijer | 66:f12dec1c0c3d | 99 | #define MT_ADD0 PB_13 |
tjerkhofmeijer | 66:f12dec1c0c3d | 100 | #define MT_ADD1 PB_14 |
tjerkhofmeijer | 66:f12dec1c0c3d | 101 | #define MT_ADD2 PB_15 |
tjerkhofmeijer | 66:f12dec1c0c3d | 102 | #define MT_MOSI PB_15 |
tjerkhofmeijer | 66:f12dec1c0c3d | 103 | #define MT_MISO PB_14 |
tjerkhofmeijer | 66:f12dec1c0c3d | 104 | #define MT_SCLK PB_13 |
tjerkhofmeijer | 66:f12dec1c0c3d | 105 | #define MT_nCS PB_6 |
tjerkhofmeijer | 66:f12dec1c0c3d | 106 | #define MT_NRESET PA_10 |
tjerkhofmeijer | 66:f12dec1c0c3d | 107 | #define MT_DRDY PB_3 |
tjerkhofmeijer | 64:8a0f00a064bb | 108 | |
tjerkhofmeijer | 68:6d6dbeefd196 | 109 | #elif defined(TARGET_NUCLEO_F401RE) |
tjerkhofmeijer | 68:6d6dbeefd196 | 110 | |
tjerkhofmeijer | 68:6d6dbeefd196 | 111 | #define PC_TX PA_2 |
tjerkhofmeijer | 68:6d6dbeefd196 | 112 | #define PC_RX PA_3 |
tjerkhofmeijer | 68:6d6dbeefd196 | 113 | #define MT_TX PA_10 |
tjerkhofmeijer | 68:6d6dbeefd196 | 114 | #define MT_RX PA_9 |
tjerkhofmeijer | 68:6d6dbeefd196 | 115 | #define MT_SDA PB_9 |
tjerkhofmeijer | 68:6d6dbeefd196 | 116 | #define MT_SCL PB_8 |
tjerkhofmeijer | 68:6d6dbeefd196 | 117 | #define MT_ADD0 PB_13 |
tjerkhofmeijer | 68:6d6dbeefd196 | 118 | #define MT_ADD1 PB_14 |
tjerkhofmeijer | 68:6d6dbeefd196 | 119 | #define MT_ADD2 PB_15 |
tjerkhofmeijer | 68:6d6dbeefd196 | 120 | #define MT_MOSI PB_15 |
tjerkhofmeijer | 68:6d6dbeefd196 | 121 | #define MT_MISO PB_14 |
tjerkhofmeijer | 68:6d6dbeefd196 | 122 | #define MT_SCLK PB_13 |
tjerkhofmeijer | 68:6d6dbeefd196 | 123 | #define MT_nCS PB_6 |
tjerkhofmeijer | 68:6d6dbeefd196 | 124 | #define MT_NRESET PC_9 |
tjerkhofmeijer | 68:6d6dbeefd196 | 125 | #define MT_DRDY PB_3 |
tjerkhofmeijer | 68:6d6dbeefd196 | 126 | |
Alex Young |
59:f9166c19451f | 127 | #elif defined(TARGET_KL46Z) |
tjerkhofmeijer | 64:8a0f00a064bb | 128 | |
tjerkhofmeijer | 66:f12dec1c0c3d | 129 | #define PC_TX USBTX |
tjerkhofmeijer | 66:f12dec1c0c3d | 130 | #define PC_RX USBRX |
tjerkhofmeijer | 66:f12dec1c0c3d | 131 | #define MT_TX PTE0 |
tjerkhofmeijer | 66:f12dec1c0c3d | 132 | #define MT_RX PTE1 |
tjerkhofmeijer | 66:f12dec1c0c3d | 133 | #define MT_SDA PTE0 |
tjerkhofmeijer | 66:f12dec1c0c3d | 134 | #define MT_SCL PTE1 |
tjerkhofmeijer | 66:f12dec1c0c3d | 135 | #define MT_ADD0 PTD5 |
tjerkhofmeijer | 66:f12dec1c0c3d | 136 | #define MT_ADD1 PTD7 |
tjerkhofmeijer | 66:f12dec1c0c3d | 137 | #define MT_ADD2 PTD6 |
tjerkhofmeijer | 66:f12dec1c0c3d | 138 | #define MT_MOSI PTD6 |
tjerkhofmeijer | 66:f12dec1c0c3d | 139 | #define MT_MISO PTD7 |
tjerkhofmeijer | 66:f12dec1c0c3d | 140 | #define MT_SCLK PTD5 |
tjerkhofmeijer | 66:f12dec1c0c3d | 141 | #define MT_nCS PTD4 |
tjerkhofmeijer | 66:f12dec1c0c3d | 142 | #define MT_NRESET PTD3 |
tjerkhofmeijer | 66:f12dec1c0c3d | 143 | #define MT_DRDY PTD2 |
tjerkhofmeijer | 64:8a0f00a064bb | 144 | |
Alex Young |
60:ab9dad3560d3 | 145 | #elif defined(TARGET_LPC4088) |
tjerkhofmeijer | 64:8a0f00a064bb | 146 | |
tjerkhofmeijer | 66:f12dec1c0c3d | 147 | #define PC_TX USBTX |
tjerkhofmeijer | 66:f12dec1c0c3d | 148 | #define PC_RX USBRX |
tjerkhofmeijer | 66:f12dec1c0c3d | 149 | #define MT_TX p9 |
tjerkhofmeijer | 66:f12dec1c0c3d | 150 | #define MT_RX p10 |
tjerkhofmeijer | 66:f12dec1c0c3d | 151 | #define MT_SDA p9 |
tjerkhofmeijer | 66:f12dec1c0c3d | 152 | #define MT_SCL p10 |
tjerkhofmeijer | 66:f12dec1c0c3d | 153 | #define MT_ADD0 p13 |
tjerkhofmeijer | 66:f12dec1c0c3d | 154 | #define MT_ADD1 p12 |
tjerkhofmeijer | 66:f12dec1c0c3d | 155 | #define MT_ADD2 p11 |
tjerkhofmeijer | 66:f12dec1c0c3d | 156 | #define MT_MOSI p11 |
tjerkhofmeijer | 66:f12dec1c0c3d | 157 | #define MT_MISO p12 |
tjerkhofmeijer | 66:f12dec1c0c3d | 158 | #define MT_SCLK p13 |
tjerkhofmeijer | 66:f12dec1c0c3d | 159 | #define MT_nCS p14 |
tjerkhofmeijer | 66:f12dec1c0c3d | 160 | #define MT_NRESET p8 |
tjerkhofmeijer | 66:f12dec1c0c3d | 161 | #define MT_DRDY p15 |
tjerkhofmeijer | 64:8a0f00a064bb | 162 | |
Alex Young |
57:c3c85ebb7375 | 163 | #else |
tjerkhofmeijer | 66:f12dec1c0c3d | 164 | |
Alex Young |
57:c3c85ebb7375 | 165 | #error "Support for selected mbed platform has not been added." |
tjerkhofmeijer | 66:f12dec1c0c3d | 166 | |
Alex Young |
57:c3c85ebb7375 | 167 | #endif |
Alex Young |
57:c3c85ebb7375 | 168 | |
Alex Young |
57:c3c85ebb7375 | 169 | |
Alex Young |
44:b3980e8ac074 | 170 | /*! |
Alex Young |
53:3891f4259901 | 171 | * \brief Baudrate used to communicate with host PC. |
Alex Young |
53:3891f4259901 | 172 | */ |
Alex Young |
53:3891f4259901 | 173 | #define PC_UART_BAUDRATE (921600) |
Alex Young |
53:3891f4259901 | 174 | |
Alex Young |
53:3891f4259901 | 175 | /*! |
Alex Young |
44:b3980e8ac074 | 176 | * \brief The number of items to hold in the memory pools. |
Alex Young |
44:b3980e8ac074 | 177 | */ |
Alex Young |
25:01356fb59467 | 178 | #define MEMORY_POOL_SIZE (4) |
Alex Young |
44:b3980e8ac074 | 179 | /*! |
Alex Young |
44:b3980e8ac074 | 180 | * \brief The size of the queue used for device responses. |
Alex Young |
44:b3980e8ac074 | 181 | * This is set to one as in typical Xbus operation each command receives a |
Alex Young |
44:b3980e8ac074 | 182 | * response before the next command is sent. |
Alex Young |
44:b3980e8ac074 | 183 | */ |
Alex Young |
26:665d3624f9ab | 184 | #define RESPONSE_QUEUE_SIZE (1) |
Alex Young |
44:b3980e8ac074 | 185 | /*! |
Alex Young |
44:b3980e8ac074 | 186 | * \brief The size of the queue used for data messages. |
Alex Young |
44:b3980e8ac074 | 187 | * This is set to two to allow some overlap between printing received data to |
Alex Young |
44:b3980e8ac074 | 188 | * the PC serial port and the reception of the subsequent data packet. In |
Alex Young |
44:b3980e8ac074 | 189 | * more complex applications it might be necessary to increase this if |
Alex Young |
44:b3980e8ac074 | 190 | * message processing might occasionally require more time than normal. |
Alex Young |
44:b3980e8ac074 | 191 | */ |
Alex Young |
43:470c019246e4 | 192 | #define DATA_QUEUE_SIZE (2) |
Alex Young |
44:b3980e8ac074 | 193 | /*! |
Alex Young |
49:38ecfbff5391 | 194 | * \brief The maximum size of an xbus message supported by the application. |
Alex Young |
44:b3980e8ac074 | 195 | * This is the size of the message buffers in the message data memory pool. |
Alex Young |
44:b3980e8ac074 | 196 | */ |
Alex Young |
25:01356fb59467 | 197 | #define MAX_XBUS_DATA_SIZE (128) |
Alex Young |
25:01356fb59467 | 198 | |
Alex Young |
44:b3980e8ac074 | 199 | /*! \brief Serial port for communication with the host PC. */ |
Alex Young |
57:c3c85ebb7375 | 200 | static Serial pc(PC_TX, PC_RX); |
tjerkhofmeijer | 64:8a0f00a064bb | 201 | |
tjerkhofmeijer | 64:8a0f00a064bb | 202 | #if defined(MTI_USES_I2C_INTERFACE) |
tjerkhofmeijer | 64:8a0f00a064bb | 203 | /*! |
tjerkhofmeijer | 64:8a0f00a064bb | 204 | * \brief I2C master used for communication with the MT. |
tjerkhofmeijer | 64:8a0f00a064bb | 205 | */ |
tjerkhofmeijer | 64:8a0f00a064bb | 206 | static I2C mt(MT_SDA, MT_SCL); |
tjerkhofmeijer | 64:8a0f00a064bb | 207 | static DigitalOut add0(MT_ADD0); |
tjerkhofmeijer | 64:8a0f00a064bb | 208 | static DigitalOut add1(MT_ADD1); |
tjerkhofmeijer | 64:8a0f00a064bb | 209 | static DigitalOut add2(MT_ADD2); |
tjerkhofmeijer | 64:8a0f00a064bb | 210 | |
tjerkhofmeijer | 64:8a0f00a064bb | 211 | #elif defined(MTI_USES_SPI_INTERFACE) |
tjerkhofmeijer | 64:8a0f00a064bb | 212 | /*! \brief SPI master used for communication with the MT. */ |
tjerkhofmeijer | 64:8a0f00a064bb | 213 | static SPI mt(MT_MOSI, MT_MISO, MT_SCLK); |
tjerkhofmeijer | 64:8a0f00a064bb | 214 | |
tjerkhofmeijer | 64:8a0f00a064bb | 215 | /*! \brief Chip select line for the MT. */ |
tjerkhofmeijer | 64:8a0f00a064bb | 216 | static DigitalOut cs(MT_nCS, 1); |
tjerkhofmeijer | 64:8a0f00a064bb | 217 | |
tjerkhofmeijer | 64:8a0f00a064bb | 218 | #elif defined(MTI_USES_UART_INTERFACE) |
Alex Young |
58:db60ef0a0d16 | 219 | /*! |
Alex Young |
58:db60ef0a0d16 | 220 | * \brief Serial port for communication with the MT. |
Alex Young |
58:db60ef0a0d16 | 221 | * |
Alex Young |
58:db60ef0a0d16 | 222 | * We use a RawSerial port as the Stream inteface used by the regular |
Alex Young |
58:db60ef0a0d16 | 223 | * Serial class can have problems with the RTOS when using interrupts. |
Alex Young |
58:db60ef0a0d16 | 224 | */ |
Alex Young |
58:db60ef0a0d16 | 225 | static RawSerial mt(MT_TX, MT_RX); |
tjerkhofmeijer | 64:8a0f00a064bb | 226 | #endif |
tjerkhofmeijer | 64:8a0f00a064bb | 227 | |
tjerkhofmeijer | 64:8a0f00a064bb | 228 | #if defined(MTI_USES_I2C_INTERFACE) || defined(MTI_USES_SPI_INTERFACE) |
tjerkhofmeijer | 64:8a0f00a064bb | 229 | /*! |
tjerkhofmeijer | 64:8a0f00a064bb | 230 | * \brief Interrput line used by MT to signal that data is available. |
tjerkhofmeijer | 64:8a0f00a064bb | 231 | */ |
tjerkhofmeijer | 64:8a0f00a064bb | 232 | static InterruptIn drdy(MT_DRDY); |
tjerkhofmeijer | 64:8a0f00a064bb | 233 | #endif |
tjerkhofmeijer | 64:8a0f00a064bb | 234 | |
Alex Young |
35:7e519b88c610 | 235 | /*! |
Alex Young |
35:7e519b88c610 | 236 | * \brief MT reset line. |
Alex Young |
35:7e519b88c610 | 237 | * |
Alex Young |
35:7e519b88c610 | 238 | * MT is held in reset on startup. |
Alex Young |
35:7e519b88c610 | 239 | */ |
Alex Young |
57:c3c85ebb7375 | 240 | static DigitalOut mtReset(MT_NRESET, 0); |
Alex Young |
44:b3980e8ac074 | 241 | /*! \brief XbusParser used to parse incoming Xbus messages from the MT. */ |
Alex Young |
4:98f063b2e6da | 242 | static XbusParser* xbusParser; |
Alex Young |
25:01356fb59467 | 243 | |
Alex Young |
44:b3980e8ac074 | 244 | /*! |
Alex Young |
44:b3980e8ac074 | 245 | * \brief Memory pool used for storing Xbus messages when passing them |
Alex Young |
44:b3980e8ac074 | 246 | * to the main thread. |
Alex Young |
44:b3980e8ac074 | 247 | */ |
Alex Young |
25:01356fb59467 | 248 | MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool; |
Alex Young |
44:b3980e8ac074 | 249 | /*! |
Alex Young |
44:b3980e8ac074 | 250 | * \brief Memory pool used for storing the payload of Xbus messages. |
Alex Young |
44:b3980e8ac074 | 251 | */ |
Alex Young |
25:01356fb59467 | 252 | MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool; |
Alex Young |
44:b3980e8ac074 | 253 | /*! |
Alex Young |
44:b3980e8ac074 | 254 | * \brief Queue used to pass data messages to the main thread for processing. |
Alex Young |
44:b3980e8ac074 | 255 | */ |
Alex Young |
44:b3980e8ac074 | 256 | Queue<XbusMessage, DATA_QUEUE_SIZE> g_dataQueue; |
Alex Young |
44:b3980e8ac074 | 257 | /*! |
Alex Young |
44:b3980e8ac074 | 258 | * \brief Queue used for passing all other messages to the main thread for processing. |
Alex Young |
44:b3980e8ac074 | 259 | */ |
Alex Young |
26:665d3624f9ab | 260 | Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue; |
Alex Young |
4:98f063b2e6da | 261 | |
Alex Young |
44:b3980e8ac074 | 262 | /*! |
Alex Young |
44:b3980e8ac074 | 263 | * \brief Allocate message data buffer from the message data pool. |
Alex Young |
44:b3980e8ac074 | 264 | */ |
Alex Young |
25:01356fb59467 | 265 | static void* allocateMessageData(size_t bufSize) |
Alex Young |
4:98f063b2e6da | 266 | { |
Alex Young |
25:01356fb59467 | 267 | return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL; |
Alex Young |
25:01356fb59467 | 268 | } |
Alex Young |
25:01356fb59467 | 269 | |
Alex Young |
44:b3980e8ac074 | 270 | /*! |
Alex Young |
44:b3980e8ac074 | 271 | * \brief Deallocate message data previously allocated from the message |
Alex Young |
44:b3980e8ac074 | 272 | * data pool. |
Alex Young |
44:b3980e8ac074 | 273 | */ |
Alex Young |
25:01356fb59467 | 274 | static void deallocateMessageData(void const* buffer) |
Alex Young |
25:01356fb59467 | 275 | { |
Alex Young |
25:01356fb59467 | 276 | g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer); |
Alex Young |
4:98f063b2e6da | 277 | } |
Alex Young |
4:98f063b2e6da | 278 | |
tjerkhofmeijer | 64:8a0f00a064bb | 279 | #if defined(MTI_USES_I2C_INTERFACE) |
tjerkhofmeijer | 64:8a0f00a064bb | 280 | #define MTI_I2C_ADDRESS (0x1D << 1) |
tjerkhofmeijer | 64:8a0f00a064bb | 281 | static void readData(uint8_t pipe, uint16_t dataLength) |
tjerkhofmeijer | 64:8a0f00a064bb | 282 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 283 | const int preambleLength = 2; |
tjerkhofmeijer | 64:8a0f00a064bb | 284 | uint8_t* buf = (uint8_t*)allocateMessageData(dataLength+preambleLength); |
tjerkhofmeijer | 64:8a0f00a064bb | 285 | if (buf) |
tjerkhofmeijer | 64:8a0f00a064bb | 286 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 287 | buf[0] = XBUS_PREAMBLE; |
tjerkhofmeijer | 64:8a0f00a064bb | 288 | buf[1] = XBUS_MASTERDEVICE; |
tjerkhofmeijer | 64:8a0f00a064bb | 289 | mt.write(MTI_I2C_ADDRESS, (char*)&pipe, sizeof(pipe), true); |
tjerkhofmeijer | 64:8a0f00a064bb | 290 | mt.read(MTI_I2C_ADDRESS, (char*)buf+preambleLength, dataLength); |
tjerkhofmeijer | 64:8a0f00a064bb | 291 | XbusParser_parseBuffer(xbusParser, buf, dataLength+preambleLength); |
tjerkhofmeijer | 64:8a0f00a064bb | 292 | deallocateMessageData(buf); |
tjerkhofmeijer | 64:8a0f00a064bb | 293 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 294 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 295 | static void mtInterruptHandler(void) |
tjerkhofmeijer | 64:8a0f00a064bb | 296 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 297 | while (true) |
tjerkhofmeijer | 64:8a0f00a064bb | 298 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 299 | uint8_t opcode = XBUS_PIPE_STATUS; |
tjerkhofmeijer | 64:8a0f00a064bb | 300 | uint8_t status[4]; |
tjerkhofmeijer | 64:8a0f00a064bb | 301 | mt.write(MTI_I2C_ADDRESS, (char*)&opcode, sizeof(opcode), true); |
tjerkhofmeijer | 64:8a0f00a064bb | 302 | mt.read(MTI_I2C_ADDRESS, (char*)status, sizeof(status)); |
tjerkhofmeijer | 64:8a0f00a064bb | 303 | |
tjerkhofmeijer | 64:8a0f00a064bb | 304 | uint16_t notificationSize = status[0] | (status[1] << 8); |
tjerkhofmeijer | 68:6d6dbeefd196 | 305 | uint16_t measurementSize = status[2] | (status[3] << 8); |
tjerkhofmeijer | 64:8a0f00a064bb | 306 | |
tjerkhofmeijer | 64:8a0f00a064bb | 307 | if (notificationSize) |
tjerkhofmeijer | 64:8a0f00a064bb | 308 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 309 | readData(XBUS_NOTIFICATION_PIPE, notificationSize); |
tjerkhofmeijer | 64:8a0f00a064bb | 310 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 311 | else if (measurementSize) |
tjerkhofmeijer | 64:8a0f00a064bb | 312 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 313 | readData(XBUS_MEASUREMENT_PIPE, measurementSize); |
tjerkhofmeijer | 64:8a0f00a064bb | 314 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 315 | else |
tjerkhofmeijer | 64:8a0f00a064bb | 316 | break; // No more data available to read. |
tjerkhofmeijer | 64:8a0f00a064bb | 317 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 318 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 319 | |
tjerkhofmeijer | 64:8a0f00a064bb | 320 | static void configureMtCommunicationInterface(void) |
tjerkhofmeijer | 64:8a0f00a064bb | 321 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 322 | mt.frequency(400000); |
tjerkhofmeijer | 64:8a0f00a064bb | 323 | //Use the addX pins to configure I2C address 0x1D |
tjerkhofmeijer | 64:8a0f00a064bb | 324 | add0.write(0); |
tjerkhofmeijer | 64:8a0f00a064bb | 325 | add1.write(0); |
tjerkhofmeijer | 64:8a0f00a064bb | 326 | add2.write(0); |
tjerkhofmeijer | 64:8a0f00a064bb | 327 | drdy.rise(&mtInterruptHandler); |
tjerkhofmeijer | 64:8a0f00a064bb | 328 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 329 | |
tjerkhofmeijer | 64:8a0f00a064bb | 330 | /*! |
tjerkhofmeijer | 64:8a0f00a064bb | 331 | * \brief Send a message to the MT |
tjerkhofmeijer | 64:8a0f00a064bb | 332 | * |
tjerkhofmeijer | 64:8a0f00a064bb | 333 | * This function formats the message data and writes this to the MT I2C |
tjerkhofmeijer | 64:8a0f00a064bb | 334 | * interface. It does not wait for any response. |
tjerkhofmeijer | 64:8a0f00a064bb | 335 | */ |
tjerkhofmeijer | 64:8a0f00a064bb | 336 | static void sendMessage(XbusMessage const* m) |
tjerkhofmeijer | 64:8a0f00a064bb | 337 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 338 | uint8_t buf[64]; |
tjerkhofmeijer | 64:8a0f00a064bb | 339 | size_t rawLength = XbusMessage_format(buf, m, XLLF_I2c); |
tjerkhofmeijer | 64:8a0f00a064bb | 340 | mt.write(MTI_I2C_ADDRESS, (char*)buf, rawLength); |
tjerkhofmeijer | 64:8a0f00a064bb | 341 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 342 | #elif defined(MTI_USES_SPI_INTERFACE) |
tjerkhofmeijer | 64:8a0f00a064bb | 343 | static void sendOpcode(uint8_t opcode) |
tjerkhofmeijer | 64:8a0f00a064bb | 344 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 345 | mt.write(opcode); |
tjerkhofmeijer | 64:8a0f00a064bb | 346 | for (int filler = 0; filler < 3; ++filler) |
tjerkhofmeijer | 64:8a0f00a064bb | 347 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 348 | mt.write(filler); |
tjerkhofmeijer | 64:8a0f00a064bb | 349 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 350 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 351 | |
tjerkhofmeijer | 64:8a0f00a064bb | 352 | static void readData(uint8_t pipe, uint16_t dataLength) |
tjerkhofmeijer | 64:8a0f00a064bb | 353 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 354 | const int preambleLength = 2; |
tjerkhofmeijer | 64:8a0f00a064bb | 355 | uint8_t* buf = (uint8_t*)allocateMessageData(dataLength+preambleLength); |
tjerkhofmeijer | 64:8a0f00a064bb | 356 | if (buf) |
tjerkhofmeijer | 64:8a0f00a064bb | 357 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 358 | uint8_t* dptr = buf; |
tjerkhofmeijer | 64:8a0f00a064bb | 359 | *dptr++ = XBUS_PREAMBLE; |
tjerkhofmeijer | 64:8a0f00a064bb | 360 | *dptr++ = XBUS_MASTERDEVICE; |
tjerkhofmeijer | 64:8a0f00a064bb | 361 | cs = 0; |
tjerkhofmeijer | 64:8a0f00a064bb | 362 | sendOpcode(pipe); |
tjerkhofmeijer | 64:8a0f00a064bb | 363 | for (int i = 0; i < dataLength; ++i) |
tjerkhofmeijer | 64:8a0f00a064bb | 364 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 365 | *dptr++ = mt.write(0); |
tjerkhofmeijer | 64:8a0f00a064bb | 366 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 367 | cs = 1; |
tjerkhofmeijer | 64:8a0f00a064bb | 368 | XbusParser_parseBuffer(xbusParser, buf, dptr - buf); |
tjerkhofmeijer | 64:8a0f00a064bb | 369 | deallocateMessageData(buf); |
tjerkhofmeijer | 64:8a0f00a064bb | 370 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 371 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 372 | static void mtInterruptHandler(void) |
tjerkhofmeijer | 64:8a0f00a064bb | 373 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 374 | while (true) |
tjerkhofmeijer | 64:8a0f00a064bb | 375 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 376 | cs = 0; |
tjerkhofmeijer | 64:8a0f00a064bb | 377 | sendOpcode(XBUS_PIPE_STATUS); |
tjerkhofmeijer | 64:8a0f00a064bb | 378 | uint8_t status[4]; |
tjerkhofmeijer | 64:8a0f00a064bb | 379 | for (int i = 0; i < sizeof(status); ++i) |
tjerkhofmeijer | 64:8a0f00a064bb | 380 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 381 | status[i] = mt.write(0); |
tjerkhofmeijer | 64:8a0f00a064bb | 382 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 383 | cs = 1; |
tjerkhofmeijer | 64:8a0f00a064bb | 384 | |
tjerkhofmeijer | 64:8a0f00a064bb | 385 | uint16_t notificationSize = status[0] | (status[1] << 8); |
tjerkhofmeijer | 64:8a0f00a064bb | 386 | uint16_t measurementSize = status[2] | (status[3] <<8); |
tjerkhofmeijer | 64:8a0f00a064bb | 387 | |
tjerkhofmeijer | 64:8a0f00a064bb | 388 | if (notificationSize) |
tjerkhofmeijer | 64:8a0f00a064bb | 389 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 390 | readData(XBUS_NOTIFICATION_PIPE, notificationSize); |
tjerkhofmeijer | 64:8a0f00a064bb | 391 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 392 | else if (measurementSize) |
tjerkhofmeijer | 64:8a0f00a064bb | 393 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 394 | readData(XBUS_MEASUREMENT_PIPE, measurementSize); |
tjerkhofmeijer | 64:8a0f00a064bb | 395 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 396 | else |
tjerkhofmeijer | 64:8a0f00a064bb | 397 | break; // No more data available to read. |
tjerkhofmeijer | 64:8a0f00a064bb | 398 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 399 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 400 | |
tjerkhofmeijer | 64:8a0f00a064bb | 401 | static void configureMtCommunicationInterface(void) |
tjerkhofmeijer | 64:8a0f00a064bb | 402 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 403 | mt.frequency(1000000); |
tjerkhofmeijer | 64:8a0f00a064bb | 404 | mt.format(8, 3); |
tjerkhofmeijer | 64:8a0f00a064bb | 405 | drdy.rise(&mtInterruptHandler); |
tjerkhofmeijer | 64:8a0f00a064bb | 406 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 407 | |
tjerkhofmeijer | 64:8a0f00a064bb | 408 | /*! |
tjerkhofmeijer | 64:8a0f00a064bb | 409 | * \brief Send a message to the MT |
tjerkhofmeijer | 64:8a0f00a064bb | 410 | * |
tjerkhofmeijer | 64:8a0f00a064bb | 411 | * This function formats the message data and writes this to the MT SPI |
tjerkhofmeijer | 64:8a0f00a064bb | 412 | * interface. It does not wait for any response. |
tjerkhofmeijer | 64:8a0f00a064bb | 413 | */ |
tjerkhofmeijer | 64:8a0f00a064bb | 414 | static void sendMessage(XbusMessage const* m) |
tjerkhofmeijer | 64:8a0f00a064bb | 415 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 416 | uint8_t buf[64]; |
tjerkhofmeijer | 64:8a0f00a064bb | 417 | size_t rawLength = XbusMessage_format(buf, m, XLLF_Spi); |
tjerkhofmeijer | 64:8a0f00a064bb | 418 | cs = 0; |
tjerkhofmeijer | 64:8a0f00a064bb | 419 | for (int i = 0; i < rawLength; ++i) |
tjerkhofmeijer | 64:8a0f00a064bb | 420 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 421 | mt.write(buf[i]); |
tjerkhofmeijer | 64:8a0f00a064bb | 422 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 423 | cs = 1; |
tjerkhofmeijer | 64:8a0f00a064bb | 424 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 425 | #elif defined(MTI_USES_UART_INTERFACE) |
Alex Young |
44:b3980e8ac074 | 426 | /*! |
Alex Young |
44:b3980e8ac074 | 427 | * \brief RX Interrupt handler for the MT serial port. |
Alex Young |
44:b3980e8ac074 | 428 | * |
Alex Young |
44:b3980e8ac074 | 429 | * Passes received data to an XbusParser to extract messages. |
Alex Young |
44:b3980e8ac074 | 430 | */ |
Alex Young |
4:98f063b2e6da | 431 | static void mtLowLevelHandler(void) |
Alex Young |
4:98f063b2e6da | 432 | { |
Alex Young |
4:98f063b2e6da | 433 | while (mt.readable()) |
Alex Young |
4:98f063b2e6da | 434 | { |
Alex Young |
4:98f063b2e6da | 435 | XbusParser_parseByte(xbusParser, mt.getc()); |
Alex Young |
4:98f063b2e6da | 436 | } |
Alex Young |
4:98f063b2e6da | 437 | } |
Alex Young |
4:98f063b2e6da | 438 | |
Alex Young |
44:b3980e8ac074 | 439 | /*! |
tjerkhofmeijer | 64:8a0f00a064bb | 440 | * \brief Configure the serial port used for communication with the |
tjerkhofmeijer | 64:8a0f00a064bb | 441 | * motion tracker. |
tjerkhofmeijer | 64:8a0f00a064bb | 442 | */ |
tjerkhofmeijer | 64:8a0f00a064bb | 443 | static void configureMtCommunicationInterface(void) |
tjerkhofmeijer | 64:8a0f00a064bb | 444 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 445 | mt.baud(115200); |
tjerkhofmeijer | 64:8a0f00a064bb | 446 | mt.format(8, Serial::None, 1); |
tjerkhofmeijer | 64:8a0f00a064bb | 447 | mt.attach(mtLowLevelHandler, Serial::RxIrq); |
tjerkhofmeijer | 64:8a0f00a064bb | 448 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 449 | |
tjerkhofmeijer | 64:8a0f00a064bb | 450 | /*! |
Alex Young |
44:b3980e8ac074 | 451 | * \brief Send a message to the MT |
Alex Young |
44:b3980e8ac074 | 452 | * |
Alex Young |
44:b3980e8ac074 | 453 | * This function formats the message data and writes this to the MT serial |
Alex Young |
44:b3980e8ac074 | 454 | * port. It does not wait for any response. |
Alex Young |
44:b3980e8ac074 | 455 | */ |
Alex Young |
34:3d7a6519a256 | 456 | static void sendMessage(XbusMessage const* m) |
Alex Young |
11:8593ba137917 | 457 | { |
Alex Young |
26:665d3624f9ab | 458 | uint8_t buf[64]; |
tjerkhofmeijer | 64:8a0f00a064bb | 459 | size_t rawLength = XbusMessage_format(buf, m, XLLF_Uart); |
Alex Young |
11:8593ba137917 | 460 | for (size_t i = 0; i < rawLength; ++i) |
Alex Young |
11:8593ba137917 | 461 | { |
Alex Young |
11:8593ba137917 | 462 | mt.putc(buf[i]); |
Alex Young |
11:8593ba137917 | 463 | } |
Alex Young |
34:3d7a6519a256 | 464 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 465 | #endif |
tjerkhofmeijer | 64:8a0f00a064bb | 466 | |
Alex Young |
34:3d7a6519a256 | 467 | |
Alex Young |
44:b3980e8ac074 | 468 | /*! |
Alex Young |
44:b3980e8ac074 | 469 | * \brief Send a message to the MT and wait for a response. |
Alex Young |
44:b3980e8ac074 | 470 | * \returns Response message from the MT, or NULL is no response received |
Alex Young |
44:b3980e8ac074 | 471 | * within 500ms. |
Alex Young |
44:b3980e8ac074 | 472 | * |
Alex Young |
44:b3980e8ac074 | 473 | * Blocking behaviour is implemented by waiting for a response to be written |
Alex Young |
44:b3980e8ac074 | 474 | * to the response queue by the XbusParser. |
Alex Young |
44:b3980e8ac074 | 475 | */ |
Alex Young |
34:3d7a6519a256 | 476 | static XbusMessage const* doTransaction(XbusMessage const* m) |
Alex Young |
34:3d7a6519a256 | 477 | { |
Alex Young |
34:3d7a6519a256 | 478 | sendMessage(m); |
Alex Young |
26:665d3624f9ab | 479 | |
Alex Young |
26:665d3624f9ab | 480 | osEvent ev = g_responseQueue.get(500); |
Alex Young |
26:665d3624f9ab | 481 | return ev.status == osEventMessage ? (XbusMessage*)ev.value.p : NULL; |
Alex Young |
26:665d3624f9ab | 482 | } |
Alex Young |
26:665d3624f9ab | 483 | |
Alex Young |
31:ce1ea9ae861e | 484 | /*! |
Alex Young |
31:ce1ea9ae861e | 485 | * \brief RAII object to manage message memory deallocation. |
Alex Young |
31:ce1ea9ae861e | 486 | * |
Alex Young |
49:38ecfbff5391 | 487 | * Will automatically free the memory used by an XbusMessage when going out |
Alex Young |
31:ce1ea9ae861e | 488 | * of scope. |
Alex Young |
31:ce1ea9ae861e | 489 | */ |
Alex Young |
31:ce1ea9ae861e | 490 | class XbusMessageMemoryManager |
Alex Young |
26:665d3624f9ab | 491 | { |
Alex Young |
31:ce1ea9ae861e | 492 | public: |
Alex Young |
31:ce1ea9ae861e | 493 | XbusMessageMemoryManager(XbusMessage const* message) |
Alex Young |
31:ce1ea9ae861e | 494 | : m_message(message) |
Alex Young |
31:ce1ea9ae861e | 495 | { |
Alex Young |
31:ce1ea9ae861e | 496 | } |
Alex Young |
31:ce1ea9ae861e | 497 | |
Alex Young |
31:ce1ea9ae861e | 498 | ~XbusMessageMemoryManager() |
Alex Young |
31:ce1ea9ae861e | 499 | { |
Alex Young |
31:ce1ea9ae861e | 500 | if (m_message) |
Alex Young |
31:ce1ea9ae861e | 501 | { |
Alex Young |
31:ce1ea9ae861e | 502 | if (m_message->data) |
Alex Young |
31:ce1ea9ae861e | 503 | deallocateMessageData(m_message->data); |
Alex Young |
31:ce1ea9ae861e | 504 | g_messagePool.free(const_cast<XbusMessage*>(m_message)); |
Alex Young |
31:ce1ea9ae861e | 505 | } |
Alex Young |
31:ce1ea9ae861e | 506 | } |
Alex Young |
31:ce1ea9ae861e | 507 | |
Alex Young |
31:ce1ea9ae861e | 508 | private: |
Alex Young |
31:ce1ea9ae861e | 509 | XbusMessage const* m_message; |
Alex Young |
31:ce1ea9ae861e | 510 | }; |
Alex Young |
26:665d3624f9ab | 511 | |
Alex Young |
44:b3980e8ac074 | 512 | /*! |
Alex Young |
44:b3980e8ac074 | 513 | * \brief Dump information from a message to the PC serial port. |
Alex Young |
44:b3980e8ac074 | 514 | */ |
Alex Young |
29:d9310e7b58b5 | 515 | static void dumpResponse(XbusMessage const* response) |
Alex Young |
29:d9310e7b58b5 | 516 | { |
Alex Young |
29:d9310e7b58b5 | 517 | switch (response->mid) |
Alex Young |
29:d9310e7b58b5 | 518 | { |
Alex Young |
29:d9310e7b58b5 | 519 | case XMID_GotoConfigAck: |
Alex Young |
52:e2197b38c029 | 520 | pc.printf("Device went to config mode.\r\n"); |
Alex Young |
29:d9310e7b58b5 | 521 | break; |
Alex Young |
29:d9310e7b58b5 | 522 | |
Alex Young |
29:d9310e7b58b5 | 523 | case XMID_Error: |
Alex Young |
29:d9310e7b58b5 | 524 | pc.printf("Device error!"); |
Alex Young |
29:d9310e7b58b5 | 525 | break; |
Alex Young |
29:d9310e7b58b5 | 526 | |
Alex Young |
29:d9310e7b58b5 | 527 | default: |
Alex Young |
52:e2197b38c029 | 528 | pc.printf("Received response MID=%X, length=%d\r\n", response->mid, response->length); |
Alex Young |
29:d9310e7b58b5 | 529 | break; |
Alex Young |
29:d9310e7b58b5 | 530 | } |
Alex Young |
29:d9310e7b58b5 | 531 | } |
Alex Young |
29:d9310e7b58b5 | 532 | |
Alex Young |
44:b3980e8ac074 | 533 | /*! |
Alex Young |
44:b3980e8ac074 | 534 | * \brief Send a command to the MT and wait for a response. |
Alex Young |
44:b3980e8ac074 | 535 | * \param cmdId The XsMessageId of the command to send. |
Alex Young |
44:b3980e8ac074 | 536 | * |
Alex Young |
44:b3980e8ac074 | 537 | * Commands are simple messages without and payload data. |
Alex Young |
44:b3980e8ac074 | 538 | */ |
Alex Young |
26:665d3624f9ab | 539 | static void sendCommand(XsMessageId cmdId) |
Alex Young |
26:665d3624f9ab | 540 | { |
Alex Young |
26:665d3624f9ab | 541 | XbusMessage m = {cmdId}; |
Alex Young |
26:665d3624f9ab | 542 | XbusMessage const* response = doTransaction(&m); |
Alex Young |
31:ce1ea9ae861e | 543 | XbusMessageMemoryManager janitor(response); |
Alex Young |
26:665d3624f9ab | 544 | |
Alex Young |
26:665d3624f9ab | 545 | if (response) |
Alex Young |
26:665d3624f9ab | 546 | { |
Alex Young |
29:d9310e7b58b5 | 547 | dumpResponse(response); |
Alex Young |
26:665d3624f9ab | 548 | } |
Alex Young |
26:665d3624f9ab | 549 | else |
Alex Young |
26:665d3624f9ab | 550 | { |
Alex Young |
52:e2197b38c029 | 551 | pc.printf("Timeout waiting for response.\r\n"); |
Alex Young |
26:665d3624f9ab | 552 | } |
Alex Young |
11:8593ba137917 | 553 | } |
Alex Young |
11:8593ba137917 | 554 | |
Alex Young |
44:b3980e8ac074 | 555 | /*! |
Alex Young |
44:b3980e8ac074 | 556 | * \brief Handle a command from the PC |
Alex Young |
44:b3980e8ac074 | 557 | * |
Alex Young |
44:b3980e8ac074 | 558 | * The example application supports single character commands from the host |
Alex Young |
44:b3980e8ac074 | 559 | * PC to switch between configuration and measurement modes. |
Alex Young |
44:b3980e8ac074 | 560 | */ |
Alex Young |
11:8593ba137917 | 561 | static void handlePcCommand(char cmd) |
Alex Young |
11:8593ba137917 | 562 | { |
Alex Young |
11:8593ba137917 | 563 | switch (cmd) |
Alex Young |
11:8593ba137917 | 564 | { |
Alex Young |
11:8593ba137917 | 565 | case 'c': |
Alex Young |
11:8593ba137917 | 566 | sendCommand(XMID_GotoConfig); |
Alex Young |
11:8593ba137917 | 567 | break; |
Alex Young |
11:8593ba137917 | 568 | |
Alex Young |
11:8593ba137917 | 569 | case 'm': |
Alex Young |
11:8593ba137917 | 570 | sendCommand(XMID_GotoMeasurement); |
Alex Young |
11:8593ba137917 | 571 | break; |
Alex Young |
11:8593ba137917 | 572 | } |
Alex Young |
11:8593ba137917 | 573 | } |
Alex Young |
11:8593ba137917 | 574 | |
Alex Young |
44:b3980e8ac074 | 575 | /*! |
Alex Young |
44:b3980e8ac074 | 576 | * \brief XbusParser callback function to handle received messages. |
Alex Young |
44:b3980e8ac074 | 577 | * \param message Pointer to the last received message. |
Alex Young |
44:b3980e8ac074 | 578 | * |
Alex Young |
44:b3980e8ac074 | 579 | * In this example received messages are copied into one of two message |
Alex Young |
44:b3980e8ac074 | 580 | * queues for later handling by the main thread. Data messages are put |
Alex Young |
49:38ecfbff5391 | 581 | * in one queue, while all other responses are placed in the second queue. |
Alex Young |
44:b3980e8ac074 | 582 | * This is done so that data and other messages can be handled separately |
Alex Young |
44:b3980e8ac074 | 583 | * by the application code. |
Alex Young |
44:b3980e8ac074 | 584 | */ |
Alex Young |
24:2cc49dc854e3 | 585 | static void mtMessageHandler(struct XbusMessage const* message) |
Alex Young |
4:98f063b2e6da | 586 | { |
Alex Young |
43:470c019246e4 | 587 | XbusMessage* m = g_messagePool.alloc(); |
Alex Young |
43:470c019246e4 | 588 | if (m) |
Alex Young |
7:c913a7cd5231 | 589 | { |
Alex Young |
43:470c019246e4 | 590 | memcpy(m, message, sizeof(XbusMessage)); |
Alex Young |
43:470c019246e4 | 591 | if (message->mid == XMID_MtData2) |
Alex Young |
43:470c019246e4 | 592 | { |
Alex Young |
43:470c019246e4 | 593 | g_dataQueue.put(m); |
Alex Young |
43:470c019246e4 | 594 | } |
Alex Young |
43:470c019246e4 | 595 | else |
Alex Young |
43:470c019246e4 | 596 | { |
Alex Young |
43:470c019246e4 | 597 | g_responseQueue.put(m); |
Alex Young |
43:470c019246e4 | 598 | } |
Alex Young |
7:c913a7cd5231 | 599 | } |
Alex Young |
43:470c019246e4 | 600 | else if (message->data) |
Alex Young |
7:c913a7cd5231 | 601 | { |
Alex Young |
43:470c019246e4 | 602 | deallocateMessageData(message->data); |
Alex Young |
25:01356fb59467 | 603 | } |
Alex Young |
4:98f063b2e6da | 604 | } |
Alex Young |
4:98f063b2e6da | 605 | |
Alex Young |
44:b3980e8ac074 | 606 | /*! |
tjerkhofmeijer | 64:8a0f00a064bb | 607 | * \brief Configure the serial port used to communicate with the host PC. |
Alex Young |
44:b3980e8ac074 | 608 | */ |
tjerkhofmeijer | 64:8a0f00a064bb | 609 | static void configurePcInterface(void) |
Alex Young |
4:98f063b2e6da | 610 | { |
Alex Young |
53:3891f4259901 | 611 | pc.baud(PC_UART_BAUDRATE); |
Alex Young |
55:9a2d6f947f0d | 612 | pc.format(8, Serial::None, 1); |
Alex Young |
4:98f063b2e6da | 613 | } |
Alex Young |
4:98f063b2e6da | 614 | |
Alex Young |
44:b3980e8ac074 | 615 | /*! |
Alex Young |
44:b3980e8ac074 | 616 | * \brief Read the device ID of the motion tracker. |
Alex Young |
44:b3980e8ac074 | 617 | */ |
Alex Young |
29:d9310e7b58b5 | 618 | static uint32_t readDeviceId(void) |
Alex Young |
29:d9310e7b58b5 | 619 | { |
Alex Young |
29:d9310e7b58b5 | 620 | XbusMessage reqDid = {XMID_ReqDid}; |
Alex Young |
29:d9310e7b58b5 | 621 | XbusMessage const* didRsp = doTransaction(&reqDid); |
Alex Young |
31:ce1ea9ae861e | 622 | XbusMessageMemoryManager janitor(didRsp); |
Alex Young |
29:d9310e7b58b5 | 623 | uint32_t deviceId = 0; |
Alex Young |
29:d9310e7b58b5 | 624 | if (didRsp) |
Alex Young |
29:d9310e7b58b5 | 625 | { |
Alex Young |
29:d9310e7b58b5 | 626 | if (didRsp->mid == XMID_DeviceId) |
Alex Young |
29:d9310e7b58b5 | 627 | { |
Alex Young |
29:d9310e7b58b5 | 628 | deviceId = *(uint32_t*)didRsp->data; |
Alex Young |
29:d9310e7b58b5 | 629 | } |
Alex Young |
29:d9310e7b58b5 | 630 | } |
Alex Young |
29:d9310e7b58b5 | 631 | return deviceId; |
Alex Young |
29:d9310e7b58b5 | 632 | } |
Alex Young |
29:d9310e7b58b5 | 633 | |
Alex Young |
44:b3980e8ac074 | 634 | /*! |
Alex Young |
44:b3980e8ac074 | 635 | * \brief Sets MT output configuration. |
Alex Young |
44:b3980e8ac074 | 636 | * \param conf Pointer to an array of OutputConfiguration elements. |
Alex Young |
44:b3980e8ac074 | 637 | * \param elements The number of elements in the configuration array. |
Alex Young |
44:b3980e8ac074 | 638 | * |
Alex Young |
44:b3980e8ac074 | 639 | * The response from the device indicates the actual values that will |
Alex Young |
44:b3980e8ac074 | 640 | * be used by the motion tracker. These may differ from the requested |
Alex Young |
44:b3980e8ac074 | 641 | * parameters as the motion tracker validates the requested parameters |
Alex Young |
44:b3980e8ac074 | 642 | * before applying them. |
Alex Young |
44:b3980e8ac074 | 643 | */ |
Alex Young |
32:fafe0f42d82b | 644 | static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements) |
Alex Young |
29:d9310e7b58b5 | 645 | { |
Alex Young |
32:fafe0f42d82b | 646 | XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf}; |
Alex Young |
32:fafe0f42d82b | 647 | XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg); |
Alex Young |
32:fafe0f42d82b | 648 | XbusMessageMemoryManager janitor(outputConfRsp); |
Alex Young |
32:fafe0f42d82b | 649 | if (outputConfRsp) |
Alex Young |
29:d9310e7b58b5 | 650 | { |
Alex Young |
32:fafe0f42d82b | 651 | if (outputConfRsp->mid == XMID_OutputConfig) |
Alex Young |
29:d9310e7b58b5 | 652 | { |
Alex Young |
52:e2197b38c029 | 653 | pc.printf("Output configuration set to:\r\n"); |
Alex Young |
32:fafe0f42d82b | 654 | OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data; |
Alex Young |
32:fafe0f42d82b | 655 | for (int i = 0; i < outputConfRsp->length; ++i) |
Alex Young |
32:fafe0f42d82b | 656 | { |
Alex Young |
52:e2197b38c029 | 657 | pc.printf("\t%s: %d Hz\r\n", XbusMessage_dataDescription(conf->dtype), conf->freq); |
Alex Young |
32:fafe0f42d82b | 658 | ++conf; |
Alex Young |
32:fafe0f42d82b | 659 | } |
Alex Young |
32:fafe0f42d82b | 660 | return true; |
Alex Young |
29:d9310e7b58b5 | 661 | } |
Alex Young |
29:d9310e7b58b5 | 662 | else |
Alex Young |
29:d9310e7b58b5 | 663 | { |
Alex Young |
32:fafe0f42d82b | 664 | dumpResponse(outputConfRsp); |
Alex Young |
29:d9310e7b58b5 | 665 | } |
Alex Young |
32:fafe0f42d82b | 666 | } |
Alex Young |
32:fafe0f42d82b | 667 | else |
Alex Young |
32:fafe0f42d82b | 668 | { |
Alex Young |
52:e2197b38c029 | 669 | pc.printf("Failed to set output configuration.\r\n"); |
Alex Young |
32:fafe0f42d82b | 670 | } |
Alex Young |
32:fafe0f42d82b | 671 | return false; |
Alex Young |
32:fafe0f42d82b | 672 | } |
Alex Young |
29:d9310e7b58b5 | 673 | |
Alex Young |
44:b3980e8ac074 | 674 | /*! |
Alex Young |
44:b3980e8ac074 | 675 | * \brief Sets the motion tracker output configuration based on the function |
Alex Young |
44:b3980e8ac074 | 676 | * of the attached device. |
Alex Young |
44:b3980e8ac074 | 677 | * |
Alex Young |
44:b3980e8ac074 | 678 | * The output configuration depends on the type of MTi-1 device connected. |
Alex Young |
49:38ecfbff5391 | 679 | * An MTI-1 (IMU) device does not have an onboard orientation filter so |
Alex Young |
44:b3980e8ac074 | 680 | * cannot output quaternion data, only inertial and magnetic measurement |
Alex Young |
44:b3980e8ac074 | 681 | * data. |
Alex Young |
44:b3980e8ac074 | 682 | * MTi-2 and MTi-3 devices have an onboard filter so can send quaternions. |
Alex Young |
44:b3980e8ac074 | 683 | */ |
Alex Young |
32:fafe0f42d82b | 684 | static bool configureMotionTracker(void) |
Alex Young |
32:fafe0f42d82b | 685 | { |
Alex Young |
32:fafe0f42d82b | 686 | uint32_t deviceId = readDeviceId(); |
Alex Young |
32:fafe0f42d82b | 687 | |
Alex Young |
32:fafe0f42d82b | 688 | if (deviceId) |
Alex Young |
32:fafe0f42d82b | 689 | { |
Alex Young |
52:e2197b38c029 | 690 | pc.printf("Found device with ID: %08X.\r\n", deviceId); |
Alex Young |
40:b77a8c10c76d | 691 | if (!XsDeviceId_isMtMk4_X(deviceId)) |
Alex Young |
40:b77a8c10c76d | 692 | { |
Alex Young |
52:e2197b38c029 | 693 | pc.printf("Device is not an MTi-1 series.\r\n"); |
Alex Young |
40:b77a8c10c76d | 694 | return false; |
Alex Young |
40:b77a8c10c76d | 695 | } |
Alex Young |
32:fafe0f42d82b | 696 | |
Alex Young |
40:b77a8c10c76d | 697 | DeviceFunction function = XsDeviceId_getFunction(deviceId); |
Alex Young |
52:e2197b38c029 | 698 | pc.printf("Device is an MTi-%d: %s.\r\n", function, XsDeviceId_functionDescription(function)); |
Alex Young |
40:b77a8c10c76d | 699 | |
Alex Young |
40:b77a8c10c76d | 700 | if (function == DF_IMU) |
Alex Young |
29:d9310e7b58b5 | 701 | { |
Alex Young |
32:fafe0f42d82b | 702 | OutputConfiguration conf[] = { |
Alex Young |
32:fafe0f42d82b | 703 | {XDI_PacketCounter, 65535}, |
Alex Young |
32:fafe0f42d82b | 704 | {XDI_SampleTimeFine, 65535}, |
Alex Young |
32:fafe0f42d82b | 705 | {XDI_Acceleration, 100}, |
Alex Young |
32:fafe0f42d82b | 706 | {XDI_RateOfTurn, 100}, |
Alex Young |
32:fafe0f42d82b | 707 | {XDI_MagneticField, 100} |
Alex Young |
32:fafe0f42d82b | 708 | }; |
Alex Young |
32:fafe0f42d82b | 709 | return setOutputConfiguration(conf, |
Alex Young |
32:fafe0f42d82b | 710 | sizeof(conf) / sizeof(OutputConfiguration)); |
Alex Young |
29:d9310e7b58b5 | 711 | } |
Alex Young |
29:d9310e7b58b5 | 712 | else |
Alex Young |
29:d9310e7b58b5 | 713 | { |
Alex Young |
32:fafe0f42d82b | 714 | OutputConfiguration conf[] = { |
Alex Young |
32:fafe0f42d82b | 715 | {XDI_PacketCounter, 65535}, |
Alex Young |
32:fafe0f42d82b | 716 | {XDI_SampleTimeFine, 65535}, |
Alex Young |
32:fafe0f42d82b | 717 | {XDI_Quaternion, 100}, |
Alex Young |
32:fafe0f42d82b | 718 | {XDI_StatusWord, 65535} |
Alex Young |
32:fafe0f42d82b | 719 | }; |
Alex Young |
32:fafe0f42d82b | 720 | return setOutputConfiguration(conf, |
Alex Young |
32:fafe0f42d82b | 721 | sizeof(conf) / sizeof(OutputConfiguration)); |
Alex Young |
29:d9310e7b58b5 | 722 | } |
Alex Young |
29:d9310e7b58b5 | 723 | } |
Alex Young |
32:fafe0f42d82b | 724 | |
Alex Young |
32:fafe0f42d82b | 725 | return false; |
Alex Young |
29:d9310e7b58b5 | 726 | } |
Alex Young |
29:d9310e7b58b5 | 727 | |
Alex Young |
35:7e519b88c610 | 728 | /*! |
Alex Young |
35:7e519b88c610 | 729 | * \brief Wait for a wakeup message from the MTi. |
Alex Young |
37:3e87bf647c68 | 730 | * \param timeout Time to wait to receive the wakeup message. |
Alex Young |
37:3e87bf647c68 | 731 | * \return true if wakeup received within timeout, else false. |
Alex Young |
35:7e519b88c610 | 732 | * |
Alex Young |
49:38ecfbff5391 | 733 | * The MTi sends an XMID_Wakeup message once it has completed its bootup |
Alex Young |
49:38ecfbff5391 | 734 | * procedure. If this is acknowledged by an XMID_WakeupAck message then the MTi |
Alex Young |
35:7e519b88c610 | 735 | * will stay in configuration mode. Otherwise it will automatically enter |
Alex Young |
35:7e519b88c610 | 736 | * measurement mode with the stored output configuration. |
Alex Young |
35:7e519b88c610 | 737 | */ |
Alex Young |
37:3e87bf647c68 | 738 | bool waitForWakeup(uint32_t timeout) |
Alex Young |
35:7e519b88c610 | 739 | { |
Alex Young |
37:3e87bf647c68 | 740 | osEvent ev = g_responseQueue.get(timeout); |
Alex Young |
35:7e519b88c610 | 741 | if (ev.status == osEventMessage) |
Alex Young |
35:7e519b88c610 | 742 | { |
Alex Young |
35:7e519b88c610 | 743 | XbusMessage const* m = (XbusMessage const*)ev.value.p; |
Alex Young |
35:7e519b88c610 | 744 | XbusMessageMemoryManager janitor(m); |
Alex Young |
35:7e519b88c610 | 745 | return m->mid == XMID_Wakeup; |
Alex Young |
35:7e519b88c610 | 746 | } |
Alex Young |
35:7e519b88c610 | 747 | return false; |
Alex Young |
35:7e519b88c610 | 748 | } |
Alex Young |
35:7e519b88c610 | 749 | |
Alex Young |
35:7e519b88c610 | 750 | /*! |
Alex Young |
37:3e87bf647c68 | 751 | * \brief Send wakeup acknowledge message to MTi. |
Alex Young |
37:3e87bf647c68 | 752 | * |
Alex Young |
37:3e87bf647c68 | 753 | * Sending a wakeup acknowledge will cause the device to stay in configuration |
Alex Young |
37:3e87bf647c68 | 754 | * mode instead of automatically transitioning to measurement mode with the |
Alex Young |
37:3e87bf647c68 | 755 | * stored output configuration. |
Alex Young |
37:3e87bf647c68 | 756 | */ |
Alex Young |
37:3e87bf647c68 | 757 | void sendWakeupAck(void) |
Alex Young |
37:3e87bf647c68 | 758 | { |
Alex Young |
37:3e87bf647c68 | 759 | XbusMessage ack = {XMID_WakeupAck}; |
Alex Young |
37:3e87bf647c68 | 760 | sendMessage(&ack); |
Alex Young |
52:e2197b38c029 | 761 | pc.printf("Device ready for operation.\r\n"); |
Alex Young |
37:3e87bf647c68 | 762 | } |
Alex Young |
37:3e87bf647c68 | 763 | |
tjerkhofmeijer | 64:8a0f00a064bb | 764 | #ifdef MTI_USES_UART_INTERFACE |
Alex Young |
37:3e87bf647c68 | 765 | /*! |
Alex Young |
37:3e87bf647c68 | 766 | * \brief Restore communication with the MTi. |
Alex Young |
37:3e87bf647c68 | 767 | * |
Alex Young |
37:3e87bf647c68 | 768 | * On bootup the MTi will listen for a magic byte to signal that it should |
Alex Young |
37:3e87bf647c68 | 769 | * return to default baudrate and output configuration. This can be used to |
Alex Young |
37:3e87bf647c68 | 770 | * recover from a bad or unknown configuration. |
Alex Young |
37:3e87bf647c68 | 771 | */ |
Alex Young |
37:3e87bf647c68 | 772 | void restoreCommunication(void) |
Alex Young |
37:3e87bf647c68 | 773 | { |
Alex Young |
37:3e87bf647c68 | 774 | pc.printf("Restoring communication with device... "); |
Alex Young |
37:3e87bf647c68 | 775 | mtReset = 0; |
Alex Young |
37:3e87bf647c68 | 776 | Thread::wait(1); |
Alex Young |
37:3e87bf647c68 | 777 | mtReset = 1; |
Alex Young |
37:3e87bf647c68 | 778 | |
Alex Young |
37:3e87bf647c68 | 779 | do |
Alex Young |
37:3e87bf647c68 | 780 | { |
Alex Young |
37:3e87bf647c68 | 781 | mt.putc(0xDE); |
Alex Young |
37:3e87bf647c68 | 782 | } |
Alex Young |
37:3e87bf647c68 | 783 | while (!waitForWakeup(1)); |
Alex Young |
52:e2197b38c029 | 784 | pc.printf("done\r\n"); |
Alex Young |
37:3e87bf647c68 | 785 | |
Alex Young |
37:3e87bf647c68 | 786 | sendWakeupAck(); |
Alex Young |
37:3e87bf647c68 | 787 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 788 | #endif |
Alex Young |
37:3e87bf647c68 | 789 | |
Alex Young |
37:3e87bf647c68 | 790 | /*! |
Alex Young |
35:7e519b88c610 | 791 | * \brief Releases the MTi reset line and waits for a wakeup message. |
Alex Young |
37:3e87bf647c68 | 792 | * |
Alex Young |
37:3e87bf647c68 | 793 | * If no wakeup message is received within 1 second the restore communications |
Alex Young |
37:3e87bf647c68 | 794 | * procedure is done to reset the MTi to default baudrate and output configuration. |
Alex Young |
35:7e519b88c610 | 795 | */ |
tjerkhofmeijer | 64:8a0f00a064bb | 796 | static bool wakeupMotionTracker(void) |
Alex Young |
35:7e519b88c610 | 797 | { |
Alex Young |
35:7e519b88c610 | 798 | mtReset.write(1); // Release MT from reset. |
Alex Young |
37:3e87bf647c68 | 799 | if (waitForWakeup(1000)) |
Alex Young |
35:7e519b88c610 | 800 | { |
Alex Young |
37:3e87bf647c68 | 801 | sendWakeupAck(); |
Alex Young |
37:3e87bf647c68 | 802 | } |
Alex Young |
37:3e87bf647c68 | 803 | else |
Alex Young |
37:3e87bf647c68 | 804 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 805 | #ifdef MTI_USES_UART_INTERFACE |
Alex Young |
37:3e87bf647c68 | 806 | restoreCommunication(); |
tjerkhofmeijer | 64:8a0f00a064bb | 807 | #else |
tjerkhofmeijer | 64:8a0f00a064bb | 808 | pc.printf("Failed to communicate with MTi device\r\n"); |
tjerkhofmeijer | 64:8a0f00a064bb | 809 | return true; |
tjerkhofmeijer | 64:8a0f00a064bb | 810 | #endif |
Alex Young |
35:7e519b88c610 | 811 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 812 | return true; |
Alex Young |
35:7e519b88c610 | 813 | } |
Alex Young |
35:7e519b88c610 | 814 | |
Alex Young |
38:d8d410d1662c | 815 | static void printIntroMessage(void) |
Alex Young |
38:d8d410d1662c | 816 | { |
Alex Young |
52:e2197b38c029 | 817 | pc.printf("\r\n\r\n\r\n\r\n\r\n"); |
Alex Young |
52:e2197b38c029 | 818 | pc.printf("MTi-1 series embedded example firmware.\r\n"); |
Alex Young |
38:d8d410d1662c | 819 | } |
Alex Young |
38:d8d410d1662c | 820 | |
xsens_mheskamp | 70:ff3afeaa31be | 821 | /*! \brief Prints usage instructions |
xsens_mheskamp | 70:ff3afeaa31be | 822 | */ |
Alex Young |
38:d8d410d1662c | 823 | static void printUsageInstructions(void) |
Alex Young |
38:d8d410d1662c | 824 | { |
Alex Young |
52:e2197b38c029 | 825 | pc.printf("\r\n"); |
Alex Young |
52:e2197b38c029 | 826 | pc.printf("Press 'm' to start measuring and 'c' to return to config mode.\r\n"); |
Alex Young |
38:d8d410d1662c | 827 | } |
Alex Young |
38:d8d410d1662c | 828 | |
Alex Young |
44:b3980e8ac074 | 829 | /*! |
Alex Young |
44:b3980e8ac074 | 830 | * \brief Output the contents of a data message to the PC serial port. |
Alex Young |
44:b3980e8ac074 | 831 | */ |
Alex Young |
43:470c019246e4 | 832 | static void printMessageData(struct XbusMessage const* message) |
Alex Young |
43:470c019246e4 | 833 | { |
Alex Young |
43:470c019246e4 | 834 | if (!message) |
Alex Young |
43:470c019246e4 | 835 | return; |
Alex Young |
43:470c019246e4 | 836 | |
Alex Young |
43:470c019246e4 | 837 | pc.printf("MTData2:"); |
Alex Young |
43:470c019246e4 | 838 | uint16_t counter; |
Alex Young |
43:470c019246e4 | 839 | if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message)) |
Alex Young |
43:470c019246e4 | 840 | { |
Alex Young |
43:470c019246e4 | 841 | pc.printf(" Packet counter: %5d", counter); |
Alex Young |
43:470c019246e4 | 842 | } |
Alex Young |
43:470c019246e4 | 843 | float ori[4]; |
Alex Young |
43:470c019246e4 | 844 | if (XbusMessage_getDataItem(ori, XDI_Quaternion, message)) |
Alex Young |
43:470c019246e4 | 845 | { |
Alex Young |
43:470c019246e4 | 846 | pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1], |
Alex Young |
43:470c019246e4 | 847 | ori[2], ori[3]); |
Alex Young |
43:470c019246e4 | 848 | } |
Alex Young |
43:470c019246e4 | 849 | float acc[3]; |
Alex Young |
43:470c019246e4 | 850 | if (XbusMessage_getDataItem(acc, XDI_Acceleration, message)) |
Alex Young |
43:470c019246e4 | 851 | { |
Alex Young |
43:470c019246e4 | 852 | pc.printf(" Acceleration: (% .3f, % .3f, % .3f)", acc[0], acc[1], acc[2]); |
Alex Young |
43:470c019246e4 | 853 | } |
Alex Young |
43:470c019246e4 | 854 | float gyr[3]; |
Alex Young |
43:470c019246e4 | 855 | if (XbusMessage_getDataItem(gyr, XDI_RateOfTurn, message)) |
Alex Young |
43:470c019246e4 | 856 | { |
Alex Young |
43:470c019246e4 | 857 | pc.printf(" Rate Of Turn: (% .3f, % .3f, % .3f)", gyr[0], gyr[1], gyr[2]); |
Alex Young |
43:470c019246e4 | 858 | } |
Alex Young |
43:470c019246e4 | 859 | float mag[3]; |
Alex Young |
43:470c019246e4 | 860 | if (XbusMessage_getDataItem(mag, XDI_MagneticField, message)) |
Alex Young |
43:470c019246e4 | 861 | { |
Alex Young |
43:470c019246e4 | 862 | pc.printf(" Magnetic Field: (% .3f, % .3f, % .3f)", mag[0], mag[1], mag[2]); |
Alex Young |
43:470c019246e4 | 863 | } |
Alex Young |
43:470c019246e4 | 864 | uint32_t status; |
Alex Young |
43:470c019246e4 | 865 | if (XbusMessage_getDataItem(&status, XDI_StatusWord, message)) |
Alex Young |
43:470c019246e4 | 866 | { |
Alex Young |
43:470c019246e4 | 867 | pc.printf(" Status:%X", status); |
Alex Young |
43:470c019246e4 | 868 | } |
Alex Young |
52:e2197b38c029 | 869 | pc.printf("\r\n"); |
Alex Young |
43:470c019246e4 | 870 | } |
Alex Young |
43:470c019246e4 | 871 | |
Alex Young |
2:b3e402dc11ca | 872 | int main(void) |
Alex Young |
2:b3e402dc11ca | 873 | { |
Alex Young |
4:98f063b2e6da | 874 | XbusParserCallback xbusCallback = {}; |
Alex Young |
25:01356fb59467 | 875 | xbusCallback.allocateBuffer = allocateMessageData; |
Alex Young |
25:01356fb59467 | 876 | xbusCallback.deallocateBuffer = deallocateMessageData; |
Alex Young |
24:2cc49dc854e3 | 877 | xbusCallback.handleMessage = mtMessageHandler; |
Alex Young |
4:98f063b2e6da | 878 | |
Alex Young |
4:98f063b2e6da | 879 | xbusParser = XbusParser_create(&xbusCallback); |
tjerkhofmeijer | 64:8a0f00a064bb | 880 | configurePcInterface(); |
tjerkhofmeijer | 64:8a0f00a064bb | 881 | configureMtCommunicationInterface(); |
tjerkhofmeijer | 68:6d6dbeefd196 | 882 | |
Alex Young |
38:d8d410d1662c | 883 | printIntroMessage(); |
tjerkhofmeijer | 64:8a0f00a064bb | 884 | if (wakeupMotionTracker()) |
Alex Young |
5:abc52dd88be2 | 885 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 886 | if (configureMotionTracker()) |
Alex Young |
26:665d3624f9ab | 887 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 888 | printUsageInstructions(); |
tjerkhofmeijer | 64:8a0f00a064bb | 889 | for (;;) |
Alex Young |
29:d9310e7b58b5 | 890 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 891 | while (pc.readable()) |
tjerkhofmeijer | 64:8a0f00a064bb | 892 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 893 | handlePcCommand(pc.getc()); |
tjerkhofmeijer | 64:8a0f00a064bb | 894 | } |
Alex Young |
43:470c019246e4 | 895 | |
tjerkhofmeijer | 64:8a0f00a064bb | 896 | osEvent ev = g_dataQueue.get(10); |
tjerkhofmeijer | 64:8a0f00a064bb | 897 | if (ev.status == osEventMessage) |
tjerkhofmeijer | 64:8a0f00a064bb | 898 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 899 | XbusMessage const* data = (XbusMessage const*)ev.value.p; |
tjerkhofmeijer | 64:8a0f00a064bb | 900 | XbusMessageMemoryManager janitor(data); |
tjerkhofmeijer | 64:8a0f00a064bb | 901 | printMessageData(data); |
tjerkhofmeijer | 64:8a0f00a064bb | 902 | } |
Alex Young |
43:470c019246e4 | 903 | } |
Alex Young |
26:665d3624f9ab | 904 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 905 | else |
tjerkhofmeijer | 64:8a0f00a064bb | 906 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 907 | pc.printf("Failed to configure motion tracker.\r\n"); |
tjerkhofmeijer | 64:8a0f00a064bb | 908 | return -1; |
tjerkhofmeijer | 64:8a0f00a064bb | 909 | } |
Alex Young |
29:d9310e7b58b5 | 910 | } |
Alex Young |
4:98f063b2e6da | 911 | } |