diff --git a/gcode.c b/gcode.c
index 235371c..8e09c53 100755
--- a/gcode.c
+++ b/gcode.c
@@ -72,12 +72,6 @@ void gc_set_current_position(int32_t x, int32_t y, int32_t z)
   gc.position[Z_AXIS] = z/settings.steps_per_mm[Z_AXIS]; 
 }
 
-// Clears and zeros g-code parser position. Called by homing routine.
-void gc_clear_position()
-{
-  clear_vector(gc.position);
-}
-
 static float to_millimeters(float value) 
 {
   return(gc.inches_mode ? (value * MM_PER_INCH) : value);
diff --git a/gcode.h b/gcode.h
index c780c41..6369dfe 100755
--- a/gcode.h
+++ b/gcode.h
@@ -106,7 +106,4 @@ uint8_t gc_execute_line(char *line);
 // Set g-code parser position. Input in steps.
 void gc_set_current_position(int32_t x, int32_t y, int32_t z); 
 
-// Clear g-code parser position
-void gc_clear_position();
-
 #endif
diff --git a/main.c b/main.c
index 32bda81..43abb64 100755
--- a/main.c
+++ b/main.c
@@ -76,12 +76,14 @@ int main(void)
       limits_init();
       st_reset(); // Clear stepper subsystem variables.
 
-      // Set cleared gcode and planner positions to current system position, which is only
-      // cleared upon startup, not a reset/abort. If Grbl does not know or ensure its position,
-      // a feedback message will be sent back to the user to let them know.
-      gc_set_current_position(sys.position[X_AXIS],sys.position[Y_AXIS],sys.position[Z_AXIS]);
-      plan_set_current_position(sys.position[X_AXIS],sys.position[Y_AXIS],sys.position[Z_AXIS]);
-      
+      // Sync cleared gcode and planner positions to current system position, which is only
+      // cleared upon startup, not a reset/abort. If Grbl does not know or can ensure its 
+      // position, a feedback message will be sent back to the user to let them know. Also,
+      // if position is lost and homing is enabled, the axes motions will be locked, and 
+      // user must either perform the homing cycle '$H' or purge the system locks '$P' to 
+      // resume.
+      sys_sync_current_position();
+
       // Reset system variables
       sys.abort = false;
       sys.execute = 0;
@@ -91,7 +93,7 @@ int main(void)
         sys.state = STATE_IDLE;
       }
       if (bit_istrue(settings.flags,BITFLAG_AUTO_START)) { sys.auto_start = true; }
-      
+
       // Execute user startup script
       protocol_execute_startup();
     }
diff --git a/motion_control.c b/motion_control.c
index 7f006ab..a7d4295 100755
--- a/motion_control.c
+++ b/motion_control.c
@@ -220,10 +220,10 @@ void mc_go_home()
   }
 
   // The machine should now be homed and machine zero has been located. Upon completion, 
-  // reset planner and system internal position vectors, but not gcode parser position yet.
-  plan_clear_position();
-  clear_vector_float(sys.position);
-
+  // reset system position and sync internal position vectors.
+  clear_vector_float(sys.position); // Set machine zero
+  sys_sync_current_position();
+  
   // Pull-off all axes from limit switches before continuing motion. This provides some initial
   // clearance off the switches and should also help prevent them from falsely tripping when 
   // hard limits are enabled.
@@ -237,8 +237,8 @@ void mc_go_home()
   st_cycle_start(); // Move it. Nothing should be in the buffer except this motion. 
   plan_synchronize(); // Make sure the motion completes.
   
-  // Explicitly update the gcode parser position since it was circumvented by the pull-off maneuver.
-  gc_set_current_position(sys.position[X_AXIS],sys.position[Y_AXIS],sys.position[Z_AXIS]);
+  // The gcode parser position was circumvented by the pull-off maneuver, so sync position vectors.
+  sys_sync_current_position();
 
   // If hard limits feature enabled, re-enable hard limits interrupt after homing cycle.
   if (bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)) { PCICR |= (1 << LIMIT_INT); }
diff --git a/nuts_bolts.c b/nuts_bolts.c
index dbd1788..921ac9a 100755
--- a/nuts_bolts.c
+++ b/nuts_bolts.c
@@ -19,8 +19,10 @@
   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "nuts_bolts.h"
 #include <util/delay.h>
+#include "nuts_bolts.h"
+#include "gcode.h"
+#include "planner.h"
 
 #define MAX_INT_DIGITS 8 // Maximum number of digits in int32 (and float)
 extern float __floatunsisf (unsigned long);
@@ -137,3 +139,10 @@ void delay_us(uint32_t us)
     }
   }
 }
+
+
+void sys_sync_current_position()
+{
+  plan_set_current_position(sys.position[X_AXIS],sys.position[Y_AXIS],sys.position[Z_AXIS]);
+  gc_set_current_position(sys.position[X_AXIS],sys.position[Y_AXIS],sys.position[Z_AXIS]);
+}
diff --git a/nuts_bolts.h b/nuts_bolts.h
index b894441..7438660 100755
--- a/nuts_bolts.h
+++ b/nuts_bolts.h
@@ -103,4 +103,7 @@ void delay_ms(uint16_t ms);
 // Delays variable-defined microseconds. Compiler compatibility fix for _delay_us().
 void delay_us(uint32_t us);
 
+// Syncs Grbl's gcode and planner position variables with the system position.
+void sys_sync_current_position();
+
 #endif
diff --git a/planner.c b/planner.c
index fac89bf..92f2ac1 100755
--- a/planner.c
+++ b/planner.c
@@ -482,12 +482,6 @@ void plan_set_current_position(int32_t x, int32_t y, int32_t z)
   pl.position[Z_AXIS] = z;
 }
 
-// Clear planner position vector. Called by homing routine.
-void plan_clear_position()
-{
-  clear_vector(pl.position);
-}
-
 // Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail.
 // Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped.
 void plan_cycle_reinitialize(int32_t step_events_remaining) 
diff --git a/planner.h b/planner.h
index a326084..37bd188 100755
--- a/planner.h
+++ b/planner.h
@@ -70,9 +70,6 @@ block_t *plan_get_current_block();
 // Reset the planner position vector (in steps)
 void plan_set_current_position(int32_t x, int32_t y, int32_t z);
 
-// Clear the planner position vector
-void plan_clear_position();
-
 // Reinitialize plan with a partially completed block
 void plan_cycle_reinitialize(int32_t step_events_remaining);
 
diff --git a/protocol.c b/protocol.c
index 5c21d73..ee0926c 100755
--- a/protocol.c
+++ b/protocol.c
@@ -183,25 +183,33 @@ uint8_t protocol_execute_line(char *line)
       // block buffer without having the planner plan them. It would need to manage de/ac-celerations 
       // on its own carefully. This approach could be effective and possibly size/memory efficient.
       case 'S' : // Switch modes
+        if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); }
         // Set helper_var as switch bitmask or clearing flag
         switch (line[++char_counter]) {
-          case 0 : helper_var = 0;  break;
-          case '0' : helper_var = BITFLAG_CHECK_GCODE; break;
+          case '0' : 
+            helper_var = BITFLAG_CHECK_GCODE; 
+            // Sync position vectors if check mode is being disabled. May be different after checking.
+            if (bit_istrue(gc.switches,helper_var)) { sys_sync_current_position(); }
+            break;
           case '1' : helper_var = BITFLAG_DRY_RUN; break;
           case '2' : helper_var = BITFLAG_BLOCK_DELETE; break;
           case '3' : helper_var = BITFLAG_SINGLE_BLOCK; break;
           case '4' : helper_var = BITFLAG_OPT_STOP; break;
           default : return(STATUS_INVALID_STATEMENT);
         }
-        if (helper_var) {
-          if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); }
-          gc.switches ^= helper_var;
-          if (bit_istrue(gc.switches,helper_var)) { report_feedback_message(MESSAGE_SWITCH_ON); }
-          else { report_feedback_message(MESSAGE_SWITCH_OFF); }
-        } else {
-          gc.switches = helper_var; // Clear all switches
-          report_feedback_message(MESSAGE_SWITCHES_CLEARED);
+        gc.switches ^= helper_var;
+        if (bit_istrue(gc.switches,helper_var)) { report_feedback_message(MESSAGE_SWITCH_ON); }
+        else { report_feedback_message(MESSAGE_SWITCH_OFF); }
+        break;
+      case 'P' : // Purge system
+        if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); }
+        if (sys.state == STATE_CYCLE) { return(STATUS_PURGE_CYCLE); } // Also prevents position error
+        plan_reset_buffer();
+        if (sys.state == STATE_LOST) { 
+          report_feedback_message(MESSAGE_PURGE_AXES_LOCK); 
+          sys_sync_current_position(); // Any motion commands during a lock can unsync position vectors.
         }
+        sys.state = STATE_IDLE;
         break;
       case 'N' : // Startup lines. 
         if ( line[++char_counter] == 0 ) { // Print startup lines
diff --git a/readme.textile b/readme.textile
index 2acf3e8..e647f92 100755
--- a/readme.textile
+++ b/readme.textile
@@ -21,13 +21,13 @@ Grbl includes full acceleration management with look ahead. That means the contr
   - Re-factored g-code parser with robust error-checking.
   - 6 work coordinate systems (G54-G59), offsets(G92), and machine coordinate system support. Work coordinate systems are stored in EEPROM and persistent.
   - G10 L2 and L20 work coordinate settings support. L2 sets one or more axes values. L20 sets the current machine position to the specified work origin.
-  - Program stop(M0,M1,M2,M30) initial support.
+  - G28.1 and G30.1 set home position support. These set the internal EEPROM parameter values to the current machine position. (G28 and G30 no longer perform homing cycle, '$H' does. They move to these stored positions.)
+  - Program stop(M0,M1,M2,M30) support.
   - Coolant control(M7*,M8,M9) support. (M7 is a compile-time option).
   - System reset re-initializes grbl without resetting the Arduino and retains machine/home position and work coordinates.
   - Settings overhauled and dozens of new settings and internal commands are now available, when most were compile-time only.
   - New startup line setting. Allows users to store a custom g-code block into Grbl's startup routine. Executes immediately upon startup or reset. May be used to set g-code defaults like G20.
   - Misc bug fixes and removed deprecated acceleration enabled code.  
-  - Planned features: Axis acceleration and max speed individual settings, full-featured status reporting.
   - Advanced compile-time options: XON/XOFF flow control (limited support), direction and step pulse time delay, and up to 5 startup lines.
   
 
diff --git a/report.c b/report.c
index a036820..42456bf 100644
--- a/report.c
+++ b/report.c
@@ -73,9 +73,11 @@ void report_status_message(uint8_t status_code)
       case STATUS_SETTING_READ_FAIL:
       printPgmString(PSTR("Failed to read EEPROM settings. Using defaults")); break;
       case STATUS_HOMING_ERROR:
-      printPgmString(PSTR("Grbl must be idle to home")); break;
+      printPgmString(PSTR("Must be idle to home")); break;
       case STATUS_ABORT_CYCLE:
-      printPgmString(PSTR("Abort during cycle")); break;
+      printPgmString(PSTR("Abort during cycle. Position may be lost")); break;
+      case STATUS_PURGE_CYCLE:
+      printPgmString(PSTR("Can't purge buffer during cycle")); break;
     }
     printPgmString(PSTR("\r\n"));
   }
@@ -95,15 +97,15 @@ void report_feedback_message(int8_t message_code)
     case MESSAGE_SYSTEM_ALARM:
     printPgmString(PSTR("ALARM: Check and reset Grbl")); break;
     case MESSAGE_POSITION_LOST:
-    printPgmString(PSTR("Position unknown. '$H' to home")); break;
+    printPgmString(PSTR("'$H' to home and enable axes")); break;
     case MESSAGE_HOMING_ENABLE:
     printPgmString(PSTR("WARNING: All limit switches must be installed before homing")); break;
     case MESSAGE_SWITCH_ON:
     printPgmString(PSTR("Switch enabled")); break;
     case MESSAGE_SWITCH_OFF:
     printPgmString(PSTR("Switch disabled")); break;    
-    case MESSAGE_SWITCHES_CLEARED:
-    printPgmString(PSTR("Switches cleared")); break;
+    case MESSAGE_PURGE_AXES_LOCK:
+    printPgmString(PSTR("WARNING: Motion lock disabled. Position unknown.")); break;
   }
   printPgmString(PSTR("]\r\n"));
 }
@@ -124,13 +126,13 @@ void report_grbl_help() {
                       "$N (print startup blocks)\r\n"
                       "$x=value (store Grbl setting)\r\n"
                       "$Nx=line (store startup block)\r\n"
-                      "$H (perform homing cycle)\r\n"
-                      "$S (clear all switches)\r\n"
                       "$S0 (toggle check gcode)\r\n"
                       "$S1 (toggle dry run)\r\n"
                       "$S2 (toggle block delete)\r\n"
                       "$S3 (toggle single block)\r\n"
                       "$S4 (toggle optional stop)\r\n"
+                      "$P (purge buffer and locks)\r\n"
+                      "$H (perform homing cycle)\r\n"
                       "~ (cycle start)\r\n"
                       "! (feed hold)\r\n"
                       "? (current position)\r\n"
diff --git a/report.h b/report.h
index f0b5604..810d199 100644
--- a/report.h
+++ b/report.h
@@ -36,6 +36,7 @@
 #define STATUS_SETTING_READ_FAIL 11
 #define STATUS_HOMING_ERROR 12
 #define STATUS_ABORT_CYCLE 13
+#define STATUS_PURGE_CYCLE 14
 
 // Define Grbl feedback message codes. Less than zero to distinguish message from error.
 #define MESSAGE_SYSTEM_ALARM -1
@@ -43,7 +44,7 @@
 #define MESSAGE_HOMING_ENABLE -3
 #define MESSAGE_SWITCH_ON -4
 #define MESSAGE_SWITCH_OFF -5
-#define MESSAGE_SWITCHES_CLEARED -6
+#define MESSAGE_PURGE_AXES_LOCK -6
 
 // Prints system status messages.
 void report_status_message(uint8_t status_code);