File content as of revision 71:9cf0c68ac380:

#include "mbed.h"
#include "rtos.h"
#include "xbusparser.h"
#include "xbusmessage.h"
#include "xsdeviceid.h"
#include "xbusdef.h"

// Select communication interface to use for MTi
#define MTI_USES_UART_INTERFACE

#if !(defined(MTI_USES_I2C_INTERFACE) || defined(MTI_USES_SPI_INTERFACE) || defined(MTI_USES_UART_INTERFACE))
#error "Must select communication interface by defining one of: MTI_USES_I2C_INTERFACE, MTI_USES_SPI_INTERFACE or MTI_USES_UART_INTERFACE"
#endif

#if defined(TARGET_KL25Z)

#define PC_TX 		USBTX
#define PC_RX 		USBRX
#define MT_TX 		PTE0
#define MT_RX 		PTE1


#else

#error "Support for selected mbed platform has not been added."

#endif

#define PC_UART_BAUDRATE (115200)

#define MEMORY_POOL_SIZE (4)

#define RESPONSE_QUEUE_SIZE (1)

#define DATA_QUEUE_SIZE (2)

#define MAX_XBUS_DATA_SIZE (128)

static Serial pc(PC_TX, PC_RX);


#if defined(MTI_USES_UART_INTERFACE)
static RawSerial mt(MT_TX, MT_RX);
#endif


static XbusParser* xbusParser;

MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool;

MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool;

Queue<XbusMessage, DATA_QUEUE_SIZE> g_dataQueue;

Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue;

static void* allocateMessageData(size_t bufSize)
{
	return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL;
}

static void deallocateMessageData(void const* buffer)
{
	g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer);
}


#if defined(MTI_USES_UART_INTERFACE)

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



static void configureMtCommunicationInterface(void)
{
	mt.baud(115200);
	mt.format(8, Serial::None, 1);
	mt.attach(mtLowLevelHandler, Serial::RxIrq);
}

#endif


class XbusMessageMemoryManager
{
	public:
		XbusMessageMemoryManager(XbusMessage const* message)
			: m_message(message)
		{
		}

		~XbusMessageMemoryManager()
		{
			if (m_message)
			{
				if (m_message->data)
					deallocateMessageData(m_message->data);
				g_messagePool.free(const_cast<XbusMessage*>(m_message));
			}
		}

	private:
		XbusMessage const* m_message;
};



static void mtMessageHandler(struct XbusMessage const* message)
{
	XbusMessage* m = g_messagePool.alloc();
	if (m)
	{
		memcpy(m, message, sizeof(XbusMessage));
		if (message->mid == XMID_MtData2)
		{
			g_dataQueue.put(m);
		}
		else
		{
			g_responseQueue.put(m);
		}
	}
	else if (message->data)
	{
		deallocateMessageData(message->data);
	}
}


static void configurePcInterface(void)
{
	pc.baud(PC_UART_BAUDRATE);
	pc.format(8, Serial::None, 1);
}



static void printMessageData(struct XbusMessage const* message)
{
	if (!message)
		return;

	float ori[4];
	if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
	{
		pc.printf("% .3f, % .3f, % .3f, % .3f", ori[0], ori[1], ori[2], ori[3]);
	}
	pc.printf("\r\n");
}

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

	xbusParser = XbusParser_create(&xbusCallback);
	configurePcInterface();
	configureMtCommunicationInterface();

			for (;;)
			{
				osEvent ev = g_dataQueue.get(10);
				if (ev.status == osEventMessage)
				{
					XbusMessage const* data = (XbusMessage const*)ev.value.p;
					XbusMessageMemoryManager janitor(data);
					printMessageData(data);
				}
			}

}