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();
 	}
 }