From c5d888fdee5c509a5d7401fd81f23dcd7f1fe740 Mon Sep 17 00:00:00 2001 From: Greg Cormier Date: Sat, 20 Dec 2025 11:39:46 -0500 Subject: [PATCH] Fix Z home movement when XY already at TLS position When X and Y coordinates are already at the tool length sensor (TLS) position during tool change restore, Z was incorrectly not moving to the home position after probing. The issue was in the restore() function where Z movement was only executed inside a conditional block that checked if XY needed to move. This caused Z to remain at the probed position when starting from the TLS XY location. Fixed by decoupling Z home movement from XY position comparison, ensuring Z always returns to home after probing (unless $346 disables full position restore). --- tool_change.c | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/tool_change.c b/tool_change.c index e70585a..ca2e80d 100644 --- a/tool_change.c +++ b/tool_change.c @@ -139,18 +139,20 @@ static void reset (void) // Restore coolant and spindle status, return controlled point to original position. static bool restore (void) { - bool ok; + bool ok = true; plan_line_data_t plan_data; plan_data_init(&plan_data); plan_data.condition.rapid_motion = On; - if(!(ok = (target.values[plane.axis_0] == previous.values[plane.axis_0] && - target.values[plane.axis_1] == previous.values[plane.axis_1]))) { + // Always move Z to home position first (unless full restore is disabled) + target.values[plane.axis_linear] = sys.home_position[plane.axis_linear]; + ok = mc_line(target.values, &plan_data); - target.values[plane.axis_linear] = sys.home_position[plane.axis_linear]; - - if((ok = mc_line(target.values, &plan_data)) && !settings.flags.no_restore_position_after_M6) { + // Then move XY to previous position if needed and full restore is enabled + if(ok && !settings.flags.no_restore_position_after_M6) { + if(target.values[plane.axis_0] != previous.values[plane.axis_0] || + target.values[plane.axis_1] != previous.values[plane.axis_1]) { memcpy(&target, &previous, sizeof(coord_data_t)); target.values[plane.axis_linear] = sys.home_position[plane.axis_linear]; ok = mc_line(target.values, &plan_data);