diff --git a/core/MyGatewayTransportEthernet.cpp b/core/MyGatewayTransportEthernet.cpp
index 498850f..a6abe91 100644
--- a/core/MyGatewayTransportEthernet.cpp
+++ b/core/MyGatewayTransportEthernet.cpp
@@ -116,9 +116,8 @@ bool gatewayTransportInit() {
 			(void)WiFi.begin(MY_ESP8266_SSID, MY_ESP8266_PASSWORD);
 			while (WiFi.status() != WL_CONNECTED)
 			{
-				delay(500);
+				wait(500);
 				MY_SERIALDEVICE.print(F("."));
-				yield();
 			}
 			MY_SERIALDEVICE.print(F("IP: "));
 			MY_SERIALDEVICE.println(WiFi.localIP());
diff --git a/core/MyGatewayTransportMQTTClient.cpp b/core/MyGatewayTransportMQTTClient.cpp
index ea98f07..339d1aa 100644
--- a/core/MyGatewayTransportMQTTClient.cpp
+++ b/core/MyGatewayTransportMQTTClient.cpp
@@ -88,7 +88,7 @@ bool reconnectMQTT() {
 bool gatewayTransportConnect() {
 	#if defined(MY_GATEWAY_ESP8266)
 		while (WiFi.status() != WL_CONNECTED) {
-			delay(500); // delay calls yield
+			wait(500);
 			MY_SERIALDEVICE.print(".");
 		}
 		MY_SERIALDEVICE.print("IP: ");
diff --git a/core/MyLeds.cpp b/core/MyLeds.cpp
index 4e28903..0da6628 100644
--- a/core/MyLeds.cpp
+++ b/core/MyLeds.cpp
@@ -26,8 +26,7 @@
 static uint8_t countRx;
 static uint8_t countTx;
 static uint8_t countErr;
-static unsigned long prevTime = hwMillis() - LED_PROCESS_INTERVAL_MS;     // Substract some, to make sure leds gets updated on first run.
-
+static unsigned long prevTime;
 
 inline void ledsInit()
 {
@@ -37,16 +36,17 @@ inline void ledsInit()
 	countErr = 0;
 
 	// Setup led pins
-#if defined(MY_DEFAULT_RX_LED_PIN)
-	pinMode(MY_DEFAULT_RX_LED_PIN,  OUTPUT);
-#endif
-#if defined(MY_DEFAULT_TX_LED_PIN)
-	pinMode(MY_DEFAULT_TX_LED_PIN,  OUTPUT);
-#endif
-#if defined(MY_DEFAULT_ERR_LED_PIN)
-	pinMode(MY_DEFAULT_ERR_LED_PIN, OUTPUT);
-#endif
-    ledsProcess();
+	#if defined(MY_DEFAULT_RX_LED_PIN)
+		pinMode(MY_DEFAULT_RX_LED_PIN,  OUTPUT);
+	#endif
+	#if defined(MY_DEFAULT_TX_LED_PIN)
+		pinMode(MY_DEFAULT_TX_LED_PIN,  OUTPUT);
+	#endif
+	#if defined(MY_DEFAULT_ERR_LED_PIN)
+		pinMode(MY_DEFAULT_ERR_LED_PIN, OUTPUT);
+	#endif
+	prevTime = hwMillis() - LED_PROCESS_INTERVAL_MS;     // Substract some, to make sure leds gets updated on first run.
+	ledsProcess();
 }
 
 void ledsProcess() {
@@ -54,46 +54,58 @@ void ledsProcess() {
 	if ((hwMillis() - prevTime) < LED_PROCESS_INTERVAL_MS) {
 		return;
 	}
-
 	prevTime = hwMillis();
 
-    uint8_t state;
+	uint8_t state;
 
-    // For an On/Off ratio of 4, the pattern repeated will be [on, on, on, off]
-    // until the counter becomes 0.
-#if defined(MY_DEFAULT_RX_LED_PIN)
-    state = (countRx & (LED_ON_OFF_RATIO-1)) ? LED_ON : LED_OFF;
-    hwDigitalWrite(MY_DEFAULT_RX_LED_PIN, state);
-    if (countRx) {
+	// For an On/Off ratio of 4, the pattern repeated will be [on, on, on, off]
+	// until the counter becomes 0.
+	#if defined(MY_DEFAULT_RX_LED_PIN)
+		if (countRx) {
 			--countRx;
 		}
-#endif
+		state = (countRx & (LED_ON_OFF_RATIO-1)) ? LED_ON : LED_OFF;
+		hwDigitalWrite(MY_DEFAULT_RX_LED_PIN, state);
+	#endif
 
-#if defined(MY_DEFAULT_TX_LED_PIN)
-    state = (countTx & (LED_ON_OFF_RATIO-1)) ? LED_ON : LED_OFF;
-    hwDigitalWrite(MY_DEFAULT_TX_LED_PIN, state);
-    if (countTx) {
+	#if defined(MY_DEFAULT_TX_LED_PIN)
+		if (countTx) {
 			--countTx;
 		}
-#endif
+		state = (countTx & (LED_ON_OFF_RATIO-1)) ? LED_ON : LED_OFF;
+		hwDigitalWrite(MY_DEFAULT_TX_LED_PIN, state);
+	#endif
 
-#if defined(MY_DEFAULT_ERR_LED_PIN)
-    state = (countErr & (LED_ON_OFF_RATIO-1)) ? LED_ON : LED_OFF;
-    hwDigitalWrite(MY_DEFAULT_ERR_LED_PIN, state);
-    if (countErr) {
+	#if defined(MY_DEFAULT_ERR_LED_PIN)
+		if (countErr) {
 			--countErr;
 		}
-#endif
+		state = (countErr & (LED_ON_OFF_RATIO-1)) ? LED_ON : LED_OFF;
+		hwDigitalWrite(MY_DEFAULT_ERR_LED_PIN, state);
+	#endif
 }
 
 void ledsBlinkRx(uint8_t cnt) {
-  if (!countRx) { countRx = cnt*LED_ON_OFF_RATIO; }
+	if (!countRx) {
+		countRx = cnt*LED_ON_OFF_RATIO;
+	}
+	ledsProcess();
 }
 
 void ledsBlinkTx(uint8_t cnt) {
-  if(!countTx) { countTx = cnt*LED_ON_OFF_RATIO; }
+	if(!countTx) {
+		countTx = cnt*LED_ON_OFF_RATIO;
+	}
+	ledsProcess();
 }
 
 void ledsBlinkErr(uint8_t cnt) {
-  if(!countErr) { countErr = cnt*LED_ON_OFF_RATIO; }
+	if(!countErr) {
+		countErr = cnt*LED_ON_OFF_RATIO;
+	}
+	ledsProcess();
+}
+
+bool ledsBlinking() {
+	return countRx || countTx || countErr;
 }
diff --git a/core/MyLeds.h b/core/MyLeds.h
index e6f982d..e0fb121 100644
--- a/core/MyLeds.h
+++ b/core/MyLeds.h
@@ -43,6 +43,11 @@
 	void ledsBlinkTx(uint8_t cnt);
 	void ledsBlinkErr(uint8_t cnt);
 	void ledsProcess(); // do the actual blinking
+	/**
+	 * Test if any LED is currently blinking.
+	 * @return true when one or more LEDs are blinking, false otherwise.
+	 */
+	bool ledsBlinking();
 
 #else
 	// Remove led functions if feature is disabled
diff --git a/core/MySensorsCore.cpp b/core/MySensorsCore.cpp
index 8586b2a..0c87a84 100644
--- a/core/MySensorsCore.cpp
+++ b/core/MySensorsCore.cpp
@@ -38,11 +38,7 @@ bool _nodeRegistered = false;
 void (*_timeCallback)(unsigned long); // Callback for requested time messages
 
 void _process(void) {
-	hwWatchdogReset();
-
-	#if defined (MY_DEFAULT_TX_LED_PIN) || defined(MY_DEFAULT_RX_LED_PIN) || defined(MY_DEFAULT_ERR_LED_PIN)
-		ledsProcess();
-	#endif
+	doYield();
 
 	#if defined(MY_INCLUSION_MODE_FEATURE)
 		inclusionProcess();
@@ -64,10 +60,7 @@ void _process(void) {
 
 void _infiniteLoop(void) {
 	while(1) {
-		yield();
-		#if defined (MY_DEFAULT_TX_LED_PIN) || defined(MY_DEFAULT_RX_LED_PIN) || defined(MY_DEFAULT_ERR_LED_PIN)
-			ledsProcess();
-		#endif
+		doYield();
 		#if defined(__linux__)
 			exit(1);
 		#endif
@@ -111,9 +104,8 @@ void _begin(void) {
 		hwWriteConfig(EEPROM_PARENT_NODE_ID_ADDRESS, MY_PARENT_NODE_ID);
 		transportInitialise();
 		while (!isTransportReady()) {
-			hwWatchdogReset();
 			transportProcess();
-			yield();
+			doYield();
 		}
 	#endif
 
@@ -447,7 +439,6 @@ void wait(const uint32_t waitingMS) {
 	const uint32_t enteringMS = hwMillis();
 	while (hwMillis() - enteringMS < waitingMS) {
 		_process();
-		yield();
 	}
 }
 
@@ -458,12 +449,21 @@ bool wait(const uint32_t waitingMS, const uint8_t cmd, const uint8_t msgtype) {
 	bool expectedResponse = false;
 	while ( (hwMillis() - enteringMS < waitingMS) && !expectedResponse ) {
 		_process();
-		yield();
 		expectedResponse = (mGetCommand(_msg) == cmd && _msg.type == msgtype);
 	}
 	return expectedResponse;
 }
 
+void doYield(void) {
+	hwWatchdogReset();
+
+	yield();
+
+	#if defined (MY_DEFAULT_TX_LED_PIN) || defined(MY_DEFAULT_RX_LED_PIN) || defined(MY_DEFAULT_ERR_LED_PIN)
+		ledsProcess();
+	#endif
+}
+
 int8_t _sleep(const uint32_t sleepingMS, const bool smartSleep, const uint8_t interrupt1, const uint8_t mode1, const uint8_t interrupt2, const uint8_t mode2) {
 	debug(PSTR("MCO:SLP:MS=%lu,SMS=%d,I1=%d,M1=%d,I2=%d,M2=%d\n"), sleepingMS, smartSleep, interrupt1, mode1, interrupt2, mode2);
 	// OTA FW feature: do not sleep if FW update ongoing
@@ -495,7 +495,6 @@ int8_t _sleep(const uint32_t sleepingMS, const bool smartSleep, const uint8_t in
 				uint32_t sleepDeltaMS = 0;
 				while (!isTransportReady() && (sleepDeltaMS < sleepingTimeMS) && (sleepDeltaMS < MY_SLEEP_TRANSPORT_RECONNECT_TIMEOUT_MS)) {
 					_process();
-					yield();
 					sleepDeltaMS = hwMillis() - sleepEnterMS;
 				}
 				// sleep remainder
@@ -521,6 +520,13 @@ int8_t _sleep(const uint32_t sleepingMS, const bool smartSleep, const uint8_t in
 			transportPowerDown();
 		#endif
 
+		#if defined (MY_DEFAULT_TX_LED_PIN) || defined(MY_DEFAULT_RX_LED_PIN) || defined(MY_DEFAULT_ERR_LED_PIN)
+			// Wait until leds finish their blinking pattern
+			while (ledsBlinking()) {
+				doYield();
+			}
+		#endif
+
 		setIndication(INDICATION_SLEEP);
 
 		int8_t result = MY_SLEEP_NOT_POSSIBLE;	// default
@@ -581,7 +587,7 @@ void nodeLock(const char* str) {
 	while (1) {
 		setIndication(INDICATION_ERR_LOCKED);
 		debug(PSTR("MCO:NLK:NODE LOCKED. TO UNLOCK, GND PIN %d AND RESET\n"), MY_NODE_UNLOCK_PIN);
-		yield();
+		doYield();
 		(void)_sendRoute(build(_msgTmp, GATEWAY_ADDRESS, NODE_SENSOR_ID,C_INTERNAL, I_LOCKED).set(str));
 		#if defined(MY_SENSOR_NETWORK)
 			transportPowerDown();
diff --git a/core/MySensorsCore.h b/core/MySensorsCore.h
index 56f04a1..094f370 100644
--- a/core/MySensorsCore.h
+++ b/core/MySensorsCore.h
@@ -246,6 +246,13 @@ void wait(const uint32_t waitingMS);
  */
 bool wait(const uint32_t waitingMS, const uint8_t cmd, const uint8_t msgtype);
 
+/**
+ * Function to allow scheduler to do some work.
+ * @remark Internally it will call yield, kick the watchdog and update led states.
+ */
+void doYield(void);
+
+
 /**
  * Sleep (PowerDownMode) the MCU and radio. Wake up on timer.
  * @param sleepingMS Number of milliseconds to sleep.
diff --git a/core/MyTransport.cpp b/core/MyTransport.cpp
index 129715d..238918b 100644
--- a/core/MyTransport.cpp
+++ b/core/MyTransport.cpp
@@ -497,7 +497,7 @@ bool transportWait(const uint32_t ms, const uint8_t cmd, const uint8_t msgtype){
 	while ((hwMillis() - enter < ms) && !expectedResponse) {
 		// process incoming messages
 		transportProcessFIFO();
-		yield();	// process esp8266 stack, ignored for AVR
+		doYield();
 		expectedResponse = (mGetCommand(_msg) == cmd && _msg.type == msgtype);
 	}
 	return expectedResponse;