David Khosravi
Add LPC1768
Dependencies: mbed-rtos mbed Xbus
Fork of
MTi-1_example
by 
Xsens
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
Go to the documentation of this file.
00001 /*! 00002 * \file 00003 * \copyright Copyright (C) Xsens Technologies B.V., 2015. 00004 * 00005 * Licensed under the Apache License, Version 2.0 (the "License"); you may not 00006 * use this file except in compliance with the License. You may obtain a copy 00007 * of the License at 00008 * 00009 * http://www.apache.org/licenses/LICENSE-2.0 00010 * 00011 * Unless required by applicable law or agreed to in writing, software 00012 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 00013 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 00014 * License for the specific language governing permissions and limitations 00015 * under the License. 00016 * 00017 * \page Overview Firmware overview 00018 * 00019 * Example firmware for communicating with an Xsens MTi-1 series motion 00020 * tracker (MT). 00021 * 00022 * The firmware uses the mbed-rtos library to provide RTOS features such as 00023 * memory pools and queues. A single thread (main) is used with reception of 00024 * data from the motion tracker. 00025 * 00026 * \section Hardware setup 00027 * The firmware has been tested with a ST Nucleo F302R8 development board. 00028 * The Nucleo board should be connected to the MTi1 development board using the 00029 * Arduino compatible headers on the Nucleo board as follows: 00030 * 00031 * | Nucleo pin | MTi1 func. | MTi1 dev. pin | Used for PSEL | 00032 * |------------|-------------|---------------|---------------| 00033 * | 5V | VDD | P300-1 | Any | 00034 * | IORef | VDDIO | P300-2 | Any | 00035 * | GND | GND | P300-3 | Any | 00036 * | D2 | nRST | P300-5 | Any | 00037 * | SCL/D15 | UART_TX/SCL | P300-9 | UART / I2C | 00038 * | SDA/D14 | UART_RX/SDA | P300-11 | UART / I2C | 00039 * | D3 | DRDY | P300-15 | SPI / I2C | 00040 * | SCK/D13 | SCK/ADD0 | P300-17 | SPI / I2C | 00041 * | MISO/D12 | MISO/ADD1 | P300-19 | SPI / I2C | 00042 * | MOSI/D11 | MOSI/ADD2 | P300-21 | SPI / I2C | 00043 * | CS/D10 | nCS | P300-23 | SPI | 00044 * 00045 * Communication with the host PC is achieved using the built-in USB serial 00046 * bridge of the Nucleo board. Communication with the MT is achieved through 00047 * either the UART, I2C or SPI interface. The active interface is chosen 00048 * on the MT's side by use of the PSEL0 and PSEL1 switch on the MTi1 00049 * development board. This example needs to be built with the matching 00050 * MTI_USES_xxxx_INTERFACE define set (see below) 00051 * 00052 * \subsection Porting 00053 * To port to a different mbed platform the following pin definitions need 00054 * to be updated. 00055 * In all cases: the reset line pin 00056 * For UART: the serial Rx/Tx lines UART_TX and UART_RX 00057 * For I2C: the SCL,SDA,DRDY and address lines 00058 * For SPI: The SCK,MISO,MOSI,nCS and DRDY lines 00059 * 00060 * \section Firmware Operation 00061 * The firmware starts by initializing the serial ports used to communicate 00062 * with the host PC and with the MT. During the initialization the MT is held 00063 * in reset using the nRST input. 00064 * 00065 * Once the firmware is ready to communicate with the MT the reset line is 00066 * released and the firmware waits for a wakeup message from the MT. If this is 00067 * not received within 1 second the firmware will try to restore communication 00068 * with the MT using a special restore communication procedure. 00069 * 00070 * When the MT is ready for communication the firmware requests the device ID 00071 * of the MT, and based on this determines which type of MTi is connected. 00072 * If the MT is an MTi-1 then it will be configured to send inertial and 00073 * magnetic measurement data. MTi-2 and MTi-3 devices have onboard orientation 00074 * estimation and will therefore be configured to provide quaternion output. 00075 */ 00076 00077 #include "mbed.h" 00078 #include "rtos.h" 00079 #include "xbusparser.h" 00080 #include "xbusmessage.h" 00081 #include "xsdeviceid.h" 00082 #include "xbusdef.h" 00083 00084 // Select communication interface to use for MTi 00085 #define MTI_USES_I2C_INTERFACE 00086 00087 #if !(defined(MTI_USES_I2C_INTERFACE) || defined(MTI_USES_SPI_INTERFACE) || defined(MTI_USES_UART_INTERFACE)) 00088 #error "Must select communication interface by defining one of: MTI_USES_I2C_INTERFACE, MTI_USES_SPI_INTERFACE or MTI_USES_UART_INTERFACE" 00089 #endif 00090 00091 // Define Target (David) 00092 //#define TARGET_MBED_LPC1768 00093 00094 #if defined(TARGET_NUCLEO_F302R8) 00095 00096 #define PC_TX PA_2 00097 #define PC_RX PA_3 00098 #define MT_TX PB_9 00099 #define MT_RX PB_8 00100 #define MT_SDA PB_9 00101 #define MT_SCL PB_8 00102 #define MT_ADD0 PB_13 00103 #define MT_ADD1 PB_14 00104 #define MT_ADD2 PB_15 00105 #define MT_MOSI PB_15 00106 #define MT_MISO PB_14 00107 #define MT_SCLK PB_13 00108 #define MT_nCS PB_6 00109 #define MT_NRESET PA_10 00110 #define MT_DRDY PB_3 00111 00112 #elif defined(TARGET_NUCLEO_F401RE) 00113 00114 #define PC_TX PA_2 00115 #define PC_RX PA_3 00116 #define MT_TX PA_10 00117 #define MT_RX PA_9 00118 #define MT_SDA PB_9 00119 #define MT_SCL PB_8 00120 #define MT_ADD0 PB_13 00121 #define MT_ADD1 PB_14 00122 #define MT_ADD2 PB_15 00123 #define MT_MOSI PB_15 00124 #define MT_MISO PB_14 00125 #define MT_SCLK PB_13 00126 #define MT_nCS PB_6 00127 #define MT_NRESET PC_9 00128 #define MT_DRDY PB_3 00129 00130 #elif defined(TARGET_KL46Z) 00131 00132 #define PC_TX USBTX 00133 #define PC_RX USBRX 00134 #define MT_TX PTE0 00135 #define MT_RX PTE1 00136 #define MT_SDA PTE0 00137 #define MT_SCL PTE1 00138 #define MT_ADD0 PTD5 00139 #define MT_ADD1 PTD7 00140 #define MT_ADD2 PTD6 00141 #define MT_MOSI PTD6 00142 #define MT_MISO PTD7 00143 #define MT_SCLK PTD5 00144 #define MT_nCS PTD4 00145 #define MT_NRESET PTD3 00146 #define MT_DRDY PTD2 00147 00148 #elif defined(TARGET_LPC4088) 00149 00150 #define PC_TX USBTX 00151 #define PC_RX USBRX 00152 #define MT_TX p9 00153 #define MT_RX p10 00154 #define MT_SDA p9 00155 #define MT_SCL p10 00156 #define MT_ADD0 p13 00157 #define MT_ADD1 p12 00158 #define MT_ADD2 p11 00159 #define MT_MOSI p11 00160 #define MT_MISO p12 00161 #define MT_SCLK p13 00162 #define MT_nCS p14 00163 #define MT_NRESET p8 00164 #define MT_DRDY p15 00165 00166 #elif defined(TARGET_MBED_LPC1768) 00167 00168 #define PC_TX USBTX 00169 #define PC_RX USBRX 00170 #define MT_TX p9 00171 #define MT_RX p10 00172 #define MT_SDA p9 00173 #define MT_SCL p10 00174 #define MT_ADD0 p13 00175 #define MT_ADD1 p12 00176 #define MT_ADD2 p11 00177 #define MT_MOSI p11 00178 #define MT_MISO p12 00179 #define MT_SCLK p13 00180 #define MT_nCS p14 00181 #define MT_NRESET p8 00182 #define MT_DRDY p15 00183 #else 00184 00185 #error "Support for selected mbed platform has not been added." 00186 00187 #endif 00188 00189 00190 /*! 00191 * \brief Baudrate used to communicate with host PC. 00192 */ 00193 #define PC_UART_BAUDRATE (921600) 00194 00195 /*! 00196 * \brief The number of items to hold in the memory pools. 00197 */ 00198 #define MEMORY_POOL_SIZE (4) 00199 /*! 00200 * \brief The size of the queue used for device responses. 00201 * This is set to one as in typical Xbus operation each command receives a 00202 * response before the next command is sent. 00203 */ 00204 #define RESPONSE_QUEUE_SIZE (1) 00205 /*! 00206 * \brief The size of the queue used for data messages. 00207 * This is set to two to allow some overlap between printing received data to 00208 * the PC serial port and the reception of the subsequent data packet. In 00209 * more complex applications it might be necessary to increase this if 00210 * message processing might occasionally require more time than normal. 00211 */ 00212 #define DATA_QUEUE_SIZE (2) 00213 /*! 00214 * \brief The maximum size of an xbus message supported by the application. 00215 * This is the size of the message buffers in the message data memory pool. 00216 */ 00217 #define MAX_XBUS_DATA_SIZE (128) 00218 00219 /*! \brief Serial port for communication with the host PC. */ 00220 static Serial pc(PC_TX, PC_RX); 00221 00222 #if defined(MTI_USES_I2C_INTERFACE) 00223 /*! 00224 * \brief I2C master used for communication with the MT. 00225 */ 00226 static I2C mt(MT_SDA, MT_SCL); 00227 static DigitalOut add0(MT_ADD0); 00228 static DigitalOut add1(MT_ADD1); 00229 static DigitalOut add2(MT_ADD2); 00230 00231 #elif defined(MTI_USES_SPI_INTERFACE) 00232 /*! \brief SPI master used for communication with the MT. */ 00233 static SPI mt(MT_MOSI, MT_MISO, MT_SCLK); 00234 00235 /*! \brief Chip select line for the MT. */ 00236 static DigitalOut cs(MT_nCS, 1); 00237 00238 #elif defined(MTI_USES_UART_INTERFACE) 00239 /*! 00240 * \brief Serial port for communication with the MT. 00241 * 00242 * We use a RawSerial port as the Stream inteface used by the regular 00243 * Serial class can have problems with the RTOS when using interrupts. 00244 */ 00245 static RawSerial mt(MT_TX, MT_RX); 00246 #endif 00247 00248 #if defined(MTI_USES_I2C_INTERFACE) || defined(MTI_USES_SPI_INTERFACE) 00249 /*! 00250 * \brief Interrput line used by MT to signal that data is available. 00251 */ 00252 static InterruptIn drdy(MT_DRDY); 00253 #endif 00254 00255 /*! 00256 * \brief MT reset line. 00257 * 00258 * MT is held in reset on startup. 00259 */ 00260 static DigitalOut mtReset(MT_NRESET, 0); 00261 /*! \brief XbusParser used to parse incoming Xbus messages from the MT. */ 00262 static XbusParser* xbusParser; 00263 00264 /*! 00265 * \brief Memory pool used for storing Xbus messages when passing them 00266 * to the main thread. 00267 */ 00268 MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool; 00269 /*! 00270 * \brief Memory pool used for storing the payload of Xbus messages. 00271 */ 00272 MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool; 00273 /*! 00274 * \brief Queue used to pass data messages to the main thread for processing. 00275 */ 00276 Queue<XbusMessage, DATA_QUEUE_SIZE> g_dataQueue; 00277 /*! 00278 * \brief Queue used for passing all other messages to the main thread for processing. 00279 */ 00280 Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue; 00281 00282 /*! 00283 * \brief Allocate message data buffer from the message data pool. 00284 */ 00285 static void* allocateMessageData(size_t bufSize) 00286 { 00287 return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL; 00288 } 00289 00290 /*! 00291 * \brief Deallocate message data previously allocated from the message 00292 * data pool. 00293 */ 00294 static void deallocateMessageData(void const* buffer) 00295 { 00296 g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer); 00297 } 00298 00299 #if defined(MTI_USES_I2C_INTERFACE) 00300 #define MTI_I2C_ADDRESS (0x1D << 1) 00301 static void readData(uint8_t pipe, uint16_t dataLength) 00302 { 00303 const int preambleLength = 2; 00304 uint8_t* buf = (uint8_t*)allocateMessageData(dataLength+preambleLength); 00305 if (buf) 00306 { 00307 buf[0] = XBUS_PREAMBLE; 00308 buf[1] = XBUS_MASTERDEVICE; 00309 mt.write(MTI_I2C_ADDRESS, (char*)&pipe, sizeof(pipe), true); 00310 mt.read(MTI_I2C_ADDRESS, (char*)buf+preambleLength, dataLength); 00311 XbusParser_parseBuffer(xbusParser, buf, dataLength+preambleLength); 00312 deallocateMessageData(buf); 00313 } 00314 } 00315 static void mtInterruptHandler(void) 00316 { 00317 while (true) 00318 { 00319 uint8_t opcode = XBUS_PIPE_STATUS; 00320 uint8_t status[4]; 00321 mt.write(MTI_I2C_ADDRESS, (char*)&opcode, sizeof(opcode), true); 00322 mt.read(MTI_I2C_ADDRESS, (char*)status, sizeof(status)); 00323 00324 uint16_t notificationSize = status[0] | (status[1] << 8); 00325 uint16_t measurementSize = status[2] | (status[3] << 8); 00326 00327 if (notificationSize) 00328 { 00329 readData(XBUS_NOTIFICATION_PIPE, notificationSize); 00330 } 00331 else if (measurementSize) 00332 { 00333 readData(XBUS_MEASUREMENT_PIPE, measurementSize); 00334 } 00335 else 00336 break; // No more data available to read. 00337 } 00338 } 00339 00340 static void configureMtCommunicationInterface(void) 00341 { 00342 mt.frequency(400000); 00343 //Use the addX pins to configure I2C address 0x1D 00344 add0.write(0); 00345 add1.write(0); 00346 add2.write(0); 00347 drdy.rise(&mtInterruptHandler); 00348 } 00349 00350 /*! 00351 * \brief Send a message to the MT 00352 * 00353 * This function formats the message data and writes this to the MT I2C 00354 * interface. It does not wait for any response. 00355 */ 00356 static void sendMessage(XbusMessage const* m) 00357 { 00358 uint8_t buf[64]; 00359 size_t rawLength = XbusMessage_format(buf, m, XLLF_I2c); 00360 mt.write(MTI_I2C_ADDRESS, (char*)buf, rawLength); 00361 } 00362 #elif defined(MTI_USES_SPI_INTERFACE) 00363 static void sendOpcode(uint8_t opcode) 00364 { 00365 mt.write(opcode); 00366 for (int filler = 0; filler < 3; ++filler) 00367 { 00368 mt.write(filler); 00369 } 00370 } 00371 00372 static void readData(uint8_t pipe, uint16_t dataLength) 00373 { 00374 const int preambleLength = 2; 00375 uint8_t* buf = (uint8_t*)allocateMessageData(dataLength+preambleLength); 00376 if (buf) 00377 { 00378 uint8_t* dptr = buf; 00379 *dptr++ = XBUS_PREAMBLE; 00380 *dptr++ = XBUS_MASTERDEVICE; 00381 cs = 0; 00382 sendOpcode(pipe); 00383 for (int i = 0; i < dataLength; ++i) 00384 { 00385 *dptr++ = mt.write(0); 00386 } 00387 cs = 1; 00388 XbusParser_parseBuffer(xbusParser, buf, dptr - buf); 00389 deallocateMessageData(buf); 00390 } 00391 } 00392 static void mtInterruptHandler(void) 00393 { 00394 while (true) 00395 { 00396 cs = 0; 00397 sendOpcode(XBUS_PIPE_STATUS); 00398 uint8_t status[4]; 00399 for (int i = 0; i < sizeof(status); ++i) 00400 { 00401 status[i] = mt.write(0); 00402 } 00403 cs = 1; 00404 00405 uint16_t notificationSize = status[0] | (status[1] << 8); 00406 uint16_t measurementSize = status[2] | (status[3] <<8); 00407 00408 if (notificationSize) 00409 { 00410 readData(XBUS_NOTIFICATION_PIPE, notificationSize); 00411 } 00412 else if (measurementSize) 00413 { 00414 readData(XBUS_MEASUREMENT_PIPE, measurementSize); 00415 } 00416 else 00417 break; // No more data available to read. 00418 } 00419 } 00420 00421 static void configureMtCommunicationInterface(void) 00422 { 00423 mt.frequency(1000000); 00424 mt.format(8, 3); 00425 drdy.rise(&mtInterruptHandler); 00426 } 00427 00428 /*! 00429 * \brief Send a message to the MT 00430 * 00431 * This function formats the message data and writes this to the MT SPI 00432 * interface. It does not wait for any response. 00433 */ 00434 static void sendMessage(XbusMessage const* m) 00435 { 00436 uint8_t buf[64]; 00437 size_t rawLength = XbusMessage_format(buf, m, XLLF_Spi); 00438 cs = 0; 00439 for (int i = 0; i < rawLength; ++i) 00440 { 00441 mt.write(buf[i]); 00442 } 00443 cs = 1; 00444 } 00445 #elif defined(MTI_USES_UART_INTERFACE) 00446 /*! 00447 * \brief RX Interrupt handler for the MT serial port. 00448 * 00449 * Passes received data to an XbusParser to extract messages. 00450 */ 00451 static void mtLowLevelHandler(void) 00452 { 00453 while (mt.readable()) 00454 { 00455 XbusParser_parseByte(xbusParser, mt.getc()); 00456 } 00457 } 00458 00459 /*! 00460 * \brief Configure the serial port used for communication with the 00461 * motion tracker. 00462 */ 00463 static void configureMtCommunicationInterface(void) 00464 { 00465 mt.baud(115200); 00466 mt.format(8, Serial::None, 1); 00467 mt.attach(mtLowLevelHandler, Serial::RxIrq); 00468 } 00469 00470 /*! 00471 * \brief Send a message to the MT 00472 * 00473 * This function formats the message data and writes this to the MT serial 00474 * port. It does not wait for any response. 00475 */ 00476 static void sendMessage(XbusMessage const* m) 00477 { 00478 uint8_t buf[64]; 00479 size_t rawLength = XbusMessage_format(buf, m, XLLF_Uart); 00480 for (size_t i = 0; i < rawLength; ++i) 00481 { 00482 mt.putc(buf[i]); 00483 } 00484 } 00485 #endif 00486 00487 00488 /*! 00489 * \brief Send a message to the MT and wait for a response. 00490 * \returns Response message from the MT, or NULL is no response received 00491 * within 500ms. 00492 * 00493 * Blocking behaviour is implemented by waiting for a response to be written 00494 * to the response queue by the XbusParser. 00495 */ 00496 static XbusMessage const* doTransaction(XbusMessage const* m) 00497 { 00498 sendMessage(m); 00499 00500 osEvent ev = g_responseQueue.get(500); 00501 return ev.status == osEventMessage ? (XbusMessage*)ev.value.p : NULL; 00502 } 00503 00504 /*! 00505 * \brief RAII object to manage message memory deallocation. 00506 * 00507 * Will automatically free the memory used by an XbusMessage when going out 00508 * of scope. 00509 */ 00510 class XbusMessageMemoryManager 00511 { 00512 public: 00513 XbusMessageMemoryManager(XbusMessage const* message) 00514 : m_message(message) 00515 { 00516 } 00517 00518 ~XbusMessageMemoryManager() 00519 { 00520 if (m_message) 00521 { 00522 if (m_message->data) 00523 deallocateMessageData(m_message->data); 00524 g_messagePool.free(const_cast<XbusMessage*>(m_message)); 00525 } 00526 } 00527 00528 private: 00529 XbusMessage const* m_message; 00530 }; 00531 00532 /*! 00533 * \brief Dump information from a message to the PC serial port. 00534 */ 00535 static void dumpResponse(XbusMessage const* response) 00536 { 00537 switch (response->mid) 00538 { 00539 case XMID_GotoConfigAck: 00540 pc.printf("Device went to config mode.\r\n"); 00541 break; 00542 00543 case XMID_Error: 00544 pc.printf("Device error!"); 00545 break; 00546 00547 default: 00548 pc.printf("Received response MID=%X, length=%d\r\n", response->mid, response->length); 00549 break; 00550 } 00551 } 00552 00553 /*! 00554 * \brief Send a command to the MT and wait for a response. 00555 * \param cmdId The XsMessageId of the command to send. 00556 * 00557 * Commands are simple messages without and payload data. 00558 */ 00559 static void sendCommand(XsMessageId cmdId) 00560 { 00561 XbusMessage m = {cmdId}; 00562 XbusMessage const* response = doTransaction(&m); 00563 XbusMessageMemoryManager janitor(response); 00564 00565 if (response) 00566 { 00567 dumpResponse(response); 00568 } 00569 else 00570 { 00571 pc.printf("Timeout waiting for response.\r\n"); 00572 } 00573 } 00574 00575 /*! 00576 * \brief Handle a command from the PC 00577 * 00578 * The example application supports single character commands from the host 00579 * PC to switch between configuration and measurement modes. 00580 */ 00581 static void handlePcCommand(char cmd) 00582 { 00583 switch (cmd) 00584 { 00585 case 'c': 00586 sendCommand(XMID_GotoConfig); 00587 break; 00588 00589 case 'm': 00590 sendCommand(XMID_GotoMeasurement); 00591 break; 00592 } 00593 } 00594 00595 /*! 00596 * \brief XbusParser callback function to handle received messages. 00597 * \param message Pointer to the last received message. 00598 * 00599 * In this example received messages are copied into one of two message 00600 * queues for later handling by the main thread. Data messages are put 00601 * in one queue, while all other responses are placed in the second queue. 00602 * This is done so that data and other messages can be handled separately 00603 * by the application code. 00604 */ 00605 static void mtMessageHandler(struct XbusMessage const* message) 00606 { 00607 XbusMessage* m = g_messagePool.alloc(); 00608 if (m) 00609 { 00610 memcpy(m, message, sizeof(XbusMessage)); 00611 if (message->mid == XMID_MtData2) 00612 { 00613 g_dataQueue.put(m); 00614 } 00615 else 00616 { 00617 g_responseQueue.put(m); 00618 } 00619 } 00620 else if (message->data) 00621 { 00622 deallocateMessageData(message->data); 00623 } 00624 } 00625 00626 /*! 00627 * \brief Configure the serial port used to communicate with the host PC. 00628 */ 00629 static void configurePcInterface(void) 00630 { 00631 pc.baud(PC_UART_BAUDRATE); 00632 pc.format(8, Serial::None, 1); 00633 } 00634 00635 /*! 00636 * \brief Read the device ID of the motion tracker. 00637 */ 00638 static uint32_t readDeviceId(void) 00639 { 00640 XbusMessage reqDid = {XMID_ReqDid}; 00641 XbusMessage const* didRsp = doTransaction(&reqDid); 00642 XbusMessageMemoryManager janitor(didRsp); 00643 uint32_t deviceId = 0; 00644 if (didRsp) 00645 { 00646 if (didRsp->mid == XMID_DeviceId) 00647 { 00648 deviceId = *(uint32_t*)didRsp->data; 00649 } 00650 } 00651 return deviceId; 00652 } 00653 00654 /*! 00655 * \brief Sets MT output configuration. 00656 * \param conf Pointer to an array of OutputConfiguration elements. 00657 * \param elements The number of elements in the configuration array. 00658 * 00659 * The response from the device indicates the actual values that will 00660 * be used by the motion tracker. These may differ from the requested 00661 * parameters as the motion tracker validates the requested parameters 00662 * before applying them. 00663 */ 00664 static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements) 00665 { 00666 XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf}; 00667 XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg); 00668 XbusMessageMemoryManager janitor(outputConfRsp); 00669 if (outputConfRsp) 00670 { 00671 if (outputConfRsp->mid == XMID_OutputConfig) 00672 { 00673 pc.printf("Output configuration set to:\r\n"); 00674 OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data; 00675 for (int i = 0; i < outputConfRsp->length; ++i) 00676 { 00677 pc.printf("\t%s: %d Hz\r\n", XbusMessage_dataDescription(conf->dtype), conf->freq); 00678 ++conf; 00679 } 00680 return true; 00681 } 00682 else 00683 { 00684 dumpResponse(outputConfRsp); 00685 } 00686 } 00687 else 00688 { 00689 pc.printf("Failed to set output configuration.\r\n"); 00690 } 00691 return false; 00692 } 00693 00694 /*! 00695 * \brief Sets the motion tracker output configuration based on the function 00696 * of the attached device. 00697 * 00698 * The output configuration depends on the type of MTi-1 device connected. 00699 * An MTI-1 (IMU) device does not have an onboard orientation filter so 00700 * cannot output quaternion data, only inertial and magnetic measurement 00701 * data. 00702 * MTi-2 and MTi-3 devices have an onboard filter so can send quaternions. 00703 */ 00704 static bool configureMotionTracker(void) 00705 { 00706 uint32_t deviceId = readDeviceId(); 00707 00708 if (deviceId) 00709 { 00710 pc.printf("Found device with ID: %08X.\r\n", deviceId); 00711 if (!XsDeviceId_isMtMk4_X(deviceId)) 00712 { 00713 pc.printf("Device is not an MTi-1 series.\r\n"); 00714 return false; 00715 } 00716 00717 DeviceFunction function = XsDeviceId_getFunction(deviceId); 00718 pc.printf("Device is an MTi-%d: %s.\r\n", function, XsDeviceId_functionDescription(function)); 00719 00720 if (function == DF_IMU) 00721 { 00722 OutputConfiguration conf[] = { 00723 {XDI_PacketCounter, 65535}, 00724 {XDI_SampleTimeFine, 65535}, 00725 {XDI_Acceleration, 100}, 00726 {XDI_RateOfTurn, 100}, 00727 {XDI_MagneticField, 100} 00728 }; 00729 return setOutputConfiguration(conf, 00730 sizeof(conf) / sizeof(OutputConfiguration)); 00731 } 00732 else 00733 { 00734 OutputConfiguration conf[] = { 00735 {XDI_PacketCounter, 65535}, 00736 {XDI_SampleTimeFine, 65535}, 00737 {XDI_Quaternion, 100}, 00738 {XDI_StatusWord, 65535} 00739 }; 00740 return setOutputConfiguration(conf, 00741 sizeof(conf) / sizeof(OutputConfiguration)); 00742 } 00743 } 00744 00745 return false; 00746 } 00747 00748 /*! 00749 * \brief Wait for a wakeup message from the MTi. 00750 * \param timeout Time to wait to receive the wakeup message. 00751 * \return true if wakeup received within timeout, else false. 00752 * 00753 * The MTi sends an XMID_Wakeup message once it has completed its bootup 00754 * procedure. If this is acknowledged by an XMID_WakeupAck message then the MTi 00755 * will stay in configuration mode. Otherwise it will automatically enter 00756 * measurement mode with the stored output configuration. 00757 */ 00758 bool waitForWakeup(uint32_t timeout) 00759 { 00760 osEvent ev = g_responseQueue.get(timeout); 00761 if (ev.status == osEventMessage) 00762 { 00763 XbusMessage const* m = (XbusMessage const*)ev.value.p; 00764 XbusMessageMemoryManager janitor(m); 00765 return m->mid == XMID_Wakeup; 00766 } 00767 return false; 00768 } 00769 00770 /*! 00771 * \brief Send wakeup acknowledge message to MTi. 00772 * 00773 * Sending a wakeup acknowledge will cause the device to stay in configuration 00774 * mode instead of automatically transitioning to measurement mode with the 00775 * stored output configuration. 00776 */ 00777 void sendWakeupAck(void) 00778 { 00779 XbusMessage ack = {XMID_WakeupAck}; 00780 sendMessage(&ack); 00781 pc.printf("Device ready for operation.\r\n"); 00782 } 00783 00784 #ifdef MTI_USES_UART_INTERFACE 00785 /*! 00786 * \brief Restore communication with the MTi. 00787 * 00788 * On bootup the MTi will listen for a magic byte to signal that it should 00789 * return to default baudrate and output configuration. This can be used to 00790 * recover from a bad or unknown configuration. 00791 */ 00792 void restoreCommunication(void) 00793 { 00794 pc.printf("Restoring communication with device... "); 00795 mtReset = 0; 00796 Thread::wait(1); 00797 mtReset = 1; 00798 00799 do 00800 { 00801 mt.putc(0xDE); 00802 } 00803 while (!waitForWakeup(1)); 00804 pc.printf("done\r\n"); 00805 00806 sendWakeupAck(); 00807 } 00808 #endif 00809 00810 /*! 00811 * \brief Releases the MTi reset line and waits for a wakeup message. 00812 * 00813 * If no wakeup message is received within 1 second the restore communications 00814 * procedure is done to reset the MTi to default baudrate and output configuration. 00815 */ 00816 static bool wakeupMotionTracker(void) 00817 { 00818 mtReset.write(1); // Release MT from reset. 00819 if (waitForWakeup(1000)) 00820 { 00821 sendWakeupAck(); 00822 } 00823 else 00824 { 00825 #ifdef MTI_USES_UART_INTERFACE 00826 restoreCommunication(); 00827 #else 00828 pc.printf("Failed to communicate with MTi device\r\n"); 00829 return true; 00830 #endif 00831 } 00832 return true; 00833 } 00834 00835 static void printIntroMessage(void) 00836 { 00837 pc.printf("\r\n\r\n\r\n\r\n\r\n"); 00838 pc.printf("MTi-1 series embedded example firmware.\r\n"); 00839 } 00840 00841 static void printUsageInstructions(void) 00842 { 00843 pc.printf("\r\n"); 00844 pc.printf("Press 'm' to start measuring and 'c' to return to config mode.\r\n"); 00845 } 00846 00847 /*! 00848 * \brief Output the contents of a data message to the PC serial port. 00849 */ 00850 static void printMessageData(struct XbusMessage const* message) 00851 { 00852 if (!message) 00853 return; 00854 00855 pc.printf("MTData2:"); 00856 uint16_t counter; 00857 if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message)) 00858 { 00859 pc.printf(" Packet counter: %5d", counter); 00860 } 00861 float ori[4]; 00862 if (XbusMessage_getDataItem(ori, XDI_Quaternion, message)) 00863 { 00864 pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1], 00865 ori[2], ori[3]); 00866 } 00867 float acc[3]; 00868 if (XbusMessage_getDataItem(acc, XDI_Acceleration, message)) 00869 { 00870 pc.printf(" Acceleration: (% .3f, % .3f, % .3f)", acc[0], acc[1], acc[2]); 00871 } 00872 float gyr[3]; 00873 if (XbusMessage_getDataItem(gyr, XDI_RateOfTurn, message)) 00874 { 00875 pc.printf(" Rate Of Turn: (% .3f, % .3f, % .3f)", gyr[0], gyr[1], gyr[2]); 00876 } 00877 float mag[3]; 00878 if (XbusMessage_getDataItem(mag, XDI_MagneticField, message)) 00879 { 00880 pc.printf(" Magnetic Field: (% .3f, % .3f, % .3f)", mag[0], mag[1], mag[2]); 00881 } 00882 uint32_t status; 00883 if (XbusMessage_getDataItem(&status, XDI_StatusWord, message)) 00884 { 00885 pc.printf(" Status:%X", status); 00886 } 00887 pc.printf("\r\n"); 00888 } 00889 00890 int main(void) 00891 { 00892 XbusParserCallback xbusCallback = {}; 00893 xbusCallback.allocateBuffer = allocateMessageData; 00894 xbusCallback.deallocateBuffer = deallocateMessageData; 00895 xbusCallback.handleMessage = mtMessageHandler; 00896 00897 xbusParser = XbusParser_create(&xbusCallback); 00898 configurePcInterface(); 00899 configureMtCommunicationInterface(); 00900 00901 printIntroMessage(); 00902 if (wakeupMotionTracker()) 00903 { 00904 if (configureMotionTracker()) 00905 { 00906 printUsageInstructions(); 00907 for (;;) 00908 { 00909 while (pc.readable()) 00910 { 00911 handlePcCommand(pc.getc()); 00912 } 00913 00914 osEvent ev = g_dataQueue.get(10); 00915 if (ev.status == osEventMessage) 00916 { 00917 XbusMessage const* data = (XbusMessage const*)ev.value.p; 00918 XbusMessageMemoryManager janitor(data); 00919 printMessageData(data); 00920 } 00921 } 00922 } 00923 else 00924 { 00925 pc.printf("Failed to configure motion tracker.\r\n"); 00926 return -1; 00927 } 00928 } 00929 }
Generated on Mon Aug 15 2022 22:29:59 by
1.7.2