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