From b1b1f296a705f3725597dccfea11895f5f1e6834 Mon Sep 17 00:00:00 2001
From: ellensp <530024+ellensp@users.noreply.github.com>
Date: Thu, 10 Feb 2022 05:46:53 +1300
Subject: [PATCH] =?UTF-8?q?=F0=9F=9A=B8=20Restore=20active=20tool=20after?=
 =?UTF-8?q?=20ABL=20G29=20(#23692)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>
---
 Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp |  4 ++--
 Marlin/src/gcode/bedlevel/G35.cpp           |  2 +-
 Marlin/src/gcode/bedlevel/abl/G29.cpp       | 11 ++++++++++-
 3 files changed, 13 insertions(+), 4 deletions(-)

diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
index 15395bcc8d58..023d0c5b1d63 100644
--- a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
+++ b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
@@ -316,7 +316,7 @@ void unified_bed_leveling::G29() {
     planner.synchronize();
     // Send 'N' to force homing before G29 (internal only)
     if (axes_should_home() || parser.seen_test('N')) gcode.home_all_axes();
-    TERN_(HAS_MULTI_HOTEND, if (active_extruder) tool_change(0));
+    TERN_(HAS_MULTI_HOTEND, if (active_extruder != 0) tool_change(0, true));
   }
 
   // Invalidate one or more nearby mesh points, possibly all.
@@ -663,7 +663,7 @@ void unified_bed_leveling::G29() {
     UNUSED(probe_deployed);
   #endif
 
-  TERN_(HAS_MULTI_HOTEND, tool_change(old_tool_index));
+  TERN_(HAS_MULTI_HOTEND, if (old_tool_index != 0) tool_change(old_tool_index));
   return;
 }
 
diff --git a/Marlin/src/gcode/bedlevel/G35.cpp b/Marlin/src/gcode/bedlevel/G35.cpp
index 4dd1323a6c29..dd828bf0c873 100644
--- a/Marlin/src/gcode/bedlevel/G35.cpp
+++ b/Marlin/src/gcode/bedlevel/G35.cpp
@@ -155,7 +155,7 @@ void GcodeSuite::G35() {
 
   // Restore the active tool after homing
   #if HAS_MULTI_HOTEND
-    tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER)); // Fetch previous toolhead if not PARKING_EXTRUDER
+    if (old_tool_index != 0) tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER)); // Fetch previous toolhead if not PARKING_EXTRUDER
   #endif
 
   #if BOTH(HAS_LEVELING, RESTORE_LEVELING_AFTER_G35)
diff --git a/Marlin/src/gcode/bedlevel/abl/G29.cpp b/Marlin/src/gcode/bedlevel/abl/G29.cpp
index eea5d4a7f251..51d658b20e57 100644
--- a/Marlin/src/gcode/bedlevel/abl/G29.cpp
+++ b/Marlin/src/gcode/bedlevel/abl/G29.cpp
@@ -93,6 +93,10 @@ class G29_State {
   bool      dryrun,
             reenable;
 
+  #if HAS_MULTI_HOTEND
+    uint8_t tool_index;
+  #endif
+
   #if EITHER(PROBE_MANUALLY, AUTO_BED_LEVELING_LINEAR)
     int abl_probe_index;
   #endif
@@ -263,7 +267,10 @@ G29_TYPE GcodeSuite::G29() {
    */
   if (!g29_in_progress) {
 
-    TERN_(HAS_MULTI_HOTEND, if (active_extruder) tool_change(0));
+    #if HAS_MULTI_HOTEND
+      abl.tool_index = active_extruder;
+      if (active_extruder != 0) tool_change(0, true);
+    #endif
 
     #if EITHER(PROBE_MANUALLY, AUTO_BED_LEVELING_LINEAR)
       abl.abl_probe_index = -1;
@@ -891,6 +898,8 @@ G29_TYPE GcodeSuite::G29() {
 
   TERN_(HAS_DWIN_E3V2_BASIC, DWIN_CompletedLeveling());
 
+  TERN_(HAS_MULTI_HOTEND, if (abl.tool_index != 0) tool_change(abl.tool_index));
+
   report_current_position();
 
   TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE));