rik freije / Mbed 2 deprecated MTi-1_rikbeun

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.

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

xbus/xbusparser.c@46:f652d199d27e, 2015-05-21 (annotated)

Committer:
Alex Young
Date:
Thu May 21 16:30:21 2015 +0200
Revision:
46:f652d199d27e
Parent:
25:01356fb59467
Child:
49:38ecfbff5391
Add documentation for xbusparser

Who changed what in which revision?

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