File content as of revision 7:c913a7cd5231:

#include "mbed.h"
#include "xbusparser.h"
#include "mtdata2.h"

static Serial pc(PA_2, PA_3);
static Serial mt(PB_9, PB_8);
static XbusParser* xbusParser;
static uint8_t rxBuffer[256];

static void* allocateBuffer(size_t bufSize)
{
	return bufSize < sizeof(rxBuffer) ? rxBuffer : NULL;
}

static void mtLowLevelHandler(void)
{
	while (mt.readable())
	{
		XbusParser_parseByte(xbusParser, mt.getc());
	}
}

static void mtDataHandler(uint8_t mid, uint16_t dataLength, uint8_t const* data)
{
	if (mid == MTDATA2_MESSAGE_ID)
	{
		pc.printf("Data packet received\n");
		uint16_t counter;
		if (MtData2_getItem(&counter, XDI_PacketCounter, data, dataLength))
		{
			pc.printf("\tPacket counter:\t%d\n", counter);
		}
		float ori[4];
		if (MtData2_getItem(ori, XDI_Quaternion, data, dataLength))
		{
			pc.printf("\tOrientation\t%.3f, %.3f, %.3f, %.3f\n", ori[0], ori[1],
					ori[2], ori[3]);
		}
		uint32_t status;
		if (MtData2_getItem(&status, XDI_StatusWord, data, dataLength))
		{
			pc.printf("\tFilter status:\t%d\n", status);
		}
		pc.printf("\n\n");
	}
	else
	{
		pc.printf("Received Xbus message. MID=%X, length=%d\n", mid, dataLength);
	}
}


static void configureSerialPorts(void)
{
	pc.baud(921600);
	pc.format(8, Serial::None, 2);

	mt.baud(921600);
	mt.format(8, Serial::None, 2);
	mt.attach(mtLowLevelHandler, Serial::RxIrq);
}

int main(void)
{
	XbusParserCallback xbusCallback = {};
	xbusCallback.allocateBuffer = allocateBuffer;
	xbusCallback.handleMessage = mtDataHandler;

	xbusParser = XbusParser_create(&xbusCallback);
	configureSerialPorts();

	for (;;)
	{
		sleep();
	}
}