David Khosravi / Mbed 2 deprecated MTi-1_example_LPC1768

Add LPC1768

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

main.cpp@68:00b3b39c60a3, 2017-01-12 (annotated)

Committer:
dakh131
Date:
Thu Jan 12 09:28:24 2017 +0000
Revision:
68:00b3b39c60a3
Parent:
66:f12dec1c0c3d
Child:
71:b9b97b6d3a8e
Add Target_MBED_LPC1768

Who changed what in which revision?

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