diff --git a/grbl/protocol.c b/grbl/protocol.c
index 5e28e504..278e4da9 100644
--- a/grbl/protocol.c
+++ b/grbl/protocol.c
@@ -173,7 +173,7 @@ void protocol_main_loop()
     // idle lock is off?
     if (settings.stepper_idle_lock_time == 255) {
     	// Will I do a block in next loop?
-    	if (plan_get_current_block() == NULL) {
+    	if (sys.state == STATE_IDLE) {
     		// Last waiting loop. so disable stepper
     		if (stepper_sleep == 1) {
     			st_disable_set(true);