main.cpp

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