mtinterface_mtssp.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_mtssp.h "
00018 #include "mbed.h"
00019 #include "rtos.h"
00020 #include "board.h "
00021 #include "xbusmessage.h "
00022 #include "xbusparser.h "
00023 #include <assert.h>
00024 #include "xbusdef.h "
00025 
00026 
00027 /*! \class MtInterfaceMtssp
00028     \brief Implementation of MtInterface for the MTSSP protocol
00029 
00030     MTSSP is the protocol used for communicating with an Xsens motion tracker over I2C or SPI.
00031 */
00032 
00033 
00034 /*! \brief Constructor
00035     \param driver Pointer to an MtsspDriver
00036 */
00037 MtInterfaceMtssp::MtInterfaceMtssp(MtsspDriver *driver)
00038     : m_driver(driver)
00039 {
00040     m_dataReady = new DigitalIn(MT_DRDY);
00041 }
00042 
00043 
00044 /*! \brief Destructor
00045 */
00046 MtInterfaceMtssp::~MtInterfaceMtssp()
00047 {
00048     delete m_dataReady;
00049 }
00050 
00051 
00052 /*! \brief Must be polled in the main application loop
00053 */
00054 void MtInterfaceMtssp::process()
00055 {
00056     if (*m_dataReady)
00057     {
00058         handleDataReady();
00059     }
00060 }
00061 
00062 
00063 /*! \brief Sends an xbus message to the motion tracker
00064     \param xbusMessage Pointer to xbus message which should be send
00065 */
00066 void MtInterfaceMtssp::sendXbusMessage(XbusMessage const* xbusMessage)
00067 {
00068     uint8_t* buf = (uint8_t*)allocateMessageData(xbusMessage->m_length + 4);
00069     size_t rawLength = XbusMessage_createRawMessage(buf, xbusMessage, m_driver->busFormat());
00070     m_driver->writeRaw(buf, rawLength);
00071     deallocateMessageData(buf);
00072 }
00073 
00074 
00075 /*! \brief Returns the low level bus format used
00076 */
00077 XbusBusFormat MtInterfaceMtssp::busFormat()
00078 {
00079     return m_driver->busFormat();
00080 }
00081 
00082 
00083 /*! \brief Should be called if the data ready line from the motion tracker signals that there is data pending
00084 */
00085 void MtInterfaceMtssp::handleDataReady()
00086 {
00087     uint16_t notificationMessageSize = 0;
00088     uint16_t measurementMessageSize = 0;
00089     readPipeStatus(&notificationMessageSize, &measurementMessageSize);
00090 
00091     uint16_t size;
00092     uint8_t pipe;
00093     if (notificationMessageSize)
00094     {
00095         size = notificationMessageSize;
00096         pipe = XBUS_NOTIFICATION_PIPE;
00097     }
00098     else if (measurementMessageSize)
00099     {
00100         size = measurementMessageSize;
00101         pipe = XBUS_MEASUREMENT_PIPE;
00102     }
00103     else
00104     {
00105         return;
00106     }
00107 
00108     uint8_t* buffer = (uint8_t*)allocateMessageData(size + 3);
00109     buffer[0] = XBUS_PREAMBLE;
00110     buffer[1] = XBUS_MASTERDEVICE;
00111     readFromPipe(&buffer[2], size, pipe);
00112     XbusParser_parseBuffer(m_xbusParser, buffer, 2 + size);
00113     deallocateMessageData(buffer);
00114 }
00115 
00116 
00117 /*! \brief Read MTSSP protocol info
00118     \param[out] version Pointer to receive the version byte
00119     \param[out] dataReadyConfig Pointer to receive the data ready configuration byte
00120     \sa configureProtocol
00121 */
00122 void MtInterfaceMtssp::readProtocolInfo(uint8_t* version, uint8_t* dataReadyConfig)
00123 {
00124     uint8_t rxdata[2];
00125     m_driver->read(XBUS_PROTOCOL_INFO, rxdata, sizeof(rxdata));
00126     *version = rxdata[0];
00127     *dataReadyConfig = rxdata[1];
00128 }
00129 
00130 
00131 /*! \brief Write MTSSP protocol settings
00132     \param dataReadyConfig The data ready configuration which must be set
00133 
00134     Bit 7:4 Reserved \n
00135     Bit 3   Measurement pipe DRDY event enable: 0 = disabled, 1 = enabled \n
00136     Bit 2   Notification pipe DRDY event enable: 0 = disabled, 1 = enabled \n
00137     Bit 1   Output type of DRDY pin: = 0 Push/pull, 1 = open drain \n
00138     Bit 0   Polarity of DRDY signal: 0 = Idle low, 1 = Idle high \n
00139     \sa readProtocolInfo
00140 */
00141 void MtInterfaceMtssp::configureProtocol(uint8_t dataReadyConfig)
00142 {
00143     m_driver->write(XBUS_CONFIGURE_PROTOCOL, &dataReadyConfig, sizeof(dataReadyConfig));
00144 }
00145 
00146 
00147 /*! \brief Read the pipe status
00148     \param[out] notificationMessageSize Pointer for returning the number of pending notification bytes
00149     \param[out] measurementMessageSize Pointer for returning the number of pending measurement bytes
00150 */
00151 void MtInterfaceMtssp::readPipeStatus(uint16_t *notificationMessageSize, uint16_t* measurementMessageSize)
00152 {
00153     uint8_t status[4];
00154     m_driver->read(XBUS_PIPE_STATUS, status, sizeof(status));
00155     *notificationMessageSize = status[0] | (status[1] << 8);
00156     *measurementMessageSize = status[2] | (status[3] << 8);
00157 }
00158 
00159 
00160 /*! \brief Read from notification or measurement data pipe
00161     \param buffer Result buffer
00162     \param size Number of bytes to read
00163     \param pipe Pipe from which to read, XBUS_NOTIFICATION_PIPE or XBUS_MEASUREMENT_PIPE
00164 */
00165 void MtInterfaceMtssp::readFromPipe(uint8_t* buffer, uint16_t size, uint8_t pipe)
00166 {
00167     m_driver->read(pipe, buffer, size);
00168 }