Xsens / Xbus

Basic implementation of Xbus message parsing and generation for embedded processors. The code has no dependencies and should also work for other MCU architectures than ARM provided a C99 compiler is available.

Dependents:   MTi-1_example_LPC1768 MTi-1_rikbeun MTi-1_example MTi-1_example ... more

Important Information

This library is deprecated and no longer maintained. There are new embedded examples available in the MT SDK folder of the MT Software Suite. For more information please visit: https://xsenstechnologies.force.com/knowledgebase/s/article/Introduction-to-the-MT-SDK-programming-examples-for-MTi-devices

For an example of using the Xbus library to communicate with an MTi-1 series device using a full-duplex UART see:

Import programMTi-1_example

Example of using Xbus library to communicate with an MTi-1 series device using a full-duplex UART connection.

Last commit 03 May 2018 by Xsens

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