From 25e5a3597fed4c9c9c0b074e4aa7c5a7c69d0403 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <thinkyhead@users.noreply.github.com>
Date: Mon, 10 Apr 2023 19:26:55 -0500
Subject: [PATCH] =?UTF-8?q?=F0=9F=9A=B8=20Fix=20G30=20behavior?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Co-Authored-By: David Fries <2767875+dfries@users.noreply.github.com>
---
 Marlin/src/gcode/probe/G30.cpp | 28 ++++++++++++++++++----------
 1 file changed, 18 insertions(+), 10 deletions(-)

diff --git a/Marlin/src/gcode/probe/G30.cpp b/Marlin/src/gcode/probe/G30.cpp
index 0a23e0981c..6548105487 100644
--- a/Marlin/src/gcode/probe/G30.cpp
+++ b/Marlin/src/gcode/probe/G30.cpp
@@ -53,15 +53,21 @@
  */
 void GcodeSuite::G30() {
 
+  xy_pos_t old_pos = current_position,
+           probepos = current_position;
+
+  const bool seenX = parser.seenval('X');
+  if (seenX) probepos.x = RAW_X_POSITION(parser.value_linear_units());
+  const bool seenY = parser.seenval('Y');
+  if (seenY) probepos.y = RAW_Y_POSITION(parser.value_linear_units());
+
   probe.use_probing_tool();
 
-  // Convert the given logical position to native position
-  const xy_pos_t pos = {
-    parser.seenval('X') ? RAW_X_POSITION(parser.value_linear_units()) : current_position.x,
-    parser.seenval('Y') ? RAW_Y_POSITION(parser.value_linear_units()) : current_position.y
-  };
+  if (probe.can_reach(probepos)) {
+
+    if (seenX) old_pos.x = probepos.x;
+    if (seenY) old_pos.y = probepos.y;
 
-  if (probe.can_reach(pos)) {
     // Disable leveling so the planner won't mess with us
     TERN_(HAS_LEVELING, set_bed_leveling_enabled(false));
 
@@ -74,15 +80,15 @@ void GcodeSuite::G30() {
     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);
+    const float measured_z = probe.probe_at_point(probepos, raise_after, 1);
     TERN_(HAS_PTC, ptc.set_enabled(true));
     if (!isnan(measured_z)) {
-      SERIAL_ECHOLNPGM("Bed X: ", pos.asLogical().x, " Y: ", pos.asLogical().y, " Z: ", measured_z);
+      SERIAL_ECHOLNPGM("Bed X: ", probepos.asLogical().x, " Y: ", probepos.asLogical().y, " Z: ", measured_z);
       #if EITHER(DWIN_LCD_PROUI, DWIN_CREALITY_LCD_JYERSUI)
         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(probepos.x, 1, 1, str_1),
+          dtostrf(probepos.y, 1, 1, str_2),
           dtostrf(measured_z, 1, 2, str_3)
         );
         ui.set_status(msg);
@@ -91,6 +97,8 @@ void GcodeSuite::G30() {
 
     restore_feedrate_and_scaling();
 
+    do_blocking_move_to(old_pos);
+
     if (raise_after == PROBE_PT_STOW)
       probe.move_z_after_probing();