Xsens / 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 Alex Young

xbus/xbusparser.c@17:680f28e00d17, 2015-05-19 (annotated)

Committer:
Alex Young
Date:
Tue May 19 14:01:33 2015 +0200
Revision:
17:680f28e00d17
Parent:
14:155f9a55ec51
Child:
19:46e88d37ecef
Better handling of zero length messages



For a zero length message there is no data buffer allocated, so the

message data pointer should be NULL. When checking if the received

message is valid it should have a correct checksum and either zero

length or a valid 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 3:abc451d82b88 18 #include <stdlib.h>
Alex Young 3:abc451d82b88 19
Alex Young 3:abc451d82b88 20 enum XbusParserState
Alex Young 3:abc451d82b88 21 {
Alex Young 3:abc451d82b88 22 XBPS_Preamble, /*!< \brief Looking for preamble. */
Alex Young 3:abc451d82b88 23 XBPS_BusId, /*!< \brief Waiting for bus ID. */
Alex Young 3:abc451d82b88 24 XBPS_MessageId, /*!< \brief Waiting for message ID. */
Alex Young 3:abc451d82b88 25 XBPS_Length, /*!< \brief Waiting for length. */
Alex Young 3:abc451d82b88 26 XBPS_ExtendedLengthMsb, /*!< \brief Waiting for extended length MSB*/
Alex Young 3:abc451d82b88 27 XBPS_ExtendedLengthLsb, /*!< \brief Waiting for extended length LSB*/
Alex Young 3:abc451d82b88 28 XBPS_Payload, /*!< \brief Reading payload. */
Alex Young 3:abc451d82b88 29 XBPS_Checksum /*!< \brief Waiting for checksum. */
Alex Young 3:abc451d82b88 30 };
Alex Young 3:abc451d82b88 31
Alex Young 3:abc451d82b88 32 struct XbusParser
Alex Young 3:abc451d82b88 33 {
Alex Young 3:abc451d82b88 34 struct XbusParserCallback callbacks;
Alex Young 14:155f9a55ec51 35 struct XbusMessage currentMessage;
Alex Young 3:abc451d82b88 36 uint16_t payloadReceived;
Alex Young 3:abc451d82b88 37 uint8_t checksum;
Alex Young 3:abc451d82b88 38 enum XbusParserState state;
Alex Young 3:abc451d82b88 39 };
Alex Young 3:abc451d82b88 40
Alex Young 3:abc451d82b88 41 size_t XbusParser_mem(void)
Alex Young 3:abc451d82b88 42 {
Alex Young 3:abc451d82b88 43 return sizeof(struct XbusParser);
Alex Young 3:abc451d82b88 44 }
Alex Young 3:abc451d82b88 45
Alex Young 3:abc451d82b88 46 struct XbusParser* XbusParser_create(struct XbusParserCallback const* callback)
Alex Young 3:abc451d82b88 47 {
Alex Young 3:abc451d82b88 48 void* mem = malloc(XbusParser_mem());
Alex Young 3:abc451d82b88 49 if (mem)
Alex Young 3:abc451d82b88 50 {
Alex Young 3:abc451d82b88 51 return XbusParser_init(mem, callback);
Alex Young 3:abc451d82b88 52 }
Alex Young 3:abc451d82b88 53 return NULL;
Alex Young 3:abc451d82b88 54 }
Alex Young 3:abc451d82b88 55
Alex Young 3:abc451d82b88 56 void XbusParser_destroy(struct XbusParser* parser)
Alex Young 3:abc451d82b88 57 {
Alex Young 3:abc451d82b88 58 free(parser);
Alex Young 3:abc451d82b88 59 }
Alex Young 3:abc451d82b88 60
Alex Young 3:abc451d82b88 61 struct XbusParser* XbusParser_init(void* parserMem, struct XbusParserCallback const* callback)
Alex Young 3:abc451d82b88 62 {
Alex Young 3:abc451d82b88 63 struct XbusParser* parser = (struct XbusParser*)parserMem;
Alex Young 3:abc451d82b88 64 parser->state = XBPS_Preamble;
Alex Young 3:abc451d82b88 65 parser->callbacks.allocateBuffer = callback->allocateBuffer;
Alex Young 3:abc451d82b88 66 parser->callbacks.handleMessage = callback->handleMessage;
Alex Young 3:abc451d82b88 67 return parser;
Alex Young 3:abc451d82b88 68 }
Alex Young 3:abc451d82b88 69
Alex Young 3:abc451d82b88 70 static void prepareForPayload(struct XbusParser* parser)
Alex Young 3:abc451d82b88 71 {
Alex Young 14:155f9a55ec51 72 parser->currentMessage.data = parser->callbacks.allocateBuffer(parser->currentMessage.length);
Alex Young 3:abc451d82b88 73 parser->payloadReceived = 0;
Alex Young 3:abc451d82b88 74 }
Alex Young 3:abc451d82b88 75
Alex Young 3:abc451d82b88 76 void XbusParser_parseByte(struct XbusParser* parser, const uint8_t byte)
Alex Young 3:abc451d82b88 77 {
Alex Young 3:abc451d82b88 78 switch (parser->state)
Alex Young 3:abc451d82b88 79 {
Alex Young 3:abc451d82b88 80 case XBPS_Preamble:
Alex Young 3:abc451d82b88 81 if (byte == XBUS_PREAMBLE)
Alex Young 3:abc451d82b88 82 {
Alex Young 3:abc451d82b88 83 parser->checksum = 0;
Alex Young 3:abc451d82b88 84 parser->state = XBPS_BusId;
Alex Young 3:abc451d82b88 85 }
Alex Young 3:abc451d82b88 86 break;
Alex Young 3:abc451d82b88 87
Alex Young 3:abc451d82b88 88 case XBPS_BusId:
Alex Young 3:abc451d82b88 89 parser->checksum += byte;
Alex Young 3:abc451d82b88 90 parser->state = XBPS_MessageId;
Alex Young 3:abc451d82b88 91 break;
Alex Young 3:abc451d82b88 92
Alex Young 3:abc451d82b88 93 case XBPS_MessageId:
Alex Young 3:abc451d82b88 94 parser->checksum += byte;
Alex Young 14:155f9a55ec51 95 parser->currentMessage.mid = (enum XsMessageId)byte;
Alex Young 3:abc451d82b88 96 parser->state = XBPS_Length;
Alex Young 3:abc451d82b88 97 break;
Alex Young 3:abc451d82b88 98
Alex Young 3:abc451d82b88 99 case XBPS_Length:
Alex Young 3:abc451d82b88 100 parser->checksum += byte;
Alex Young 3:abc451d82b88 101 if (byte == XBUS_NO_PAYLOAD)
Alex Young 3:abc451d82b88 102 {
Alex Young 14:155f9a55ec51 103 parser->currentMessage.length = byte;
Alex Young 17:680f28e00d17 104 parser->currentMessage.data = NULL;
Alex Young 3:abc451d82b88 105 parser->state = XBPS_Checksum;
Alex Young 3:abc451d82b88 106 }
Alex Young 3:abc451d82b88 107 else if (byte < XBUS_EXTENDED_LENGTH)
Alex Young 3:abc451d82b88 108 {
Alex Young 14:155f9a55ec51 109 parser->currentMessage.length = byte;
Alex Young 3:abc451d82b88 110 prepareForPayload(parser);
Alex Young 3:abc451d82b88 111 parser->state = XBPS_Payload;
Alex Young 3:abc451d82b88 112 }
Alex Young 3:abc451d82b88 113 else
Alex Young 3:abc451d82b88 114 {
Alex Young 3:abc451d82b88 115 parser->state = XBPS_ExtendedLengthMsb;
Alex Young 3:abc451d82b88 116 }
Alex Young 3:abc451d82b88 117 break;
Alex Young 3:abc451d82b88 118
Alex Young 3:abc451d82b88 119 case XBPS_ExtendedLengthMsb:
Alex Young 3:abc451d82b88 120 parser->checksum += byte;
Alex Young 14:155f9a55ec51 121 parser->currentMessage.length = ((uint16_t)byte) << 8;
Alex Young 3:abc451d82b88 122 parser->state = XBPS_ExtendedLengthLsb;
Alex Young 3:abc451d82b88 123 break;
Alex Young 3:abc451d82b88 124
Alex Young 3:abc451d82b88 125 case XBPS_ExtendedLengthLsb:
Alex Young 3:abc451d82b88 126 parser->checksum += byte;
Alex Young 14:155f9a55ec51 127 parser->currentMessage.length |= byte;
Alex Young 3:abc451d82b88 128 prepareForPayload(parser);
Alex Young 3:abc451d82b88 129 parser->state = XBPS_Payload;
Alex Young 3:abc451d82b88 130 break;
Alex Young 3:abc451d82b88 131
Alex Young 3:abc451d82b88 132 case XBPS_Payload:
Alex Young 3:abc451d82b88 133 parser->checksum += byte;
Alex Young 14:155f9a55ec51 134 if (parser->currentMessage.data)
Alex Young 3:abc451d82b88 135 {
Alex Young 14:155f9a55ec51 136 parser->currentMessage.data[parser->payloadReceived] = byte;
Alex Young 3:abc451d82b88 137 }
Alex Young 14:155f9a55ec51 138 if (++parser->payloadReceived == parser->currentMessage.length)
Alex Young 3:abc451d82b88 139 {
Alex Young 3:abc451d82b88 140 parser->state = XBPS_Checksum;
Alex Young 3:abc451d82b88 141 }
Alex Young 3:abc451d82b88 142 break;
Alex Young 3:abc451d82b88 143
Alex Young 3:abc451d82b88 144 case XBPS_Checksum:
Alex Young 3:abc451d82b88 145 parser->checksum += byte;
Alex Young 17:680f28e00d17 146 if ((parser->checksum == 0) &&
Alex Young 17:680f28e00d17 147 ((parser->currentMessage.length == 0) ||
Alex Young 17:680f28e00d17 148 parser->currentMessage.data))
Alex Young 3:abc451d82b88 149 {
Alex Young 14:155f9a55ec51 150 parser->callbacks.handleMessage(&parser->currentMessage);
Alex Young 3:abc451d82b88 151 }
Alex Young 3:abc451d82b88 152 parser->state = XBPS_Preamble;
Alex Young 3:abc451d82b88 153 break;
Alex Young 3:abc451d82b88 154 }
Alex Young 3:abc451d82b88 155 }
Alex Young 3:abc451d82b88 156
Alex Young 3:abc451d82b88 157 void XbusParser_parseBuffer(struct XbusParser* parser, uint8_t const* buf, size_t bufSize)
Alex Young 3:abc451d82b88 158 {
Alex Young 3:abc451d82b88 159 for (size_t i = 0; i < bufSize; ++i)
Alex Young 3:abc451d82b88 160 {
Alex Young 3:abc451d82b88 161 XbusParser_parseByte(parser, buf[i]);
Alex Young 3:abc451d82b88 162 }
Alex Young 3:abc451d82b88 163 }
Alex Young 3:abc451d82b88 164