File content as of revision 24:2cc49dc854e3:

#include "mbed.h"
#include "xbusparser.h"
#include "xbusmessage.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 sendCommand(XsMessageId cmdId)
{
	uint8_t buf[8];
	XbusMessage m = {cmdId};
	size_t rawLength = XbusMessage_format(buf, &m);
	for (size_t i = 0; i < rawLength; ++i)
	{
		mt.putc(buf[i]);
	}
}

static void handlePcCommand(char cmd)
{
	switch (cmd)
	{
		case 'c':
			sendCommand(XMID_GotoConfig);
			break;

		case 'm':
			sendCommand(XMID_GotoMeasurement);
			break;

		case 'd':
			sendCommand(XMID_ReqDid);
			break;

		case 'o':
			sendCommand(XMID_ReqOutputConfig);
			break;
	}
}

static void pcHandler(void)
{
	while (pc.readable())
	{
		handlePcCommand(pc.getc());
	}
}

static void handleDataMessage(struct XbusMessage const* message)
{
	pc.printf("MTData2:");
	uint16_t counter;
	if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
	{
		pc.printf(" Packet counter: %5d", counter);
	}
	float ori[4];
	if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
	{
		pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1],
				ori[2], ori[3]);
	}
	uint32_t status;
	if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
	{
		pc.printf(" Status:%X", status);
	}
	pc.printf("\n");
}

static void mtMessageHandler(struct XbusMessage const* message)
{
	if (message->mid == XMID_MtData2)
	{
		handleDataMessage(message);
	}
	else if (message->mid == XMID_DeviceId)
	{
		pc.printf("Device ID: %8X\n", *(uint32_t*)message->data);
	}
	else if (message->mid == XMID_OutputConfig)
	{
		pc.printf("Output configuration:\n");
		struct OutputConfiguration* conf = (struct OutputConfiguration*)message->data;
		for (int i = 0; i < message->length; ++i)
		{
			pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
			++conf;
		}
	}
	else
	{
		pc.printf("Received Xbus message. MID=%X, length=%d\n", message->mid, message->length);
	}
}

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

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

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

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

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