From a876b320bcad5c29a2cf931b6be68636c5b5f162 Mon Sep 17 00:00:00 2001
From: tekka007 <myscontroller@gmail.com>
Date: Thu, 11 Feb 2016 21:56:10 +0100
Subject: [PATCH] Implement conditional Wait() until time passed or specific
 message received

---
 libraries/MySensors/core/MySensorCore.cpp | 13 +++++++++++++
 libraries/MySensors/core/MySensorCore.h   | 11 +++++++++++
 libraries/MySensors/core/MyTransport.cpp  |  4 ++--
 3 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/libraries/MySensors/core/MySensorCore.cpp b/libraries/MySensors/core/MySensorCore.cpp
index f74cd13..39ab3b3 100644
--- a/libraries/MySensors/core/MySensorCore.cpp
+++ b/libraries/MySensors/core/MySensorCore.cpp
@@ -338,6 +338,19 @@ void wait(unsigned long ms) {
 	}
 }
 
+void wait(unsigned long ms, uint8_t cmd, uint8_t msgtype) {
+	unsigned long enter = hwMillis();
+	// invalidate msg type
+	_msg.type = !msgtype;
+	while ( (hwMillis() - enter < ms) && !(mGetCommand(_msg)==cmd && _msg.type==msgtype) ) {
+		_process();
+		#if defined(MY_GATEWAY_ESP8266)
+			yield();
+		#endif
+	}
+}
+
+
 int8_t sleep(unsigned long ms) {
 	#if defined(MY_OTA_FIRMWARE_FEATURE)
 	if (_fwUpdateOngoing) {
diff --git a/libraries/MySensors/core/MySensorCore.h b/libraries/MySensors/core/MySensorCore.h
index d8cac58..b188174 100644
--- a/libraries/MySensors/core/MySensorCore.h
+++ b/libraries/MySensors/core/MySensorCore.h
@@ -168,6 +168,17 @@ uint8_t loadState(uint8_t pos);
  */
 void wait(unsigned long ms);
 
+/**
+ * Wait for a specified amount of time to pass or until specified message received.  Keeps process()ing.
+ * This does not power-down the radio nor the Arduino.
+ * Because this calls process() in a loop, it is a good way to wait
+ * in your loop() on a repeater node or sensor that listens to messages.
+ * @param ms Number of milliseconds to sleep.
+ * @param cmd Command of incoming message.
+ * @param msgtype Message type.
+ */
+void wait(unsigned long ms, uint8_t cmd, uint8_t msgtype);
+
 /**
  * Sleep (PowerDownMode) the MCU and radio. Wake up on timer.
  * @param ms Number of milliseconds to sleep.
diff --git a/libraries/MySensors/core/MyTransport.cpp b/libraries/MySensors/core/MyTransport.cpp
index f0a2723..e975f8b 100644
--- a/libraries/MySensors/core/MyTransport.cpp
+++ b/libraries/MySensors/core/MyTransport.cpp
@@ -443,7 +443,7 @@ void transportRequestNodeId() {
 	transportSetAddress(_nc.nodeId);
 	build(_msg, _nc.nodeId, GATEWAY_ADDRESS, NODE_SENSOR_ID, C_INTERNAL, I_ID_REQUEST, false).set("");
 	transportSendWrite(_nc.parentNodeId, _msg);
-	wait(2000);
+	wait(2000, C_INTERNAL, I_ID_RESPONSE);
 }
 
 void transportPresentNode() {
@@ -467,7 +467,7 @@ void transportPresentNode() {
 			_sendRoute(build(_msg, _nc.nodeId, GATEWAY_ADDRESS, NODE_SENSOR_ID, C_INTERNAL, I_CONFIG, false).set(_nc.parentNodeId));
 
 			// Wait configuration reply.
-			wait(2000);
+			wait(2000, C_INTERNAL, I_CONFIG);
 
 			#ifdef MY_OTA_FIRMWARE_FEATURE
 				RequestFirmwareConfig *reqFWConfig = (RequestFirmwareConfig *)_msg.data;