From 24a15332b3b5adcdffaf55ecba3c8eff82ba561e Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Thu, 28 Apr 2016 02:15:53 -0700
Subject: [PATCH] Encapsulate dual Z endstop handling

---
 Marlin/endstops.cpp | 35 +++++++++++++++++++----------------
 Marlin/endstops.h   |  6 ++++++
 2 files changed, 25 insertions(+), 16 deletions(-)

diff --git a/Marlin/endstops.cpp b/Marlin/endstops.cpp
index f87b4c9b6..ac37c5805 100644
--- a/Marlin/endstops.cpp
+++ b/Marlin/endstops.cpp
@@ -193,6 +193,21 @@ void Endstops::M119() {
   #endif
 } // Endstops::M119
 
+#if ENABLED(Z_DUAL_ENDSTOPS)
+
+  // Pass the result of the endstop test
+  void Endstops::test_dual_z_endstops(EndstopEnum es1, EndstopEnum es2) {
+    byte z_test = TEST_ENDSTOP(es1) | (TEST_ENDSTOP(es2) << 1); // bit 0 for Z, bit 1 for Z2
+    if (stepper.current_block->steps[Z_AXIS] > 0) {
+      stepper.endstop_triggered(Z_AXIS);
+      SBI(endstop_hit_bits, Z_MIN);
+      if (!stepper.performing_homing || (z_test == 0x3))  //if not performing home or if both endstops were trigged during homing...
+        stepper.kill_current_block();
+    }
+  }
+
+#endif
+
 // Check endstops - Called from ISR!
 void Endstops::update() {
 
@@ -290,14 +305,7 @@ void Endstops::update() {
               COPY_BIT(current_endstop_bits, Z_MIN, Z2_MIN);
             #endif
 
-            byte z_test = TEST_ENDSTOP(Z_MIN) | (TEST_ENDSTOP(Z2_MIN) << 1); // bit 0 for Z, bit 1 for Z2
-
-            if (z_test && stepper.current_block->steps[Z_AXIS] > 0) { // z_test = Z_MIN || Z2_MIN
-              stepper.endstop_triggered(Z_AXIS);
-              SBI(endstop_hit_bits, Z_MIN);
-              if (!performing_homing || (z_test == 0x3))  //if not performing home or if both endstops were trigged during homing...
-                stepper.kill_current_block();
-            }
+            test_dual_z_endstops(Z_MIN, Z2_MIN);
 
           #else // !Z_DUAL_ENDSTOPS
 
@@ -330,14 +338,7 @@ void Endstops::update() {
               COPY_BIT(current_endstop_bits, Z_MAX, Z2_MAX);
             #endif
 
-            byte z_test = TEST_ENDSTOP(Z_MAX) | (TEST_ENDSTOP(Z2_MAX) << 1); // bit 0 for Z, bit 1 for Z2
-
-            if (z_test && stepper.current_block->steps[Z_AXIS] > 0) {  // t_test = Z_MAX || Z2_MAX
-              stepper.endstop_triggered(Z_AXIS);
-              SBI(endstop_hit_bits, Z_MIN);
-              if (!performing_homing || (z_test == 0x3))  //if not performing home or if both endstops were trigged during homing...
-                stepper.kill_current_block();
-            }
+            test_dual_z_endstops(Z_MAX, Z2_MAX);
 
           #else // !Z_DUAL_ENDSTOPS
 
@@ -349,5 +350,7 @@ void Endstops::update() {
   #if ENABLED(COREXZ)
     }
   #endif
+
   old_endstop_bits = current_endstop_bits;
+
 } // Endstops::update()
diff --git a/Marlin/endstops.h b/Marlin/endstops.h
index c7e9c0a38..a72306728 100644
--- a/Marlin/endstops.h
+++ b/Marlin/endstops.h
@@ -92,6 +92,12 @@ class Endstops {
       volatile bool z_probe_enabled = false;
       FORCE_INLINE void enable_z_probe(bool onoff=true) { z_probe_enabled = onoff; }
     #endif
+
+  private:
+
+    #if ENABLED(Z_DUAL_ENDSTOPS)
+      void test_dual_z_endstops(EndstopEnum es1, EndstopEnum es2);
+    #endif
 };
 
 extern Endstops endstops;