FRDM-KL25Zand Xsens MTi-3
Dependencies: mbed mbed-rtos Xbus
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},