mtinterface.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 #include "mtinterface.h "
00018 #include <stdlib.h>
00019 #include <assert.h>
00020 
00021 #define XBUS_ERROR_MID  0x42
00022 
00023 /*! \class MtInterface
00024     \brief Abstract interface for communicating with an Xsens Motion Tracker (MT) on an mbed platform
00025 
00026     MtInterface defines an abstract interface for communicating with a Motion Tracker (MT) via xbus
00027     messages. Depending on the type of communication interface a specific implementation
00028     must be made which inherits from MtInterface.
00029 */
00030 
00031 static MtInterface* g_thisPtr = 0;
00032 
00033 
00034 /*! \brief Callback function for XbusParserCallback for allocating a block of memory
00035     \param bufferSize buffer size
00036 */
00037 static void *allocateMessageDataWrapper(size_t bufferSize)
00038 {
00039     return g_thisPtr->allocateMessageData(bufferSize);
00040 }
00041 
00042 
00043 /*! \brief Callback function for XbusParserCallback for deallocating a block of memory
00044     \param buffer Pointer to the buffer that should be deallocated
00045 */
00046 static void deallocateMessageDataWrapper(void const *buffer)
00047 {
00048     g_thisPtr->deallocateMessageData(buffer);
00049 }
00050 
00051 
00052 /*! \brief Callback function for XbusParserCallback that handles completely received xbus messages from the motion tracker
00053     \param Pointer to the received xbus message
00054 */
00055 static void xbusParserCallbackFunctionWrapper(struct XbusMessage const *message)
00056 {
00057     g_thisPtr->xbusParserCallbackFunction(message);
00058 }
00059 
00060 
00061 /*! \brief Constructs an MtInterface.
00062  *  \note Only a single instance of MtInterface is allowed
00063 */
00064 MtInterface::MtInterface()
00065 {
00066     assert(g_thisPtr == 0);
00067     g_thisPtr = this;
00068 
00069     // Create an xbusParser:
00070     XbusParserCallback xbusCallback = {};
00071     xbusCallback.allocateBuffer = allocateMessageDataWrapper;
00072     xbusCallback.deallocateBuffer = deallocateMessageDataWrapper;
00073     xbusCallback.handleMessage = xbusParserCallbackFunctionWrapper;
00074     m_xbusParser = XbusParser_create(&xbusCallback);
00075 }
00076 
00077 
00078 /*! \brief Destructor
00079 */
00080 MtInterface::~MtInterface()
00081 {
00082     g_thisPtr = 0;
00083 }
00084 
00085 
00086 /*! \brief Returns the next message from the rx queue or NULL if the queue is empty
00087     \note The caller must dealocate the message with releaseXbusMessage() after use.
00088 */
00089 XbusMessage *MtInterface::getXbusMessage()
00090 {
00091     XbusMessage *xbusMessage = NULL;
00092     osEvent ev = m_xbusRxQueue.get(1);
00093     if (ev.status == osEventMessage)
00094     {
00095         xbusMessage = (XbusMessage*)ev.value.p;
00096     }
00097     return xbusMessage;
00098 }
00099 
00100 
00101 /*! \brief Releases an xbus message previously obtained by a call to getXbusMessage()
00102     \param xbusMessage that should be released
00103 */
00104 void MtInterface::releaseXbusMessage(XbusMessage *xbusMessage)
00105 {
00106     if (xbusMessage != NULL)
00107     {
00108         deallocateMessageData(xbusMessage->m_data);
00109         m_xbusMessagePool.free(xbusMessage);
00110     }
00111 }
00112 
00113 
00114 /*! \brief Callback function for XbusParserCallback to allocate a block of memory
00115     \param bufferSize buffer size
00116 */
00117 void *MtInterface::allocateMessageData(size_t bufferSize)
00118 {
00119     assert(bufferSize < m_rxBufferSize);
00120     void *ptr = m_memoryPool.alloc();
00121     assert(ptr);
00122     return ptr;
00123 }
00124 
00125 
00126 /*! \brief Callback function for XbusParserCallback to deallocate a block of memory
00127     \param buffer Pointer to the buffer that should be deallocated
00128 */
00129 void MtInterface::deallocateMessageData(void const *buffer)
00130 {
00131     m_memoryPool.free((uint8_t(*)[m_rxBufferSize])buffer);
00132 }
00133 
00134 
00135 /*! \brief Callback function for XbusParserCallback that handles a completely received xbus message from the motion tracker
00136     \param message Pointer to the received xbus message
00137 */
00138 void MtInterface::xbusParserCallbackFunction(struct XbusMessage const *message)
00139 {
00140     XbusMessage *xbusMessage = m_xbusMessagePool.alloc();
00141     assert(xbusMessage);
00142     memcpy(xbusMessage, message, sizeof(XbusMessage));
00143     m_xbusRxQueue.put(xbusMessage);
00144 }
00145 
00146 /*! \brief Sends an Xbus message and waits for the related acknowledge
00147     \returns 0 if no related message was received. Pointer to the message otherwise. This message can also be an error message.
00148 */
00149 XbusMessage* MtInterface::sendAndWait(const XbusMessage* xbusMessage)
00150 {
00151     sendXbusMessage(xbusMessage);
00152     Timer timer;
00153     timer.start();
00154     bool waiting = true;
00155     
00156     uint8_t expectedMid = xbusMessage->m_mid + 1;
00157     
00158     while(waiting && (timer.read_ms() < 1000))
00159     {
00160         process();
00161         XbusMessage* xbusMessage = getXbusMessage();
00162         if (xbusMessage)
00163         {
00164             if (xbusMessage->m_mid == expectedMid || xbusMessage->m_mid == XBUS_ERROR_MID)
00165             {
00166                 return xbusMessage;
00167             }
00168             releaseXbusMessage(xbusMessage);
00169         }
00170     }
00171     
00172     return NULL;
00173 }
00174