only backoff after homing if triggered and asked to move

diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp
index 99e0f24..727408d 100644 (file)
--- a/src/modules/tools/endstops/Endstops.cpp
+++ b/src/modules/tools/endstops/Endstops.cpp
@@ -230,12 +230,15 @@ void Endstops::on_idle(void *argument)
 }
 
 // if limit switches are enabled, then we must move off of the endstop otherwise we won't be able to move
-// TODO should check if triggered and only back off if triggered
-void Endstops::back_off_home()
+// checks if triggered and only backs off if triggered
+void Endstops::back_off_home(char axes_to_move)
 {
     this->status = BACK_OFF_HOME;
     for( int c = X_AXIS; c <= Z_AXIS; c++ ) {
+        if( ((axes_to_move >> c ) & 1) == 0) continue; // only for axes we asked to move
         if(this->limit_enable[c]) {
+            if( !this->pins[c + (this->home_direction[c] ? 0 : 3)].get() ) continue; // if not triggered no need to move off
+
             // Move off of the endstop using a regular relative move
             char buf[32];
             snprintf(buf, sizeof(buf), "G0 %c%1.4f F%1.4f", c+'X', this->retract_mm[c]*(this->home_direction[c]?1:-1), this->slow_rates[c]*60.0F);
@@ -528,7 +531,7 @@ void Endstops::on_gcode_received(void *argument)
 
             // if limit switches are enabled we must back off endstop after setting home
             // TODO should maybe be done before setting home so X0 does not retrigger?
-            back_off_home();
+            back_off_home(axes_to_move);
         }
     } else if (gcode->has_m) {
         switch (gcode->m) {

diff --git a/src/modules/tools/endstops/Endstops.h b/src/modules/tools/endstops/Endstops.h
index 332a200..86866e0 100644 (file)
--- a/src/modules/tools/endstops/Endstops.h
+++ b/src/modules/tools/endstops/Endstops.h
@@ -29,7 +29,7 @@ class Endstops : public Module{
         void wait_for_homed(char axes_to_move);
         void wait_for_homed_corexy(int axis);
         void corexy_home(int home_axis, bool dirx, bool diry, float fast_rate, float slow_rate, unsigned int retract_steps);
-        void back_off_home();
+        void back_off_home(char axes_to_move);
         void on_get_public_data(void* argument);
         void on_set_public_data(void* argument);
         void on_idle(void *argument);