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;