Diff: main.cpp

Revision:

40:b77a8c10c76d

Parent:

39:9014c5236864

Child:

41:504b6821a96f

diff -r 9014c5236864 -r b77a8c10c76d main.cpp
--- a/main.cpp	Thu May 21 13:10:39 2015 +0200
+++ b/main.cpp	Thu May 21 14:36:52 2015 +0200
@@ -17,6 +17,7 @@
 #include "rtos.h"
 #include "xbusparser.h"
 #include "xbusmessage.h"
+#include "xsdeviceid.h"
 
 #define MEMORY_POOL_SIZE (4)
 #define RESPONSE_QUEUE_SIZE (1)
@@ -247,10 +248,17 @@
 
 	if (deviceId)
 	{
-		uint8_t deviceType = (deviceId >> 24) & 0x0F;
-		pc.printf("Found MTi-%d\n", deviceType);
+		pc.printf("Found device with ID: %08X.\n", deviceId);
+		if (!XsDeviceId_isMtMk4_X(deviceId))
+		{
+			pc.printf("Device is not an MTi-1 series.\n");
+			return false;
+		}
 
-		if (deviceType == 1)
+		DeviceFunction function = XsDeviceId_getFunction(deviceId);
+		pc.printf("Device is an MTi-%d: %s.\n", function, XsDeviceId_functionDescription(function));
+
+		if (function == DF_IMU)
 		{
 			OutputConfiguration conf[] = {
 				{XDI_PacketCounter, 65535},