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@61:b9d3e7e5ba0c, 2015-06-12 (annotated)

Committer:
Alex Young
Date:
Fri Jun 12 13:23:26 2015 +0200
Revision:
61:b9d3e7e5ba0c
Parent:
60:ab9dad3560d3
Child:
63:138c196f0b88
Change to Apache license

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