KugleFirmware/Libraries/Devices/MTI200/xbusmessage.c
/*!
 * \brief Format a message into the raw Xbus format ready for transmission to
 * a motion tracker.
 */
\brief Format a message into the raw Xbus format ready for transmission to
a motion tracker.
\ brief Format a message into the raw Xbus format ready for transmission to a motion tracker .
size_t XbusMessage_format(uint8_t* raw, struct XbusMessage const* message, enum XbusLowLevelFormat format)
{
	uint8_t* dptr = raw;
	switch (format)
	{
		case XLLF_I2c:
			{
				*dptr++ = XBUS_CONTROL_PIPE;
			}
			break;
		case XLLF_Spi:
			{
				*dptr++ = XBUS_CONTROL_PIPE;
				*dptr++ = 0;
				*dptr++ = 0;
				*dptr++ = 0;
			}
			break;
		case XLLF_Uart:
			{
				*dptr++ = XBUS_PREAMBLE;
				*dptr++ = XBUS_MASTERDEVICE;
			}
			break;
	}
	uint8_t checksum = (uint8_t)(-XBUS_MASTERDEVICE);
	*dptr = message->mid;
	checksum -= *dptr++;
	uint16_t length = messageLength(message);
	if (length < XBUS_EXTENDED_LENGTH)
	{
		*dptr = length;
		checksum -= *dptr++;
	}
	else
	{
		*dptr = XBUS_EXTENDED_LENGTH;
		checksum -= *dptr++;
		*dptr = length >> 8;
		checksum -= *dptr++;
		*dptr = length & 0xFF;
		checksum -= *dptr++;
	}
	formatPayload(dptr, message);
	for (int i = 0; i < length; ++i)
	{
		checksum -= *dptr++;
	}
	*dptr++ = checksum;
	return dptr - raw;
}
size_t XbusMessage_format ( uint8_t * raw , struct XbusMessage const * message , enum XbusLowLevelFormat format ) { uint8_t * dptr = raw ; switch ( format ) { case XLLF_I2c : { * dptr ++ = XBUS_CONTROL_PIPE ; } break ; case XLLF_Spi : { * dptr ++ = XBUS_CONTROL_PIPE ; * dptr ++ = 0 ; * dptr ++ = 0 ; * dptr ++ = 0 ; } break ; case XLLF_Uart : { * dptr ++ = XBUS_PREAMBLE ; * dptr ++ = XBUS_MASTERDEVICE ; } break ; } uint8_t checksum = ( uint8_t ) ( - XBUS_MASTERDEVICE ) ; * dptr = message -> mid ; checksum -= * dptr ++ ; uint16_t length = messageLength ( message ) ; if ( length < XBUS_EXTENDED_LENGTH ) { * dptr = length ; checksum -= * dptr ++ ; } else { * dptr = XBUS_EXTENDED_LENGTH ; checksum -= * dptr ++ ; * dptr = length >> 8 ; checksum -= * dptr ++ ; * dptr = length & 0xFF ; checksum -= * dptr ++ ; } formatPayload ( dptr , message ) ; for ( int i = 0 ; i < length ; ++ i ) { checksum -= * dptr ++ ; } * dptr ++ = checksum ; return dptr - raw ; }
\brief Format a message into the raw Xbus format ready for transmission to
 a motion tracker.
\ brief Format a message into the raw Xbus format ready for transmission to a motion tracker .
// Fill bytes required to allow MT to process data