diff --git a/ecmc_plugin_grbl/ecmcGrbl.cpp b/ecmc_plugin_grbl/ecmcGrbl.cpp
index 96ad669..c529925 100644
--- a/ecmc_plugin_grbl/ecmcGrbl.cpp
+++ b/ecmc_plugin_grbl/ecmcGrbl.cpp
@@ -514,12 +514,13 @@ void ecmcGrbl::preExeAxes() {
     autoEnableExecuted_ = 1;
     autoEnableTimeOutCounter_ = 0;
   } else {
-    if(cfgAutoEnable_) {
+    if(cfgAutoEnable_ && !errorCode_) {
       if(autoEnableTimeOutCounter_ >= cfgAutoEnableTimeOutSecs_/exeSampleTimeMs_*1000) {
         errorCode_ = ECMC_PLUGIN_AUTO_ENABLE_TIMEOUT_ERROR_CODE;
         if(errorCode_ != errorCodeOld_) {
           printf("GRBL: ERROR: Auto enable timeout 0x%x\n",errorCode_);
         }
+        setExecute(0);
         setAllAxesEnable(0);
       } else {
         autoEnableTimeOutCounter_++;
@@ -686,16 +687,18 @@ int  ecmcGrbl::grblRTexecute(int ecmcError) {
   if(getEcmcEpicsIOCState()!=16 || !grblInitDone_) {
     return 0;
   }
-
-  readEcmcStatus( ecmcError);
   
+  // Read all ecmc data
+  readEcmcStatus(ecmcError);
+  
+  // Error handling
   if((ecmcData_.errorOld == 0 && ecmcData_.error > 0) ||
-     (errorCode_>0 && errorCodeOld_ == 0)) {
+     (errorCode_ > 0 && errorCodeOld_ == 0)) {
     setHalt(0);
     setHalt(1);
 
-    if(ecmcError != errorCode_) {  // ecmc error then reset
-      if(ecmcError>0 && ecmcData_.errorOld == 0) {
+    if(ecmcData_.error != errorCode_) {  // ecmc error then reset
+      if(ecmcData_.error > 0 && ecmcData_.errorOld == 0) {
         setReset(0);
         setReset(1);
       }
@@ -706,6 +709,10 @@ int  ecmcGrbl::grblRTexecute(int ecmcError) {
     return errorCode_;
   }
 
+  if(errorCodeOld_) {    
+    writerBusy_ = false;
+  }
+
   // auto start
   if (!autoStartDone_) {
     if(cfgAutoStart_) {
@@ -768,7 +775,10 @@ void ecmcGrbl::postExeAxes() {
 int ecmcGrbl::setExecute(int exe) {
   if(!exe) {
     writerBusy_ = 0;
+    autoEnableTimeOutCounter_ = 0;
+    autoEnableExecuted_       = 0;
   }
+
   if(!executeCmd_ && exe) {
     grblCommandBufferIndex_ = 0;
     writerBusy_ = 1;
@@ -820,6 +830,15 @@ std::string ecmcGrbl::to_string(int value) {
   return os.str();
 }
 
+int ecmcGrbl::getError() {
+  return errorCode_;
+}
+
+void ecmcGrbl::resetError() {
+  errorCode_ = 0;
+  errorCodeOld_ = 0;
+}
+
 void ecmcGrbl::addCommand(std::string command) {
   if(cfgDbgMode_){
     printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
@@ -832,6 +851,7 @@ void ecmcGrbl::addCommand(std::string command) {
            __FILE__,__FUNCTION__,__LINE__,grblCommandBuffer_.size());
   }
 }
+
 void ecmcGrbl::loadFile(std::string fileName, int append) {
   if(cfgDbgMode_){
     printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
diff --git a/ecmc_plugin_grbl/ecmcGrbl.h b/ecmc_plugin_grbl/ecmcGrbl.h
index 2bb0784..90788a0 100644
--- a/ecmc_plugin_grbl/ecmcGrbl.h
+++ b/ecmc_plugin_grbl/ecmcGrbl.h
@@ -77,6 +77,8 @@ class ecmcGrbl : public asynPortDriver {
   int                      getParserBusy();
   int                      getCodeRowNum();
   int                      setAllAxesEnable(int enable);
+  int                      getError();
+  void                     resetError();
 
  private:
   void                     parseConfigStr(char *configStr);
diff --git a/ecmc_plugin_grbl/ecmcGrblDefs.h b/ecmc_plugin_grbl/ecmcGrblDefs.h
index abebcae..eab1e9a 100644
--- a/ecmc_plugin_grbl/ecmcGrblDefs.h
+++ b/ecmc_plugin_grbl/ecmcGrblDefs.h
@@ -33,7 +33,7 @@
 #define ECMC_PLUGIN_SPINDLE_ACC_ERROR_CODE 0x105
 #define ECMC_PLUGIN_AUTO_ENABLE_TIMEOUT_ERROR_CODE 0x106
 
-#define ECMC_PLUGIN_AUTO_ENABLE_TIME_OUT_SEC 5 
+#define ECMC_PLUGIN_AUTO_ENABLE_TIME_OUT_SEC 10 
 
 #define ECMC_PLUGIN_GRBL_GRBL_STARTUP_STRING "for help]"
 #define ECMC_PLUGIN_GRBL_GRBL_OK_STRING "ok"
diff --git a/ecmc_plugin_grbl/ecmcGrblWrap.cpp b/ecmc_plugin_grbl/ecmcGrblWrap.cpp
index 8f950e7..793c410 100644
--- a/ecmc_plugin_grbl/ecmcGrblWrap.cpp
+++ b/ecmc_plugin_grbl/ecmcGrblWrap.cpp
@@ -113,7 +113,6 @@ int getCodeRowNum() {
   return 0;
 }
 
-
 int setReset(int reset) {
   if(grbl){
     return grbl->setReset(reset);
@@ -121,6 +120,21 @@ int setReset(int reset) {
   return 0;
 }
 
+int getError() {
+  if(grbl){
+    return grbl->getError();
+  }
+  return 0;
+}
+
+int resetError() {
+  if(grbl){
+    grbl->resetError();
+  }
+  return 0;
+}
+
+
 void deleteGrbl() {
   if(grbl) {
     delete (grbl);
diff --git a/ecmc_plugin_grbl/ecmcGrblWrap.h b/ecmc_plugin_grbl/ecmcGrblWrap.h
index 7e841d6..73403d4 100644
--- a/ecmc_plugin_grbl/ecmcGrblWrap.h
+++ b/ecmc_plugin_grbl/ecmcGrblWrap.h
@@ -66,6 +66,13 @@ int getParserBusy();
   */
 int getCodeRowNum();
 
+/** \brief get error code\n
+  */
+int getError();
+
+/** \brief reset error code\n
+  */
+int resetError();
 
 // Delete object
 void deleteGrbl();
diff --git a/ecmc_plugin_grbl/ecmcPluginGrbl.c b/ecmc_plugin_grbl/ecmcPluginGrbl.c
index fd20d39..0291c69 100644
--- a/ecmc_plugin_grbl/ecmcPluginGrbl.c
+++ b/ecmc_plugin_grbl/ecmcPluginGrbl.c
@@ -134,6 +134,14 @@ double grbl_get_code_row_num() {
   return getCodeRowNum();
 }
 
+double grbl_reset_error() {
+  return resetError();
+}
+
+double grbl_get_error() {
+  return getError();
+}
+
 // Register data for plugin so ecmc know what to use
 struct ecmcPluginData pluginDataDef = {
   // Allways use ECMC_PLUG_VERSION_MAGIC
@@ -325,7 +333,54 @@ struct ecmcPluginData pluginDataDef = {
         .funcArg10 = NULL,
         .funcGenericObj = NULL,
       },
-  .funcs[7] = {0},  // last element set all to zero..
+  .funcs[7] =
+      { /*----grbl_get_error----*/
+        // Function name (this is the name you use in ecmc plc-code)
+        .funcName = "grbl_get_error",
+        // Function description
+        .funcDesc = "double grbl_get_error() :  Get error code.",
+        /**
+        * 7 different prototypes allowed (only doubles since reg in plc).
+        * Only funcArg${argCount} func shall be assigned the rest set to NULL.
+        **/
+        .funcArg0 = grbl_get_error,
+        .funcArg1 = NULL,
+        .funcArg2 = NULL,
+        .funcArg3 = NULL,
+        .funcArg4 = NULL,
+        .funcArg5 = NULL,
+        .funcArg6 = NULL,
+        .funcArg7 = NULL,
+        .funcArg8 = NULL,
+        .funcArg9 = NULL,
+        .funcArg10 = NULL,
+        .funcGenericObj = NULL,
+      },
+  .funcs[8] =
+      { /*----grbl_reset_error----*/
+        // Function name (this is the name you use in ecmc plc-code)
+        .funcName = "grbl_reset_error",
+        // Function description
+        .funcDesc = "double grbl_reset_error() :  Reset error.",
+        /**
+        * 7 different prototypes allowed (only doubles since reg in plc).
+        * Only funcArg${argCount} func shall be assigned the rest set to NULL.
+        **/
+        .funcArg0 = grbl_reset_error,
+        .funcArg1 = NULL,
+        .funcArg2 = NULL,
+        .funcArg3 = NULL,
+        .funcArg4 = NULL,
+        .funcArg5 = NULL,
+        .funcArg6 = NULL,
+        .funcArg7 = NULL,
+        .funcArg8 = NULL,
+        .funcArg9 = NULL,
+        .funcArg10 = NULL,
+        .funcGenericObj = NULL,
+      },
+
+  .funcs[9] = {0},  // last element set all to zero..
   // PLC consts
   .consts[0] = {0}, // last element set all to zero..
 };
diff --git a/iocsh/plc/grbl.plc b/iocsh/plc/grbl.plc
index b719b60..175551e 100644
--- a/iocsh/plc/grbl.plc
+++ b/iocsh/plc/grbl.plc
@@ -4,7 +4,13 @@
 
 println('GRBL busy  :', grbl_get_busy());
 println('Parser busy:', grbl_get_parser_busy());
-if(not(grbl_get_parser_busy())) {
+println('Error:', grbl_get_error());
+
+if(grbl_get_error()) {
+  println('Try reset');
+  grbl_reset_error();
+  ec_err_rst();
+} else if(not(grbl_get_parser_busy())) {
   static.counter+=1;
   println('Retrigger g-code. Counter: ', static.counter);
   grbl_set_execute(0);