Gabriel Delgado / Mbed 2 deprecated MTi-1_uart

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 mbed-rtos Xbus

xbus/xbusparser.c@19:46e88d37ecef, 2015-05-19 (annotated)

Committer:
Alex Young
Date:
Tue May 19 15:49:29 2015 +0200
Revision:
19:46e88d37ecef
Parent:
17:680f28e00d17
Child:
22:3eab999c5076
Add support for parsing individual xbus messages.



Most xbus messages, except from MTData2 messages have a fixed format. To

make life easy for the user we will parse these and return the parsed

data in the memory pointed to by the XbusMessage data pointer. This way

we can just define structs for the different messages and users can

switch on the mid to decide how to cast the data pointer.

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 19:46e88d37ecef 21 #include <assert.h>
Alex Young 19:46e88d37ecef 22
Alex Young 19:46e88d37ecef 23 /*!
Alex Young 19:46e88d37ecef 24 * \brief Max message length for parsed message types.
Alex Young 19:46e88d37ecef 25 * Unparsed types, e.g. MtData2 packets, will have buffers dynamically
Alex Young 19:46e88d37ecef 26 * requested, so are not constrained by this value.
Alex Young 19:46e88d37ecef 27 */
Alex Young 19:46e88d37ecef 28 #define XBUS_MAX_MESSAGE_LENGTH (64)
Alex Young 3:abc451d82b88 29
Alex Young 3:abc451d82b88 30 enum XbusParserState
Alex Young 3:abc451d82b88 31 {
Alex Young 3:abc451d82b88 32 XBPS_Preamble, /*!< \brief Looking for preamble. */
Alex Young 3:abc451d82b88 33 XBPS_BusId, /*!< \brief Waiting for bus ID. */
Alex Young 3:abc451d82b88 34 XBPS_MessageId, /*!< \brief Waiting for message ID. */
Alex Young 3:abc451d82b88 35 XBPS_Length, /*!< \brief Waiting for length. */
Alex Young 3:abc451d82b88 36 XBPS_ExtendedLengthMsb, /*!< \brief Waiting for extended length MSB*/
Alex Young 3:abc451d82b88 37 XBPS_ExtendedLengthLsb, /*!< \brief Waiting for extended length LSB*/
Alex Young 3:abc451d82b88 38 XBPS_Payload, /*!< \brief Reading payload. */
Alex Young 3:abc451d82b88 39 XBPS_Checksum /*!< \brief Waiting for checksum. */
Alex Young 3:abc451d82b88 40 };
Alex Young 3:abc451d82b88 41
Alex Young 3:abc451d82b88 42 struct XbusParser
Alex Young 3:abc451d82b88 43 {
Alex Young 3:abc451d82b88 44 struct XbusParserCallback callbacks;
Alex Young 14:155f9a55ec51 45 struct XbusMessage currentMessage;
Alex Young 19:46e88d37ecef 46 uint8_t rxBuffer[XBUS_MAX_MESSAGE_LENGTH];
Alex Young 3:abc451d82b88 47 uint16_t payloadReceived;
Alex Young 3:abc451d82b88 48 uint8_t checksum;
Alex Young 3:abc451d82b88 49 enum XbusParserState state;
Alex Young 3:abc451d82b88 50 };
Alex Young 3:abc451d82b88 51
Alex Young 3:abc451d82b88 52 size_t XbusParser_mem(void)
Alex Young 3:abc451d82b88 53 {
Alex Young 3:abc451d82b88 54 return sizeof(struct XbusParser);
Alex Young 3:abc451d82b88 55 }
Alex Young 3:abc451d82b88 56
Alex Young 3:abc451d82b88 57 struct XbusParser* XbusParser_create(struct XbusParserCallback const* callback)
Alex Young 3:abc451d82b88 58 {
Alex Young 3:abc451d82b88 59 void* mem = malloc(XbusParser_mem());
Alex Young 3:abc451d82b88 60 if (mem)
Alex Young 3:abc451d82b88 61 {
Alex Young 3:abc451d82b88 62 return XbusParser_init(mem, callback);
Alex Young 3:abc451d82b88 63 }
Alex Young 3:abc451d82b88 64 return NULL;
Alex Young 3:abc451d82b88 65 }
Alex Young 3:abc451d82b88 66
Alex Young 3:abc451d82b88 67 void XbusParser_destroy(struct XbusParser* parser)
Alex Young 3:abc451d82b88 68 {
Alex Young 3:abc451d82b88 69 free(parser);
Alex Young 3:abc451d82b88 70 }
Alex Young 3:abc451d82b88 71
Alex Young 3:abc451d82b88 72 struct XbusParser* XbusParser_init(void* parserMem, struct XbusParserCallback const* callback)
Alex Young 3:abc451d82b88 73 {
Alex Young 3:abc451d82b88 74 struct XbusParser* parser = (struct XbusParser*)parserMem;
Alex Young 3:abc451d82b88 75 parser->state = XBPS_Preamble;
Alex Young 3:abc451d82b88 76 parser->callbacks.allocateBuffer = callback->allocateBuffer;
Alex Young 3:abc451d82b88 77 parser->callbacks.handleMessage = callback->handleMessage;
Alex Young 3:abc451d82b88 78 return parser;
Alex Young 3:abc451d82b88 79 }
Alex Young 3:abc451d82b88 80
Alex Young 19:46e88d37ecef 81 static bool canParseMessagePayload(enum XsMessageId mid)
Alex Young 19:46e88d37ecef 82 {
Alex Young 19:46e88d37ecef 83 switch (mid)
Alex Young 19:46e88d37ecef 84 {
Alex Young 19:46e88d37ecef 85 case XMID_DeviceId:
Alex Young 19:46e88d37ecef 86 return true;
Alex Young 19:46e88d37ecef 87
Alex Young 19:46e88d37ecef 88 default:
Alex Young 19:46e88d37ecef 89 return false;
Alex Young 19:46e88d37ecef 90 }
Alex Young 19:46e88d37ecef 91 }
Alex Young 19:46e88d37ecef 92
Alex Young 19:46e88d37ecef 93 static void parseMessagePayload(struct XbusParser* parser)
Alex Young 3:abc451d82b88 94 {
Alex Young 19:46e88d37ecef 95 switch (parser->currentMessage.mid)
Alex Young 19:46e88d37ecef 96 {
Alex Young 19:46e88d37ecef 97 case XMID_DeviceId:
Alex Young 19:46e88d37ecef 98 {
Alex Young 19:46e88d37ecef 99 uint32_t* deviceId = parser->callbacks.allocateBuffer(sizeof(uint32_t));
Alex Young 19:46e88d37ecef 100 if (deviceId)
Alex Young 19:46e88d37ecef 101 {
Alex Young 19:46e88d37ecef 102 XbusUtility_readU32(deviceId, parser->rxBuffer);
Alex Young 19:46e88d37ecef 103 parser->currentMessage.data = deviceId;
Alex Young 19:46e88d37ecef 104 }
Alex Young 19:46e88d37ecef 105 else
Alex Young 19:46e88d37ecef 106 {
Alex Young 19:46e88d37ecef 107 parser->currentMessage.data = NULL;
Alex Young 19:46e88d37ecef 108 }
Alex Young 19:46e88d37ecef 109 }
Alex Young 19:46e88d37ecef 110 break;
Alex Young 19:46e88d37ecef 111
Alex Young 19:46e88d37ecef 112 default:
Alex Young 19:46e88d37ecef 113 assert(!canParseMessagePayload(parser->currentMessage.mid));
Alex Young 19:46e88d37ecef 114 break;
Alex Young 19:46e88d37ecef 115 }
Alex Young 19:46e88d37ecef 116 }
Alex Young 19:46e88d37ecef 117
Alex Young 19:46e88d37ecef 118 void prepareForPayload(struct XbusParser* parser)
Alex Young 19:46e88d37ecef 119 {
Alex Young 19:46e88d37ecef 120 parser->currentMessage.data = NULL;
Alex Young 3:abc451d82b88 121 parser->payloadReceived = 0;
Alex Young 19:46e88d37ecef 122
Alex Young 19:46e88d37ecef 123 if (canParseMessagePayload(parser->currentMessage.mid))
Alex Young 19:46e88d37ecef 124 {
Alex Young 19:46e88d37ecef 125 assert(parser->currentMessage.length < XBUS_MAX_MESSAGE_LENGTH);
Alex Young 19:46e88d37ecef 126 if (parser->currentMessage.length < XBUS_MAX_MESSAGE_LENGTH)
Alex Young 19:46e88d37ecef 127 {
Alex Young 19:46e88d37ecef 128 parser->currentMessage.data = parser->rxBuffer;
Alex Young 19:46e88d37ecef 129 }
Alex Young 19:46e88d37ecef 130 }
Alex Young 19:46e88d37ecef 131 else
Alex Young 19:46e88d37ecef 132 {
Alex Young 19:46e88d37ecef 133 parser->currentMessage.data = parser->callbacks.allocateBuffer(parser->currentMessage.length);
Alex Young 19:46e88d37ecef 134 }
Alex Young 3:abc451d82b88 135 }
Alex Young 3:abc451d82b88 136
Alex Young 3:abc451d82b88 137 void XbusParser_parseByte(struct XbusParser* parser, const uint8_t byte)
Alex Young 3:abc451d82b88 138 {
Alex Young 3:abc451d82b88 139 switch (parser->state)
Alex Young 3:abc451d82b88 140 {
Alex Young 3:abc451d82b88 141 case XBPS_Preamble:
Alex Young 3:abc451d82b88 142 if (byte == XBUS_PREAMBLE)
Alex Young 3:abc451d82b88 143 {
Alex Young 3:abc451d82b88 144 parser->checksum = 0;
Alex Young 3:abc451d82b88 145 parser->state = XBPS_BusId;
Alex Young 3:abc451d82b88 146 }
Alex Young 3:abc451d82b88 147 break;
Alex Young 3:abc451d82b88 148
Alex Young 3:abc451d82b88 149 case XBPS_BusId:
Alex Young 3:abc451d82b88 150 parser->checksum += byte;
Alex Young 3:abc451d82b88 151 parser->state = XBPS_MessageId;
Alex Young 3:abc451d82b88 152 break;
Alex Young 3:abc451d82b88 153
Alex Young 3:abc451d82b88 154 case XBPS_MessageId:
Alex Young 3:abc451d82b88 155 parser->checksum += byte;
Alex Young 14:155f9a55ec51 156 parser->currentMessage.mid = (enum XsMessageId)byte;
Alex Young 3:abc451d82b88 157 parser->state = XBPS_Length;
Alex Young 3:abc451d82b88 158 break;
Alex Young 3:abc451d82b88 159
Alex Young 3:abc451d82b88 160 case XBPS_Length:
Alex Young 3:abc451d82b88 161 parser->checksum += byte;
Alex Young 3:abc451d82b88 162 if (byte == XBUS_NO_PAYLOAD)
Alex Young 3:abc451d82b88 163 {
Alex Young 14:155f9a55ec51 164 parser->currentMessage.length = byte;
Alex Young 17:680f28e00d17 165 parser->currentMessage.data = NULL;
Alex Young 3:abc451d82b88 166 parser->state = XBPS_Checksum;
Alex Young 3:abc451d82b88 167 }
Alex Young 3:abc451d82b88 168 else if (byte < XBUS_EXTENDED_LENGTH)
Alex Young 3:abc451d82b88 169 {
Alex Young 14:155f9a55ec51 170 parser->currentMessage.length = byte;
Alex Young 3:abc451d82b88 171 prepareForPayload(parser);
Alex Young 3:abc451d82b88 172 parser->state = XBPS_Payload;
Alex Young 3:abc451d82b88 173 }
Alex Young 3:abc451d82b88 174 else
Alex Young 3:abc451d82b88 175 {
Alex Young 3:abc451d82b88 176 parser->state = XBPS_ExtendedLengthMsb;
Alex Young 3:abc451d82b88 177 }
Alex Young 3:abc451d82b88 178 break;
Alex Young 3:abc451d82b88 179
Alex Young 3:abc451d82b88 180 case XBPS_ExtendedLengthMsb:
Alex Young 3:abc451d82b88 181 parser->checksum += byte;
Alex Young 14:155f9a55ec51 182 parser->currentMessage.length = ((uint16_t)byte) << 8;
Alex Young 3:abc451d82b88 183 parser->state = XBPS_ExtendedLengthLsb;
Alex Young 3:abc451d82b88 184 break;
Alex Young 3:abc451d82b88 185
Alex Young 3:abc451d82b88 186 case XBPS_ExtendedLengthLsb:
Alex Young 3:abc451d82b88 187 parser->checksum += byte;
Alex Young 14:155f9a55ec51 188 parser->currentMessage.length |= byte;
Alex Young 3:abc451d82b88 189 prepareForPayload(parser);
Alex Young 3:abc451d82b88 190 parser->state = XBPS_Payload;
Alex Young 3:abc451d82b88 191 break;
Alex Young 3:abc451d82b88 192
Alex Young 3:abc451d82b88 193 case XBPS_Payload:
Alex Young 3:abc451d82b88 194 parser->checksum += byte;
Alex Young 14:155f9a55ec51 195 if (parser->currentMessage.data)
Alex Young 3:abc451d82b88 196 {
Alex Young 19:46e88d37ecef 197 ((uint8_t*)parser->currentMessage.data)[parser->payloadReceived] = byte;
Alex Young 3:abc451d82b88 198 }
Alex Young 14:155f9a55ec51 199 if (++parser->payloadReceived == parser->currentMessage.length)
Alex Young 3:abc451d82b88 200 {
Alex Young 3:abc451d82b88 201 parser->state = XBPS_Checksum;
Alex Young 3:abc451d82b88 202 }
Alex Young 3:abc451d82b88 203 break;
Alex Young 3:abc451d82b88 204
Alex Young 3:abc451d82b88 205 case XBPS_Checksum:
Alex Young 3:abc451d82b88 206 parser->checksum += byte;
Alex Young 17:680f28e00d17 207 if ((parser->checksum == 0) &&
Alex Young 17:680f28e00d17 208 ((parser->currentMessage.length == 0) ||
Alex Young 17:680f28e00d17 209 parser->currentMessage.data))
Alex Young 3:abc451d82b88 210 {
Alex Young 19:46e88d37ecef 211 parseMessagePayload(parser);
Alex Young 14:155f9a55ec51 212 parser->callbacks.handleMessage(&parser->currentMessage);
Alex Young 3:abc451d82b88 213 }
Alex Young 3:abc451d82b88 214 parser->state = XBPS_Preamble;
Alex Young 3:abc451d82b88 215 break;
Alex Young 3:abc451d82b88 216 }
Alex Young 3:abc451d82b88 217 }
Alex Young 3:abc451d82b88 218
Alex Young 3:abc451d82b88 219 void XbusParser_parseBuffer(struct XbusParser* parser, uint8_t const* buf, size_t bufSize)
Alex Young 3:abc451d82b88 220 {
Alex Young 3:abc451d82b88 221 for (size_t i = 0; i < bufSize; ++i)
Alex Young 3:abc451d82b88 222 {
Alex Young 3:abc451d82b88 223 XbusParser_parseByte(parser, buf[i]);
Alex Young 3:abc451d82b88 224 }
Alex Young 3:abc451d82b88 225 }
Alex Young 3:abc451d82b88 226