xbusmessage.c

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 
00018 #include "xbusmessage.h "
00019 #include "xbusdef.h "
00020 #include "xbusutility.h "
00021 
00022 /*!
00023  * \brief Calculate the number of bytes needed for \a message payload.
00024  */
00025 static uint16_t messageLength(struct XbusMessage const* message)
00026 {
00027     switch (message->mid)
00028     {
00029         case XMID_SetOutputConfig:
00030             return message->length * 2 * sizeof(uint16_t);
00031 
00032         default:
00033             return message->length;
00034     }
00035 }
00036 
00037 /*!
00038  * \brief Format a message with a pointer to an array of OutputConfiguration elements.
00039  */
00040 static void formatOutputConfig(uint8_t* raw, struct XbusMessage const* message)
00041 {
00042     struct OutputConfiguration* conf = message->data;
00043     for (int i = 0; i < message->length; ++i)
00044     {
00045         raw = XbusUtility_writeU16(raw, conf->dtype);
00046         raw = XbusUtility_writeU16(raw, conf->freq);
00047         ++conf;
00048     }
00049 }
00050 
00051 /*!
00052  * \brief Format the payload of a message from a native data type to
00053  * raw bytes.
00054  */
00055 static void formatPayload(uint8_t* raw, struct XbusMessage const* message)
00056 {
00057     switch (message->mid)
00058     {
00059         case XMID_SetOutputConfig:
00060             formatOutputConfig(raw, message);
00061             break;
00062 
00063         default:
00064             for (int i = 0; i < message->length; ++i)
00065             {
00066                 *raw++ = ((uint8_t*)message->data)[i];
00067             }
00068             break;
00069     }
00070 }
00071 
00072 /*!
00073  * \brief Format a message into the raw Xbus format ready for transmission to
00074  * a motion tracker.
00075  */
00076 size_t XbusMessage_format(uint8_t* raw, struct XbusMessage const* message, enum XbusLowLevelFormat format)
00077 {
00078     uint8_t* dptr = raw;
00079     switch (format)
00080     {
00081         case XLLF_I2c:
00082             {
00083                 *dptr++ = XBUS_CONTROL_PIPE;
00084             }
00085             break;
00086 
00087         case XLLF_Spi:
00088             {
00089                 *dptr++ = XBUS_CONTROL_PIPE;
00090                 // Fill bytes required to allow MT to process data
00091                 *dptr++ = 0;
00092                 *dptr++ = 0;
00093                 *dptr++ = 0;
00094             }
00095             break;
00096 
00097         case XLLF_Uart:
00098             {
00099                 *dptr++ = XBUS_PREAMBLE;
00100                 *dptr++ = XBUS_MASTERDEVICE;
00101             }
00102             break;
00103     }
00104 
00105     uint8_t checksum = (uint8_t)(-XBUS_MASTERDEVICE);
00106 
00107     *dptr = message->mid;
00108     checksum -= *dptr++;
00109 
00110     uint16_t length = messageLength(message);
00111 
00112     if (length < XBUS_EXTENDED_LENGTH)
00113     {
00114         *dptr = length;
00115         checksum -= *dptr++;
00116     }
00117     else
00118     {
00119         *dptr = XBUS_EXTENDED_LENGTH;
00120         checksum -= *dptr++;
00121         *dptr = length >> 8;
00122         checksum -= *dptr++;
00123         *dptr = length & 0xFF;
00124         checksum -= *dptr++;
00125     }
00126 
00127     formatPayload(dptr, message);
00128     for (int i = 0; i < length; ++i)
00129     {
00130         checksum -= *dptr++;
00131     }
00132     *dptr++ = checksum;
00133 
00134     return dptr - raw;
00135 }
00136 
00137 /*!
00138  * \brief Get a pointer to the data corresponding to \a id.
00139  * \param id The data identifier to find in the message.
00140  * \param data Pointer to the raw message payload.
00141  * \param dataLength The length of the payload in bytes.
00142  * \returns Pointer to data item, or NULL if the identifier is not present in
00143  * the message.
00144  */
00145 static uint8_t const* getPointerToData(enum XsDataIdentifier id, uint8_t const* data, uint16_t dataLength)
00146 {
00147     uint8_t const* dptr = data;
00148     while (dptr < data + dataLength)
00149     {
00150         uint16_t itemId;
00151         uint8_t itemSize;
00152         dptr = XbusUtility_readU16(&itemId, dptr);
00153         dptr = XbusUtility_readU8(&itemSize, dptr);
00154 
00155         if (id == itemId)
00156             return dptr;
00157 
00158         dptr += itemSize;
00159     }
00160     return NULL;
00161 }
00162 
00163 /*!
00164  * \brief Read a number of floats from a message payload.
00165  * \param out Pointer to where to output data.
00166  * \param raw Pointer to the start of the raw float data.
00167  * \param floats The number of floats to read.
00168  */
00169 static void readFloats(float* out, uint8_t const* raw, uint8_t floats)
00170 {
00171     for (int i = 0; i < floats; ++i)
00172     {
00173         raw = XbusUtility_readU32((uint32_t*)&out[i], raw);
00174     }
00175 }
00176 
00177 /*!
00178  * \brief Get a data item from an XMID_MtData2 Xbus message.
00179  * \param item Pointer to where to store the data.
00180  * \param id The data identifier to get.
00181  * \param message The message to read the data item from.
00182  * \returns true if the data item is found in the message, else false.
00183  */
00184 bool XbusMessage_getDataItem(void* item, enum XsDataIdentifier id, struct XbusMessage const* message)
00185 {
00186     uint8_t const* raw = getPointerToData(id, message->data, message->length);
00187     if (raw)
00188     {
00189         switch (id)
00190         {
00191             case XDI_PacketCounter:
00192                 raw = XbusUtility_readU16(item, raw);
00193                 break;
00194 
00195             case XDI_SampleTimeFine:
00196             case XDI_StatusWord:
00197                 raw = XbusUtility_readU32(item, raw);
00198                 break;
00199 
00200             case XDI_Quaternion:
00201             case XDI_DeltaQ:
00202                 readFloats(item, raw, 4);
00203                 break;
00204 
00205             case XDI_DeltaV:
00206             case XDI_Acceleration:
00207             case XDI_RateOfTurn:
00208             case XDI_MagneticField:
00209                 readFloats(item, raw, 3);
00210                 break;
00211 
00212             default:
00213                 return false;
00214         }
00215         return true;
00216     }
00217     else
00218     {
00219         return false;
00220     }
00221 }
00222 
00223 /*!
00224  * \brief Get a string description for the passed data identifier.
00225  */
00226 char const* XbusMessage_dataDescription(enum XsDataIdentifier id)
00227 {
00228     switch (id)
00229     {
00230         case XDI_PacketCounter:
00231             return "Packet counter";
00232 
00233         case XDI_SampleTimeFine:
00234             return "Sample time fine";
00235 
00236         case XDI_Quaternion:
00237             return "Quaternion";
00238 
00239         case XDI_DeltaV:
00240             return "Velocity increment";
00241 
00242         case XDI_Acceleration:
00243             return "Acceleration";
00244 
00245         case XDI_RateOfTurn:
00246             return "Rate of turn";
00247 
00248         case XDI_DeltaQ:
00249             return "Orientation increment";
00250 
00251         case XDI_MagneticField:
00252             return "Magnetic field";
00253 
00254         case XDI_StatusWord:
00255             return "Status word";
00256 
00257         default:
00258             return "Unknown data type";
00259     }
00260 }