Diff: main.cpp

Revision:

35:7e519b88c610

Parent:

34:3d7a6519a256

Child:

36:21198d933917

--- a/main.cpp	Thu May 21 12:19:50 2015 +0200
+++ b/main.cpp	Thu May 21 12:20:39 2015 +0200
@@ -9,6 +9,12 @@
 
 static Serial pc(PA_2, PA_3);
 static Serial mt(PB_9, PB_8);
+/*!
+ * \brief MT reset line.
+ *
+ * MT is held in reset on startup.
+ */
+static DigitalOut mtReset(PA_10, 0);
 static XbusParser* xbusParser;
 
 MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool;
@@ -281,6 +287,40 @@
 	return false;
 }
 
+/*!
+ * \brief Wait for a wakeup message from the MTi.
+ * \return true if wakeup received within 1 second, 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)
+{
+	osEvent ev = g_responseQueue.get(1000);
+	if (ev.status == osEventMessage)
+	{
+		XbusMessage const* m = (XbusMessage const*)ev.value.p;
+		XbusMessageMemoryManager janitor(m);
+		return m->mid == XMID_Wakeup;
+	}
+	return false;
+}
+
+/*!
+ * \brief Releases the MTi reset line and waits for a wakeup message.
+ */
+static void wakeupMotionTracker(void)
+{
+	mtReset.write(1); // Release MT from reset.
+	if (waitForWakeup())
+	{
+		XbusMessage ack = {XMID_WakeupAck};
+		sendMessage(&ack);
+	}
+}
+
 int main(void)
 {
 	XbusParserCallback xbusCallback = {};
@@ -290,6 +330,7 @@
 
 	xbusParser = XbusParser_create(&xbusCallback);
 	configureSerialPorts();
+	wakeupMotionTracker();
 	if (configureMotionTracker())
 	{
 		for (;;)