Diff: main.cpp

Revision:

32:fafe0f42d82b

Parent:

31:ce1ea9ae861e

Child:

34:3d7a6519a256

diff -r ce1ea9ae861e -r fafe0f42d82b main.cpp
--- a/main.cpp	Thu May 21 11:43:11 2015 +0200
+++ b/main.cpp	Thu May 21 11:47:29 2015 +0200
@@ -209,67 +209,71 @@
 	return deviceId;
 }
 
-static bool configureMotionTracker(void)
+static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements)
 {
-	uint32_t deviceId = readDeviceId();
-	uint8_t deviceType = (deviceId >> 24) & 0x0F;
-
-	if (deviceId)
+	XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf};
+	XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg);
+	XbusMessageMemoryManager janitor(outputConfRsp);
+	if (outputConfRsp)
 	{
-		pc.printf("Found MTi-%d\n", deviceType);
-
-		OutputConfiguration conf[5] = {
-			{XDI_PacketCounter, 65535},
-			{XDI_SampleTimeFine, 65535}
-		};
-		if (deviceType == 1)
+		if (outputConfRsp->mid == XMID_OutputConfig)
 		{
-			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;
+			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
 		{
-			conf[2].dtype = XDI_Quaternion;
-			conf[2].freq = 100;
-			conf[3].dtype = XDI_StatusWord;
-			conf[3].freq = 65535;
+			dumpResponse(outputConfRsp);
 		}
+	}
+	else
+	{
+		pc.printf("Failed to set output configuration.\n");
+	}
+	return false;
+}
 
-		XbusMessage outputConfMsg = {XMID_SetOutputConfig, 5, &conf};
-		XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg);
-		XbusMessageMemoryManager janitor(outputConfRsp);
-		if (outputConfRsp)
+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)
 		{
-			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);
-			}
+			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
 		{
-			pc.printf("Failed to set output configuration.\n");
+			OutputConfiguration conf[] = {
+				{XDI_PacketCounter, 65535},
+				{XDI_SampleTimeFine, 65535},
+				{XDI_Quaternion, 100},
+				{XDI_StatusWord, 65535}
+			};
+			return setOutputConfiguration(conf,
+					sizeof(conf) / sizeof(OutputConfiguration));
 		}
-
-		return true;
 	}
-	else
-	{
-		return false;
-	}
+
+	return false;
 }
 
 int main(void)