Add LPC1768

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

Committer:
tjerkhofmeijer
Date:
Tue May 26 08:41:24 2015 +0000
Revision:
56:041d3d9c300a
Parent:
55:9a2d6f947f0d
Child:
57:c3c85ebb7375
-fixed: typos in names of mti-2 and mti-3

Who changed what in which revision?

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