Debug messages for homing and leveling

Marlin/Marlin_main.cpp

@@ -1033,6 +1033,19 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir, HOME_DIR);
 
 #endif //DUAL_X_CARRIAGE
 
+#ifdef DEBUG_LEVELING
+  void print_xyz(const char *prefix, const float x, const float y, const float z) {
+    SERIAL_ECHO(prefix);
+    SERIAL_ECHOPAIR(": (", x);
+    SERIAL_ECHOPAIR(", ", y);
+    SERIAL_ECHOPAIR(", ", z);
+    SERIAL_ECHOLNPGM(")");
+  }
+  void print_xyz(const char *prefix, const float xyz[]) {
+    print_xyz(prefix, xyz[X_AXIS], xyz[Y_AXIS], xyz[Z_AXIS]);
+  }
+#endif
+
 static void set_axis_is_at_home(AxisEnum axis) {
 
   #if ENABLED(DUAL_X_CARRIAGE)
@@ -1098,6 +1111,12 @@ static void set_axis_is_at_home(AxisEnum axis) {
     #if ENABLED(AUTO_BED_LEVELING_FEATURE) && Z_HOME_DIR < 0
       if (axis == Z_AXIS) current_position[Z_AXIS] -= zprobe_zoffset;
     #endif
+
+    #ifdef DEBUG_LEVELING
+      SERIAL_ECHOPAIR("set_axis_is_at_home ", (unsigned long)axis);
+      SERIAL_ECHOPAIR(" > (home_offset[axis]==", home_offset[axis]);
+      print_xyz(") > current_position", current_position);
+    #endif
   }
 }
 
@@ -1143,6 +1162,9 @@ static void setup_for_endstop_move() {
   saved_feedrate_multiplier = feedrate_multiplier;
   feedrate_multiplier = 100;
   refresh_cmd_timeout();
+  #ifdef DEBUG_LEVELING
+    SERIAL_ECHOLNPGM("setup_for_endstop_move > enable_endstops(true)");
+  #endif
   enable_endstops(true);
 }
 
@@ -1153,6 +1175,9 @@ static void setup_for_endstop_move() {
      * Calculate delta, start a line, and set current_position to destination
      */
     void prepare_move_raw() {
+      #ifdef DEBUG_LEVELING
+        print_xyz("prepare_move_raw > destination", destination);
+      #endif
       refresh_cmd_timeout();
       calculate_delta(destination);
       plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedrate_multiplier/100.0), active_extruder);
@@ -1180,6 +1205,10 @@ static void setup_for_endstop_move() {
         current_position[Y_AXIS] = corrected_position.y;
         current_position[Z_AXIS] = corrected_position.z;
 
+        #ifdef DEBUG_LEVELING
+          print_xyz("set_bed_level_equation_lsq > current_position", current_position);
+        #endif
+
         sync_plan_position();
       }
 
@@ -1209,6 +1238,10 @@ static void setup_for_endstop_move() {
       current_position[Y_AXIS] = corrected_position.y;
       current_position[Z_AXIS] = corrected_position.z;
 
+      #ifdef DEBUG_LEVELING
+        print_xyz("set_bed_level_equation_3pts > current_position", current_position);
+      #endif
+
       sync_plan_position();
     }
 
@@ -1221,6 +1254,10 @@ static void setup_for_endstop_move() {
       float start_z = current_position[Z_AXIS];
       long start_steps = st_get_position(Z_AXIS);
     
+      #ifdef DEBUG_LEVELING
+        SERIAL_ECHOLNPGM("run_z_probe (DELTA) 1");
+      #endif
+
       // move down slowly until you find the bed
       feedrate = homing_feedrate[Z_AXIS] / 4;
       destination[Z_AXIS] = -10;
@@ -1232,6 +1269,11 @@ static void setup_for_endstop_move() {
       long stop_steps = st_get_position(Z_AXIS);
       float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
       current_position[Z_AXIS] = mm;
+
+      #ifdef DEBUG_LEVELING
+        print_xyz("run_z_probe (DELTA) 2 > current_position", current_position);
+      #endif
+
       sync_plan_position_delta();
       
     #else // !DELTA
@@ -1266,6 +1308,10 @@ static void setup_for_endstop_move() {
       current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
       sync_plan_position();
 
+      #ifdef DEBUG_LEVELING
+        print_xyz("run_z_probe > current_position", current_position);
+      #endif
+      
     #endif // !DELTA
   }
 
@@ -1276,6 +1322,10 @@ static void setup_for_endstop_move() {
   static void do_blocking_move_to(float x, float y, float z) {
     float oldFeedRate = feedrate;
 
+    #ifdef DEBUG_LEVELING
+      print_xyz("do_blocking_move_to", x, y, z);
+    #endif
+
     #if ENABLED(DELTA)
 
       feedrate = XY_TRAVEL_SPEED;
@@ -1312,6 +1362,9 @@ static void setup_for_endstop_move() {
 
   static void clean_up_after_endstop_move() {
     #if ENABLED(ENDSTOPS_ONLY_FOR_HOMING)
+      #if ENABLED(DEBUG_LEVELING)
+        SERIAL_ECHOLNPGM("clean_up_after_endstop_move > ENDSTOPS_ONLY_FOR_HOMING > enable_endstops(false)");
+      #endif
       enable_endstops(false);
     #endif
     feedrate = saved_feedrate;
@@ -1321,6 +1374,10 @@ static void setup_for_endstop_move() {
 
   static void deploy_z_probe() {
 
+    #ifdef DEBUG_LEVELING
+      print_xyz("deploy_z_probe > current_position", current_position);
+    #endif
+
     #if HAS_SERVO_ENDSTOPS
 
       // Engage Z Servo endstop if enabled
@@ -1411,6 +1468,10 @@ static void setup_for_endstop_move() {
 
   static void stow_z_probe(bool doRaise=true) {
 
+    #ifdef DEBUG_LEVELING
+      print_xyz("stow_z_probe > current_position", current_position);
+    #endif
+
     #if HAS_SERVO_ENDSTOPS
 
       // Retract Z Servo endstop if enabled
@@ -1418,6 +1479,12 @@ static void setup_for_endstop_move() {
 
         #if Z_RAISE_AFTER_PROBING > 0
           if (doRaise) {
+            #ifdef DEBUG_LEVELING
+              SERIAL_ECHOPAIR("Raise Z (after) by ", (float)Z_RAISE_AFTER_PROBING);
+              SERIAL_EOL;
+              SERIAL_ECHOPAIR("> SERVO_ENDSTOPS > do_blocking_move_to_z ", current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING);
+              SERIAL_EOL;
+            #endif
             do_blocking_move_to_z(current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING); // this also updates current_position
             st_synchronize();
           }
@@ -1506,19 +1573,51 @@ static void setup_for_endstop_move() {
 
   // Probe bed height at position (x,y), returns the measured z value
   static float probe_pt(float x, float y, float z_before, ProbeAction probe_action=ProbeDeployAndStow, int verbose_level=1) {
+
+    #ifdef DEBUG_LEVELING
+      SERIAL_ECHOLNPGM("probe_pt >>>");
+      SERIAL_ECHOPAIR("> ProbeAction:", (unsigned long)probe_action);
+      SERIAL_EOL;
+      print_xyz("> current_position", current_position);
+    #endif
+
+    #ifdef DEBUG_LEVELING
+      SERIAL_ECHOPAIR("Z Raise to z_before ", z_before);
+      SERIAL_EOL;
+      SERIAL_ECHOPAIR("> do_blocking_move_to_z ", z_before);
+      SERIAL_EOL;
+    #endif
+
     // Move Z up to the z_before height, then move the Z probe to the given XY
     do_blocking_move_to_z(z_before); // this also updates current_position
+
+    #ifdef DEBUG_LEVELING
+      SERIAL_ECHOPAIR("> do_blocking_move_to_xy ", x - X_PROBE_OFFSET_FROM_EXTRUDER);
+      SERIAL_ECHOPAIR(", ", y - Y_PROBE_OFFSET_FROM_EXTRUDER);
+      SERIAL_EOL;
+    #endif
+
     do_blocking_move_to_xy(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER); // this also updates current_position
 
     #if DISABLED(Z_PROBE_SLED) && DISABLED(Z_PROBE_ALLEN_KEY)
-      if (probe_action & ProbeDeploy) deploy_z_probe();
+      if (probe_action & ProbeDeploy) {
+        #ifdef DEBUG_LEVELING
+          SERIAL_ECHOLNPGM("> ProbeDeploy");
+        #endif
+        deploy_z_probe();
+      }
     #endif
 
     run_z_probe();
     float measured_z = current_position[Z_AXIS];
 
     #if DISABLED(Z_PROBE_SLED) && DISABLED(Z_PROBE_ALLEN_KEY)
-      if (probe_action & ProbeStow) stow_z_probe();
+      if (probe_action & ProbeStow) {
+        #ifdef DEBUG_LEVELING
+          SERIAL_ECHOLNPGM("> ProbeStow (stow_z_probe will do Z Raise)");
+        #endif
+        stow_z_probe();
+      }
     #endif
 
     if (verbose_level > 2) {
@@ -1530,6 +1629,11 @@ static void setup_for_endstop_move() {
       SERIAL_PROTOCOL_F(measured_z, 3);
       SERIAL_EOL;
     }
+
+    #ifdef DEBUG_LEVELING
+      SERIAL_ECHOLNPGM("<<< probe_pt");
+    #endif
+
     return measured_z;
   }
 
@@ -1585,6 +1689,9 @@ static void setup_for_endstop_move() {
 
     // Reset calibration results to zero.
     void reset_bed_level() {
+      #ifdef DEBUG_LEVELING
+        SERIAL_ECHOLNPGM("reset_bed_level");
+      #endif
       for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
         for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
           bed_level[x][y] = 0.0;
@@ -1620,6 +1727,10 @@ static void setup_for_endstop_move() {
    * offset[in]   The additional distance to move to adjust docking location
    */
   static void dock_sled(bool dock, int offset=0) {
+    #ifdef DEBUG_LEVELING
+      SERIAL_ECHOPAIR("dock_sled", dock);
+      SERIAL_EOL;
+    #endif
     if (!axis_known_position[X_AXIS] || !axis_known_position[Y_AXIS]) {
       LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
       SERIAL_ECHO_START;
@@ -1654,6 +1765,11 @@ static void setup_for_endstop_move() {
 #define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS)
 
 static void homeaxis(AxisEnum axis) {
+  #ifdef DEBUG_LEVELING
+    SERIAL_ECHOPAIR(">>> homeaxis(", (unsigned long)axis);
+    SERIAL_CHAR(')');
+    SERIAL_EOL;
+  #endif
   #define HOMEAXIS_DO(LETTER) \
     ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
 
@@ -1706,6 +1822,9 @@ static void homeaxis(AxisEnum axis) {
     current_position[axis] = 0;
     sync_plan_position();
 
+    #ifdef DEBUG_LEVELING
+      SERIAL_ECHOLNPGM("> enable_endstops(false)");
+    #endif
     enable_endstops(false); // Disable endstops while moving away
 
     // Move away from the endstop by the axis HOME_BUMP_MM
@@ -1713,6 +1832,9 @@ static void homeaxis(AxisEnum axis) {
     line_to_destination();
     st_synchronize();
 
+    #ifdef DEBUG_LEVELING
+      SERIAL_ECHOLNPGM("> enable_endstops(true)");
+    #endif
     enable_endstops(true); // Enable endstops for next homing move
 
     // Slow down the feedrate for the next move
@@ -1723,6 +1845,10 @@ static void homeaxis(AxisEnum axis) {
     line_to_destination();
     st_synchronize();
 
+    #ifdef DEBUG_LEVELING
+      print_xyz("> TRIGGER ENDSTOP > current_position", current_position);
+    #endif
+
     #if ENABLED(Z_DUAL_ENDSTOPS)
       if (axis == Z_AXIS) {
         float adj = fabs(z_endstop_adj);
@@ -1751,19 +1877,39 @@ static void homeaxis(AxisEnum axis) {
     #if ENABLED(DELTA)
       // retrace by the amount specified in endstop_adj
       if (endstop_adj[axis] * axis_home_dir < 0) {
+        #ifdef DEBUG_LEVELING
+          SERIAL_ECHOLNPGM("> enable_endstops(false)");
+        #endif
         enable_endstops(false); // Disable endstops while moving away
         sync_plan_position();
         destination[axis] = endstop_adj[axis];
+        #ifdef DEBUG_LEVELING
+          SERIAL_ECHOPAIR("> endstop_adj = ", endstop_adj[axis]);
+          print_xyz(" > destination", destination);
+        #endif
         line_to_destination();
         st_synchronize();
+        #ifdef DEBUG_LEVELING
+          SERIAL_ECHOLNPGM("> enable_endstops(true)");
+        #endif
         enable_endstops(true); // Enable endstops for next homing move
       }
+      #ifdef DEBUG_LEVELING
+        else {
+          SERIAL_ECHOPAIR("> endstop_adj * axis_home_dir = ", endstop_adj[axis] * axis_home_dir);
+          SERIAL_EOL;
+        }
+      #endif
     #endif
 
     // Set the axis position to its home position (plus home offsets)
     set_axis_is_at_home(axis);
     sync_plan_position();
 
+    #ifdef DEBUG_LEVELING
+      print_xyz("> AFTER set_axis_is_at_home > current_position", current_position);
+    #endif
+
     destination[axis] = current_position[axis];
     feedrate = 0.0;
     endstops_hit_on_purpose(); // clear endstop hit flags
@@ -1780,7 +1926,12 @@ static void homeaxis(AxisEnum axis) {
 
       // Deploy a Z probe if there is one, and homing towards the bed
       if (axis == Z_AXIS) {
-        if (axis_home_dir < 0) stow_z_probe();
+        if (axis_home_dir < 0) {
+          #ifdef DEBUG_LEVELING
+            SERIAL_ECHOLNPGM("> SERVO_LEVELING > stow_z_probe");
+          #endif
+          stow_z_probe();
+        }
       }
       else
 
@@ -1789,12 +1940,22 @@ static void homeaxis(AxisEnum axis) {
     {
       #if HAS_SERVO_ENDSTOPS
         // Retract Servo endstop if enabled
-        if (servo_endstop_id[axis] >= 0)
+        if (servo_endstop_id[axis] >= 0) {
+          #ifdef DEBUG_LEVELING
+            SERIAL_ECHOLNPGM("> SERVO_ENDSTOPS > Stow with servo.move()");
+          #endif
           servo[servo_endstop_id[axis]].move(servo_endstop_angle[axis][1]);
+        }
       #endif
     }
 
   }
+
+  #ifdef DEBUG_LEVELING
+    SERIAL_ECHOPAIR("<<< homeaxis(", (unsigned long)axis);
+    SERIAL_CHAR(')');
+    SERIAL_EOL;
+  #endif
 }
 
 #if ENABLED(FWRETRACT)
@@ -1996,6 +2157,10 @@ inline void gcode_G4() {
  */
 inline void gcode_G28() {
 
+  #ifdef DEBUG_LEVELING
+    SERIAL_ECHOLNPGM("gcode_G28 >>>");
+  #endif
+
   // Wait for planner moves to finish!
   st_synchronize();
 
@@ -2044,6 +2209,10 @@ inline void gcode_G28() {
 
     sync_plan_position_delta();
 
+    #ifdef DEBUG_LEVELING
+      print_xyz("(DELTA) > current_position", current_position);
+    #endif
+
   #else // NOT DELTA
 
     bool  homeX = code_seen(axis_codes[X_AXIS]),
@@ -2057,12 +2226,20 @@ inline void gcode_G28() {
       #if Z_HOME_DIR > 0  // If homing away from BED do Z first
 
         HOMEAXIS(Z);
+        #ifdef DEBUG_LEVELING
+          print_xyz("> HOMEAXIS(Z) > current_position", current_position);
+        #endif
 
       #elif DISABLED(Z_SAFE_HOMING) && defined(Z_RAISE_BEFORE_HOMING) && Z_RAISE_BEFORE_HOMING > 0
 
         // Raise Z before homing any other axes
         // (Does this need to be "negative home direction?" Why not just use Z_RAISE_BEFORE_HOMING?)
         destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);
+        #ifdef DEBUG_LEVELING
+          SERIAL_ECHOPAIR("Raise Z (before homing) by ", (float)Z_RAISE_BEFORE_HOMING);
+          SERIAL_EOL;
+          print_xyz("> (home_all_axis || homeZ) > destination", destination);
+        #endif
         feedrate = max_feedrate[Z_AXIS] * 60;
         line_to_destination();
         st_synchronize();
@@ -2099,6 +2276,10 @@ inline void gcode_G28() {
         set_axis_is_at_home(Y_AXIS);
         sync_plan_position();
 
+        #ifdef DEBUG_LEVELING
+          print_xyz("> QUICK_HOME > current_position 1", current_position);
+        #endif
+
         destination[X_AXIS] = current_position[X_AXIS];
         destination[Y_AXIS] = current_position[Y_AXIS];
         line_to_destination();
@@ -2111,6 +2292,10 @@ inline void gcode_G28() {
         #if DISABLED(SCARA)
           current_position[Z_AXIS] = destination[Z_AXIS];
         #endif
+
+        #ifdef DEBUG_LEVELING
+          print_xyz("> QUICK_HOME > current_position 2", current_position);
+        #endif
       }
 
     #endif // QUICK_HOME
@@ -2137,11 +2322,19 @@ inline void gcode_G28() {
       #else
         HOMEAXIS(X);
       #endif
+      #ifdef DEBUG_LEVELING
+        print_xyz("> homeX", current_position);
+      #endif
     }
 
     #if DISABLED(HOME_Y_BEFORE_X)
       // Home Y
-      if (home_all_axis || homeY) HOMEAXIS(Y);
+      if (home_all_axis || homeY) {
+        HOMEAXIS(Y);
+        #ifdef DEBUG_LEVELING
+          print_xyz("> homeY", current_position);
+        #endif
+      }
     #endif
 
     // Home Z last if homing towards the bed
@@ -2151,6 +2344,10 @@ inline void gcode_G28() {
 
         #if ENABLED(Z_SAFE_HOMING)
 
+          #ifdef DEBUG_LEVELING
+            SERIAL_ECHOLNPGM("> Z_SAFE_HOMING >>>");
+          #endif
+
           if (home_all_axis) {
 
             current_position[Z_AXIS] = 0;
@@ -2165,6 +2362,14 @@ inline void gcode_G28() {
             destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - Y_PROBE_OFFSET_FROM_EXTRUDER);
             destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
             feedrate = XY_TRAVEL_SPEED;
+
+            #ifdef DEBUG_LEVELING
+              SERIAL_ECHOPAIR("Raise Z (before homing) by ", (float)Z_RAISE_BEFORE_HOMING);
+              SERIAL_EOL;
+              print_xyz("> home_all_axis > current_position", current_position);
+              print_xyz("> home_all_axis > destination", destination);
+            #endif
+
             // This could potentially move X, Y, Z all together
             line_to_destination();
             st_synchronize();
@@ -2197,6 +2402,14 @@ inline void gcode_G28() {
                 // NOTE: This should always just be Z_RAISE_BEFORE_HOMING unless...???
                 destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);
                 feedrate = max_feedrate[Z_AXIS] * 60;  // feedrate (mm/m) = max_feedrate (mm/s)
+
+                #ifdef DEBUG_LEVELING
+                  SERIAL_ECHOPAIR("Raise Z (before homing) by ", (float)Z_RAISE_BEFORE_HOMING);
+                  SERIAL_EOL;
+                  print_xyz("> homeZ > current_position", current_position);
+                  print_xyz("> homeZ > destination", destination);
+                #endif
+
                 line_to_destination();
                 st_synchronize();
 
@@ -2217,12 +2430,20 @@ inline void gcode_G28() {
 
           } // !home_all_axes && homeZ
 
+          #ifdef DEBUG_LEVELING
+            SERIAL_ECHOLNPGM("<<< Z_SAFE_HOMING");
+          #endif
+
         #else // !Z_SAFE_HOMING
 
           HOMEAXIS(Z);
 
         #endif // !Z_SAFE_HOMING
 
+        #ifdef DEBUG_LEVELING
+          print_xyz("> (home_all_axis || homeZ) > final", current_position);
+        #endif
+
       } // home_all_axis || homeZ
 
     #endif // Z_HOME_DIR < 0
@@ -2236,6 +2457,9 @@ inline void gcode_G28() {
   #endif
 
   #if ENABLED(ENDSTOPS_ONLY_FOR_HOMING)
+    #ifdef DEBUG_LEVELING
+      SERIAL_ECHOLNPGM("ENDSTOPS_ONLY_FOR_HOMING enable_endstops(false)");
+    #endif
     enable_endstops(false);
   #endif
 
@@ -2251,6 +2475,9 @@ inline void gcode_G28() {
       current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
       sync_plan_position();
       mbl.active = 1;
+      #ifdef DEBUG_LEVELING
+        print_xyz("mbl_was_active > current_position", current_position);
+      #endif
     }
   #endif
 
@@ -2258,6 +2485,11 @@ inline void gcode_G28() {
   feedrate_multiplier = saved_feedrate_multiplier;
   refresh_cmd_timeout();
   endstops_hit_on_purpose(); // clear endstop hit flags
+
+  #ifdef DEBUG_LEVELING
+    SERIAL_ECHOLNPGM("<<< gcode_G28");
+  #endif
+
 }
 
 #if ENABLED(MESH_BED_LEVELING)
@@ -2443,6 +2675,10 @@ inline void gcode_G28() {
    */
   inline void gcode_G29() {
 
+    #ifdef DEBUG_LEVELING
+      SERIAL_ECHOLNPGM("gcode_G29 >>>");
+    #endif
+
     // Don't allow auto-leveling without homing first
     if (!axis_known_position[X_AXIS] || !axis_known_position[Y_AXIS]) {
       LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
@@ -2601,6 +2837,19 @@ inline void gcode_G28() {
           float measured_z,
                 z_before = probePointCounter ? Z_RAISE_BETWEEN_PROBINGS + current_position[Z_AXIS] : Z_RAISE_BEFORE_PROBING;
 
+          if (probePointCounter) {
+            #ifdef DEBUG_LEVELING
+              SERIAL_ECHOPAIR("z_before = (between) ", (float)(Z_RAISE_BETWEEN_PROBINGS + current_position[Z_AXIS]));
+              SERIAL_EOL;
+            #endif
+          }
+          else {
+            #ifdef DEBUG_LEVELING
+              SERIAL_ECHOPAIR("z_before = (before) ", (float)Z_RAISE_BEFORE_PROBING);
+              SERIAL_EOL;
+            #endif
+          }
+
           #if ENABLED(DELTA)
             // Avoid probing the corners (outside the round or hexagon print surface) on a delta printer.
             float distance_from_center = sqrt(xProbe*xProbe + yProbe*yProbe);
@@ -2638,6 +2887,10 @@ inline void gcode_G28() {
         } //xProbe
       } //yProbe
 
+      #ifdef DEBUG_LEVELING
+        print_xyz("> probing complete > current_position", current_position);
+      #endif
+
       clean_up_after_endstop_move();
 
       #if ENABLED(DELTA)
@@ -2734,6 +2987,10 @@ inline void gcode_G28() {
 
     #else // !AUTO_BED_LEVELING_GRID
 
+      #ifdef DEBUG_LEVELING
+        SERIAL_ECHOLNPGM("> 3-point Leveling");
+      #endif
+
       // Actions for each probe
       ProbeAction p1, p2, p3;
       if (deploy_probe_for_each_reading)
@@ -2763,6 +3020,13 @@ inline void gcode_G28() {
               z_tmp = current_position[Z_AXIS],
               real_z = st_get_position_mm(Z_AXIS);  //get the real Z (since plan_get_position is now correcting the plane)
 
+        #ifdef DEBUG_LEVELING
+          SERIAL_ECHOPAIR("> BEFORE apply_rotation_xyz > z_tmp  = ", z_tmp);
+          SERIAL_EOL;
+          SERIAL_ECHOPAIR("> BEFORE apply_rotation_xyz > real_z = ", real_z);
+          SERIAL_EOL;
+        #endif
+
         apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); // Apply the correction sending the Z probe offset
 
         // Get the current Z position and send it to the planner.
@@ -2781,6 +3045,11 @@ inline void gcode_G28() {
         //      adjust for inaccurate endstops, not for reasonably accurate probes. If it were
         //      added here, it could be seen as a compensating factor for the Z probe.
         //
+        #ifdef DEBUG_LEVELING
+          SERIAL_ECHOPAIR("> AFTER apply_rotation_xyz > z_tmp  = ", z_tmp);
+          SERIAL_EOL;
+        #endif
+
         current_position[Z_AXIS] = -zprobe_zoffset + (z_tmp - real_z)
           #if HAS_SERVO_ENDSTOPS || ENABLED(Z_PROBE_ALLEN_KEY) || ENABLED(Z_PROBE_SLED)
              + Z_RAISE_AFTER_PROBING
@@ -2788,6 +3057,10 @@ inline void gcode_G28() {
           ;
         // current_position[Z_AXIS] += home_offset[Z_AXIS]; // The Z probe determines Z=0, not "Z home"
         sync_plan_position();
+
+        #ifdef DEBUG_LEVELING
+          print_xyz("> corrected Z in G29", current_position);
+        #endif
       }
     #endif // !DELTA
 
@@ -2798,9 +3071,18 @@ inline void gcode_G28() {
     #endif
 
     #ifdef Z_PROBE_END_SCRIPT
+      #ifdef DEBUG_LEVELING
+        SERIAL_ECHO("Z Probe End Script: ");
+        SERIAL_ECHOLNPGM(Z_PROBE_END_SCRIPT);
+      #endif
       enqueuecommands_P(PSTR(Z_PROBE_END_SCRIPT));
       st_synchronize();
     #endif
+
+    #ifdef DEBUG_LEVELING
+      SERIAL_ECHOLNPGM("<<< gcode_G29");
+    #endif
+
   }
 
   #if DISABLED(Z_PROBE_SLED)
@@ -4095,11 +4377,23 @@ inline void gcode_M206() {
    * M666: Set delta endstop adjustment
    */
   inline void gcode_M666() {
+    #ifdef DEBUG_LEVELING
+      SERIAL_ECHOLNPGM(">>> gcode_M666");
+    #endif
     for (int8_t i = X_AXIS; i <= Z_AXIS; i++) {
       if (code_seen(axis_codes[i])) {
         endstop_adj[i] = code_value();
+        #ifdef DEBUG_LEVELING
+          SERIAL_ECHOPGM("endstop_adj[");
+          SERIAL_ECHO(axis_codes[i]);
+          SERIAL_ECHOPAIR("] = ", endstop_adj[i]);
+          SERIAL_EOL;
+        #endif
       }
     }
+    #ifdef DEBUG_LEVELING
+      SERIAL_ECHOLNPGM("<<< gcode_M666");
+    #endif
   }
 #elif ENABLED(Z_DUAL_ENDSTOPS) // !DELTA && ENABLED(Z_DUAL_ENDSTOPS)
   /**
@@ -5801,7 +6095,13 @@ void clamp_to_software_endstops(float target[3]) {
     float negative_z_offset = 0;
     #if ENABLED(AUTO_BED_LEVELING_FEATURE)
       if (zprobe_zoffset < 0) negative_z_offset += zprobe_zoffset;
-      if (home_offset[Z_AXIS] < 0) negative_z_offset += home_offset[Z_AXIS];
+      if (home_offset[Z_AXIS] < 0) {
+        #ifdef DEBUG_LEVELING
+          SERIAL_ECHOPAIR("> clamp_to_software_endstops > Add home_offset[Z_AXIS]:", home_offset[Z_AXIS]);
+          SERIAL_EOL;
+        #endif
+        negative_z_offset += home_offset[Z_AXIS];
+      }
     #endif
     NOLESS(target[Z_AXIS], min_pos[Z_AXIS] + negative_z_offset);
   }