Diff: main.cpp

Revision:

29:d9310e7b58b5

Parent:

26:665d3624f9ab

Child:

31:ce1ea9ae861e

--- a/main.cpp	Wed May 20 16:46:54 2015 +0200
+++ b/main.cpp	Wed May 20 16:57:39 2015 +0200
@@ -53,6 +53,40 @@
 	g_messagePool.free((XbusMessage*)m);
 }
 
+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};
@@ -60,33 +94,7 @@
 
 	if (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;
-
-
-			default:
-				pc.printf("Received response MID=%X, length=%d\n", response->mid, response->length);
-				break;
-		}
+		dumpResponse(response);
 		freeMessage(response);
 	}
 	else
@@ -164,6 +172,85 @@
 	mt.attach(mtLowLevelHandler, Serial::RxIrq);
 }
 
+static uint32_t readDeviceId(void)
+{
+	XbusMessage reqDid = {XMID_ReqDid};
+	XbusMessage const* didRsp = doTransaction(&reqDid);
+	uint32_t deviceId = 0;
+	if (didRsp)
+	{
+		if (didRsp->mid == XMID_DeviceId)
+		{
+			deviceId = *(uint32_t*)didRsp->data;
+		}
+		freeMessage(didRsp);
+	}
+	return deviceId;
+}
+
+static bool configureMotionTracker(void)
+{
+	uint32_t deviceId = readDeviceId();
+	uint8_t deviceType = (deviceId >> 24) & 0x0F;
+
+	if (deviceId)
+	{
+		pc.printf("Found MTi-%d\n", deviceType);
+
+		OutputConfiguration conf[5] = {
+			{XDI_PacketCounter, 65535},
+			{XDI_SampleTimeFine, 65535}
+		};
+		if (deviceType == 1)
+		{
+			conf[2].dtype = XDI_Acceleration;
+			conf[2].freq = 100;
+			conf[3].dtype = XDI_RateOfTurn;
+			conf[3].freq = 100;
+			conf[4].dtype = XDI_MagneticField;
+			conf[4].freq = 100;
+		}
+		else
+		{
+			conf[2].dtype = XDI_Quaternion;
+			conf[2].freq = 100;
+			conf[3].dtype = XDI_StatusWord;
+			conf[3].freq = 65535;
+		}
+
+		XbusMessage outputConfMsg = {XMID_SetOutputConfig, 5, &conf};
+		XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg);
+		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;
+				}
+			}
+			else
+			{
+				dumpResponse(outputConfRsp);
+			}
+			freeMessage(outputConfRsp);
+		}
+		else
+		{
+			pc.printf("Failed to set output configuration.\n");
+		}
+
+		return true;
+	}
+	else
+	{
+		return false;
+	}
+}
+
 int main(void)
 {
 	XbusParserCallback xbusCallback = {};
@@ -173,12 +260,19 @@
 
 	xbusParser = XbusParser_create(&xbusCallback);
 	configureSerialPorts();
-
-	for (;;)
+	if (configureMotionTracker())
 	{
-		while (pc.readable())
+		for (;;)
 		{
-			handlePcCommand(pc.getc());
+			while (pc.readable())
+			{
+				handlePcCommand(pc.getc());
+			}
 		}
 	}
+	else
+	{
+		pc.printf("Failed to configure motion tracker.\n");
+		return -1;
+	}
 }