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 #include "xbusmessage.h "
00018 #include "xbusdef.h "
00019 
00020 /*! \brief Format a message into the raw mtssp format ready for transmission to a motion tracker.
00021 */
00022 size_t XbusMessage_createRawMessage(uint8_t *raw, struct XbusMessage const *message, enum XbusBusFormat format)
00023 {
00024     int n;
00025     uint8_t checksum;
00026     uint16_t length;
00027     uint8_t *dptr = raw;
00028 
00029     if (raw == 0)
00030     {
00031         switch (format)
00032         {
00033         case XBF_I2c:
00034             return (message->m_length < 255) ? message->m_length + 4 : message->m_length + 6;
00035         case XBF_Spi:
00036             return (message->m_length < 255) ? message->m_length + 7 : message->m_length + 9;
00037         case XBF_Uart:
00038             return (message->m_length < 255) ? message->m_length + 5 : message->m_length + 7;
00039         }
00040     }
00041 
00042     switch (format)
00043     {
00044     case XBF_I2c:
00045     {
00046         *dptr++ = XBUS_CONTROL_PIPE;
00047     }
00048         break;
00049 
00050     case XBF_Spi:
00051     {
00052         *dptr++ = XBUS_CONTROL_PIPE;
00053         // Fill bytes required to allow MT to process data
00054         *dptr++ = 0;
00055         *dptr++ = 0;
00056         *dptr++ = 0;
00057     }
00058         break;
00059 
00060     case XBF_Uart:
00061     {
00062         *dptr++ = XBUS_PREAMBLE;
00063         *dptr++ = XBUS_MASTERDEVICE;
00064     }
00065         break;
00066     }
00067 
00068     checksum = (uint8_t)(-XBUS_MASTERDEVICE);
00069 
00070     *dptr = message->m_mid;
00071     checksum -= *dptr++;
00072 
00073     length = message->m_length;
00074 
00075     if (length < XBUS_EXTENDED_LENGTH)
00076     {
00077         *dptr = length;
00078         checksum -= *dptr++;
00079     }
00080     else
00081     {
00082         *dptr = XBUS_EXTENDED_LENGTH;
00083         checksum -= *dptr++;
00084         *dptr = length >> 8;
00085         checksum -= *dptr++;
00086         *dptr = length & 0xFF;
00087         checksum -= *dptr++;
00088     }
00089 
00090     for (n = 0; n < message->m_length; n++)
00091     {
00092         *dptr = message->m_data[n];
00093         checksum -= *dptr++;
00094     }
00095 
00096     *dptr++ = checksum;
00097 
00098     return dptr - raw;
00099 }