Consolidate probe clearance, add section debug (#18576)

* Better section / function log
* Add do_z_clearance motion function

Marlin/src/core/debug_out.h

@@ -26,6 +26,7 @@
 //  (or not) in a given .cpp file
 //
 
+#undef DEBUG_SECTION
 #undef DEBUG_PRINT_P
 #undef DEBUG_ECHO_START
 #undef DEBUG_ERROR_START
@@ -53,6 +54,10 @@
 #undef DEBUG_DELAY
 
 #if DEBUG_OUT
+
+  #include "debug_section.h"
+  #define DEBUG_SECTION(N,S,D)    SectionLog N(PSTR(S),D)
+
   #define DEBUG_PRINT_P(P)        serialprintPGM(P)
   #define DEBUG_ECHO_START        SERIAL_ECHO_START
   #define DEBUG_ERROR_START       SERIAL_ERROR_START
@@ -78,7 +83,10 @@
   #define DEBUG_POS               SERIAL_POS
   #define DEBUG_XYZ               SERIAL_XYZ
   #define DEBUG_DELAY(ms)         serial_delay(ms)
+
 #else
+
+  #define DEBUG_SECTION(...)        NOOP
   #define DEBUG_PRINT_P(P)          NOOP
   #define DEBUG_ECHO_START()        NOOP
   #define DEBUG_ERROR_START()       NOOP
@@ -104,6 +112,7 @@
   #define DEBUG_POS(...)            NOOP
   #define DEBUG_XYZ(...)            NOOP
   #define DEBUG_DELAY(...)          NOOP
+
 #endif
 
 #undef DEBUG_OUT

Marlin/src/core/debug_section.h

@@ -0,0 +1,49 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "serial.h"
+#include "../module/motion.h"
+
+class SectionLog {
+public:
+  SectionLog(PGM_P const msg=nullptr, bool inbug=true) {
+    the_msg = msg;
+    if ((debug = inbug)) echo_msg(PSTR(">>>"));
+  }
+
+  ~SectionLog() { if (debug) echo_msg(PSTR("<<<")); }
+
+private:
+  PGM_P the_msg;
+  bool debug;
+
+  void echo_msg(PGM_P const pre) {
+    serialprintPGM(pre);
+    if (the_msg) {
+      SERIAL_CHAR(' ');
+      serialprintPGM(the_msg);
+    }
+    SERIAL_CHAR(' ');
+    print_xyz(current_position);
+  }
+};

Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp

@@ -462,7 +462,7 @@
             // Manually Probe Mesh in areas that can't be reached by the probe
             //
             SERIAL_ECHOLNPGM("Manually probing unreachable points.");
-            do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
+            do_z_clearance(Z_CLEARANCE_BETWEEN_PROBES);
 
             if (parser.seen('C') && !xy_seen) {
 
@@ -780,9 +780,7 @@
       probe.stow();
       TERN_(HAS_LCD_MENU, ui.capture());
 
-      #ifdef Z_AFTER_PROBING
       probe.move_z_after_probing();
-      #endif
 
       restore_ubl_active_state_and_leave();
 
@@ -858,7 +856,6 @@
       echo_and_take_a_measurement();
 
       const float z2 = measure_point_with_encoder();
-
       do_blocking_move_to_z(current_position.z + Z_CLEARANCE_BETWEEN_PROBES);
 
       const float thickness = ABS(z1 - z2);
@@ -899,7 +896,7 @@
         LCD_MESSAGEPGM(MSG_UBL_MOVING_TO_NEXT);
 
         do_blocking_move_to(ppos);
-        do_blocking_move_to_z(z_clearance);
+        do_z_clearance(z_clearance);
 
         KEEPALIVE_STATE(PAUSED_FOR_USER);
         ui.capture();
@@ -915,7 +912,7 @@
 
         if (click_and_hold()) {
           SERIAL_ECHOLNPGM("\nMesh only partially populated.");
-          do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
+          do_z_clearance(Z_CLEARANCE_DEPLOY_PROBE);
           return restore_ubl_active_state_and_leave();
         }
 
@@ -940,7 +937,7 @@
 
     void abort_fine_tune() {
       ui.return_to_status();
-      do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
+      do_z_clearance(Z_CLEARANCE_BETWEEN_PROBES);
       set_message_with_feedback(GET_TEXT(MSG_EDITING_STOPPED));
     }
 
@@ -1415,9 +1412,7 @@
         }
 
         probe.stow();
-        #ifdef Z_AFTER_PROBING
         probe.move_z_after_probing();
-        #endif
 
         if (abort_flag) {
           SERIAL_ECHOLNPGM("?Error probing point. Aborting operation.");
@@ -1478,9 +1473,7 @@
         }
       }
       probe.stow();
-      #ifdef Z_AFTER_PROBING
       probe.move_z_after_probing();
-      #endif
 
       if (abort_flag || finish_incremental_LSF(&lsf_results)) {
         SERIAL_ECHOPGM("Could not complete LSF!");

Marlin/src/gcode/bedlevel/G26.cpp

@@ -622,8 +622,7 @@ void GcodeSuite::G26() {
    */
   set_bed_leveling_enabled(!parser.seen('D'));
 
-  if (current_position.z < Z_CLEARANCE_BETWEEN_PROBES)
+  do_z_clearance(Z_CLEARANCE_BETWEEN_PROBES);
-    do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
 
   #if DISABLED(NO_VOLUMETRICS)
     bool volumetric_was_enabled = parser.volumetric_enabled;

Marlin/src/gcode/bedlevel/G35.cpp

@@ -75,10 +75,9 @@ static_assert(G35_PROBE_COUNT > 2, "TRAMMING_POINT_XY requires at least 3 XY pos
  *               51 - Counter-Clockwise M5
  **/
 void GcodeSuite::G35() {
-  if (DEBUGGING(LEVELING)) {
+  DEBUG_SECTION(log_G35, "G35", DEBUGGING(LEVELING));
-    DEBUG_ECHOLNPGM(">>> G35");
+
-    log_machine_info();
+  if (DEBUGGING(LEVELING)) log_machine_info();
-  }
 
   float z_measured[G35_PROBE_COUNT] = { 0 };
 
@@ -181,8 +180,6 @@ void GcodeSuite::G35() {
 
   // Home Z after the alignment procedure
   process_subcommands_now_P(PSTR("G28Z"));
-
-  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< G35");
 }
 
 #endif // ASSISTED_TRAMMING

Marlin/src/gcode/bedlevel/abl/G29.cpp

@@ -172,10 +172,8 @@ G29_TYPE GcodeSuite::G29() {
   #if ENABLED(DEBUG_LEVELING_FEATURE)
     const uint8_t old_debug_flags = marlin_debug_flags;
     if (seenQ) marlin_debug_flags |= MARLIN_DEBUG_LEVELING;
-    if (DEBUGGING(LEVELING)) {
+    DEBUG_SECTION(log_G29, "G29", DEBUGGING(LEVELING));
-      DEBUG_POS(">>> G29", current_position);
+    if (DEBUGGING(LEVELING)) log_machine_info();
-      log_machine_info();
-    }
     marlin_debug_flags = old_debug_flags;
     if (DISABLED(PROBE_MANUALLY) && seenQ) G29_RETURN(false);
   #endif
@@ -188,7 +186,7 @@ G29_TYPE GcodeSuite::G29() {
   if (axis_unhomed_error()) G29_RETURN(false);
 
   if (!no_action && planner.leveling_active && parser.boolval('O')) { // Auto-level only if needed
-    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Auto-level not needed, skip\n<<< G29");
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Auto-level not needed, skip");
     G29_RETURN(false);
   }
 
@@ -416,7 +414,7 @@ G29_TYPE GcodeSuite::G29() {
     // Deploy certain probes before starting probing
     #if HAS_BED_PROBE
       if (ENABLED(BLTOUCH))
-        do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
+        do_z_clearance(Z_CLEARANCE_DEPLOY_PROBE);
       else if (probe.deploy()) {
         set_bed_leveling_enabled(abl_should_enable);
         G29_RETURN(false);
@@ -884,7 +882,7 @@ G29_TYPE GcodeSuite::G29() {
   // Sync the planner from the current_position
   if (planner.leveling_active) sync_plan_position();
 
-  #if HAS_BED_PROBE && defined(Z_AFTER_PROBING)
+  #if HAS_BED_PROBE
     probe.move_z_after_probing();
   #endif
 
@@ -900,8 +898,6 @@ G29_TYPE GcodeSuite::G29() {
 
   report_current_position();
 
-  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< G29");
-
   G29_RETURN(isnan(measured_z));
 }
 

Marlin/src/gcode/calibrate/G28.cpp

@@ -115,11 +115,11 @@
 #if ENABLED(Z_SAFE_HOMING)
 
   inline void home_z_safely() {
+    DEBUG_SECTION(log_G28, "home_z_safely", DEBUGGING(LEVELING));
+
     // Disallow Z homing if X or Y homing is needed
     if (axis_unhomed_error(_BV(X_AXIS) | _BV(Y_AXIS))) return;
 
-    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("home_z_safely >>>");
-
     sync_plan_position();
 
     /**
@@ -146,8 +146,6 @@
       LCD_MESSAGEPGM(MSG_ZPROBE_OUT);
       SERIAL_ECHO_MSG(STR_ZPROBE_OUT_SER);
     }
-
-    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< home_z_safely");
   }
 
 #endif // Z_SAFE_HOMING
@@ -197,15 +195,10 @@
  *
  */
 void GcodeSuite::G28() {
+  DEBUG_SECTION(log_G28, "G28", DEBUGGING(LEVELING));
+  if (DEBUGGING(LEVELING)) log_machine_info();
 
-  if (DEBUGGING(LEVELING)) {
+  TERN_(LASER_MOVE_G28_OFF, cutter.set_inline_enabled(false));  // turn off laser
-    DEBUG_ECHOLNPGM(">>> G28");
-    log_machine_info();
-  }
-
-  #if ENABLED(LASER_MOVE_G28_OFF)
-    cutter.set_inline_enabled(false);       // turn off laser
-  #endif
 
   TERN_(DWIN_CREALITY_LCD, HMI_flag.home_flag = true);
 
@@ -220,14 +213,13 @@ void GcodeSuite::G28() {
       sync_plan_position();
       SERIAL_ECHOLNPGM("Simulated Homing");
       report_current_position();
-      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< G28");
       return;
     }
   #endif
 
   // Home (O)nly if position is unknown
   if (!homing_needed() && parser.boolval('O')) {
-    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> homing not needed, skip\n<<< G28");
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> homing not needed, skip");
     return;
   }
 
@@ -313,8 +305,6 @@ void GcodeSuite::G28() {
                home_all = homeX == homeY && homeX == homeZ, // All or None
                doX = home_all || homeX, doY = home_all || homeY, doZ = home_all || homeZ;
 
-    destination = current_position;
-
     #if Z_HOME_DIR > 0  // If homing away from BED do Z first
 
       if (doZ) homeaxis(Z_AXIS);
@@ -322,17 +312,14 @@ void GcodeSuite::G28() {
     #endif
 
     const float z_homing_height =
-      (DISABLED(UNKNOWN_Z_NO_RAISE) || TEST(axis_known_position, Z_AXIS))
+      ENABLED(UNKNOWN_Z_NO_RAISE) && TEST(axis_known_position, Z_AXIS)
-        ? (parser.seenval('R') ? parser.value_linear_units() : Z_HOMING_HEIGHT)
+        ? 0
-        : 0;
+        : (parser.seenval('R') ? parser.value_linear_units() : Z_HOMING_HEIGHT);
 
-    if (z_homing_height && (doX || doY || ENABLED(Z_SAFE_HOMING))) {
+    if (z_homing_height && (doX || doY || (ENABLED(Z_SAFE_HOMING) && doZ))) {
       // Raise Z before homing any other axes and z is not already high enough (never lower z)
-      destination.z = z_homing_height + (TEST(axis_known_position, Z_AXIS) ? 0.0f : current_position.z);
+      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Raise Z (before homing) by ", z_homing_height);
-      if (destination.z > current_position.z) {
+      do_z_clearance(z_homing_height, TEST(axis_known_position, Z_AXIS), DISABLED(UNKNOWN_Z_NO_RAISE));
-        if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Raise Z (before homing) to ", destination.z);
-        do_blocking_move_to_z(destination.z);
-      }
     }
 
     #if ENABLED(QUICK_HOME)
@@ -387,15 +374,7 @@ void GcodeSuite::G28() {
 
         TERN(Z_SAFE_HOMING, home_z_safely(), homeaxis(Z_AXIS));
 
-        #if HOMING_Z_WITH_PROBE && defined(Z_AFTER_PROBING)
+        probe.move_z_after_homing();
-          #if Z_AFTER_HOMING > Z_AFTER_PROBING
-            do_blocking_move_to_z(Z_AFTER_HOMING);
-          #else
-            probe.move_z_after_probing();
-          #endif
-        #elif defined(Z_AFTER_HOMING)
-          do_blocking_move_to_z(Z_AFTER_HOMING);
-        #endif
 
       } // doZ
 
@@ -485,8 +464,6 @@ void GcodeSuite::G28() {
   if (ENABLED(NANODLP_Z_SYNC) && (doZ || ENABLED(NANODLP_ALL_AXIS)))
     SERIAL_ECHOLNPGM(STR_Z_MOVE_COMP);
 
-  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< G28");
-
   #if HAS_L64XX
     // Set L6470 absolute position registers to counts
     // constexpr *might* move this to PROGMEM.

Marlin/src/gcode/calibrate/G34_M422.cpp

@@ -56,10 +56,8 @@
  *   R<recalculate> points based on current probe offsets
  */
 void GcodeSuite::G34() {
-  if (DEBUGGING(LEVELING)) {
+  DEBUG_SECTION(log_G34, "G34", DEBUGGING(LEVELING));
-    DEBUG_ECHOLNPGM(">>> G34");
+  if (DEBUGGING(LEVELING)) log_machine_info();
-    log_machine_info();
-  }
 
   do { // break out on error
 
@@ -367,8 +365,6 @@ void GcodeSuite::G34() {
     #endif
 
   }while(0);
-
-  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< G34");
 }
 
 /**

Marlin/src/gcode/calibrate/G76_M871.cpp

@@ -104,7 +104,7 @@ void GcodeSuite::G76() {
   };
 
   auto g76_probe = [](const TempSensorID sid, uint16_t &targ, const xy_pos_t &nozpos) {
-    do_blocking_move_to_z(5.0); // Raise nozzle before probing
+    do_z_clearance(5.0); // Raise nozzle before probing
     const float measured_z = probe.probe_at_point(nozpos, PROBE_PT_STOW, 0, false);  // verbose=0, probe_relative=false
     if (isnan(measured_z))
       SERIAL_ECHOLNPGM("!Received NAN. Aborting.");

Marlin/src/gcode/calibrate/M666.cpp

@@ -38,7 +38,7 @@
    * M666: Set delta endstop adjustment
    */
   void GcodeSuite::M666() {
-    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM(">>> M666");
+    DEBUG_SECTION(log_M666, "M666", DEBUGGING(LEVELING));
     LOOP_XYZ(i) {
       if (parser.seen(XYZ_CHAR(i))) {
         const float v = parser.value_linear_units();
@@ -46,7 +46,6 @@
         if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("delta_endstop_adj[", XYZ_CHAR(i), "] = ", delta_endstop_adj[i]);
       }
     }
-    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< M666");
   }
 
 #elif HAS_EXTRA_ENDSTOPS

Marlin/src/gcode/control/T.cpp

@@ -48,10 +48,8 @@
  */
 void GcodeSuite::T(const uint8_t tool_index) {
 
-  if (DEBUGGING(LEVELING)) {
+  DEBUG_SECTION(log_T, "T", DEBUGGING(LEVELING));
-    DEBUG_ECHOLNPAIR(">>> T(", tool_index, ")");
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("...(", tool_index, ")");
-    DEBUG_POS("BEFORE", current_position);
-  }
 
   // Count this command as movement / activity
   reset_stepper_timeout();
@@ -75,9 +73,4 @@ void GcodeSuite::T(const uint8_t tool_index) {
     );
 
   #endif
-
-  if (DEBUGGING(LEVELING)) {
-    DEBUG_POS("AFTER", current_position);
-    DEBUG_ECHOLNPGM("<<< T()");
-  }
 }

Marlin/src/gcode/probe/G30.cpp

@@ -57,9 +57,8 @@ void GcodeSuite::G30() {
 
   restore_feedrate_and_scaling();
 
-  #ifdef Z_AFTER_PROBING
+  if (raise_after == PROBE_PT_STOW)
-    if (raise_after == PROBE_PT_STOW) probe.move_z_after_probing();
+    probe.move_z_after_probing();
-  #endif
 
   report_current_position();
 }

Marlin/src/gcode/probe/M401_M402.cpp

@@ -41,9 +41,7 @@ void GcodeSuite::M401() {
  */
 void GcodeSuite::M402() {
   probe.stow();
-  #ifdef Z_AFTER_PROBING
   probe.move_z_after_probing();
-  #endif
   report_current_position();
 }
 

Marlin/src/module/delta.cpp

@@ -233,7 +233,8 @@ void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3)
  * This is like quick_home_xy() but for 3 towers.
  */
 void home_delta() {
-  if (DEBUGGING(LEVELING)) DEBUG_POS(">>> home_delta", current_position);
+  DEBUG_SECTION(log_home_delta, "home_delta", DEBUGGING(LEVELING));
+
   // Init the current position of all carriages to 0,0,0
   current_position.reset();
   destination.reset();
@@ -283,8 +284,6 @@ void home_delta() {
       line_to_current_position(homing_feedrate(Z_AXIS));
     }
   #endif
-
-  if (DEBUGGING(LEVELING)) DEBUG_POS("<<< home_delta", current_position);
 }
 
 #endif // DELTA

Marlin/src/module/motion.cpp

@@ -387,7 +387,8 @@ void _internal_move_to_destination(const feedRate_t &fr_mm_s/*=0.0f*/
  * Plan a move to (X, Y, Z) and set the current_position
  */
 void do_blocking_move_to(const float rx, const float ry, const float rz, const feedRate_t &fr_mm_s/*=0.0*/) {
-  if (DEBUGGING(LEVELING)) DEBUG_XYZ(">>> do_blocking_move_to", rx, ry, rz);
+  DEBUG_SECTION(log_move, "do_blocking_move_to", DEBUGGING(LEVELING));
+  if (DEBUGGING(LEVELING)) DEBUG_XYZ("> ", rx, ry, rz);
 
   const feedRate_t z_feedrate = fr_mm_s ?: homing_feedrate(Z_AXIS),
                   xy_feedrate = fr_mm_s ?: feedRate_t(XY_PROBE_FEEDRATE_MM_S);
@@ -471,8 +472,6 @@ void do_blocking_move_to(const float rx, const float ry, const float rz, const f
 
   #endif
 
-  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< do_blocking_move_to");
-
   planner.synchronize();
 }
 
@@ -507,6 +506,13 @@ void do_blocking_move_to_xy_z(const xy_pos_t &raw, const float &z, const feedRat
   do_blocking_move_to(raw.x, raw.y, z, fr_mm_s);
 }
 
+void do_z_clearance(const float &zclear, const bool z_known/*=true*/, const bool raise_on_unknown/*=true*/, const bool lower_allowed/*=false*/) {
+  const bool rel = raise_on_unknown && !z_known;
+  float zdest = zclear + (rel ? current_position.z : 0.0f);
+  if (!lower_allowed) NOLESS(zdest, current_position.z);
+  do_blocking_move_to_z(_MIN(zdest, Z_MAX_POS), MMM_TO_MMS(Z_PROBE_SPEED_FAST));
+}
+
 //
 // Prepare to do endstop or probe moves with custom feedrates.
 //  - Save / restore current feedrate and multiplier
@@ -1272,11 +1278,12 @@ feedRate_t get_homing_bump_feedrate(const AxisEnum axis) {
  * Home an individual linear axis
  */
 void do_homing_move(const AxisEnum axis, const float distance, const feedRate_t fr_mm_s=0.0) {
+  DEBUG_SECTION(log_move, "do_homing_move", DEBUGGING(LEVELING));
 
   const feedRate_t real_fr_mm_s = fr_mm_s ?: homing_feedrate(axis);
 
   if (DEBUGGING(LEVELING)) {
-    DEBUG_ECHOPAIR(">>> do_homing_move(", axis_codes[axis], ", ", distance, ", ");
+    DEBUG_ECHOPAIR("...(", axis_codes[axis], ", ", distance, ", ");
     if (fr_mm_s)
       DEBUG_ECHO(fr_mm_s);
     else
@@ -1349,8 +1356,6 @@ void do_homing_move(const AxisEnum axis, const float distance, const feedRate_t
     // Re-enable stealthChop if used. Disable diag1 pin on driver.
     TERN_(SENSORLESS_HOMING, end_sensorless_homing_per_axis(axis, stealth_states));
   }
-
-  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("<<< do_homing_move(", axis_codes[axis], ")");
 }
 
 /**

Marlin/src/module/motion.h

@@ -234,6 +234,8 @@ void remember_feedrate_and_scaling();
 void remember_feedrate_scaling_off();
 void restore_feedrate_and_scaling();
 
+void do_z_clearance(const float &zclear, const bool z_known=true, const bool raise_on_unknown=true, const bool lower_allowed=false);
+
 //
 // Homing
 //

Marlin/src/module/probe.cpp

@@ -260,15 +260,10 @@ xyz_pos_t Probe::offset; // Initialized by settings.load()
  * Raise Z to a minimum height to make room for a probe to move
  */
 void Probe::do_z_raise(const float z_raise) {
-  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Probe::move_z(", z_raise, ")");
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Probe::do_z_raise(", z_raise, ")");
-
   float z_dest = z_raise;
   if (offset.z < 0) z_dest -= offset.z;
-
+  do_z_clearance(z_dest);
-  NOMORE(z_dest, Z_MAX_POS);
-
-  if (z_dest > current_position.z)
-    do_blocking_move_to_z(z_dest);
 }
 
 FORCE_INLINE void probe_specific_action(const bool deploy) {
@@ -410,16 +405,6 @@ bool Probe::set_deployed(const bool deploy) {
   return false;
 }
 
-#ifdef Z_AFTER_PROBING
-  // After probing move to a preferred Z position
-  void Probe::move_z_after_probing() {
-    if (current_position.z != Z_AFTER_PROBING) {
-      do_blocking_move_to_z(Z_AFTER_PROBING);
-      current_position.z = Z_AFTER_PROBING;
-    }
-  }
-#endif
-
 /**
  * @brief Used by run_z_probe to do a single Z probe move.
  *
@@ -439,7 +424,7 @@ bool Probe::set_deployed(const bool deploy) {
  * @return TRUE if the probe failed to trigger.
  */
 bool Probe::probe_down_to_z(const float z, const feedRate_t fr_mm_s) {
-  if (DEBUGGING(LEVELING)) DEBUG_POS(">>> Probe::probe_down_to_z", current_position);
+  DEBUG_SECTION(log_probe, "Probe::probe_down_to_z", DEBUGGING(LEVELING));
 
   #if BOTH(HAS_HEATED_BED, WAIT_FOR_BED_HEATER)
     thermalManager.wait_for_bed_heating();
@@ -499,8 +484,6 @@ bool Probe::probe_down_to_z(const float z, const feedRate_t fr_mm_s) {
   // Tell the planner where we actually are
   sync_plan_position();
 
-  if (DEBUGGING(LEVELING)) DEBUG_POS("<<< Probe::probe_down_to_z", current_position);
-
   return !probe_triggered;
 }
 
@@ -513,8 +496,7 @@ bool Probe::probe_down_to_z(const float z, const feedRate_t fr_mm_s) {
  * @return The Z position of the bed at the current XY or NAN on error.
  */
 float Probe::run_z_probe(const bool sanity_check/*=true*/) {
-
+  DEBUG_SECTION(log_probe, "Probe::run_z_probe", DEBUGGING(LEVELING));
-  if (DEBUGGING(LEVELING)) DEBUG_POS(">>> Probe::run_z_probe", current_position);
 
   auto try_to_probe = [&](PGM_P const plbl, const float &z_probe_low_point, const feedRate_t fr_mm_s, const bool scheck, const float clearance) {
     // Do a first probe at the fast speed
@@ -527,7 +509,6 @@ float Probe::run_z_probe(const bool sanity_check/*=true*/) {
         if (probe_fail) DEBUG_ECHOPGM(" No trigger.");
         if (early_fail) DEBUG_ECHOPGM(" Triggered early.");
         DEBUG_EOL();
-        DEBUG_POS("<<< run_z_probe", current_position);
       }
     #else
       UNUSED(plbl);
@@ -651,8 +632,6 @@ float Probe::run_z_probe(const bool sanity_check/*=true*/) {
 
   #endif
 
-  if (DEBUGGING(LEVELING)) DEBUG_POS("<<< run_z_probe", current_position);
-
   return measured_z;
 }
 
@@ -666,9 +645,11 @@ float Probe::run_z_probe(const bool sanity_check/*=true*/) {
  * - Return the probed Z position
  */
 float Probe::probe_at_point(const float &rx, const float &ry, const ProbePtRaise raise_after/*=PROBE_PT_NONE*/, const uint8_t verbose_level/*=0*/, const bool probe_relative/*=true*/, const bool sanity_check/*=true*/) {
+  DEBUG_SECTION(log_probe, "Probe::probe_at_point", DEBUGGING(LEVELING));
+
   if (DEBUGGING(LEVELING)) {
     DEBUG_ECHOLNPAIR(
-      ">>> Probe::probe_at_point(", LOGICAL_X_POSITION(rx), ", ", LOGICAL_Y_POSITION(ry),
+      "...(", LOGICAL_X_POSITION(rx), ", ", LOGICAL_Y_POSITION(ry),
       ", ", raise_after == PROBE_PT_RAISE ? "raise" : raise_after == PROBE_PT_STOW ? "stow" : "none",
       ", ", int(verbose_level),
       ", ", probe_relative ? "probe" : "nozzle", "_relative)"
@@ -729,8 +710,6 @@ float Probe::probe_at_point(const float &rx, const float &ry, const ProbePtRaise
     #endif
   }
 
-  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< Probe::probe_at_point");
-
   return measured_z;
 }
 

Marlin/src/module/probe.h

@@ -79,9 +79,18 @@ public:
 
     #endif
 
+    static inline void move_z_after_probing() {
       #ifdef Z_AFTER_PROBING
-      static void move_z_after_probing();
+        do_z_clearance(Z_AFTER_PROBING, true, true, true); // Move down still permitted
       #endif
+    }
+    static inline void move_z_after_homing() {
+      #ifdef Z_AFTER_HOMING
+        do_z_clearance(Z_AFTER_HOMING, true, true, true);
+      #elif defined(Z_AFTER_PROBING)
+        move_z_after_probing();
+      #endif
+    }
     static float probe_at_point(const float &rx, const float &ry, const ProbePtRaise raise_after=PROBE_PT_NONE, const uint8_t verbose_level=0, const bool probe_relative=true, const bool sanity_check=true);
     static inline float probe_at_point(const xy_pos_t &pos, const ProbePtRaise raise_after=PROBE_PT_NONE, const uint8_t verbose_level=0, const bool probe_relative=true, const bool sanity_check=true) {
       return probe_at_point(pos.x, pos.y, raise_after, verbose_level, probe_relative, sanity_check);
@@ -89,6 +98,8 @@ public:
 
   #else
 
+    FORCE_INLINE static void move_z_after_homing() {}
+
     static constexpr xyz_pos_t offset = xyz_pos_t({ 0, 0, 0 }); // See #16767
 
     static bool set_deployed(const bool) { return false; }