diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h
index 1d3e57e3..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 = "20210305";
+const char* const GRBL_VERSION_BUILD = "20210306";
 
 //#include <sdkconfig.h>
 #include <Arduino.h>
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();
+        }
     }
 }