rik freije / Mbed 2 deprecated MTi-1_rikbeun

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/xbusmessage.c

Committer:
Alex Young
Date:
2015-05-13
Revision:
10:18a6661b7e59
Child:
16:4bdcdac223d8

File content as of revision 10:18a6661b7e59:

/*!
 * \file
 * \copyright
 * Copyright (C) Xsens Technologies B.V., 2015.  All rights reserved.
 *
 * This source code is intended for use only by Xsens Technologies BV and
 * those that have explicit written permission to use it from
 * Xsens Technologies BV.
 *
 * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
 * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
 * PARTICULAR PURPOSE.
 */

#include "xbusmessage.h"
#include "xbusdef.h"

size_t XbusMessage_format(uint8_t* raw, struct XbusMessage const* message)
{
	uint8_t* dptr = raw;
	*dptr++ = XBUS_PREAMBLE;

	*dptr = XBUS_BUS_ID_STANDALONE;
	uint8_t checksum = 0;
	checksum -= *dptr++;

	*dptr = message->mid;
	checksum -= *dptr++;

	if (message->length < XBUS_EXTENDED_LENGTH)
	{
		*dptr = message->length;
		checksum -= *dptr++;
	}
	else
	{
		*dptr = XBUS_EXTENDED_LENGTH;
		checksum -= *dptr++;
		*dptr = message->length >> 8;
		checksum -= *dptr++;
		*dptr = message->length & 0xFF;
		checksum -= *dptr++;
	}

	uint8_t* sptr = message->data;
	for (int i = 0; i < message->length; ++i)
	{
		*dptr = *sptr++;
		checksum -= *dptr++;
	}
	*dptr++ = checksum;

	return dptr - raw;
}