V N / xbus

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

xbusparser.c@0:eb25b1785ee4, 2015-06-16 (annotated)

Committer:
alexandery
Date:
Tue Jun 16 07:54:23 2015 +0000
Revision:
0:eb25b1785ee4
Make Xbus code into a library;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alexandery 0:eb25b1785ee4 1 /*!
alexandery 0:eb25b1785ee4 2 * \file
alexandery 0:eb25b1785ee4 3 * \copyright Copyright (C) Xsens Technologies B.V., 2015.
alexandery 0:eb25b1785ee4 4 *
alexandery 0:eb25b1785ee4 5 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
alexandery 0:eb25b1785ee4 6 * use this file except in compliance with the License. You may obtain a copy
alexandery 0:eb25b1785ee4 7 * of the License at
alexandery 0:eb25b1785ee4 8 *
alexandery 0:eb25b1785ee4 9 * http://www.apache.org/licenses/LICENSE-2.0
alexandery 0:eb25b1785ee4 10 *
alexandery 0:eb25b1785ee4 11 * Unless required by applicable law or agreed to in writing, software
alexandery 0:eb25b1785ee4 12 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
alexandery 0:eb25b1785ee4 13 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
alexandery 0:eb25b1785ee4 14 * License for the specific language governing permissions and limitations
alexandery 0:eb25b1785ee4 15 * under the License.
alexandery 0:eb25b1785ee4 16 */
alexandery 0:eb25b1785ee4 17
alexandery 0:eb25b1785ee4 18 #include "xbusparser.h"
alexandery 0:eb25b1785ee4 19 #include "xbusdef.h"
alexandery 0:eb25b1785ee4 20 #include "xbusutility.h"
alexandery 0:eb25b1785ee4 21 #include <stdlib.h>
alexandery 0:eb25b1785ee4 22 #include <string.h>
alexandery 0:eb25b1785ee4 23
alexandery 0:eb25b1785ee4 24 /*! \brief XbusParser states. */
alexandery 0:eb25b1785ee4 25 enum XbusParserState
alexandery 0:eb25b1785ee4 26 {
alexandery 0:eb25b1785ee4 27 XBPS_Preamble, /*!< \brief Looking for preamble. */
alexandery 0:eb25b1785ee4 28 XBPS_BusId, /*!< \brief Waiting for bus ID. */
alexandery 0:eb25b1785ee4 29 XBPS_MessageId, /*!< \brief Waiting for message ID. */
alexandery 0:eb25b1785ee4 30 XBPS_Length, /*!< \brief Waiting for length. */
alexandery 0:eb25b1785ee4 31 XBPS_ExtendedLengthMsb, /*!< \brief Waiting for extended length MSB*/
alexandery 0:eb25b1785ee4 32 XBPS_ExtendedLengthLsb, /*!< \brief Waiting for extended length LSB*/
alexandery 0:eb25b1785ee4 33 XBPS_Payload, /*!< \brief Reading payload. */
alexandery 0:eb25b1785ee4 34 XBPS_Checksum /*!< \brief Waiting for checksum. */
alexandery 0:eb25b1785ee4 35 };
alexandery 0:eb25b1785ee4 36
alexandery 0:eb25b1785ee4 37 /*!
alexandery 0:eb25b1785ee4 38 * \brief Xbus Parser state structure.
alexandery 0:eb25b1785ee4 39 */
alexandery 0:eb25b1785ee4 40 struct XbusParser
alexandery 0:eb25b1785ee4 41 {
alexandery 0:eb25b1785ee4 42 /*! \brief Callbacks for memory management, and message handling. */
alexandery 0:eb25b1785ee4 43 struct XbusParserCallback callbacks;
alexandery 0:eb25b1785ee4 44 /*! \brief Storage for the current message being received. */
alexandery 0:eb25b1785ee4 45 struct XbusMessage currentMessage;
alexandery 0:eb25b1785ee4 46 /*! \brief The number of bytes of payload received for the current message. */
alexandery 0:eb25b1785ee4 47 uint16_t payloadReceived;
alexandery 0:eb25b1785ee4 48 /*! \brief The calculated checksum for the current message. */
alexandery 0:eb25b1785ee4 49 uint8_t checksum;
alexandery 0:eb25b1785ee4 50 /*! \brief The state of the parser. */
alexandery 0:eb25b1785ee4 51 enum XbusParserState state;
alexandery 0:eb25b1785ee4 52 };
alexandery 0:eb25b1785ee4 53
alexandery 0:eb25b1785ee4 54 /*!
alexandery 0:eb25b1785ee4 55 * \brief Get the amount of memory needed for the XbusParser structure.
alexandery 0:eb25b1785ee4 56 */
alexandery 0:eb25b1785ee4 57 size_t XbusParser_mem(void)
alexandery 0:eb25b1785ee4 58 {
alexandery 0:eb25b1785ee4 59 return sizeof(struct XbusParser);
alexandery 0:eb25b1785ee4 60 }
alexandery 0:eb25b1785ee4 61
alexandery 0:eb25b1785ee4 62 /*!
alexandery 0:eb25b1785ee4 63 * \brief Create a new XbusParser object.
alexandery 0:eb25b1785ee4 64 * \param callback Pointer to callback structure containing callback functions
alexandery 0:eb25b1785ee4 65 * for memory management and handling received messages.
alexandery 0:eb25b1785ee4 66 * \returns Pointer the new XbusParser structure.
alexandery 0:eb25b1785ee4 67 *
alexandery 0:eb25b1785ee4 68 * Uses malloc to allocate the memory required for the parser.
alexandery 0:eb25b1785ee4 69 */
alexandery 0:eb25b1785ee4 70 struct XbusParser* XbusParser_create(struct XbusParserCallback const* callback)
alexandery 0:eb25b1785ee4 71 {
alexandery 0:eb25b1785ee4 72 void* mem = malloc(XbusParser_mem());
alexandery 0:eb25b1785ee4 73 if (mem)
alexandery 0:eb25b1785ee4 74 {
alexandery 0:eb25b1785ee4 75 return XbusParser_init(mem, callback);
alexandery 0:eb25b1785ee4 76 }
alexandery 0:eb25b1785ee4 77 return NULL;
alexandery 0:eb25b1785ee4 78 }
alexandery 0:eb25b1785ee4 79
alexandery 0:eb25b1785ee4 80 /*!
alexandery 0:eb25b1785ee4 81 * \brief Frees an XbusParser structure allocated by XbusParser_create().
alexandery 0:eb25b1785ee4 82 */
alexandery 0:eb25b1785ee4 83 void XbusParser_destroy(struct XbusParser* parser)
alexandery 0:eb25b1785ee4 84 {
alexandery 0:eb25b1785ee4 85 free(parser);
alexandery 0:eb25b1785ee4 86 }
alexandery 0:eb25b1785ee4 87
alexandery 0:eb25b1785ee4 88 /*!
alexandery 0:eb25b1785ee4 89 * \brief Initializes an XbusParser in the passed memory location.
alexandery 0:eb25b1785ee4 90 * \param parserMem Pointer to memory to use for storing parser state. Should
alexandery 0:eb25b1785ee4 91 * be at least as big as the value returned by XbusParser_mem().
alexandery 0:eb25b1785ee4 92 * \param callback Pointer to callback structure containing callback functions
alexandery 0:eb25b1785ee4 93 * for memory management and handling received messages.
alexandery 0:eb25b1785ee4 94 * \returns Initialized XbusParser structure.
alexandery 0:eb25b1785ee4 95 */
alexandery 0:eb25b1785ee4 96 struct XbusParser* XbusParser_init(void* parserMem, struct XbusParserCallback const* callback)
alexandery 0:eb25b1785ee4 97 {
alexandery 0:eb25b1785ee4 98 struct XbusParser* parser = (struct XbusParser*)parserMem;
alexandery 0:eb25b1785ee4 99 parser->state = XBPS_Preamble;
alexandery 0:eb25b1785ee4 100 parser->callbacks.allocateBuffer = callback->allocateBuffer;
alexandery 0:eb25b1785ee4 101 parser->callbacks.deallocateBuffer = callback->deallocateBuffer;
alexandery 0:eb25b1785ee4 102 parser->callbacks.handleMessage = callback->handleMessage;
alexandery 0:eb25b1785ee4 103 return parser;
alexandery 0:eb25b1785ee4 104 }
alexandery 0:eb25b1785ee4 105
alexandery 0:eb25b1785ee4 106 /*!
alexandery 0:eb25b1785ee4 107 * \brief Parse an XMID_DeviceId message to extract the device ID value.
alexandery 0:eb25b1785ee4 108
alexandery 0:eb25b1785ee4 109 * Replaces the raw Xbus message data with the device ID.
alexandery 0:eb25b1785ee4 110 */
alexandery 0:eb25b1785ee4 111 static void parseDeviceId(struct XbusParser* parser, uint8_t const* rawData)
alexandery 0:eb25b1785ee4 112 {
alexandery 0:eb25b1785ee4 113 uint32_t* deviceId = parser->callbacks.allocateBuffer(sizeof(uint32_t));
alexandery 0:eb25b1785ee4 114 if (deviceId)
alexandery 0:eb25b1785ee4 115 {
alexandery 0:eb25b1785ee4 116 XbusUtility_readU32(deviceId, rawData);
alexandery 0:eb25b1785ee4 117 parser->currentMessage.data = deviceId;
alexandery 0:eb25b1785ee4 118 parser->currentMessage.length = 1;
alexandery 0:eb25b1785ee4 119 }
alexandery 0:eb25b1785ee4 120 else
alexandery 0:eb25b1785ee4 121 {
alexandery 0:eb25b1785ee4 122 parser->currentMessage.data = NULL;
alexandery 0:eb25b1785ee4 123 }
alexandery 0:eb25b1785ee4 124 }
alexandery 0:eb25b1785ee4 125
alexandery 0:eb25b1785ee4 126 /*!
alexandery 0:eb25b1785ee4 127 * \brief Parse an XMID_OutputConfig message.
alexandery 0:eb25b1785ee4 128 *
alexandery 0:eb25b1785ee4 129 * Replaces the raw Xbus message data with an array of OutputConfiguration
alexandery 0:eb25b1785ee4 130 * structures.
alexandery 0:eb25b1785ee4 131 */
alexandery 0:eb25b1785ee4 132 static void parseOutputConfig(struct XbusParser* parser, uint8_t const* rawData)
alexandery 0:eb25b1785ee4 133 {
alexandery 0:eb25b1785ee4 134 uint8_t fields = parser->currentMessage.length / 4;
alexandery 0:eb25b1785ee4 135 struct OutputConfiguration* conf = parser->callbacks.allocateBuffer(fields * sizeof(struct OutputConfiguration));
alexandery 0:eb25b1785ee4 136 if (conf)
alexandery 0:eb25b1785ee4 137 {
alexandery 0:eb25b1785ee4 138 parser->currentMessage.data = conf;
alexandery 0:eb25b1785ee4 139 parser->currentMessage.length = fields;
alexandery 0:eb25b1785ee4 140
alexandery 0:eb25b1785ee4 141 for (int i = 0; i < fields; ++i)
alexandery 0:eb25b1785ee4 142 {
alexandery 0:eb25b1785ee4 143 rawData = XbusUtility_readU16((uint16_t*)&conf->dtype, rawData);
alexandery 0:eb25b1785ee4 144 rawData = XbusUtility_readU16(&conf->freq, rawData);
alexandery 0:eb25b1785ee4 145 ++conf;
alexandery 0:eb25b1785ee4 146 }
alexandery 0:eb25b1785ee4 147 }
alexandery 0:eb25b1785ee4 148 else
alexandery 0:eb25b1785ee4 149 {
alexandery 0:eb25b1785ee4 150 parser->currentMessage.data = NULL;
alexandery 0:eb25b1785ee4 151 }
alexandery 0:eb25b1785ee4 152 }
alexandery 0:eb25b1785ee4 153
alexandery 0:eb25b1785ee4 154 /*!
alexandery 0:eb25b1785ee4 155 * \brief Converts raw Xbus payload data to native structures if possible.
alexandery 0:eb25b1785ee4 156 *
alexandery 0:eb25b1785ee4 157 * Raw data payloads are converted to native data structures and the
alexandery 0:eb25b1785ee4 158 * message data pointer is changed to point to the native structure.
alexandery 0:eb25b1785ee4 159 * The raw data is automatically deallocated.
alexandery 0:eb25b1785ee4 160 */
alexandery 0:eb25b1785ee4 161 static void parseMessagePayload(struct XbusParser* parser)
alexandery 0:eb25b1785ee4 162 {
alexandery 0:eb25b1785ee4 163 uint8_t const* const rawData = parser->currentMessage.data;
alexandery 0:eb25b1785ee4 164 switch (parser->currentMessage.mid)
alexandery 0:eb25b1785ee4 165 {
alexandery 0:eb25b1785ee4 166 default:
alexandery 0:eb25b1785ee4 167 // Leave parsing and memory management to user code
alexandery 0:eb25b1785ee4 168 return;
alexandery 0:eb25b1785ee4 169
alexandery 0:eb25b1785ee4 170 case XMID_DeviceId:
alexandery 0:eb25b1785ee4 171 parseDeviceId(parser, rawData);
alexandery 0:eb25b1785ee4 172 break;
alexandery 0:eb25b1785ee4 173
alexandery 0:eb25b1785ee4 174 case XMID_OutputConfig:
alexandery 0:eb25b1785ee4 175 parseOutputConfig(parser, rawData);
alexandery 0:eb25b1785ee4 176 break;
alexandery 0:eb25b1785ee4 177 }
alexandery 0:eb25b1785ee4 178
alexandery 0:eb25b1785ee4 179 if (rawData)
alexandery 0:eb25b1785ee4 180 parser->callbacks.deallocateBuffer(rawData);
alexandery 0:eb25b1785ee4 181 }
alexandery 0:eb25b1785ee4 182
alexandery 0:eb25b1785ee4 183 /*!
alexandery 0:eb25b1785ee4 184 * \brief Prepare for receiving a message payload.
alexandery 0:eb25b1785ee4 185 *
alexandery 0:eb25b1785ee4 186 * Requests a memory area to store the received data to using the
alexandery 0:eb25b1785ee4 187 * registered callbacks.
alexandery 0:eb25b1785ee4 188 */
alexandery 0:eb25b1785ee4 189 void prepareForPayload(struct XbusParser* parser)
alexandery 0:eb25b1785ee4 190 {
alexandery 0:eb25b1785ee4 191 parser->payloadReceived = 0;
alexandery 0:eb25b1785ee4 192 parser->currentMessage.data = parser->callbacks.allocateBuffer(parser->currentMessage.length);
alexandery 0:eb25b1785ee4 193 }
alexandery 0:eb25b1785ee4 194
alexandery 0:eb25b1785ee4 195 /*!
alexandery 0:eb25b1785ee4 196 * \brief Parse a byte of data from a motion tracker.
alexandery 0:eb25b1785ee4 197 *
alexandery 0:eb25b1785ee4 198 * When a complete message is received the user will be notified by a call
alexandery 0:eb25b1785ee4 199 * to the handleMessage() callback function.
alexandery 0:eb25b1785ee4 200 */
alexandery 0:eb25b1785ee4 201 void XbusParser_parseByte(struct XbusParser* parser, const uint8_t byte)
alexandery 0:eb25b1785ee4 202 {
alexandery 0:eb25b1785ee4 203 switch (parser->state)
alexandery 0:eb25b1785ee4 204 {
alexandery 0:eb25b1785ee4 205 case XBPS_Preamble:
alexandery 0:eb25b1785ee4 206 if (byte == XBUS_PREAMBLE)
alexandery 0:eb25b1785ee4 207 {
alexandery 0:eb25b1785ee4 208 parser->checksum = 0;
alexandery 0:eb25b1785ee4 209 parser->state = XBPS_BusId;
alexandery 0:eb25b1785ee4 210 }
alexandery 0:eb25b1785ee4 211 break;
alexandery 0:eb25b1785ee4 212
alexandery 0:eb25b1785ee4 213 case XBPS_BusId:
alexandery 0:eb25b1785ee4 214 parser->checksum += byte;
alexandery 0:eb25b1785ee4 215 parser->state = XBPS_MessageId;
alexandery 0:eb25b1785ee4 216 break;
alexandery 0:eb25b1785ee4 217
alexandery 0:eb25b1785ee4 218 case XBPS_MessageId:
alexandery 0:eb25b1785ee4 219 parser->checksum += byte;
alexandery 0:eb25b1785ee4 220 parser->currentMessage.mid = (enum XsMessageId)byte;
alexandery 0:eb25b1785ee4 221 parser->state = XBPS_Length;
alexandery 0:eb25b1785ee4 222 break;
alexandery 0:eb25b1785ee4 223
alexandery 0:eb25b1785ee4 224 case XBPS_Length:
alexandery 0:eb25b1785ee4 225 parser->checksum += byte;
alexandery 0:eb25b1785ee4 226 if (byte == XBUS_NO_PAYLOAD)
alexandery 0:eb25b1785ee4 227 {
alexandery 0:eb25b1785ee4 228 parser->currentMessage.length = byte;
alexandery 0:eb25b1785ee4 229 parser->currentMessage.data = NULL;
alexandery 0:eb25b1785ee4 230 parser->state = XBPS_Checksum;
alexandery 0:eb25b1785ee4 231 }
alexandery 0:eb25b1785ee4 232 else if (byte < XBUS_EXTENDED_LENGTH)
alexandery 0:eb25b1785ee4 233 {
alexandery 0:eb25b1785ee4 234 parser->currentMessage.length = byte;
alexandery 0:eb25b1785ee4 235 prepareForPayload(parser);
alexandery 0:eb25b1785ee4 236 parser->state = XBPS_Payload;
alexandery 0:eb25b1785ee4 237 }
alexandery 0:eb25b1785ee4 238 else
alexandery 0:eb25b1785ee4 239 {
alexandery 0:eb25b1785ee4 240 parser->state = XBPS_ExtendedLengthMsb;
alexandery 0:eb25b1785ee4 241 }
alexandery 0:eb25b1785ee4 242 break;
alexandery 0:eb25b1785ee4 243
alexandery 0:eb25b1785ee4 244 case XBPS_ExtendedLengthMsb:
alexandery 0:eb25b1785ee4 245 parser->checksum += byte;
alexandery 0:eb25b1785ee4 246 parser->currentMessage.length = ((uint16_t)byte) << 8;
alexandery 0:eb25b1785ee4 247 parser->state = XBPS_ExtendedLengthLsb;
alexandery 0:eb25b1785ee4 248 break;
alexandery 0:eb25b1785ee4 249
alexandery 0:eb25b1785ee4 250 case XBPS_ExtendedLengthLsb:
alexandery 0:eb25b1785ee4 251 parser->checksum += byte;
alexandery 0:eb25b1785ee4 252 parser->currentMessage.length |= byte;
alexandery 0:eb25b1785ee4 253 prepareForPayload(parser);
alexandery 0:eb25b1785ee4 254 parser->state = XBPS_Payload;
alexandery 0:eb25b1785ee4 255 break;
alexandery 0:eb25b1785ee4 256
alexandery 0:eb25b1785ee4 257 case XBPS_Payload:
alexandery 0:eb25b1785ee4 258 parser->checksum += byte;
alexandery 0:eb25b1785ee4 259 if (parser->currentMessage.data)
alexandery 0:eb25b1785ee4 260 {
alexandery 0:eb25b1785ee4 261 ((uint8_t*)parser->currentMessage.data)[parser->payloadReceived] = byte;
alexandery 0:eb25b1785ee4 262 }
alexandery 0:eb25b1785ee4 263 if (++parser->payloadReceived == parser->currentMessage.length)
alexandery 0:eb25b1785ee4 264 {
alexandery 0:eb25b1785ee4 265 parser->state = XBPS_Checksum;
alexandery 0:eb25b1785ee4 266 }
alexandery 0:eb25b1785ee4 267 break;
alexandery 0:eb25b1785ee4 268
alexandery 0:eb25b1785ee4 269 case XBPS_Checksum:
alexandery 0:eb25b1785ee4 270 parser->checksum += byte;
alexandery 0:eb25b1785ee4 271 if ((parser->checksum == 0) &&
alexandery 0:eb25b1785ee4 272 ((parser->currentMessage.length == 0) ||
alexandery 0:eb25b1785ee4 273 parser->currentMessage.data))
alexandery 0:eb25b1785ee4 274 {
alexandery 0:eb25b1785ee4 275 parseMessagePayload(parser);
alexandery 0:eb25b1785ee4 276 parser->callbacks.handleMessage(&parser->currentMessage);
alexandery 0:eb25b1785ee4 277 }
alexandery 0:eb25b1785ee4 278 else if (parser->currentMessage.data)
alexandery 0:eb25b1785ee4 279 {
alexandery 0:eb25b1785ee4 280 parser->callbacks.deallocateBuffer(parser->currentMessage.data);
alexandery 0:eb25b1785ee4 281 }
alexandery 0:eb25b1785ee4 282 parser->state = XBPS_Preamble;
alexandery 0:eb25b1785ee4 283 break;
alexandery 0:eb25b1785ee4 284 }
alexandery 0:eb25b1785ee4 285 }
alexandery 0:eb25b1785ee4 286
alexandery 0:eb25b1785ee4 287 /*!
alexandery 0:eb25b1785ee4 288 * \brief Parse a buffer of data received from a motion tracker.
alexandery 0:eb25b1785ee4 289 */
alexandery 0:eb25b1785ee4 290 void XbusParser_parseBuffer(struct XbusParser* parser, uint8_t const* buf, size_t bufSize)
alexandery 0:eb25b1785ee4 291 {
alexandery 0:eb25b1785ee4 292 for (size_t i = 0; i < bufSize; ++i)
alexandery 0:eb25b1785ee4 293 {
alexandery 0:eb25b1785ee4 294 XbusParser_parseByte(parser, buf[i]);
alexandery 0:eb25b1785ee4 295 }
alexandery 0:eb25b1785ee4 296 }
alexandery 0:eb25b1785ee4 297