diff --git a/.gitignore b/.gitignore
index dec931d0..44cc0076 100644
--- a/.gitignore
+++ b/.gitignore
@@ -20,3 +20,4 @@ __vm/
 *.vcxproj.filters
 *.suo
 Grbl_Esp32.ino.cpp
+/packages
diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h
index f53ca288..ffba4148 100644
--- a/Grbl_Esp32/src/Config.h
+++ b/Grbl_Esp32/src/Config.h
@@ -446,17 +446,7 @@ const int DWELL_TIME_STEP = 50;  // Integer (1-255) (milliseconds)
 // While this is experimental, it is intended to be the future default method after testing
 //#define USE_RMT_STEPS
 
-// Creates a delay between the direction pin setting and corresponding step pulse by creating
-// another interrupt (Timer2 compare) to manage it. The main Grbl interrupt (Timer1 compare)
-// sets the direction pins, and does not immediately set the stepper pins, as it would in
-// normal operation. The Timer2 compare fires next to set the stepper pins after the step
-// pulse delay time, and Timer2 overflow will complete the step pulse, except now delayed
-// by the step pulse time plus the step pulse delay. (Thanks langwadt for the idea!)
-// NOTE: Uncomment to enable. The recommended delay must be > 3us, and, when added with the
-// user-supplied step pulse time, the total time must not exceed 127us. Reported successful
-// values for certain setups have ranged from 5 to 20us.
-// must use #define USE_RMT_STEPS for this to work
-//#define STEP_PULSE_DELAY 10 // Step pulse delay in microseconds. Default disabled.
+// STEP_PULSE_DELAY is now a setting...$Stepper/Direction/Delay
 
 // The number of linear motions in the planner buffer to be planned at any give time. The vast
 // majority of RAM that Grbl uses is based on this buffer size. Only increase if there is extra
diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h
index 91c9e396..9c5ac586 100644
--- a/Grbl_Esp32/src/Defaults.h
+++ b/Grbl_Esp32/src/Defaults.h
@@ -46,6 +46,10 @@
 #    define DEFAULT_STEP_ENABLE_DELAY 0
 #endif
 
+#ifndef STEP_PULSE_DELAY
+#    define STEP_PULSE_DELAY 0
+#endif
+
 #ifndef DEFAULT_STEPPER_IDLE_LOCK_TIME
 #    define DEFAULT_STEPPER_IDLE_LOCK_TIME 250  // $1 msec (0-254, 255 keeps steppers enabled)
 #endif
@@ -674,4 +678,4 @@
 
 #ifndef DEFAULT_USER_MACRO3
 #    define DEFAULT_USER_MACRO3 ""
-#endif
\ No newline at end of file
+#endif
diff --git a/Grbl_Esp32/src/Error.cpp b/Grbl_Esp32/src/Error.cpp
index 34dd1cd4..c18ef200 100644
--- a/Grbl_Esp32/src/Error.cpp
+++ b/Grbl_Esp32/src/Error.cpp
@@ -61,16 +61,16 @@ std::map<Error, const char*> ErrorNames = {
     { Error::GcodeG43DynamicAxisError, "Gcode G43 dynamic axis error" },
     { Error::GcodeMaxValueExceeded, "Gcode max value exceeded" },
     { Error::PParamMaxExceeded, "P param max exceeded" },
-    { Error::SdFailedMount, "SD failed mount" },
-    { Error::SdFailedRead, "SD failed read" },
-    { Error::SdFailedOpenDir, "SD failed to open directory" },
-    { Error::SdDirNotFound, "SD directory not found" },
-    { Error::SdFileEmpty, "SD file empty" },
-    { Error::SdFileNotFound, "SD file not found" },
-    { Error::SdFailedOpenFile, "SD failed to open file" },
-    { Error::SdFailedBusy, "SD is busy" },
-    { Error::SdFailedDelDir, "SD failed to delete directory" },
-    { Error::SdFailedDelFile, "SD failed to delete file" },
+    { Error::FsFailedMount, "Failed to mount device" },
+    { Error::FsFailedRead, "Failed to read" },
+    { Error::FsFailedOpenDir, "Failed to open directory" },
+    { Error::FsDirNotFound, "Directory not found" },
+    { Error::FsFileEmpty, "File empty" },
+    { Error::FsFileNotFound, "File not found" },
+    { Error::FsFailedOpenFile, "Failed to open file" },
+    { Error::FsFailedBusy, "Device is busy" },
+    { Error::FsFailedDelDir, "Failed to delete directory" },
+    { Error::FsFailedDelFile, "Failed to delete file" },
     { Error::BtFailBegin, "Bluetooth failed to start" },
     { Error::WifiFailBegin, "WiFi failed to start" },
     { Error::NumberRange, "Number out of range for setting" },
@@ -79,5 +79,5 @@ std::map<Error, const char*> ErrorNames = {
     { Error::NvsSetFailed, "Failed to store setting" },
     { Error::NvsGetStatsFailed, "Failed to get setting status" },
     { Error::AuthenticationFailed, "Authentication failed!" },
-    { Error::AnotherInterfaceBusy, "Another interface is busy"},
+    { Error::AnotherInterfaceBusy, "Another interface is busy" },
 };
diff --git a/Grbl_Esp32/src/Error.h b/Grbl_Esp32/src/Error.h
index 80f40a39..14b443eb 100644
--- a/Grbl_Esp32/src/Error.h
+++ b/Grbl_Esp32/src/Error.h
@@ -64,16 +64,16 @@ enum class Error : uint8_t {
     GcodeG43DynamicAxisError    = 37,
     GcodeMaxValueExceeded       = 38,
     PParamMaxExceeded           = 39,
-    SdFailedMount               = 60,  // SD Failed to mount
-    SdFailedRead                = 61,  // SD Failed to read file
-    SdFailedOpenDir             = 62,  // SD card failed to open directory
-    SdDirNotFound               = 63,  // SD Card directory not found
-    SdFileEmpty                 = 64,  // SD Card directory not found
-    SdFileNotFound              = 65,  // SD Card file not found
-    SdFailedOpenFile            = 66,  // SD card failed to open file
-    SdFailedBusy                = 67,  // SD card is busy
-    SdFailedDelDir              = 68,
-    SdFailedDelFile             = 69,
+    FsFailedMount               = 60,  // SD Failed to mount
+    FsFailedRead                = 61,  // SD Failed to read file
+    FsFailedOpenDir             = 62,  // SD card failed to open directory
+    FsDirNotFound               = 63,  // SD Card directory not found
+    FsFileEmpty                 = 64,  // SD Card directory not found
+    FsFileNotFound              = 65,  // SD Card file not found
+    FsFailedOpenFile            = 66,  // SD card failed to open file
+    FsFailedBusy                = 67,  // SD card is busy
+    FsFailedDelDir              = 68,
+    FsFailedDelFile             = 69,
     BtFailBegin                 = 70,  // Bluetooth failed to start
     WifiFailBegin               = 71,  // WiFi failed to start
     NumberRange                 = 80,  // Setting number range problem
diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp
index 94ea3237..bd1bf2b9 100644
--- a/Grbl_Esp32/src/GCode.cpp
+++ b/Grbl_Esp32/src/GCode.cpp
@@ -1435,7 +1435,7 @@ Error gc_execute_line(char* line, uint8_t client) {
 #endif
     // [10. Dwell ]:
     if (gc_block.non_modal_command == NonModal::Dwell) {
-        mc_dwell(gc_block.values.p);
+        mc_dwell(int32_t(gc_block.values.p * 1000.0f));
     }
     // [11. Set active plane ]:
     gc_state.modal.plane_select = gc_block.modal.plane_select;
diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h
index 23ae52b5..5a56918b 100644
--- a/Grbl_Esp32/src/Grbl.h
+++ b/Grbl_Esp32/src/Grbl.h
@@ -22,7 +22,7 @@
 
 // Grbl versioning system
 const char* const GRBL_VERSION       = "1.3a";
-const char* const GRBL_VERSION_BUILD = "20210226";
+const char* const GRBL_VERSION_BUILD = "20210306";
 
 //#include <sdkconfig.h>
 #include <Arduino.h>
diff --git a/Grbl_Esp32/src/Machines/3axis_rs485.h b/Grbl_Esp32/src/Machines/3axis_rs485.h
deleted file mode 100644
index 39a565cb..00000000
--- a/Grbl_Esp32/src/Machines/3axis_rs485.h
+++ /dev/null
@@ -1,59 +0,0 @@
-#pragma once
-// clang-format off
-
-/*
-	3axis_xyx.h
-	Part of Grbl_ESP32
-
-	This is a general XYZ-axis RS-485 CNC machine. The schematic is quite 
-	easy, you basically need a MAX485 wired through a logic level converter
-	for the VFD, and a few pins wired through an ULN2803A to the external 
-	stepper drivers. It's common to have a dual gantry for the Y axis.
-
-	Optional limit pins are slightly more difficult, as these require a 
-	Schmitt trigger and optocouplers.
-
-	2020    - Stefan de Bruijn
-
-	Grbl_ESP32 is free software: you can redistribute it and/or modify
-	it under the terms of the GNU General Public License as published by
-	the Free Software Foundation, either version 3 of the License, or
-	(at your option) any later version.
-
-	Grbl is distributed in the hope that it will be useful,
-	but WITHOUT ANY WARRANTY; without even the implied warranty of
-	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-	GNU General Public License for more details.
-
-	You should have received a copy of the GNU General Public License
-	along with Grbl_ESP32.  If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#define MACHINE_NAME            "ESP32_XYZ_RS485"
-#define X_STEP_PIN              GPIO_NUM_4   // labeled X
-#define X_DIRECTION_PIN         GPIO_NUM_16  // labeled X
-#define Y_STEP_PIN              GPIO_NUM_17  // labeled Y
-#define Y_DIRECTION_PIN         GPIO_NUM_18  // labeled Y
-#define Y2_STEP_PIN             GPIO_NUM_19  // labeled Y2
-#define Y2_DIRECTION_PIN        GPIO_NUM_21  // labeled Y2
-#define Z_STEP_PIN              GPIO_NUM_22  // labeled Z
-#define Z_DIRECTION_PIN         GPIO_NUM_23  // labeled Z
-
-#define SPINDLE_TYPE            SpindleType::H2A
-#define VFD_RS485_TXD_PIN       GPIO_NUM_13 // RS485 TX
-#define VFD_RS485_RTS_PIN       GPIO_NUM_15 // RS485 RTS
-#define VFD_RS485_RXD_PIN       GPIO_NUM_2  // RS485 RX
-
-#define X_LIMIT_PIN             GPIO_NUM_33
-#define Y_LIMIT_PIN             GPIO_NUM_32
-#define Y2_LIMIT_PIN            GPIO_NUM_35
-#define Z_LIMIT_PIN             GPIO_NUM_34
-
-// Set $Homing/Cycle0=X and $Homing/Cycle=XY
-
-#define PROBE_PIN               GPIO_NUM_14  // labeled Probe
-#define CONTROL_RESET_PIN       GPIO_NUM_27  // labeled Reset
-#define CONTROL_FEED_HOLD_PIN   GPIO_NUM_26  // labeled Hold
-#define CONTROL_CYCLE_START_PIN GPIO_NUM_25  // labeled Start
-
-// #define VFD_DEBUG_MODE
diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp
index 5ea64cb2..1b8cda8f 100644
--- a/Grbl_Esp32/src/MotionControl.cpp
+++ b/Grbl_Esp32/src/MotionControl.cpp
@@ -5,8 +5,8 @@
   Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
   Copyright (c) 2009-2011 Simen Svale Skogsrud
 
-	2018 -	Bart Dring This file was modifed for use on the ESP32
-					CPU. Do not use this with Grbl for atMega328P
+    2018 -	Bart Dring This file was modifed for use on the ESP32
+            CPU. Do not use this with Grbl for atMega328P
 
   Grbl is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
@@ -226,12 +226,12 @@ void mc_arc(float*            target,
 }
 
 // Execute dwell in seconds.
-void mc_dwell(float seconds) {
-    if (seconds == 0 || sys.state == State::CheckMode) {
-        return;
+bool mc_dwell(int32_t milliseconds) {
+    if (milliseconds <= 0 || sys.state == State::CheckMode) {
+        return false;
     }
     protocol_buffer_synchronize();
-    delay_sec(seconds, DwellMode::Dwell);
+    return delay_msec(milliseconds, DwellMode::Dwell);
 }
 
 // return true if the mask has exactly one bit set,
diff --git a/Grbl_Esp32/src/MotionControl.h b/Grbl_Esp32/src/MotionControl.h
index c9177f0b..c7adebc5 100644
--- a/Grbl_Esp32/src/MotionControl.h
+++ b/Grbl_Esp32/src/MotionControl.h
@@ -53,7 +53,7 @@ void mc_arc(float*            target,
             uint8_t           is_clockwise_arc);
 
 // Dwell for a specific number of seconds
-void mc_dwell(float seconds);
+bool mc_dwell(int32_t milliseconds);
 
 // Perform homing cycle to locate machine zero. Requires limit switches.
 void mc_homing_cycle(uint8_t cycle_mask);
diff --git a/Grbl_Esp32/src/Motors/Motors.cpp b/Grbl_Esp32/src/Motors/Motors.cpp
index bedfcd0a..4cb5289a 100644
--- a/Grbl_Esp32/src/Motors/Motors.cpp
+++ b/Grbl_Esp32/src/Motors/Motors.cpp
@@ -478,7 +478,7 @@ uint8_t motors_set_homing_mode(uint8_t homing_mask, bool isHoming) {
     return can_home;
 }
 
-void motors_step(uint8_t step_mask, uint8_t dir_mask) {
+bool motors_direction(uint8_t dir_mask) {
     auto n_axis = number_axis->get();
     //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "motors_set_direction_pins:0x%02X", onMask);
 
@@ -493,7 +493,17 @@ void motors_step(uint8_t step_mask, uint8_t dir_mask) {
             myMotor[axis][0]->set_direction(thisDir);
             myMotor[axis][1]->set_direction(thisDir);
         }
+
+        return true;
+    } else {
+        return false;
     }
+}
+
+void motors_step(uint8_t step_mask) {
+    auto n_axis = number_axis->get();
+    //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "motors_set_direction_pins:0x%02X", onMask);
+
     // Turn on step pulses for motors that are supposed to step now
     for (uint8_t axis = X_AXIS; axis < n_axis; axis++) {
         if (bitnum_istrue(step_mask, axis)) {
@@ -513,4 +523,4 @@ void motors_unstep() {
         myMotor[axis][0]->unstep();
         myMotor[axis][1]->unstep();
     }
-}
\ No newline at end of file
+}
diff --git a/Grbl_Esp32/src/Motors/Motors.h b/Grbl_Esp32/src/Motors/Motors.h
index 14550f68..d29fa3c4 100644
--- a/Grbl_Esp32/src/Motors/Motors.h
+++ b/Grbl_Esp32/src/Motors/Motors.h
@@ -40,7 +40,8 @@ void    motors_read_settings();
 // The return value is a bitmask of axes that can home
 uint8_t motors_set_homing_mode(uint8_t homing_mask, bool isHoming);
 void    motors_set_disable(bool disable, uint8_t mask = B11111111);  // default is all axes
-void    motors_step(uint8_t step_mask, uint8_t dir_mask);
+bool    motors_direction(uint8_t dir_mask);
+void    motors_step(uint8_t step_mask);
 void    motors_unstep();
 
 void servoUpdateTask(void* pvParameters);
diff --git a/Grbl_Esp32/src/Motors/RcServo.cpp b/Grbl_Esp32/src/Motors/RcServo.cpp
index 2eefbfc2..bc0fe8b5 100644
--- a/Grbl_Esp32/src/Motors/RcServo.cpp
+++ b/Grbl_Esp32/src/Motors/RcServo.cpp
@@ -50,7 +50,7 @@ namespace Motors {
         ledcSetup(_channel_num, SERVO_PULSE_FREQ, SERVO_PULSE_RES_BITS);
         ledcAttachPin(_pwm_pin, _channel_num);
         _current_pwm_duty = 0;
-
+        _disabled         = true;
         config_message();
         startUpdateTask();
     }
@@ -78,10 +78,6 @@ namespace Motors {
 
     // sets the PWM to zero. This allows most servos to be manually moved
     void RcServo::set_disable(bool disable) {
-        if (_disabled == disable) {
-            return;
-        }
-
         _disabled = disable;
         if (_disabled) {
             _write_pwm(0);
@@ -107,11 +103,6 @@ namespace Motors {
         if (_disabled)
             return;
 
-        if (sys.state == State::Alarm) {
-            set_disable(true);
-            return;
-        }
-
         read_settings();
 
         mpos = system_convert_axis_steps_to_mpos(sys_position, _axis_index);  // get the axis machine position in mm
@@ -127,10 +118,14 @@ namespace Motors {
     }
 
     void RcServo::read_settings() {
-        _pwm_pulse_min = SERVO_MIN_PULSE * rc_servo_cal_min->get();
-        _pwm_pulse_max = SERVO_MAX_PULSE * rc_servo_cal_max->get();
+        if (bitnum_istrue(dir_invert_mask->get(), _axis_index)) {
+            // swap the pwm values
+            _pwm_pulse_min = SERVO_MAX_PULSE * (1.0 + (1.0 - rc_servo_cal_min->get()));
+            _pwm_pulse_max = SERVO_MIN_PULSE * (1.0 + (1.0 - rc_servo_cal_max->get()));
 
-        if (bitnum_istrue(dir_invert_mask->get(), _axis_index))  // normal direction
-            swap(_pwm_pulse_min, _pwm_pulse_max);
+        } else {
+            _pwm_pulse_min = SERVO_MIN_PULSE * rc_servo_cal_min->get();
+            _pwm_pulse_max = SERVO_MAX_PULSE * rc_servo_cal_max->get();
+        }
     }
 }
diff --git a/Grbl_Esp32/src/Motors/StandardStepper.cpp b/Grbl_Esp32/src/Motors/StandardStepper.cpp
index 8e782ba4..3f5a91da 100644
--- a/Grbl_Esp32/src/Motors/StandardStepper.cpp
+++ b/Grbl_Esp32/src/Motors/StandardStepper.cpp
@@ -41,6 +41,9 @@ namespace Motors {
 
     StandardStepper::StandardStepper(uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin) :
         Motor(axis_index), _step_pin(step_pin), _dir_pin(dir_pin), _disable_pin(disable_pin) {
+#ifdef USE_RMT_STEPS
+        _rmt_chan_num = get_next_RMT_chan_num();
+#endif
     }
 
     void StandardStepper::init() {
@@ -48,9 +51,7 @@ namespace Motors {
         config_message();
     }
 
-    void StandardStepper::read_settings() { 
-        init_step_dir_pins(); 
-    }
+    void StandardStepper::read_settings() { init_step_dir_pins(); }
 
     void StandardStepper::init_step_dir_pins() {
         _invert_step_pin = bitnum_istrue(step_invert_mask->get(), _axis_index);
@@ -68,17 +69,13 @@ namespace Motors {
         rmtConfig.tx_config.carrier_level        = RMT_CARRIER_LEVEL_LOW;
         rmtConfig.tx_config.idle_output_en       = true;
 
-#    ifdef STEP_PULSE_DELAY
-        rmtItem[0].duration0 = STEP_PULSE_DELAY * 4;
-#    else
-        rmtItem[0].duration0 = 1;
-#    endif
+        auto stepPulseDelay  = direction_delay_microseconds->get();
+        rmtItem[0].duration0 = stepPulseDelay < 1 ? 1 : stepPulseDelay * 4;
 
         rmtItem[0].duration1 = 4 * pulse_microseconds->get();
         rmtItem[1].duration0 = 0;
         rmtItem[1].duration1 = 0;
 
-        _rmt_chan_num = get_next_RMT_chan_num();
         if (_rmt_chan_num == RMT_CHANNEL_MAX) {
             return;
         }
@@ -126,5 +123,8 @@ namespace Motors {
 
     void StandardStepper::set_direction(bool dir) { digitalWrite(_dir_pin, dir ^ _invert_dir_pin); }
 
-    void StandardStepper::set_disable(bool disable) { digitalWrite(_disable_pin, disable); }
+    void StandardStepper::set_disable(bool disable) {
+        disable ^= step_enable_invert->get();
+        digitalWrite(_disable_pin, disable);
+    }
 }
diff --git a/Grbl_Esp32/src/NutsBolts.cpp b/Grbl_Esp32/src/NutsBolts.cpp
index 51245465..8039bdd6 100644
--- a/Grbl_Esp32/src/NutsBolts.cpp
+++ b/Grbl_Esp32/src/NutsBolts.cpp
@@ -112,11 +112,14 @@ void delay_ms(uint16_t ms) {
 }
 
 // Non-blocking delay function used for general operation and suspend features.
-void delay_sec(float seconds, DwellMode mode) {
-    uint16_t i = ceil(1000 / DWELL_TIME_STEP * seconds);
+bool delay_msec(int32_t milliseconds, DwellMode mode) {
+    // Note: i must be signed, because of the 'i-- > 0' check below.
+    int32_t i         = milliseconds / DWELL_TIME_STEP;
+    int32_t remainder = i < 0 ? 0 : (milliseconds - DWELL_TIME_STEP * i);
+
     while (i-- > 0) {
         if (sys.abort) {
-            return;
+            return false;
         }
         if (mode == DwellMode::Dwell) {
             protocol_execute_realtime();
@@ -124,11 +127,13 @@ void delay_sec(float seconds, DwellMode mode) {
             // Execute rt_system() only to avoid nesting suspend loops.
             protocol_exec_rt_system();
             if (sys.suspend.bit.restartRetract) {
-                return;  // Bail, if safety door reopens.
+                return false;  // Bail, if safety door reopens.
             }
         }
         delay(DWELL_TIME_STEP);  // Delay DWELL_TIME_STEP increment
     }
+    delay(remainder);
+    return true;
 }
 
 // Simple hypotenuse computation function.
diff --git a/Grbl_Esp32/src/NutsBolts.h b/Grbl_Esp32/src/NutsBolts.h
index 15705065..77dd45f0 100644
--- a/Grbl_Esp32/src/NutsBolts.h
+++ b/Grbl_Esp32/src/NutsBolts.h
@@ -26,7 +26,7 @@
 // #define true 1
 
 enum class DwellMode : uint8_t {
-    Dwell = 0,   // (Default: Must be zero)
+    Dwell      = 0,  // (Default: Must be zero)
     SysSuspend = 1,  //G92.1 (Do not alter value)
 };
 
@@ -62,7 +62,6 @@ static inline int toMotor2(int axis) {
 const double MM_PER_INCH = (25.40);
 const double INCH_PER_MM = (0.0393701);
 
-
 // Useful macros
 #define clear_vector(a) memset(a, 0, sizeof(a))
 #define clear_vector_float(a) memset(a, 0.0, sizeof(float) * MAX_N_AXIS)
@@ -90,7 +89,7 @@ const double INCH_PER_MM = (0.0393701);
 uint8_t read_float(const char* line, uint8_t* char_counter, float* float_ptr);
 
 // Non-blocking delay function used for general operation and suspend features.
-void delay_sec(float seconds, DwellMode mode);
+bool delay_msec(int32_t milliseconds, DwellMode mode);
 
 // Delays variable-defined milliseconds. Compiler compatibility fix for _delay_ms().
 void delay_ms(uint16_t ms);
diff --git a/Grbl_Esp32/src/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp
index 126015e8..0a331983 100644
--- a/Grbl_Esp32/src/Protocol.cpp
+++ b/Grbl_Esp32/src/Protocol.cpp
@@ -5,8 +5,8 @@
   Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
   Copyright (c) 2009-2011 Simen Svale Skogsrud
 
-	2018 -	Bart Dring This file was modifed for use on the ESP32
-					CPU. Do not use this with Grbl for atMega328P
+    2018 -	Bart Dring This file was modifed for use on the ESP32
+                    CPU. Do not use this with Grbl for atMega328P
 
   Grbl is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
@@ -193,7 +193,7 @@ void protocol_main_loop() {
             return;  // Bail to main() program loop to reset system.
         }
         // check to see if we should disable the stepper drivers ... esp32 work around for disable in main loop.
-        if (stepper_idle) {
+        if (stepper_idle && stepper_idle_lock_time->get() != 0xff) {
             if (esp_timer_get_time() > stepper_idle_counter) {
                 motors_set_disable(true);
             }
@@ -272,7 +272,7 @@ void protocol_exec_rt_system() {
     }
     ExecState rt_exec_state;
     rt_exec_state.value = sys_rt_exec_state.value;  // Copy volatile sys_rt_exec_state.
-    if (rt_exec_state.value != 0 || cycle_stop) {                 // Test if any bits are on
+    if (rt_exec_state.value != 0 || cycle_stop) {   // Test if any bits are on
         // Execute system abort.
         if (rt_exec_state.bit.reset) {
             sys.abort = true;  // Only place this is set true.
@@ -664,10 +664,10 @@ static void protocol_exec_rt_suspend() {
                                 if (spindle->inLaserMode()) {
                                     // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts.
                                     sys.step_control.updateSpindleRpm = true;
-                                } else {                                    
+                                } else {
                                     spindle->set_state(restore_spindle, (uint32_t)restore_spindle_speed);
                                     // restore delay is done in the spindle class
-                                    //delay_sec(spindle_delay_spinup->get(), DwellMode::SysSuspend);
+                                    //delay_sec(int32_t(1000.0 * spindle_delay_spinup->get()), DwellMode::SysSuspend);
                                 }
                             }
                         }
@@ -676,7 +676,7 @@ static void protocol_exec_rt_suspend() {
                             if (!sys.suspend.bit.restartRetract) {
                                 // NOTE: Laser mode will honor this delay. An exhaust system is often controlled by this pin.
                                 coolant_set_state(restore_coolant);
-                                delay_sec(coolant_start_delay->get(), DwellMode::SysSuspend);
+                                delay_msec(int32_t(1000.0 * coolant_start_delay->get()), DwellMode::SysSuspend);
                             }
                         }
 #ifdef PARKING_ENABLE
diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp
index 892230e8..b4728154 100644
--- a/Grbl_Esp32/src/Report.cpp
+++ b/Grbl_Esp32/src/Report.cpp
@@ -291,10 +291,14 @@ std::map<Message, const char*> MessageText = {
 // NOTE: For interfaces, messages are always placed within brackets. And if silent mode
 // is installed, the message number codes are less than zero.
 void report_feedback_message(Message message) {  // ok to send to all clients
+#if defined (ENABLE_SD_CARD) 
     if (message == Message::SdFileQuit) {
         grbl_notifyf("SD print canceled", "Reset during SD file at line: %d", sd_get_current_line_number());
         grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Reset during SD file at line: %d", sd_get_current_line_number());
-    } else {
+
+    } else 
+#endif //ENABLE_SD_CARD  
+    {
         auto it = MessageText.find(message);
         if (it != MessageText.end()) {
             grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, it->second);
diff --git a/Grbl_Esp32/src/SDCard.cpp b/Grbl_Esp32/src/SDCard.cpp
index 32dd95ef..eadac19f 100644
--- a/Grbl_Esp32/src/SDCard.cpp
+++ b/Grbl_Esp32/src/SDCard.cpp
@@ -18,7 +18,9 @@
   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "SDCard.h"
+#include "Config.h"
+#ifdef ENABLE_SD_CARD
+#    include "SDCard.h"
 
 File                       myFile;
 bool                       SD_ready_next = false;  // Grbl has processed a line and is waiting for another
@@ -31,7 +33,7 @@ static char                comment[LINE_BUFFER_SIZE];  // Line to be executed. Z
 /*bool sd_mount()
 {
   if(!SD.begin()) {
-    report_status_message(Error::SdFailedMount, CLIENT_SERIAL);
+    report_status_message(Error::FsFailedMount, CLIENT_SERIAL);
     return false;
   }
   return true;
@@ -41,11 +43,11 @@ void listDir(fs::FS& fs, const char* dirname, uint8_t levels, uint8_t client) {
     //char temp_filename[128]; // to help filter by extension	TODO: 128 needs a definition based on something
     File root = fs.open(dirname);
     if (!root) {
-        report_status_message(Error::SdFailedOpenDir, client);
+        report_status_message(Error::FsFailedOpenDir, client);
         return;
     }
     if (!root.isDirectory()) {
-        report_status_message(Error::SdDirNotFound, client);
+        report_status_message(Error::FsDirNotFound, client);
         return;
     }
     File file = root.openNextFile();
@@ -64,7 +66,7 @@ void listDir(fs::FS& fs, const char* dirname, uint8_t levels, uint8_t client) {
 boolean openFile(fs::FS& fs, const char* path) {
     myFile = fs.open(path);
     if (!myFile) {
-        //report_status_message(Error::SdFailedRead, CLIENT_SERIAL);
+        //report_status_message(Error::FsFailedRead, CLIENT_SERIAL);
         return false;
     }
     set_sd_state(SDState::BusyPrinting);
@@ -94,7 +96,7 @@ boolean closeFile() {
 */
 boolean readFileLine(char* line, int maxlen) {
     if (!myFile) {
-        report_status_message(Error::SdFailedRead, SD_client);
+        report_status_message(Error::FsFailedRead, SD_client);
         return false;
     }
     sd_current_line_number += 1;
@@ -169,3 +171,4 @@ void sd_get_current_filename(char* name) {
         name[0] = 0;
     }
 }
+#endif  //ENABLE_SD_CARD
diff --git a/Grbl_Esp32/src/Serial.cpp b/Grbl_Esp32/src/Serial.cpp
index 9a4b9e4e..c5d0220b 100644
--- a/Grbl_Esp32/src/Serial.cpp
+++ b/Grbl_Esp32/src/Serial.cpp
@@ -159,16 +159,20 @@ void serialCheckTask(void* pvParameters) {
             if (is_realtime_command(data)) {
                 execute_realtime_command(static_cast<Cmd>(data), client);
             } else {
+#if defined(ENABLE_SD_CARD)
                 if (get_sd_state(false) < SDState::Busy) {
+#endif //ENABLE_SD_CARD
                     vTaskEnterCritical(&myMutex);
                     client_buffer[client].write(data);
                     vTaskExitCritical(&myMutex);
+#if defined(ENABLE_SD_CARD)
                 } else {
                     if (data == '\r' || data == '\n') {
                         grbl_sendf(client, "error %d\r\n", Error::AnotherInterfaceBusy);
                         grbl_msg_sendf(client, MsgLevel::Info, "SD card job running");
                     }
                 }
+#endif //ENABLE_SD_CARD
             }
         }  // if something available
         WebUI::COMMANDS::handle();
diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp
index 8d0aecc2..9d7ad8f6 100644
--- a/Grbl_Esp32/src/SettingsDefinitions.cpp
+++ b/Grbl_Esp32/src/SettingsDefinitions.cpp
@@ -10,6 +10,7 @@ StringSetting* build_info;
 
 IntSetting* pulse_microseconds;
 IntSetting* stepper_idle_lock_time;
+IntSetting* direction_delay_microseconds;
 IntSetting* enable_delay_microseconds;
 
 AxisMaskSetting* step_invert_mask;
@@ -351,8 +352,7 @@ void make_settings() {
         new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinUp", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30, checkSpindleChange);
     spindle_delay_spindown =
         new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinDown", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30, checkSpindleChange);
-    coolant_start_delay =
-        new FloatSetting(EXTENDED, WG, NULL, "Coolant/Delay/TurnOn", DEFAULT_COOLANT_DELAY_TURNON, 0, 30);
+    coolant_start_delay = new FloatSetting(EXTENDED, WG, NULL, "Coolant/Delay/TurnOn", DEFAULT_COOLANT_DELAY_TURNON, 0, 30);
 
     spindle_enbl_off_with_zero_speed =
         new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/OffWithSpeed", DEFAULT_SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED, checkSpindleChange);
@@ -393,13 +393,17 @@ void make_settings() {
     junction_deviation = new FloatSetting(GRBL, WG, "11", "GCode/JunctionDeviation", DEFAULT_JUNCTION_DEVIATION, 0, 10);
     status_mask        = new IntSetting(GRBL, WG, "10", "Report/Status", DEFAULT_STATUS_REPORT_MASK, 0, 3);
 
-    probe_invert           = new FlagSetting(GRBL, WG, "6", "Probe/Invert", DEFAULT_INVERT_PROBE_PIN);
-    limit_invert           = new FlagSetting(GRBL, WG, "5", "Limits/Invert", DEFAULT_INVERT_LIMIT_PINS);
-    step_enable_invert     = new FlagSetting(GRBL, WG, "4", "Stepper/EnableInvert", DEFAULT_INVERT_ST_ENABLE);
-    dir_invert_mask        = new AxisMaskSetting(GRBL, WG, "3", "Stepper/DirInvert", DEFAULT_DIRECTION_INVERT_MASK, postMotorSetting);
-    step_invert_mask       = new AxisMaskSetting(GRBL, WG, "2", "Stepper/StepInvert", DEFAULT_STEPPING_INVERT_MASK, postMotorSetting);
-    stepper_idle_lock_time = new IntSetting(GRBL, WG, "1", "Stepper/IdleTime", DEFAULT_STEPPER_IDLE_LOCK_TIME, 0, 255);
-    pulse_microseconds     = new IntSetting(GRBL, WG, "0", "Stepper/Pulse", DEFAULT_STEP_PULSE_MICROSECONDS, 3, 1000);
+    probe_invert                 = new FlagSetting(GRBL, WG, "6", "Probe/Invert", DEFAULT_INVERT_PROBE_PIN);
+    limit_invert                 = new FlagSetting(GRBL, WG, "5", "Limits/Invert", DEFAULT_INVERT_LIMIT_PINS);
+    step_enable_invert           = new FlagSetting(GRBL, WG, "4", "Stepper/EnableInvert", DEFAULT_INVERT_ST_ENABLE);
+    dir_invert_mask              = new AxisMaskSetting(GRBL, WG, "3", "Stepper/DirInvert", DEFAULT_DIRECTION_INVERT_MASK, postMotorSetting);
+    step_invert_mask             = new AxisMaskSetting(GRBL, WG, "2", "Stepper/StepInvert", DEFAULT_STEPPING_INVERT_MASK, postMotorSetting);
+    stepper_idle_lock_time       = new IntSetting(GRBL, WG, "1", "Stepper/IdleTime", DEFAULT_STEPPER_IDLE_LOCK_TIME, 0, 255);
+    pulse_microseconds           = new IntSetting(GRBL, WG, "0", "Stepper/Pulse", DEFAULT_STEP_PULSE_MICROSECONDS, 3, 1000);
+    direction_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Direction/Delay", 0, 0, 1000);
+    enable_delay_microseconds    = new IntSetting(EXTENDED, WG, NULL, "Stepper/Enable/Delay", 0, 0, 1000);  // microseconds
+
+    direction_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Direction/Delay", STEP_PULSE_DELAY, 0, 1000);
     enable_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Enable/Delay", DEFAULT_STEP_ENABLE_DELAY, 0, 1000);  // microseconds
 
     stallguard_debug_mask = new AxisMaskSetting(EXTENDED, WG, NULL, "Report/StallGuard", 0, postMotorSetting);
diff --git a/Grbl_Esp32/src/SettingsDefinitions.h b/Grbl_Esp32/src/SettingsDefinitions.h
index 84c62367..b079786e 100644
--- a/Grbl_Esp32/src/SettingsDefinitions.h
+++ b/Grbl_Esp32/src/SettingsDefinitions.h
@@ -18,6 +18,7 @@ extern StringSetting* build_info;
 
 extern IntSetting* pulse_microseconds;
 extern IntSetting* stepper_idle_lock_time;
+extern IntSetting* direction_delay_microseconds;
 extern IntSetting* enable_delay_microseconds;
 
 extern AxisMaskSetting* step_invert_mask;
diff --git a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp
index b18249e5..a62112fd 100644
--- a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp
+++ b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp
@@ -24,7 +24,7 @@
     Remove power before changing bits.
 
     The documentation is okay once you get how it works, but unfortunately
-    incomplete... See H2ASpindle.md for the remainder of the docs that I 
+    incomplete... See H2ASpindle.md for the remainder of the docs that I
     managed to piece together.
 */
 
@@ -80,31 +80,35 @@ namespace Spindles {
         data.msg[5] = uint8_t(speed & 0xFF);
     }
 
-    H2A::response_parser H2A::get_max_rpm(ModbusCommand& data) {
-        // NOTE: data length is excluding the CRC16 checksum.
-        data.tx_length = 6;
-        data.rx_length = 8;
+    VFD::response_parser H2A::initialization_sequence(int index, ModbusCommand& data) {
+        if (index == -1) {
+            // NOTE: data length is excluding the CRC16 checksum.
+            data.tx_length = 6;
+            data.rx_length = 8;
 
-        // Send: 01 03 B005 0002
-        data.msg[1] = 0x03;  // READ
-        data.msg[2] = 0xB0;  // B0.05 = Get RPM
-        data.msg[3] = 0x05;
-        data.msg[4] = 0x00;  // Read 2 values
-        data.msg[5] = 0x02;
+            // Send: 01 03 B005 0002
+            data.msg[1] = 0x03;  // READ
+            data.msg[2] = 0xB0;  // B0.05 = Get RPM
+            data.msg[3] = 0x05;
+            data.msg[4] = 0x00;  // Read 2 values
+            data.msg[5] = 0x02;
 
-        //  Recv: 01 03 00 04 5D C0 03 F6
-        //                    -- -- = 24000 (val #1)
-        return [](const uint8_t* response, Spindles::VFD* vfd) -> bool {
-            uint16_t rpm  = (uint16_t(response[4]) << 8) | uint16_t(response[5]);
-            vfd->_max_rpm = rpm;
+            //  Recv: 01 03 00 04 5D C0 03 F6
+            //                    -- -- = 24000 (val #1)
+            return [](const uint8_t* response, Spindles::VFD* vfd) -> bool {
+                uint16_t rpm  = (uint16_t(response[4]) << 8) | uint16_t(response[5]);
+                vfd->_max_rpm = rpm;
 
-            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "H2A spindle is initialized at %d RPM", int(rpm));
+                grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "H2A spindle is initialized at %d RPM", int(rpm));
 
-            return true;
-        };
+                return true;
+            };
+        } else {
+            return nullptr;
+        }
     }
 
-    H2A::response_parser H2A::get_current_rpm(ModbusCommand& data) {
+    VFD::response_parser H2A::get_current_rpm(ModbusCommand& data) {
         // NOTE: data length is excluding the CRC16 checksum.
         data.tx_length = 6;
         data.rx_length = 8;
@@ -118,16 +122,16 @@ namespace Spindles {
 
         //  Recv: 01 03 0004 095D 0000
         //                   ---- = 2397 (val #1)
-
-        // TODO: What are we going to do with this? Update sys.spindle_speed? Update vfd state?
         return [](const uint8_t* response, Spindles::VFD* vfd) -> bool {
             uint16_t rpm = (uint16_t(response[4]) << 8) | uint16_t(response[5]);
+
             // Set current RPM value? Somewhere?
+            vfd->_sync_rpm = rpm;
             return true;
         };
     }
 
-    H2A::response_parser H2A::get_current_direction(ModbusCommand& data) {
+    VFD::response_parser H2A::get_current_direction(ModbusCommand& data) {
         // NOTE: data length is excluding the CRC16 checksum.
         data.tx_length = 6;
         data.rx_length = 6;
diff --git a/Grbl_Esp32/src/Spindles/H2ASpindle.h b/Grbl_Esp32/src/Spindles/H2ASpindle.h
index d206603b..8243867a 100644
--- a/Grbl_Esp32/src/Spindles/H2ASpindle.h
+++ b/Grbl_Esp32/src/Spindles/H2ASpindle.h
@@ -29,9 +29,12 @@ namespace Spindles {
         void direction_command(SpindleState mode, ModbusCommand& data) override;
         void set_speed_command(uint32_t rpm, ModbusCommand& data) override;
 
-        response_parser get_max_rpm(ModbusCommand& data) override;
+        response_parser initialization_sequence(int index, ModbusCommand& data) override;
         response_parser get_current_rpm(ModbusCommand& data) override;
         response_parser get_current_direction(ModbusCommand& data) override;
         response_parser get_status_ok(ModbusCommand& data) override { return nullptr; }
+
+        bool supports_actual_rpm() const override { return true; }
+        bool safety_polling() const override { return false; }
     };
 }
diff --git a/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp b/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp
index e0172de3..22fddb9d 100644
--- a/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp
+++ b/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp
@@ -41,24 +41,31 @@
 
     Protocol Details
 
-    VFD frequencies are in Hz. Multiply by 60 for RPM
+    A lot of good information about the details of all these parameters and how they should 
+    be setup can be found on this page: 
+    https://community.carbide3d.com/t/vfd-parameters-huanyang-model/15459/7 . 
 
-    before using spindle, VFD must be setup for RS485 and match your spindle
-    PD001   2   RS485 Control of run commands
-    PD002   2   RS485 Control of operating frequency
-    PD005   400 Maximum frequency Hz (Typical for spindles)
-    PD011   120 Min Speed (Recommend Aircooled=120 Water=100)
-    PD014   10  Acceleration time (Test to optimize)
-    PD015   10  Deceleration time (Test to optimize)
-    PD023   1   Reverse run enabled
-    PD142   3.7 Max current Amps (0.8kw=3.7 1.5kw=7.0, 2.2kw=??)
-    PD143   2   Poles most are 2 (I think this is only used for RPM calc from Hz)
-    PD163   1   RS485 Address: 1 (Typical. OK to change...see below)
-    PD164   1   RS485 Baud rate: 9600 (Typical. OK to change...see below)
-    PD165   3   RS485 Mode: RTU, 8N1
+    Before using spindle, VFD must be setup for RS485 and match your spindle:
+
+    PD004   400  Base frequency as rated on my spindle (default was 50)
+    PD005   400  Maximum frequency Hz (Typical for spindles)
+    PD011   120  Min Speed (Recommend Aircooled=120 Water=100)
+    PD014   10   Acceleration time (Test to optimize)
+    PD015   10   Deceleration time (Test to optimize)
+    PD023   1    Reverse run enabled
+    PD141   220  Spindle max rated voltage
+    PD142   3.7  Max current Amps (0.8kw=3.7 1.5kw=7.0, 2.2kw=??)
+    PD143   2    Poles most are 2 (I think this is only used for RPM calc from Hz)
+    PD144   3000 Max rated motor revolution at 50 Hz => 24000@400Hz = 3000@50HZ
+
+    PD001   2    RS485 Control of run commands
+    PD002   2    RS485 Control of operating frequency
+    PD163   1    RS485 Address: 1 (Typical. OK to change...see below)
+    PD164   1    RS485 Baud rate: 9600 (Typical. OK to change...see below)
+    PD165   3    RS485 Mode: RTU, 8N1
 
     The official documentation of the RS485 is horrible. I had to piece it together from
-    a lot of different sources
+    a lot of different sources:
 
     Manuals: https://github.com/RobertOlechowski/Huanyang_VFD/tree/master/Documentations/pdf
     Reference: https://github.com/Smoothieware/Smoothieware/blob/edge/src/modules/tools/spindle/HuanyangSpindleControl.cpp
@@ -95,6 +102,17 @@
 
     ==========================================================================
 
+    Setting registers
+    Addr    Read    Len     Reg     DataH   DataL   CRC     CRC
+    0x01    0x01    0x03    5       0x00    0x00    CRC     CRC     //  PD005
+    0x01    0x01    0x03    11      0x00    0x00    CRC     CRC     //  PD011
+    0x01    0x01    0x03    143     0x00    0x00    CRC     CRC     //  PD143
+    0x01    0x01    0x03    144     0x00    0x00    CRC     CRC     //  PD144
+
+    Message is returned with requested value = (DataH * 16) + DataL (see decimal offset above)
+
+    ==========================================================================
+
     Status registers
     Addr    Read    Len     Reg     DataH   DataL   CRC     CRC
     0x01    0x04    0x03    0x00    0x00    0x00    CRC     CRC     //  Set Frequency * 100 (25Hz = 2500)
@@ -105,8 +123,30 @@
     0x01    0x04    0x03    0x05    0x00    0x00    CRC     CRC     //  AC voltage
     0x01    0x04    0x03    0x06    0x00    0x00    CRC     CRC     //  Cont
     0x01    0x04    0x03    0x07    0x00    0x00    CRC     CRC     //  VFD Temp
+    
     Message is returned with requested value = (DataH * 16) + DataL (see decimal offset above)
 
+    ==========================================================================
+
+    The math:
+
+        PD005   400  Maximum frequency Hz (Typical for spindles)
+        PD011   120  Min Speed (Recommend Aircooled=120 Water=100)
+        PD143   2    Poles most are 2 (I think this is only used for RPM calc from Hz)
+        PD144   3000 Max rated motor revolution at 50 Hz => 24000@400Hz = 3000@50HZ
+
+    During initialization these 4 are pulled from the VFD registers. It then sets min and max RPM
+    of the spindle. So:
+
+        MinRPM = PD011 * PD144 / 50 = 120 * 3000 / 50 = 7200 RPM min
+        MaxRPM = PD005 * PD144 / 50 = 400 * 3000 / 50 = 24000 RPM max
+
+    If you then set 12000 RPM, it calculates the frequency:
+
+        int targetFrequency = targetRPM * PD005 / MaxRPM = targetRPM * PD005 / (PD005 * PD144 / 50) = 
+                              targetRPM * 50 / PD144 = 12000 * 50 / 3000 = 200
+
+    If the frequency is -say- 25 Hz, Huanyang wants us to send 2500 (eg. 25.00 Hz).
 */
 
 #include <driver/uart.h>
@@ -151,13 +191,148 @@ namespace Spindles {
         data.msg[1] = 0x05;
         data.msg[2] = 0x02;
 
-        uint16_t value = (uint16_t)(rpm * 100 / 60);  // send Hz * 10  (Ex:1500 RPM = 25Hz .... Send 2500)
+        // Frequency comes from a conversion of revolutions per second to revolutions per minute
+        // (factor of 60) and a factor of 2 from counting the number of poles. E.g. rpm * 120 / 100.
+
+        // int targetFrequency = targetRPM * PD005 / MaxRPM = targetRPM * PD005 / (PD005 * PD144 / 50) =
+        // targetRPM * 50 / PD144
+        //
+        // Huanyang wants a factor 100 bigger numbers. So, 1500 rpm -> 25 HZ. Send 2500.
+
+        uint16_t value = rpm * 5000 / _maxRpmAt50Hz;
 
         data.msg[3] = (value >> 8) & 0xFF;
         data.msg[4] = (value & 0xFF);
     }
 
-    Huanyang::response_parser Huanyang::get_status_ok(ModbusCommand& data) {
+    VFD::response_parser Huanyang::initialization_sequence(int index, ModbusCommand& data) {
+        // NOTE: data length is excluding the CRC16 checksum.
+        data.tx_length = 6;
+        data.rx_length = 6;
+
+        // data.msg[0] is omitted (modbus address is filled in later)
+        data.msg[1] = 0x01;  // Read setting
+        data.msg[2] = 0x03;  // Len
+        //      [3] = set below...
+        data.msg[4] = 0x00;
+        data.msg[5] = 0x00;
+
+        if (index == -1) {
+            // Max frequency
+            data.msg[3] = 5;  // PD005: max frequency the VFD will allow. Normally 400.
+
+            return [](const uint8_t* response, Spindles::VFD* vfd) -> bool {
+                uint16_t value = (response[4] << 8) | response[5];
+#ifdef VFD_DEBUG_MODE
+                grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Max frequency = %d", value);
+#endif
+
+                // Set current RPM value? Somewhere?
+                auto huanyang           = static_cast<Huanyang*>(vfd);
+                huanyang->_maxFrequency = value;
+                return true;
+            };
+
+        } else if (index == -2) {
+            // Min Frequency
+            data.msg[3] = 11;  // PD011: frequency lower limit. Normally 0.
+
+            return [](const uint8_t* response, Spindles::VFD* vfd) -> bool {
+                uint16_t value = (response[4] << 8) | response[5];
+
+#ifdef VFD_DEBUG_MODE
+                grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Min frequency set to %d", value);
+#endif
+
+                // Set current RPM value? Somewhere?
+                auto huanyang           = static_cast<Huanyang*>(vfd);
+                huanyang->_minFrequency = value;
+                return true;
+            };
+        } else if (index == -3) {
+            // Max rated revolutions @ 50Hz
+
+            data.msg[3] = 144;  // PD144: max rated motor revolution at 50Hz => 24000@400Hz = 3000@50HZ
+
+            return [](const uint8_t* response, Spindles::VFD* vfd) -> bool {
+                uint16_t value = (response[4] << 8) | response[5];
+#ifdef VFD_DEBUG_MODE
+                grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Max rated revolutions @ 50Hz = %d", value);
+#endif
+                // Set current RPM value? Somewhere?
+                auto huanyang           = static_cast<Huanyang*>(vfd);
+                huanyang->_maxRpmAt50Hz = value;
+
+                // Regarding PD144, the 2 versions of the manuals both say "This is set according to the
+                // actual revolution of the motor. The displayed value is the same as this set value. It
+                // can be used as a monitoring parameter, which is convenient to the user. This set value
+                // corresponds to the revolution at 50Hz".
+
+                // Calculate the VFD settings:
+                huanyang->updateRPM();
+
+                return true;
+            };
+        }
+        /*
+        The number of poles seems to be over constrained information with PD144. If we're wrong, here's how 
+        to get this information. Note that you probably have to call 'updateRPM' here then:
+        --
+
+        else if (index == -4) {
+            // Number Poles
+
+            data.msg[3] = 143;  // PD143: 4 or 2 poles in motor. Default is 4. A spindle being 24000RPM@400Hz implies 2 poles
+
+            return [](const uint8_t* response, Spindles::VFD* vfd) -> bool {
+                uint8_t value = response[4]; // Single byte response.
+
+                // Sanity check. We expect something like 2 or 4 poles.
+                if (value <= 4 && value >= 2) {
+#ifdef VFD_DEBUG_MODE
+                    // Set current RPM value? Somewhere?
+                    grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Number of poles set to %d", value);
+#endif
+
+                    auto huanyang = static_cast<Huanyang*>(vfd);
+                    huanyang->_numberPoles = value;
+                    return true;
+                }
+                else {
+                    grbl_msg_sendf(CLIENT_ALL,
+                        MsgLevel::Error,
+                        "Initialization of Huanyang spindle failed. Number of poles (PD143, expected 2-4, got %d) is not sane.",
+                        value);
+                    return false;
+                }
+            };
+
+        }
+        */
+
+        // Done.
+        return nullptr;
+    }
+
+    void Huanyang::updateRPM() {
+        /*
+        PD005 = 400 ; max frequency the VFD will allow
+        MaxRPM = PD005 * 50 / PD176
+
+        Frequencies are a factor 100 of what they should be.
+        */
+
+        if (_minFrequency > _maxFrequency) {
+            _minFrequency = _maxFrequency;
+        }
+
+        this->_min_rpm = uint32_t(_minFrequency) * uint32_t(_maxRpmAt50Hz) / 5000;  //   0 * 3000 / 50 =   0 RPM.
+        this->_max_rpm = uint32_t(_maxFrequency) * uint32_t(_maxRpmAt50Hz) / 5000;  // 400 * 3000 / 50 = 24k RPM.
+
+        grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: VFD settings read: RPM Range(%d, %d)]", _min_rpm, _max_rpm);
+    }
+
+    VFD::response_parser Huanyang::get_status_ok(ModbusCommand& data) {
         // NOTE: data length is excluding the CRC16 checksum.
         data.tx_length = 6;
         data.rx_length = 6;
@@ -176,4 +351,42 @@ namespace Spindles {
         }
         return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { return true; };
     }
+
+    VFD::response_parser Huanyang::get_current_rpm(ModbusCommand& data) {
+        // NOTE: data length is excluding the CRC16 checksum.
+        data.tx_length = 6;
+        data.rx_length = 6;
+
+        // data.msg[0] is omitted (modbus address is filled in later)
+        data.msg[1] = 0x04;
+        data.msg[2] = 0x03;
+        data.msg[3] = 0x01;  // Output frequency
+        data.msg[4] = 0x00;
+        data.msg[5] = 0x00;
+
+        return [](const uint8_t* response, Spindles::VFD* vfd) -> bool {
+            uint16_t frequency = (response[4] << 8) | response[5];
+
+            auto huanyang = static_cast<Huanyang*>(vfd);
+
+            // Since we set a frequency, it's only fair to check if that's output in the spindle sync.
+            // The reason we check frequency instead of RPM is because RPM assumes a linear relation
+            // between RPM and frequency - which isn't necessarily true.
+            //
+            // We calculate the frequency back to RPM the same way as we calculated the frequency in the
+            // first place. In other words, this code must match the set_speed_command method. Note that
+            // we test sync_rpm instead of frequency, because that matches whatever a vfd can throw at us.
+            //
+            // value = rpm * 5000 / maxRpmAt50Hz
+            //
+            // It follows that:
+            // frequency_value_x100 * maxRpmAt50Hz / 5000 = rpm
+
+            auto rpm = uint32_t(frequency) * uint32_t(huanyang->_maxRpmAt50Hz) / 5000;
+
+            // Store RPM for synchronization
+            vfd->_sync_rpm = rpm;
+            return true;
+        };
+    }
 }
diff --git a/Grbl_Esp32/src/Spindles/HuanyangSpindle.h b/Grbl_Esp32/src/Spindles/HuanyangSpindle.h
index 48fecfdf..9c6ee875 100644
--- a/Grbl_Esp32/src/Spindles/HuanyangSpindle.h
+++ b/Grbl_Esp32/src/Spindles/HuanyangSpindle.h
@@ -6,7 +6,7 @@
     HuanyangSpindle.h
 
     Part of Grbl_ESP32
-    2020 -    Bart Dring
+    2020 -  Bart Dring
     2020 -  Stefan de Bruijn
 
     Grbl is free software: you can redistribute it and/or modify
@@ -28,11 +28,22 @@ namespace Spindles {
         int reg;
 
     protected:
+        uint16_t _minFrequency = 0;    // PD011: frequency lower limit. Normally 0.
+        uint16_t _maxFrequency = 400;  // PD005: max frequency the VFD will allow. Normally 400.
+        uint16_t _maxRpmAt50Hz = 100;  // PD144: rated motor revolution at 50Hz => 24000@400Hz = 3000@50HZ
+        // uint16_t _numberPoles  = 2; // PD143: 4 or 2 poles in motor. Default is 4. A spindle being 24000RPM@400Hz implies 2 poles
+
+        void updateRPM();
+
         void default_modbus_settings(uart_config_t& uart) override;
 
         void direction_command(SpindleState mode, ModbusCommand& data) override;
         void set_speed_command(uint32_t rpm, ModbusCommand& data) override;
 
+        response_parser initialization_sequence(int index, ModbusCommand& data) override;
         response_parser get_status_ok(ModbusCommand& data) override;
+        response_parser get_current_rpm(ModbusCommand& data) override;
+
+        bool supports_actual_rpm() const override { return true; }
     };
 }
diff --git a/Grbl_Esp32/src/Spindles/Spindle.cpp b/Grbl_Esp32/src/Spindles/Spindle.cpp
index f705aea7..b9cfed56 100644
--- a/Grbl_Esp32/src/Spindles/Spindle.cpp
+++ b/Grbl_Esp32/src/Spindles/Spindle.cpp
@@ -104,4 +104,4 @@ namespace Spindles {
     void Spindle::deinit() { stop(); }
 }
 
-Spindles::Spindle* spindle;
+ Spindles::Spindle* spindle;
diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp
index 76e30c80..a332d0fd 100644
--- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp
+++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp
@@ -1,9 +1,9 @@
 /*
     VFDSpindle.cpp
 
-    This is for a VFD based spindles via RS485 Modbus. The details of the 
-    VFD protocol heavily depend on the VFD in question here. We have some 
-    implementations, but if yours is not here, the place to start is the 
+    This is for a VFD based spindles via RS485 Modbus. The details of the
+    VFD protocol heavily depend on the VFD in question here. We have some
+    implementations, but if yours is not here, the place to start is the
     manual. This VFD class implements the modbus functionality.
 
     Part of Grbl_ESP32
@@ -26,7 +26,7 @@
     Remove power before changing bits.
 
     TODO:
-      - We can report spindle_state and rpm better with VFD's that support 
+      - We can report spindle_state and rpm better with VFD's that support
         either mode, register RPM or actual RPM.
       - Destructor should break down the task.
       - Move min/max RPM to protected members.
@@ -34,11 +34,19 @@
 */
 #include "VFDSpindle.h"
 
+#include <freertos/task.h>
+
+// Timing and modbus... The manual states that between communications, we should respect a
+// silent interval of 3,5 characters. If we received communications between these times, we
+// have to assume that the message is broken. We use a poll rate of 250 ms here, which should
+// be plenty: assuming 9600 8N1, that's roughly 250 chars. A message of 2x16 chars with 4x4
+// chars buffering is just 40 chars.
+
 const uart_port_t VFD_RS485_UART_PORT  = UART_NUM_2;  // hard coded for this port right now
 const int         VFD_RS485_BUF_SIZE   = 127;
-const int         VFD_RS485_QUEUE_SIZE = 10;   // numv\ber of commands that can be queued up.
-const int         RESPONSE_WAIT_TICKS  = 50;   // how long to wait for a response
-const int         VFD_RS485_POLL_RATE  = 200;  // in milliseconds between commands
+const int         VFD_RS485_QUEUE_SIZE = 10;    // numv\ber of commands that can be queued up.
+const int         RESPONSE_WAIT_MILLIS = 1000;  // how long to wait for a response in milliseconds
+const int         VFD_RS485_POLL_RATE  = 250;   // in milliseconds between commands
 
 // OK to change these
 // #define them in your machine definition file if you want different values
@@ -53,11 +61,12 @@ namespace Spindles {
     // The communications task
     void VFD::vfd_cmd_task(void* pvParameters) {
         static bool unresponsive = false;  // to pop off a message once each time it becomes unresponsive
-        static int  pollidx      = 0;
+        static int  pollidx      = -1;     // -1 starts the VFD initialization sequence
 
         VFD*          instance = static_cast<VFD*>(pvParameters);
         ModbusCommand next_cmd;
         uint8_t       rx_message[VFD_RS485_MAX_MSG_SIZE];
+        bool          safetyPollingEnabled = instance->safety_polling();
 
         while (true) {
             response_parser parser = nullptr;
@@ -66,10 +75,10 @@ namespace Spindles {
 
             // First check if we should ask the VFD for the max RPM value as part of the initialization. We
             // should also query this is max_rpm is 0, because that means a previous initialization failed:
-            if (pollidx == 0 || (instance->_max_rpm == 0 && (parser = instance->get_max_rpm(next_cmd)) != nullptr)) {
-                pollidx           = 1;
-                next_cmd.critical = true;
+            if ((pollidx < 0 || instance->_max_rpm == 0) && (parser = instance->initialization_sequence(pollidx, next_cmd)) != nullptr) {
+                next_cmd.critical = false;
             } else {
+                pollidx           = 1;  // Done with initialization. Main sequence.
                 next_cmd.critical = false;
             }
 
@@ -77,34 +86,39 @@ namespace Spindles {
             if (parser == nullptr && xQueueReceive(vfd_cmd_queue, &next_cmd, 0) != pdTRUE) {
                 // We poll in a cycle. Note that the switch will fall through unless we encounter a hit.
                 // The weakest form here is 'get_status_ok' which should be implemented if the rest fails.
-                switch (pollidx) {
-                    case 1:
-                        parser = instance->get_current_rpm(next_cmd);
-                        if (parser) {
-                            pollidx = 2;
-                            break;
-                        }
-                        // fall through intentionally:
-                    case 2:
-                        parser = instance->get_current_direction(next_cmd);
-                        if (parser) {
-                            pollidx = 3;
-                            break;
-                        }
-                        // fall through intentionally:
-                    case 3:
-                        parser  = instance->get_status_ok(next_cmd);
-                        pollidx = 1;
+                if (instance->_syncing) {
+                    parser = instance->get_current_rpm(next_cmd);
+                } else if (safetyPollingEnabled) {
+                    switch (pollidx) {
+                        case 1:
+                            parser = instance->get_current_rpm(next_cmd);
+                            if (parser) {
+                                pollidx = 2;
+                                break;
+                            }
+                            // fall through intentionally:
+                        case 2:
+                            parser = instance->get_current_direction(next_cmd);
+                            if (parser) {
+                                pollidx = 3;
+                                break;
+                            }
+                            // fall through intentionally:
+                        case 3:
+                        default:
+                            parser  = instance->get_status_ok(next_cmd);
+                            pollidx = 1;
 
-                        // we could complete this in case parser == nullptr with some ifs, but let's
-                        // just keep it easy and wait an iteration.
-                        break;
+                            // we could complete this in case parser == nullptr with some ifs, but let's
+                            // just keep it easy and wait an iteration.
+                            break;
+                    }
                 }
 
                 // If we have no parser, that means get_status_ok is not implemented (and we have
                 // nothing resting in our queue). Let's fall back on a simple continue.
                 if (parser == nullptr) {
-                    vTaskDelay(VFD_RS485_POLL_RATE);
+                    vTaskDelay(VFD_RS485_POLL_RATE / portTICK_PERIOD_MS);
                     continue;  // main while loop
                 }
             }
@@ -120,7 +134,7 @@ namespace Spindles {
                 next_cmd.msg[next_cmd.tx_length - 1] = (crc16 & 0xFF00) >> 8;
                 next_cmd.msg[next_cmd.tx_length - 2] = (crc16 & 0xFF);
 
-#ifdef VFD_DEBUG_MODE
+#ifdef VFD_DEBUG_MODE2
                 if (parser == nullptr) {
                     report_hex_msg(next_cmd.msg, "RS485 Tx: ", next_cmd.tx_length);
                 }
@@ -130,12 +144,36 @@ namespace Spindles {
             // Assume for the worst, and retry...
             int retry_count = 0;
             for (; retry_count < MAX_RETRIES; ++retry_count) {
-                // Flush the UART and write the data:
+                // Flush the UART:
                 uart_flush(VFD_RS485_UART_PORT);
+
+                // Write the data:
                 uart_write_bytes(VFD_RS485_UART_PORT, reinterpret_cast<const char*>(next_cmd.msg), next_cmd.tx_length);
+                uart_wait_tx_done(VFD_RS485_UART_PORT, RESPONSE_WAIT_MILLIS / portTICK_PERIOD_MS);
 
                 // Read the response
-                uint16_t read_length = uart_read_bytes(VFD_RS485_UART_PORT, rx_message, next_cmd.rx_length, RESPONSE_WAIT_TICKS);
+                uint16_t read_length = 0;
+                uint16_t current_read =
+                    uart_read_bytes(VFD_RS485_UART_PORT, rx_message, next_cmd.rx_length, RESPONSE_WAIT_MILLIS / portTICK_PERIOD_MS);
+                read_length += current_read;
+
+                // Apparently some Huanyang report modbus errors in the correct way, and the rest not. Sigh.
+                // Let's just check for the condition, and truncate the first byte.
+                if (read_length > 0 && VFD_RS485_ADDR != 0 && rx_message[0] == 0) {
+                    memmove(rx_message + 1, rx_message, read_length - 1);
+                }
+
+                while (read_length < next_cmd.rx_length && current_read > 0) {
+                    // Try to read more; we're not there yet...
+                    current_read = uart_read_bytes(VFD_RS485_UART_PORT,
+                                                   rx_message + read_length,
+                                                   next_cmd.rx_length - read_length,
+                                                   RESPONSE_WAIT_MILLIS / portTICK_PERIOD_MS);
+                    read_length += current_read;
+                }
+                if (current_read < 0) {
+                    read_length = 0;
+                }
 
                 // Generate crc16 for the response:
                 auto crc16response = ModRTU_CRC(rx_message, next_cmd.rx_length - 2);
@@ -145,20 +183,30 @@ namespace Spindles {
                     rx_message[read_length - 1] == (crc16response & 0xFF00) >> 8 &&  // check CRC byte 1
                     rx_message[read_length - 2] == (crc16response & 0xFF)) {         // check CRC byte 1
 
-                    // success
+                    // Success
                     unresponsive = false;
                     retry_count  = MAX_RETRIES + 1;  // stop retry'ing
 
                     // Should we parse this?
-                    if (parser != nullptr && !parser(rx_message, instance)) {
+                    if (parser != nullptr) {
+                        if (parser(rx_message, instance)) {
+                            // If we're initializing, move to the next initialization command:
+                            if (pollidx < 0) {
+                                --pollidx;
+                            }
+                        } else {
+                            // If we were initializing, move back to where we started.
 #ifdef VFD_DEBUG_MODE
-                        report_hex_msg(next_cmd.msg, "RS485 Tx: ", next_cmd.tx_length);
-                        report_hex_msg(rx_message, "RS485 Rx: ", read_length);
+                            // Parsing failed
+                            report_hex_msg(next_cmd.msg, "RS485 Tx: ", next_cmd.tx_length);
+                            report_hex_msg(rx_message, "RS485 Rx: ", read_length);
 #endif
 
-                        // Not succesful! Now what?
-                        unresponsive = true;
-                        grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 did not give a satisfying response");
+                            // Not succesful! Now what?
+                            unresponsive = true;
+                            pollidx      = -1;  // Re-initializing the VFD seems like a plan
+                            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 did not give a satisfying response");
+                        }
                     }
                 } else {
 #ifdef VFD_DEBUG_MODE
@@ -184,7 +232,7 @@ namespace Spindles {
 
                     // Wait a bit before we retry. Set the delay to poll-rate. Not sure
                     // if we should use a different value...
-                    vTaskDelay(VFD_RS485_POLL_RATE);
+                    vTaskDelay(VFD_RS485_POLL_RATE / portTICK_PERIOD_MS);
 
                     static UBaseType_t uxHighWaterMark = 0;
                     reportTaskStackSize(uxHighWaterMark);
@@ -193,17 +241,18 @@ namespace Spindles {
 
             if (retry_count == MAX_RETRIES) {
                 if (!unresponsive) {
-                    grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length);
+                    grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length);
                     unresponsive = true;
+                    pollidx      = -1;
                 }
                 if (next_cmd.critical) {
-                    grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Critical Spindle RS485 Unresponsive");
+                    grbl_msg_sendf(CLIENT_ALL, MsgLevel::Error, "Critical Spindle RS485 Unresponsive");
                     mc_reset();
                     sys_rt_exec_alarm = ExecAlarm::SpindleControl;
                 }
             }
 
-            vTaskDelay(VFD_RS485_POLL_RATE);  // TODO: What is the best value here?
+            vTaskDelay(VFD_RS485_POLL_RATE / portTICK_PERIOD_MS);
         }
     }
 
@@ -217,7 +266,9 @@ namespace Spindles {
     }
 
     void VFD::init() {
-        vfd_ok = false;  // initialize
+        vfd_ok    = false;  // initialize
+        _sync_rpm = 0;
+        _syncing  = false;
 
         grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Initializing RS485 VFD spindle");
 
@@ -266,6 +317,15 @@ namespace Spindles {
             return;
         }
 
+        // We have to initialize the constants before starting the task:
+        is_reversable = true;  // these VFDs are always reversable
+        use_delays    = true;
+        vfd_ok        = true;
+
+        // Initially we initialize this to 0; over time, we might poll better information from the VFD.
+        _current_rpm   = 0;
+        _current_state = SpindleState::Disable;
+
         // Initialization is complete, so now it's okay to run the queue task:
         if (!_task_running) {  // init can happen many times, we only want to start one task
             vfd_cmd_queue = xQueueCreate(VFD_RS485_QUEUE_SIZE, sizeof(ModbusCommand));
@@ -280,14 +340,6 @@ namespace Spindles {
             _task_running = true;
         }
 
-        is_reversable = true;  // these VFDs are always reversable
-        use_delays    = true;
-        vfd_ok        = true;
-
-        // Initially we initialize this to 0; over time, we might poll better information from the VFD.
-        _current_rpm   = 0;
-        _current_state = SpindleState::Disable;
-
         config_message();
     }
 
@@ -323,6 +375,7 @@ namespace Spindles {
             pins_settings_ok = false;
         }
 
+        // Use config for initial RPM values:
         _min_rpm = rpm_min->get();
         _max_rpm = rpm_max->get();
 
@@ -346,32 +399,93 @@ namespace Spindles {
             return;  // Block during abort.
         }
 
-        bool critical = (sys.state == State::Cycle || state != SpindleState::Disable);
+        bool shouldWait = state != _current_state || state != SpindleState::Disable;
+        bool critical   = (sys.state == State::Cycle || state != SpindleState::Disable);
+
+        int32_t delayMillis = 1000;
 
         if (_current_state != state) {  // already at the desired state. This function gets called a lot.
             set_mode(state, critical);  // critical if we are in a job
             set_rpm(rpm);
 
-            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spin1");
-
             if (state == SpindleState::Disable) {
                 sys.spindle_speed = 0;
-                delay(_spindown_delay);
+                delayMillis       = _spindown_delay;
+                rpm               = 0;
 
             } else {
-                delay(_spinup_delay);
+                delayMillis = _spinup_delay;
+            }
+
+            if (_current_state != state && !supports_actual_rpm()) {
+                delay(delayMillis);
             }
-            
-            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spin2");
         } else {
             if (_current_rpm != rpm) {
                 set_rpm(rpm);
+
+                if (rpm > _current_rpm) {
+                    delayMillis = _spinup_delay;
+                } else {
+                    delayMillis = _spindown_delay;
+                }
             }
         }
 
-        _current_state = state;  // store locally for faster get_state()
+        if (shouldWait) {
+            if (supports_actual_rpm()) {
+                _syncing = true;
 
-        sys.report_ovr_counter = 0;  // Set to report change immediately
+                // Allow 2.5% difference from what we asked for. Should be fine.
+                uint32_t drpm = (_max_rpm - _min_rpm) / 40;
+                if (drpm < 100) {
+                    drpm = 100;
+                }  // Just a sanity check
+
+                auto minRpmAllowed = _current_rpm > drpm ? (_current_rpm - drpm) : 0;
+                auto maxRpmAllowed = _current_rpm + drpm;
+
+                int       unchanged = 0;
+                const int limit     = 20;  // 20 * 0.5s = 10 sec
+                auto      last      = _sync_rpm;
+
+                while ((_sync_rpm < minRpmAllowed || _sync_rpm > maxRpmAllowed) && unchanged < limit) {
+#ifdef VFD_DEBUG_MODE
+                    grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Syncing RPM. Requested %d, current %d", int(rpm), int(_sync_rpm));
+#endif
+                    if (!mc_dwell(500)) {
+                        // Something happened while we were dwelling, like a safety door.
+                        unchanged = limit;
+                        last      = _sync_rpm;
+                        break;
+                    }
+
+                    if (_sync_rpm == last) {
+                        ++unchanged;
+                    } else {
+                        unchanged = 0;
+                    }
+                    last = _sync_rpm;
+                }
+
+                if (unchanged == limit) {
+                    grbl_msg_sendf(CLIENT_ALL,
+                                   MsgLevel::Error,
+                                   "Critical Spindle RS485 did not reach speed %d. Reported speed is %d rpm.",
+                                   rpm,
+                                   _sync_rpm);
+                    mc_reset();
+                    sys_rt_exec_alarm = ExecAlarm::SpindleControl;
+                }
+
+                _syncing = false;
+            } else {
+                delay(delayMillis);
+            }
+        }
+
+        _current_state         = state;  // store locally for faster get_state()
+        sys.report_ovr_counter = 0;      // Set to report change immediately
 
         return;
     }
@@ -407,26 +521,32 @@ namespace Spindles {
             return 0;
         }
 
-#ifdef VFD_DEBUG_MODE
-        grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Setting spindle speed to %d rpm (%d, %d)", int(rpm), int(_min_rpm), int(_max_rpm));
-#endif
-
         // apply override
         rpm = rpm * sys.spindle_speed_ovr / 100;  // Scale by spindle speed override value (uint8_t percent)
 
-        // apply limits
-        if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) {
-            rpm = _max_rpm;
-        } else if (rpm != 0 && rpm <= _min_rpm) {
-            rpm = _min_rpm;
+        if (rpm < _min_rpm || rpm > _max_rpm) {
+            grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Requested speed %d outside range:(%d,%d)", rpm, _min_rpm, _max_rpm);
+            rpm = constrain(rpm, _min_rpm, _max_rpm);
+            grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Requested speed changed to %d", rpm);
         }
 
+        // apply limits
+        // if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) {
+        //     rpm = _max_rpm;
+        // } else if (rpm != 0 && rpm <= _min_rpm) {
+        //     rpm = _min_rpm;
+        // }
+
         sys.spindle_speed = rpm;
 
         if (rpm == _current_rpm) {  // prevent setting same RPM twice
             return rpm;
         }
 
+#ifdef VFD_DEBUG_MODE2
+        grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Setting spindle speed to %d rpm (%d, %d)", int(rpm), int(_min_rpm), int(_max_rpm));
+#endif
+
         _current_rpm = rpm;
 
         // TODO add the speed modifiers override, linearization, etc.
@@ -434,9 +554,10 @@ namespace Spindles {
         ModbusCommand rpm_cmd;
         rpm_cmd.msg[0] = VFD_RS485_ADDR;
 
+        
         set_speed_command(rpm, rpm_cmd);
 
-        rpm_cmd.critical = false;
+        rpm_cmd.critical = (rpm == 0);
 
         if (xQueueSend(vfd_cmd_queue, &rpm_cmd, 0) != pdTRUE) {
             grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD Queue Full");
@@ -445,7 +566,7 @@ namespace Spindles {
         return rpm;
     }
 
-    void VFD::stop() { set_mode(SpindleState::Disable, false); }
+    void VFD::stop() { set_mode(SpindleState::Disable, true); }
 
     // state is cached rather than read right now to prevent delays
     SpindleState VFD::get_state() { return _current_state; }
@@ -471,4 +592,4 @@ namespace Spindles {
 
         return crc;
     }
-}
\ No newline at end of file
+}
diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.h b/Grbl_Esp32/src/Spindles/VFDSpindle.h
index 7e81443d..2661d246 100644
--- a/Grbl_Esp32/src/Spindles/VFDSpindle.h
+++ b/Grbl_Esp32/src/Spindles/VFDSpindle.h
@@ -2,11 +2,11 @@
 
 /*
     VFDSpindle.h
-    
+
     Part of Grbl_ESP32
     2020 -	Bart Dring
     2020 -  Stefan de Bruijn
-    
+
     Grbl is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation, either version 3 of the License, or
@@ -29,7 +29,7 @@ namespace Spindles {
     class VFD : public Spindle {
     private:
         static const int VFD_RS485_MAX_MSG_SIZE = 16;  // more than enough for a modbus message
-        static const int MAX_RETRIES            = 3;   // otherwise the spindle is marked 'unresponsive'
+        static const int MAX_RETRIES            = 5;   // otherwise the spindle is marked 'unresponsive'
 
         bool set_mode(SpindleState mode, bool critical);
         bool get_pins_and_settings();
@@ -66,10 +66,12 @@ namespace Spindles {
         // Commands that return the status. Returns nullptr if unavailable by this VFD (default):
         using response_parser = bool (*)(const uint8_t* response, VFD* spindle);
 
-        virtual response_parser get_max_rpm(ModbusCommand& data) { return nullptr; }
+        virtual response_parser initialization_sequence(int index, ModbusCommand& data) { return nullptr; }
         virtual response_parser get_current_rpm(ModbusCommand& data) { return nullptr; }
         virtual response_parser get_current_direction(ModbusCommand& data) { return nullptr; }
         virtual response_parser get_status_ok(ModbusCommand& data) = 0;
+        virtual bool            supports_actual_rpm() const { return false; }
+        virtual bool            safety_polling() const { return true; }
 
     public:
         VFD()           = default;
@@ -82,6 +84,8 @@ namespace Spindles {
         // Should hide them and use a member function.
         volatile uint32_t _min_rpm;
         volatile uint32_t _max_rpm;
+        volatile uint32_t _sync_rpm;
+        volatile bool     _syncing;
 
         void         init();
         void         config_message();
@@ -92,4 +96,4 @@ namespace Spindles {
 
         virtual ~VFD() {}
     };
-}
\ No newline at end of file
+}
diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp
index d4919614..c0d81a18 100644
--- a/Grbl_Esp32/src/Stepper.cpp
+++ b/Grbl_Esp32/src/Stepper.cpp
@@ -25,6 +25,8 @@
 
 #include "Grbl.h"
 
+#include <atomic>
+
 // Stores the planner block Bresenham algorithm execution data for the segments in the segment
 // buffer. Normally, this buffer is partially in-use, but, for the worst case scenario, it will
 // never exceed the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1).
@@ -58,10 +60,7 @@ typedef struct {
 
     uint32_t counter[MAX_N_AXIS];  // Counter variables for the bresenham line tracer
 
-#ifdef STEP_PULSE_DELAY
-    uint8_t step_bits;  // Stores out_bits output to complete the step pulse delay
-#endif
-
+    uint8_t  step_bits;        // Stores out_bits output to complete the step pulse delay
     uint8_t  execute_step;     // Flags step execution for each interrupt.
     uint8_t  step_pulse_time;  // Step pulse reset time after step rise
     uint8_t  step_outbits;     // The next stepping-bits to be output
@@ -81,7 +80,7 @@ static uint8_t          segment_buffer_head;
 static uint8_t          segment_next_head;
 
 // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though.
-static volatile uint8_t busy;
+static std::atomic<bool> busy;
 
 // Pointers for the step segment being prepped from the planner buffer. Accessed only by the
 // main program. Pointers may be planning segments or planner blocks ahead of what being executed.
@@ -182,7 +181,7 @@ stepper_id_t current_stepper = DEFAULT_STEPPER;
 
 	 The complete step timing should look this...
 		Direction pin is set
-		An optional (via STEP_PULSE_DELAY in config.h) is put after this
+		An optional delay (direction_delay_microseconds) is put after this
 		The step pin is started
 		A pulse length is determine (via option $0 ... pulse_microseconds)
 		The pulse is ended
@@ -196,19 +195,21 @@ static void stepper_pulse_func();
 // TODO: Replace direct updating of the int32 position counters in the ISR somehow. Perhaps use smaller
 // int8 variables and update position counters only when a segment completes. This can get complicated
 // with probing and homing cycles that require true real-time positions.
-void IRAM_ATTR onStepperDriverTimer(
-    void* para) {  // ISR It is time to take a step =======================================================================================
-    //const int timer_idx = (int)para;  // get the timer index
+void IRAM_ATTR onStepperDriverTimer(void* para) {
+    // Timer ISR, normally takes a step.
+    //
+    // When handling an interrupt within an interrupt serivce routine (ISR), the interrupt status bit
+    // needs to be explicitly cleared.
     TIMERG0.int_clr_timers.t0 = 1;
-    if (busy) {
-        return;  // The busy-flag is used to avoid reentering this interrupt
+
+    bool expected = false;
+    if (busy.compare_exchange_strong(expected, true)) {
+        stepper_pulse_func();
+
+        TIMERG0.hw_timer[STEP_TIMER_INDEX].config.alarm_en = TIMER_ALARM_EN;
+
+        busy.store(false);
     }
-    busy = true;
-
-    stepper_pulse_func();
-
-    TIMERG0.hw_timer[STEP_TIMER_INDEX].config.alarm_en = TIMER_ALARM_EN;
-    busy                                               = false;
 }
 
 /**
@@ -221,14 +222,42 @@ void IRAM_ATTR onStepperDriverTimer(
 static void stepper_pulse_func() {
     auto n_axis = number_axis->get();
 
-    motors_step(st.step_outbits, st.dir_outbits);
+    if (motors_direction(st.dir_outbits)) {
+        auto wait_direction = direction_delay_microseconds->get();
+        if (wait_direction > 0) {
+            // Stepper drivers need some time between changing direction and doing a pulse.
+            switch (current_stepper) {
+                case ST_I2S_STREAM:
+                    i2s_out_push_sample(wait_direction);
+                    break;
+                case ST_I2S_STATIC:
+                case ST_TIMED: {
+                    // wait for step pulse time to complete...some time expired during code above
+                    //
+                    // If we are using GPIO stepping as opposed to RMT, record the
+                    // time that we turned on the direction pins so we can delay a bit.
+                    // If we are using RMT, we can't delay here.
+                    auto direction_pulse_start_time = esp_timer_get_time() + wait_direction;
+                    while ((esp_timer_get_time() - direction_pulse_start_time) < 0) {
+                        NOP();  // spin here until time to turn off step
+                    }
+                    break;
+                }
+                case ST_RMT:
+                    break;
+            }
+        }
+    }
 
     // If we are using GPIO stepping as opposed to RMT, record the
     // time that we turned on the step pins so we can turn them off
     // at the end of this routine without incurring another interrupt.
     // This is unnecessary with RMT and I2S stepping since both of
     // those methods time the turn off automatically.
+    //
+    // NOTE: We could use direction_pulse_start_time + wait_direction, but let's play it safe
     uint64_t step_pulse_start_time = esp_timer_get_time();
+    motors_step(st.step_outbits);
 
     // If there is no step segment, attempt to pop one from the stepper buffer
     if (st.exec_segment == NULL) {
@@ -323,6 +352,8 @@ static void stepper_pulse_func() {
 }
 
 void stepper_init() {
+    busy.store(false); 
+    
     grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Axis count %d", number_axis->get());
     grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s", stepper_names[current_stepper]);
 
@@ -359,13 +390,20 @@ void st_wake_up() {
     // Enable stepper drivers.
     motors_set_disable(false);
     stepper_idle = false;
+
     // Initialize step pulse timing from settings. Here to ensure updating after re-writing.
-#ifdef STEP_PULSE_DELAY
+#ifdef USE_RMT_STEPS
     // Step pulse delay handling is not require with ESP32...the RMT function does it.
+    if (direction_delay_microseconds->get() < 1)
+    {
+        // Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement.
+        st.step_pulse_time = -(((pulse_microseconds->get() - 2) * ticksPerMicrosecond) >> 3);
+    }
 #else  // Normal operation
     // Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement.
     st.step_pulse_time = -(((pulse_microseconds->get() - 2) * ticksPerMicrosecond) >> 3);
 #endif
+
     // Enable Stepper Driver Interrupt
     Stepper_Timer_Start();
 }
@@ -390,7 +428,6 @@ void st_reset() {
     segment_buffer_tail = 0;
     segment_buffer_head = 0;  // empty = tail
     segment_next_head   = 1;
-    busy                = false;
     st.step_outbits     = 0;
     st.dir_outbits      = 0;  // Initialize direction bits to default.
     // TODO do we need to turn step pins off?
@@ -400,7 +437,6 @@ void st_reset() {
 void st_go_idle() {
     // Disable Stepper Driver Interrupt. Allow Stepper Port Reset Interrupt to finish, if active.
     Stepper_Timer_Stop();
-    busy = false;
 
     // Set stepper driver idle state, disabled or enabled, depending on settings and circumstances.
     if (((stepper_idle_lock_time->get() != 0xff) || sys_rt_exec_alarm != ExecAlarm::None || sys.state == State::Sleep) &&
diff --git a/Grbl_Esp32/src/WebUI/WebServer.cpp b/Grbl_Esp32/src/WebUI/WebServer.cpp
index 311c980a..b4a1cef5 100644
--- a/Grbl_Esp32/src/WebUI/WebServer.cpp
+++ b/Grbl_Esp32/src/WebUI/WebServer.cpp
@@ -480,10 +480,11 @@ namespace WebUI {
                 }
             }
             if (silent || !espresponse->anyOutput()) {
-                _webserver->send(err != Error::Ok ? 401 : 200, "text/plain", answer);
+                _webserver->send(err != Error::Ok ? 500 : 200, "text/plain", answer);
             } else {
                 espresponse->flush();
             }
+            if(espresponse) delete(espresponse);
         } else {  //execute GCODE
             if (auth_level == AuthenticationLevel::LEVEL_GUEST) {
                 _webserver->send(401, "text/plain", "Authentication failed!\n");
@@ -491,7 +492,7 @@ namespace WebUI {
             }
             //Instead of send several commands one by one by web  / send full set and split here
             String      scmd;
-            const char* res    = "";
+            bool hasError =false;
             uint8_t     sindex = 0;
             // TODO Settings - this is very inefficient.  get_Splited_Value() is O(n^2)
             // when it could easily be O(n).  Also, it would be just as easy to push
@@ -512,10 +513,10 @@ namespace WebUI {
                     scmd += "\n";
                 }
                 if (!Serial2Socket.push(scmd.c_str())) {
-                    res = "Error";
+                    hasError = true;
                 }
             }
-            _webserver->send(200, "text/plain", res);
+            _webserver->send(200, "text/plain", hasError?"Error":"");
         }
     }
 
diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp
index 84e2ec18..f2a312f2 100644
--- a/Grbl_Esp32/src/WebUI/WebSettings.cpp
+++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp
@@ -302,11 +302,11 @@ namespace WebUI {
         }
         if (!SPIFFS.exists(path)) {
             webPrintln("Error: No such file!");
-            return Error::SdFileNotFound;
+            return Error::FsFileNotFound;
         }
         File currentfile = SPIFFS.open(path, FILE_READ);
         if (!currentfile) {  //if file open success
-            return Error::SdFailedOpenFile;
+            return Error::FsFailedOpenFile;
         }
         //until no line in file
         Error   err;
@@ -338,11 +338,11 @@ namespace WebUI {
         }
         if (!SPIFFS.exists(path)) {
             webPrintln("Error: No such file!");
-            return Error::SdFileNotFound;
+            return Error::FsFileNotFound;
         }
         File currentfile = SPIFFS.open(path, FILE_READ);
         if (!currentfile) {
-            return Error::SdFailedOpenFile;
+            return Error::FsFailedOpenFile;
         }
         while (currentfile.available()) {
             // String currentline = currentfile.readStringUntil('\n');
@@ -686,16 +686,16 @@ namespace WebUI {
         if (state != SDState::Idle) {
             if (state == SDState::NotPresent) {
                 webPrintln("No SD Card");
-                return Error::SdFailedMount;
+                return Error::FsFailedMount;
             } else {
                 webPrintln("SD Card Busy");
-                return Error::SdFailedBusy;
+                return Error::FsFailedBusy;
             }
         }
         if (!openFile(SD, path.c_str())) {
-            report_status_message(Error::SdFailedRead, (espresponse) ? espresponse->client() : CLIENT_ALL);
+            report_status_message(Error::FsFailedRead, (espresponse) ? espresponse->client() : CLIENT_ALL);
             webPrintln("");
-            return Error::SdFailedOpenFile;
+            return Error::FsFailedOpenFile;
         }
         return Error::Ok;
     }
@@ -764,18 +764,18 @@ namespace WebUI {
         File file2del = SD.open(path);
         if (!file2del) {
             webPrintln("Cannot stat file!");
-            return Error::SdFileNotFound;
+            return Error::FsFileNotFound;
         }
         if (file2del.isDirectory()) {
             if (!SD.rmdir(path)) {
                 webPrintln("Cannot delete directory! Is directory empty?");
-                return Error::SdFailedDelDir;
+                return Error::FsFailedDelDir;
             }
             webPrintln("Directory deleted.");
         } else {
             if (!SD.remove(path)) {
                 webPrintln("Cannot delete file!");
-                return Error::SdFailedDelFile;
+                return Error::FsFailedDelFile;
             }
             webPrintln("File deleted.");
         }
@@ -788,10 +788,10 @@ namespace WebUI {
         if (state != SDState::Idle) {
             if (state == SDState::NotPresent) {
                 webPrintln("No SD Card");
-                return Error::SdFailedMount;
+                return Error::FsFailedMount;
             } else {
                 webPrintln("SD Card Busy");
-                return Error::SdFailedBusy;
+                return Error::FsFailedBusy;
             }
         }
         webPrintln("");
@@ -806,9 +806,35 @@ namespace WebUI {
     }
 #endif
 
+    void listDirLocalFS(fs::FS& fs, const char* dirname, uint8_t levels, uint8_t client) {
+        //char temp_filename[128]; // to help filter by extension	TODO: 128 needs a definition based on something
+        File root = fs.open(dirname);
+        if (!root) {
+            //FIXME: need proper error for FS and not usd sd one
+            report_status_message(Error::FsFailedOpenDir, client);
+            return;
+        }
+        if (!root.isDirectory()) {
+            //FIXME: need proper error for FS and not usd sd one
+            report_status_message(Error::FsDirNotFound, client);
+            return;
+        }
+        File file = root.openNextFile();
+        while (file) {
+            if (file.isDirectory()) {
+                if (levels) {
+                    listDirLocalFS(fs, file.name(), levels - 1, client);
+                }
+            } else {
+                grbl_sendf(CLIENT_ALL, "[FILE:%s|SIZE:%d]\r\n", file.name(), file.size());
+            }
+            file = root.openNextFile();
+        }
+    }
+
     static Error listLocalFiles(char* parameter, AuthenticationLevel auth_level) {  // No ESP command
         webPrintln("");
-        listDir(SPIFFS, "/", 10, espresponse->client());
+        listDirLocalFS(SPIFFS, "/", 10, espresponse->client());
         String ssd = "[Local FS Free:" + ESPResponseStream::formatBytes(SPIFFS.totalBytes() - SPIFFS.usedBytes());
         ssd += " Used:" + ESPResponseStream::formatBytes(SPIFFS.usedBytes());
         ssd += " Total:" + ESPResponseStream::formatBytes(SPIFFS.totalBytes());