File content as of revision 33:542e4b568631:

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

#define MEMORY_POOL_SIZE (4)
#define RESPONSE_QUEUE_SIZE (1)
#define MAX_XBUS_DATA_SIZE (128)

static Serial pc(PA_2, PA_3);
static Serial mt(PB_9, PB_8);
static XbusParser* xbusParser;

MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool;
MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool;
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);
}

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

XbusMessage const* doTransaction(XbusMessage* m)
{
	uint8_t buf[64];
	size_t rawLength = XbusMessage_format(buf, m);
	for (size_t i = 0; i < rawLength; ++i)
	{
		mt.putc(buf[i]);
	}

	osEvent ev = g_responseQueue.get(500);
	return ev.status == osEventMessage ? (XbusMessage*)ev.value.p : NULL;
}

/*!
 * \brief RAII object to manage message memory deallocation.
 *
 * Will automatically free the memory used by a XbusMessage when going out
 * of scope.
 */
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 dumpResponse(XbusMessage const* response)
{
	switch (response->mid)
	{
		case XMID_GotoConfigAck:
			pc.printf("Device went to config mode\n");
			break;

		case XMID_DeviceId:
			pc.printf("Device ID: %08X\n", *(uint32_t*)response->data);
			break;

		case XMID_OutputConfig:
			{
				pc.printf("Output configuration\n");
				OutputConfiguration* conf = (OutputConfiguration*)response->data;
				for (int i = 0; i < response->length; ++i)
				{
					pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
					++conf;
				}
			}
			break;

		case XMID_Error:
			pc.printf("Device error!");
			break;

		default:
			pc.printf("Received response MID=%X, length=%d\n", response->mid, response->length);
			break;
	}
}

static void sendCommand(XsMessageId cmdId)
{
	XbusMessage m = {cmdId};
	XbusMessage const* response = doTransaction(&m);
	XbusMessageMemoryManager janitor(response);

	if (response)
	{
		dumpResponse(response);
	}
	else
	{
		pc.printf("Timeout waiting for response.\n");
	}
}

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 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");
	deallocateMessageData(message->data);
}

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

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);
}

static uint32_t readDeviceId(void)
{
	XbusMessage reqDid = {XMID_ReqDid};
	XbusMessage const* didRsp = doTransaction(&reqDid);
	XbusMessageMemoryManager janitor(didRsp);
	uint32_t deviceId = 0;
	if (didRsp)
	{
		if (didRsp->mid == XMID_DeviceId)
		{
			deviceId = *(uint32_t*)didRsp->data;
		}
	}
	return deviceId;
}

static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements)
{
	XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf};
	XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg);
	XbusMessageMemoryManager janitor(outputConfRsp);
	if (outputConfRsp)
	{
		if (outputConfRsp->mid == XMID_OutputConfig)
		{
			pc.printf("Output configuration set to:\n");
			OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data;
			for (int i = 0; i < outputConfRsp->length; ++i)
			{
				pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
				++conf;
			}
			return true;
		}
		else
		{
			dumpResponse(outputConfRsp);
		}
	}
	else
	{
		pc.printf("Failed to set output configuration.\n");
	}
	return false;
}

static bool configureMotionTracker(void)
{
	uint32_t deviceId = readDeviceId();

	if (deviceId)
	{
		uint8_t deviceType = (deviceId >> 24) & 0x0F;
		pc.printf("Found MTi-%d\n", deviceType);

		if (deviceType == 1)
		{
			OutputConfiguration conf[] = {
				{XDI_PacketCounter, 65535},
				{XDI_SampleTimeFine, 65535},
				{XDI_Acceleration, 100},
				{XDI_RateOfTurn, 100},
				{XDI_MagneticField, 100}
			};
			return setOutputConfiguration(conf,
					sizeof(conf) / sizeof(OutputConfiguration));
		}
		else
		{
			OutputConfiguration conf[] = {
				{XDI_PacketCounter, 65535},
				{XDI_SampleTimeFine, 65535},
				{XDI_Quaternion, 100},
				{XDI_StatusWord, 65535}
			};
			return setOutputConfiguration(conf,
					sizeof(conf) / sizeof(OutputConfiguration));
		}
	}

	return false;
}

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

	xbusParser = XbusParser_create(&xbusCallback);
	configureSerialPorts();
	if (configureMotionTracker())
	{
		for (;;)
		{
			while (pc.readable())
			{
				handlePcCommand(pc.getc());
			}
		}
	}
	else
	{
		pc.printf("Failed to configure motion tracker.\n");
		return -1;
	}
}