xbusparser.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 "xbusparser.h "
00018 #include "xbusdef.h "
00019 #include <stdlib.h>
00020 
00021 
00022 /*! \brief XbusParser states. */
00023 enum XbusParserState
00024 {
00025     XBPS_Preamble,          /*!< \brief Looking for preamble. */
00026     XBPS_BusId,             /*!< \brief Waiting for bus ID. */
00027     XBPS_MessageId,         /*!< \brief Waiting for message ID. */
00028     XBPS_Length,            /*!< \brief Waiting for length. */
00029     XBPS_ExtendedLengthMsb, /*!< \brief Waiting for extended length MSB*/
00030     XBPS_ExtendedLengthLsb, /*!< \brief Waiting for extended length LSB*/
00031     XBPS_Payload,           /*!< \brief Reading payload. */
00032     XBPS_Checksum           /*!< \brief Waiting for checksum. */
00033 };
00034 
00035 
00036 /*!
00037  * \brief Xbus Parser state structure.
00038  */
00039 struct XbusParser
00040 {
00041     /*! \brief Callbacks for memory management, and message handling. */
00042     struct XbusParserCallback callbacks;
00043     /*! \brief Storage for the current message being received. */
00044     struct XbusMessage currentMessage;
00045     /*! \brief The number of bytes of payload received for the current message. */
00046     uint16_t payloadReceived;
00047     /*! \brief The calculated checksum for the current message. */
00048     uint8_t checksum;
00049     /*! \brief The state of the parser. */
00050     enum XbusParserState state;
00051 };
00052 
00053 
00054 /*!
00055  * \brief Get the amount of memory needed for the XbusParser structure.
00056  */
00057 size_t XbusParser_mem(void)
00058 {
00059     return sizeof(struct XbusParser);
00060 }
00061 
00062 
00063 /*!
00064  * \brief Create a new XbusParser object.
00065  * \param callback Pointer to callback structure containing callback functions
00066  * for memory management and handling received messages.
00067  * \returns Pointer the new XbusParser structure.
00068  *
00069  * Uses malloc to allocate the memory required for the parser.
00070  */
00071 struct XbusParser* XbusParser_create(struct XbusParserCallback const* callback)
00072 {
00073     void* mem = malloc(XbusParser_mem());
00074     if (mem)
00075     {
00076         return XbusParser_init(mem, callback);
00077     }
00078     return NULL;
00079 }
00080 
00081 
00082 /*!
00083  * \brief Frees an XbusParser structure allocated by XbusParser_create().
00084  */
00085 void XbusParser_destroy(struct XbusParser* parser)
00086 {
00087     free(parser);
00088 }
00089 
00090 
00091 /*!
00092  * \brief Initializes an XbusParser in the passed memory location.
00093  * \param parserMem Pointer to memory to use for storing parser state. Should
00094  * be at least as big as the value returned by XbusParser_mem().
00095  * \param callback Pointer to callback structure containing callback functions
00096  * for memory management and handling received messages.
00097  * \returns Initialized XbusParser structure.
00098  */
00099 struct XbusParser* XbusParser_init(void* parserMem, struct XbusParserCallback const* callback)
00100 {
00101     struct XbusParser* parser = (struct XbusParser*)parserMem;
00102     parser->state = XBPS_Preamble;
00103     parser->callbacks.allocateBuffer = callback->allocateBuffer;
00104     parser->callbacks.deallocateBuffer = callback->deallocateBuffer;
00105     parser->callbacks.handleMessage = callback->handleMessage;
00106     return parser;
00107 }
00108 
00109 
00110 /*!
00111  * \brief Prepare for receiving a message payload.
00112  *
00113  * Requests a memory area to store the received data to using the
00114  * registered callbacks.
00115  */
00116 void prepareForPayload(struct XbusParser* parser)
00117 {
00118     parser->payloadReceived = 0;
00119     parser->currentMessage.m_data = parser->callbacks.allocateBuffer(parser->currentMessage.m_length);
00120 }
00121 
00122 
00123 /*!
00124  * \brief Parse a byte of data from a motion tracker.
00125  *
00126  * When a complete message is received the user will be notified by a call
00127  * to the handleMessage() callback function.
00128  */
00129 void XbusParser_parseByte(struct XbusParser *parser, const uint8_t byte)
00130 {
00131     switch (parser->state)
00132     {
00133         case XBPS_Preamble:
00134             if (byte == XBUS_PREAMBLE)
00135             {
00136                 parser->checksum = 0;
00137                 parser->state = XBPS_BusId;
00138             }
00139             break;
00140 
00141         case XBPS_BusId:
00142             parser->checksum += byte;
00143             parser->state = XBPS_MessageId;
00144             break;
00145 
00146         case XBPS_MessageId:
00147             parser->checksum += byte;
00148             parser->currentMessage.m_mid = byte;
00149             parser->state = XBPS_Length;
00150             break;
00151 
00152         case XBPS_Length:
00153             parser->checksum += byte;
00154             if (byte == XBUS_NO_PAYLOAD)
00155             {
00156                 parser->currentMessage.m_length = byte;
00157                 parser->currentMessage.m_data = NULL;
00158                 parser->state = XBPS_Checksum;
00159             }
00160             else if (byte < XBUS_EXTENDED_LENGTH)
00161             {
00162                 parser->currentMessage.m_length = byte;
00163                 prepareForPayload(parser);
00164                 parser->state = XBPS_Payload;
00165             }
00166             else
00167             {
00168                 parser->state = XBPS_ExtendedLengthMsb;
00169             }
00170             break;
00171 
00172         case XBPS_ExtendedLengthMsb:
00173             parser->checksum += byte;
00174             parser->currentMessage.m_length = ((uint16_t)byte) << 8;
00175             parser->state = XBPS_ExtendedLengthLsb;
00176             break;
00177 
00178         case XBPS_ExtendedLengthLsb:
00179             parser->checksum += byte;
00180             parser->currentMessage.m_length |= byte;
00181             prepareForPayload(parser);
00182             parser->state = XBPS_Payload;
00183             break;
00184 
00185         case XBPS_Payload:
00186             parser->checksum += byte;
00187             if (parser->currentMessage.m_data)
00188             {
00189                 ((uint8_t*)parser->currentMessage.m_data)[parser->payloadReceived] = byte;
00190             }
00191             if (++parser->payloadReceived == parser->currentMessage.m_length)
00192             {
00193                 parser->state = XBPS_Checksum;
00194             }
00195             break;
00196 
00197         case XBPS_Checksum:
00198             parser->checksum += byte;
00199             if ((parser->checksum == 0) &&
00200                     ((parser->currentMessage.m_length == 0) ||
00201                      parser->currentMessage.m_data))
00202             {
00203                 parser->callbacks.handleMessage(&parser->currentMessage);
00204             }
00205             else if (parser->currentMessage.m_data)
00206             {
00207                 parser->callbacks.deallocateBuffer(parser->currentMessage.m_data);
00208             }
00209             parser->state = XBPS_Preamble;
00210             break;
00211     }
00212 }
00213 
00214 
00215 /*!
00216  * \brief Parse a buffer of data received from a motion tracker.
00217  */
00218 void XbusParser_parseBuffer(struct XbusParser* parser, uint8_t const* buf, size_t bufSize)
00219 {
00220     size_t i;
00221     for (i = 0; i < bufSize; ++i)
00222     {
00223         XbusParser_parseByte(parser, buf[i]);
00224     }
00225 }
00226 
00227