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