Add LPC1768

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

Committer:
Alex Young
Date:
Fri Jun 12 11:32:35 2015 +0200
Revision:
58:db60ef0a0d16
Parent:
57:c3c85ebb7375
Child:
59:f9166c19451f
Use RawSerial port for communication with MTi.

The Stream interface used by the Serial class can have problems when
used with the combination of interrupts and RTOS.

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