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