Diff: xbus/xbusmessage.c

Revision:

10:18a6661b7e59

Child:

16:4bdcdac223d8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/xbus/xbusmessage.c	Wed May 13 17:42:38 2015 +0200
@@ -0,0 +1,56 @@
+/*!
+ * \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;
+}
+