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@22:3eab999c5076, 2015-05-19 (annotated)

Committer:
Alex Young
Date:
Tue May 19 16:59:53 2015 +0200
Revision:
22:3eab999c5076
Parent:
19:46e88d37ecef
Child:
23:8171449f0dc3
Add support for reading back output configuration.

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 22:3eab999c5076 86 case XMID_OutputConfig:
Alex Young 19:46e88d37ecef 87 return true;
Alex Young 19:46e88d37ecef 88
Alex Young 19:46e88d37ecef 89 default:
Alex Young 19:46e88d37ecef 90 return false;
Alex Young 19:46e88d37ecef 91 }
Alex Young 19:46e88d37ecef 92 }
Alex Young 19:46e88d37ecef 93
Alex Young 19:46e88d37ecef 94 static void parseMessagePayload(struct XbusParser* parser)
Alex Young 3:abc451d82b88 95 {
Alex Young 19:46e88d37ecef 96 switch (parser->currentMessage.mid)
Alex Young 19:46e88d37ecef 97 {
Alex Young 19:46e88d37ecef 98 case XMID_DeviceId:
Alex Young 19:46e88d37ecef 99 {
Alex Young 19:46e88d37ecef 100 uint32_t* deviceId = parser->callbacks.allocateBuffer(sizeof(uint32_t));
Alex Young 19:46e88d37ecef 101 if (deviceId)
Alex Young 19:46e88d37ecef 102 {
Alex Young 19:46e88d37ecef 103 XbusUtility_readU32(deviceId, parser->rxBuffer);
Alex Young 19:46e88d37ecef 104 parser->currentMessage.data = deviceId;
Alex Young 19:46e88d37ecef 105 }
Alex Young 19:46e88d37ecef 106 else
Alex Young 19:46e88d37ecef 107 {
Alex Young 19:46e88d37ecef 108 parser->currentMessage.data = NULL;
Alex Young 19:46e88d37ecef 109 }
Alex Young 19:46e88d37ecef 110 }
Alex Young 19:46e88d37ecef 111 break;
Alex Young 19:46e88d37ecef 112
Alex Young 22:3eab999c5076 113 case XMID_OutputConfig:
Alex Young 22:3eab999c5076 114 {
Alex Young 22:3eab999c5076 115 uint8_t fields = parser->currentMessage.length / 4;
Alex Young 22:3eab999c5076 116 struct OutputConfiguration* conf = parser->callbacks.allocateBuffer(fields * sizeof(struct OutputConfiguration));
Alex Young 22:3eab999c5076 117 uint8_t const* dptr = parser->rxBuffer;
Alex Young 22:3eab999c5076 118 if (conf)
Alex Young 22:3eab999c5076 119 {
Alex Young 22:3eab999c5076 120 parser->currentMessage.data = conf;
Alex Young 22:3eab999c5076 121 parser->currentMessage.length = fields;
Alex Young 22:3eab999c5076 122
Alex Young 22:3eab999c5076 123 for (int i = 0; i < fields; ++i)
Alex Young 22:3eab999c5076 124 {
Alex Young 22:3eab999c5076 125 dptr = XbusUtility_readU16((uint16_t*)&conf->dtype, dptr);
Alex Young 22:3eab999c5076 126 dptr = XbusUtility_readU16(&conf->freq, dptr);
Alex Young 22:3eab999c5076 127 ++conf;
Alex Young 22:3eab999c5076 128 }
Alex Young 22:3eab999c5076 129 }
Alex Young 22:3eab999c5076 130 else
Alex Young 22:3eab999c5076 131 {
Alex Young 22:3eab999c5076 132 parser->currentMessage.data = NULL;
Alex Young 22:3eab999c5076 133 }
Alex Young 22:3eab999c5076 134 }
Alex Young 22:3eab999c5076 135 break;
Alex Young 22:3eab999c5076 136
Alex Young 19:46e88d37ecef 137 default:
Alex Young 19:46e88d37ecef 138 assert(!canParseMessagePayload(parser->currentMessage.mid));
Alex Young 19:46e88d37ecef 139 break;
Alex Young 19:46e88d37ecef 140 }
Alex Young 19:46e88d37ecef 141 }
Alex Young 19:46e88d37ecef 142
Alex Young 19:46e88d37ecef 143 void prepareForPayload(struct XbusParser* parser)
Alex Young 19:46e88d37ecef 144 {
Alex Young 19:46e88d37ecef 145 parser->currentMessage.data = NULL;
Alex Young 3:abc451d82b88 146 parser->payloadReceived = 0;
Alex Young 19:46e88d37ecef 147
Alex Young 19:46e88d37ecef 148 if (canParseMessagePayload(parser->currentMessage.mid))
Alex Young 19:46e88d37ecef 149 {
Alex Young 19:46e88d37ecef 150 assert(parser->currentMessage.length < XBUS_MAX_MESSAGE_LENGTH);
Alex Young 19:46e88d37ecef 151 if (parser->currentMessage.length < XBUS_MAX_MESSAGE_LENGTH)
Alex Young 19:46e88d37ecef 152 {
Alex Young 19:46e88d37ecef 153 parser->currentMessage.data = parser->rxBuffer;
Alex Young 19:46e88d37ecef 154 }
Alex Young 19:46e88d37ecef 155 }
Alex Young 19:46e88d37ecef 156 else
Alex Young 19:46e88d37ecef 157 {
Alex Young 19:46e88d37ecef 158 parser->currentMessage.data = parser->callbacks.allocateBuffer(parser->currentMessage.length);
Alex Young 19:46e88d37ecef 159 }
Alex Young 3:abc451d82b88 160 }
Alex Young 3:abc451d82b88 161
Alex Young 3:abc451d82b88 162 void XbusParser_parseByte(struct XbusParser* parser, const uint8_t byte)
Alex Young 3:abc451d82b88 163 {
Alex Young 3:abc451d82b88 164 switch (parser->state)
Alex Young 3:abc451d82b88 165 {
Alex Young 3:abc451d82b88 166 case XBPS_Preamble:
Alex Young 3:abc451d82b88 167 if (byte == XBUS_PREAMBLE)
Alex Young 3:abc451d82b88 168 {
Alex Young 3:abc451d82b88 169 parser->checksum = 0;
Alex Young 3:abc451d82b88 170 parser->state = XBPS_BusId;
Alex Young 3:abc451d82b88 171 }
Alex Young 3:abc451d82b88 172 break;
Alex Young 3:abc451d82b88 173
Alex Young 3:abc451d82b88 174 case XBPS_BusId:
Alex Young 3:abc451d82b88 175 parser->checksum += byte;
Alex Young 3:abc451d82b88 176 parser->state = XBPS_MessageId;
Alex Young 3:abc451d82b88 177 break;
Alex Young 3:abc451d82b88 178
Alex Young 3:abc451d82b88 179 case XBPS_MessageId:
Alex Young 3:abc451d82b88 180 parser->checksum += byte;
Alex Young 14:155f9a55ec51 181 parser->currentMessage.mid = (enum XsMessageId)byte;
Alex Young 3:abc451d82b88 182 parser->state = XBPS_Length;
Alex Young 3:abc451d82b88 183 break;
Alex Young 3:abc451d82b88 184
Alex Young 3:abc451d82b88 185 case XBPS_Length:
Alex Young 3:abc451d82b88 186 parser->checksum += byte;
Alex Young 3:abc451d82b88 187 if (byte == XBUS_NO_PAYLOAD)
Alex Young 3:abc451d82b88 188 {
Alex Young 14:155f9a55ec51 189 parser->currentMessage.length = byte;
Alex Young 17:680f28e00d17 190 parser->currentMessage.data = NULL;
Alex Young 3:abc451d82b88 191 parser->state = XBPS_Checksum;
Alex Young 3:abc451d82b88 192 }
Alex Young 3:abc451d82b88 193 else if (byte < XBUS_EXTENDED_LENGTH)
Alex Young 3:abc451d82b88 194 {
Alex Young 14:155f9a55ec51 195 parser->currentMessage.length = byte;
Alex Young 3:abc451d82b88 196 prepareForPayload(parser);
Alex Young 3:abc451d82b88 197 parser->state = XBPS_Payload;
Alex Young 3:abc451d82b88 198 }
Alex Young 3:abc451d82b88 199 else
Alex Young 3:abc451d82b88 200 {
Alex Young 3:abc451d82b88 201 parser->state = XBPS_ExtendedLengthMsb;
Alex Young 3:abc451d82b88 202 }
Alex Young 3:abc451d82b88 203 break;
Alex Young 3:abc451d82b88 204
Alex Young 3:abc451d82b88 205 case XBPS_ExtendedLengthMsb:
Alex Young 3:abc451d82b88 206 parser->checksum += byte;
Alex Young 14:155f9a55ec51 207 parser->currentMessage.length = ((uint16_t)byte) << 8;
Alex Young 3:abc451d82b88 208 parser->state = XBPS_ExtendedLengthLsb;
Alex Young 3:abc451d82b88 209 break;
Alex Young 3:abc451d82b88 210
Alex Young 3:abc451d82b88 211 case XBPS_ExtendedLengthLsb:
Alex Young 3:abc451d82b88 212 parser->checksum += byte;
Alex Young 14:155f9a55ec51 213 parser->currentMessage.length |= byte;
Alex Young 3:abc451d82b88 214 prepareForPayload(parser);
Alex Young 3:abc451d82b88 215 parser->state = XBPS_Payload;
Alex Young 3:abc451d82b88 216 break;
Alex Young 3:abc451d82b88 217
Alex Young 3:abc451d82b88 218 case XBPS_Payload:
Alex Young 3:abc451d82b88 219 parser->checksum += byte;
Alex Young 14:155f9a55ec51 220 if (parser->currentMessage.data)
Alex Young 3:abc451d82b88 221 {
Alex Young 19:46e88d37ecef 222 ((uint8_t*)parser->currentMessage.data)[parser->payloadReceived] = byte;
Alex Young 3:abc451d82b88 223 }
Alex Young 14:155f9a55ec51 224 if (++parser->payloadReceived == parser->currentMessage.length)
Alex Young 3:abc451d82b88 225 {
Alex Young 3:abc451d82b88 226 parser->state = XBPS_Checksum;
Alex Young 3:abc451d82b88 227 }
Alex Young 3:abc451d82b88 228 break;
Alex Young 3:abc451d82b88 229
Alex Young 3:abc451d82b88 230 case XBPS_Checksum:
Alex Young 3:abc451d82b88 231 parser->checksum += byte;
Alex Young 17:680f28e00d17 232 if ((parser->checksum == 0) &&
Alex Young 17:680f28e00d17 233 ((parser->currentMessage.length == 0) ||
Alex Young 17:680f28e00d17 234 parser->currentMessage.data))
Alex Young 3:abc451d82b88 235 {
Alex Young 19:46e88d37ecef 236 parseMessagePayload(parser);
Alex Young 14:155f9a55ec51 237 parser->callbacks.handleMessage(&parser->currentMessage);
Alex Young 3:abc451d82b88 238 }
Alex Young 3:abc451d82b88 239 parser->state = XBPS_Preamble;
Alex Young 3:abc451d82b88 240 break;
Alex Young 3:abc451d82b88 241 }
Alex Young 3:abc451d82b88 242 }
Alex Young 3:abc451d82b88 243
Alex Young 3:abc451d82b88 244 void XbusParser_parseBuffer(struct XbusParser* parser, uint8_t const* buf, size_t bufSize)
Alex Young 3:abc451d82b88 245 {
Alex Young 3:abc451d82b88 246 for (size_t i = 0; i < bufSize; ++i)
Alex Young 3:abc451d82b88 247 {
Alex Young 3:abc451d82b88 248 XbusParser_parseByte(parser, buf[i]);
Alex Young 3:abc451d82b88 249 }
Alex Young 3:abc451d82b88 250 }
Alex Young 3:abc451d82b88 251