HyunGyu Kim / Mbed 2 deprecated MTi-1_example

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@23:8171449f0dc3, 2015-05-20 (annotated)

Committer:
Alex Young
Date:
Wed May 20 12:39:11 2015 +0200
Revision:
23:8171449f0dc3
Parent:
22:3eab999c5076
Child:
25:01356fb59467
Factor out specific message parsing functions.

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