From feafa321d7caa90f886ef7d66f6b0513d5c595e3 Mon Sep 17 00:00:00 2001
From: InsanityAutomation
 <38436470+InsanityAutomation@users.noreply.github.com>
Date: Tue, 19 Jul 2022 18:33:49 -0400
Subject: [PATCH] =?UTF-8?q?=F0=9F=9A=B8=20Use=20Tool=200=20for=20G30=20(#2?=
 =?UTF-8?q?4511)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Marlin/src/gcode/probe/G30.cpp | 74 ++++++++++++++++++++--------------
 1 file changed, 43 insertions(+), 31 deletions(-)

diff --git a/Marlin/src/gcode/probe/G30.cpp b/Marlin/src/gcode/probe/G30.cpp
index a16853bdf8..eae04f2817 100644
--- a/Marlin/src/gcode/probe/G30.cpp
+++ b/Marlin/src/gcode/probe/G30.cpp
@@ -33,6 +33,10 @@
   #include "../../feature/probe_temp_comp.h"
 #endif
 
+#if HAS_MULTI_HOTEND
+  #include "../../module/tool_change.h"
+#endif
+
 #if ENABLED(DWIN_LCD_PROUI)
   #include "../../lcd/marlinui.h"
 #endif
@@ -49,6 +53,11 @@
  */
 void GcodeSuite::G30() {
 
+  #if HAS_MULTI_HOTEND
+    const uint8_t old_tool_index = active_extruder;
+    tool_change(0);
+  #endif
+
   const xy_pos_t pos = { parser.linearval('X', current_position.x + probe.offset_xy.x),
                          parser.linearval('Y', current_position.y + probe.offset_xy.y) };
 
@@ -57,40 +66,43 @@ void GcodeSuite::G30() {
       SERIAL_ECHOLNF(GET_EN_TEXT_F(MSG_ZPROBE_OUT));
       LCD_MESSAGE(MSG_ZPROBE_OUT);
     #endif
-    return;
   }
-
-  // Disable leveling so the planner won't mess with us
-  TERN_(HAS_LEVELING, set_bed_leveling_enabled(false));
-
-  remember_feedrate_scaling_off();
-
-  TERN_(DWIN_LCD_PROUI, process_subcommands_now(F("G28O")));
-
-  const ProbePtRaise raise_after = parser.boolval('E', true) ? PROBE_PT_STOW : PROBE_PT_NONE;
-
-  TERN_(HAS_PTC, ptc.set_enabled(!parser.seen('C') || parser.value_bool()));
-  const float measured_z = probe.probe_at_point(pos, raise_after, 1);
-  TERN_(HAS_PTC, ptc.set_enabled(true));
-  if (!isnan(measured_z)) {
-    SERIAL_ECHOLNPGM("Bed X: ", pos.x, " Y: ", pos.y, " Z: ", measured_z);
-    #if ENABLED(DWIN_LCD_PROUI)
-      char msg[31], str_1[6], str_2[6], str_3[6];
-      sprintf_P(msg, PSTR("X:%s, Y:%s, Z:%s"),
-        dtostrf(pos.x, 1, 1, str_1),
-        dtostrf(pos.y, 1, 1, str_2),
-        dtostrf(measured_z, 1, 2, str_3)
-      );
-      ui.set_status(msg);
-    #endif
+  else {
+    // Disable leveling so the planner won't mess with us
+    TERN_(HAS_LEVELING, set_bed_leveling_enabled(false));
+
+    remember_feedrate_scaling_off();
+
+    TERN_(DWIN_LCD_PROUI, process_subcommands_now(F("G28O")));
+
+    const ProbePtRaise raise_after = parser.boolval('E', true) ? PROBE_PT_STOW : PROBE_PT_NONE;
+
+    TERN_(HAS_PTC, ptc.set_enabled(!parser.seen('C') || parser.value_bool()));
+    const float measured_z = probe.probe_at_point(pos, raise_after, 1);
+    TERN_(HAS_PTC, ptc.set_enabled(true));
+    if (!isnan(measured_z)) {
+      SERIAL_ECHOLNPGM("Bed X: ", pos.x, " Y: ", pos.y, " Z: ", measured_z);
+      #if ENABLED(DWIN_LCD_PROUI)
+        char msg[31], str_1[6], str_2[6], str_3[6];
+        sprintf_P(msg, PSTR("X:%s, Y:%s, Z:%s"),
+          dtostrf(pos.x, 1, 1, str_1),
+          dtostrf(pos.y, 1, 1, str_2),
+          dtostrf(measured_z, 1, 2, str_3)
+        );
+        ui.set_status(msg);
+      #endif
+    }
+
+    restore_feedrate_and_scaling();
+
+    if (raise_after == PROBE_PT_STOW)
+      probe.move_z_after_probing();
+
+    report_current_position();
   }
 
-  restore_feedrate_and_scaling();
-
-  if (raise_after == PROBE_PT_STOW)
-    probe.move_z_after_probing();
-
-  report_current_position();
+  // Restore the active tool
+  TERN_(HAS_MULTI_HOTEND, tool_change(old_tool_index));
 }
 
 #endif // HAS_BED_PROBE