FRDM-KL25Zand Xsens MTi-3
Dependencies: mbed mbed-rtos Xbus
Diff: main.cpp
- Revision:
- 37:3e87bf647c68
- Parent:
- 36:21198d933917
- Child:
- 38:d8d410d1662c
--- a/main.cpp Thu May 21 12:24:01 2015 +0200 +++ b/main.cpp Thu May 21 12:54:35 2015 +0200 @@ -214,7 +214,7 @@ pc.baud(921600); pc.format(8, Serial::None, 2); - mt.baud(921600); + mt.baud(115200); mt.format(8, Serial::None, 2); mt.attach(mtLowLevelHandler, Serial::RxIrq); } @@ -304,16 +304,17 @@ /*! * \brief Wait for a wakeup message from the MTi. - * \return true if wakeup received within 1 second, else false. + * \param timeout Time to wait to receive the wakeup message. + * \return true if wakeup received within timeout, else false. * * The MTi sends a XMID_Wakeup message once it has completed its bootup * procedure. If this is acknowledged by a XMID_WakeupAck message then the MTi * will stay in configuration mode. Otherwise it will automatically enter * measurement mode with the stored output configuration. */ -bool waitForWakeup(void) +bool waitForWakeup(uint32_t timeout) { - osEvent ev = g_responseQueue.get(1000); + osEvent ev = g_responseQueue.get(timeout); if (ev.status == osEventMessage) { XbusMessage const* m = (XbusMessage const*)ev.value.p; @@ -324,15 +325,59 @@ } /*! + * \brief Send wakeup acknowledge message to MTi. + * + * Sending a wakeup acknowledge will cause the device to stay in configuration + * mode instead of automatically transitioning to measurement mode with the + * stored output configuration. + */ +void sendWakeupAck(void) +{ + XbusMessage ack = {XMID_WakeupAck}; + sendMessage(&ack); + pc.printf("Device ready for operation.\n"); +} + +/*! + * \brief Restore communication with the MTi. + * + * On bootup the MTi will listen for a magic byte to signal that it should + * return to default baudrate and output configuration. This can be used to + * recover from a bad or unknown configuration. + */ +void restoreCommunication(void) +{ + pc.printf("Restoring communication with device... "); + mtReset = 0; + Thread::wait(1); + mtReset = 1; + + do + { + mt.putc(0xDE); + } + while (!waitForWakeup(1)); + pc.printf("done\n"); + + sendWakeupAck(); +} + +/*! * \brief Releases the MTi reset line and waits for a wakeup message. + * + * If no wakeup message is received within 1 second the restore communications + * procedure is done to reset the MTi to default baudrate and output configuration. */ static void wakeupMotionTracker(void) { mtReset.write(1); // Release MT from reset. - if (waitForWakeup()) + if (waitForWakeup(1000)) { - XbusMessage ack = {XMID_WakeupAck}; - sendMessage(&ack); + sendWakeupAck(); + } + else + { + restoreCommunication(); } }