Implement conditional Wait() until time passed or specific message received

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) {

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.

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;