Bed leveling that accounts for home XYZ

Marlin/Conditionals_post.h

     #ifndef Z_SAFE_HOMING_Y_POINT
       #define Z_SAFE_HOMING_Y_POINT ((Y_MIN_POS + Y_MAX_POS) / 2)
     #endif
+    #define X_TILT_FULCRUM Z_SAFE_HOMING_X_POINT
+    #define Y_TILT_FULCRUM Z_SAFE_HOMING_Y_POINT
+  #else
+    #define X_TILT_FULCRUM X_HOME_POS
+    #define Y_TILT_FULCRUM Y_HOME_POS
   #endif
 
   /**

Marlin/Marlin_main.cpp

 
 #if ENABLED(DELTA)
 
-  #define TOWER_1 X_AXIS
-  #define TOWER_2 Y_AXIS
-  #define TOWER_3 Z_AXIS
-
-  float delta[ABC];
-  float cartesian_position[XYZ] = { 0 };
   #define SIN_60 0.8660254037844386
   #define COS_60 0.5
-  float endstop_adj[ABC] = { 0 };
+
+  float delta[ABC],
+        cartesian_position[XYZ] = { 0 },
+        endstop_adj[ABC] = { 0 };
+
   // these are the default values, can be overriden with M665
-  float delta_radius = DELTA_RADIUS;
-  float delta_tower1_x = -SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1); // front left tower
-  float delta_tower1_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1);
-  float delta_tower2_x =  SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2); // front right tower
-  float delta_tower2_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2);
-  float delta_tower3_x = 0;                                                    // back middle tower
-  float delta_tower3_y = (delta_radius + DELTA_RADIUS_TRIM_TOWER_3);
-  float delta_diagonal_rod = DELTA_DIAGONAL_ROD;
-  float delta_diagonal_rod_trim_tower_1 = DELTA_DIAGONAL_ROD_TRIM_TOWER_1;
-  float delta_diagonal_rod_trim_tower_2 = DELTA_DIAGONAL_ROD_TRIM_TOWER_2;
-  float delta_diagonal_rod_trim_tower_3 = DELTA_DIAGONAL_ROD_TRIM_TOWER_3;
-  float delta_diagonal_rod_2_tower_1 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_1);
-  float delta_diagonal_rod_2_tower_2 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_2);
-  float delta_diagonal_rod_2_tower_3 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_3);
-  float delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
-  float delta_clip_start_height = Z_MAX_POS;
+  float delta_radius = DELTA_RADIUS,
+        delta_tower1_x = -SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1), // front left tower
+        delta_tower1_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1),
+        delta_tower2_x =  SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2), // front right tower
+        delta_tower2_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2),
+        delta_tower3_x = 0,                                                    // back middle tower
+        delta_tower3_y = (delta_radius + DELTA_RADIUS_TRIM_TOWER_3),
+        delta_diagonal_rod = DELTA_DIAGONAL_ROD,
+        delta_diagonal_rod_trim_tower_1 = DELTA_DIAGONAL_ROD_TRIM_TOWER_1,
+        delta_diagonal_rod_trim_tower_2 = DELTA_DIAGONAL_ROD_TRIM_TOWER_2,
+        delta_diagonal_rod_trim_tower_3 = DELTA_DIAGONAL_ROD_TRIM_TOWER_3,
+        delta_diagonal_rod_2_tower_1 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_1),
+        delta_diagonal_rod_2_tower_2 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_2),
+        delta_diagonal_rod_2_tower_3 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_3),
+        delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND,
+        delta_clip_start_height = Z_MAX_POS;
+
   #if ENABLED(AUTO_BED_LEVELING_FEATURE)
     int delta_grid_spacing[2] = { 0, 0 };
     float bed_level[AUTO_BED_LEVELING_GRID_POINTS][AUTO_BED_LEVELING_GRID_POINTS];
   #endif
+
   float delta_safe_distance_from_top();
+  void set_cartesian_from_steppers();
+
 #else
+
   static bool home_all_axis = true;
+
 #endif
 
 #if ENABLED(SCARA)
-  float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND;
-  float delta[ABC];
-  float axis_scaling[ABC] = { 1, 1, 1 };    // Build size scaling, default to 1
+  float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND,
+        delta[ABC],
+        axis_scaling[ABC] = { 1, 1, 1 },    // Build size scaling, default to 1
+        cartesian_position[XYZ] = { 0 };
+  void set_cartesian_from_steppers() { }    // to be written later
 #endif
 
 #if ENABLED(FILAMENT_WIDTH_SENSOR)
 
 #if ENABLED(AUTO_BED_LEVELING_FEATURE)
 
-  #if ENABLED(AUTO_BED_LEVELING_GRID)
-
-    #if DISABLED(DELTA)
-
-      static void set_bed_level_equation_lsq(double* plane_equation_coefficients) {
-
-        //planner.bed_level_matrix.debug("bed level before");
-
-        #if ENABLED(DEBUG_LEVELING_FEATURE)
-          planner.bed_level_matrix.set_to_identity();
-          if (DEBUGGING(LEVELING)) {
-            vector_3 uncorrected_position = planner.adjusted_position();
-            DEBUG_POS(">>> set_bed_level_equation_lsq", uncorrected_position);
-            DEBUG_POS(">>> set_bed_level_equation_lsq", current_position);
-          }
-        #endif
-
-        vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
-        planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
-
-        vector_3 corrected_position = planner.adjusted_position();
-        current_position[X_AXIS] = corrected_position.x;
-        current_position[Y_AXIS] = corrected_position.y;
-        current_position[Z_AXIS] = corrected_position.z;
+  #if DISABLED(DELTA)
 
-        #if ENABLED(DEBUG_LEVELING_FEATURE)
-          if (DEBUGGING(LEVELING)) DEBUG_POS("<<< set_bed_level_equation_lsq", corrected_position);
-        #endif
-
-        SYNC_PLAN_POSITION_KINEMATIC();
-      }
-
-    #endif // !DELTA
-
-  #else // !AUTO_BED_LEVELING_GRID
-
-    static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
-
-      planner.bed_level_matrix.set_to_identity();
+    /**
+     * Get the stepper positions, apply the rotation matrix
+     * using the home XY and Z0 position as the fulcrum.
+     */
+    vector_3 untilted_stepper_position() {
+      vector_3 pos = vector_3(
+        RAW_X_POSITION(stepper.get_axis_position_mm(X_AXIS)) - X_TILT_FULCRUM,
+        RAW_Y_POSITION(stepper.get_axis_position_mm(Y_AXIS)) - Y_TILT_FULCRUM,
+        RAW_Z_POSITION(stepper.get_axis_position_mm(Z_AXIS))
+      );
 
-      #if ENABLED(DEBUG_LEVELING_FEATURE)
-        if (DEBUGGING(LEVELING)) {
-          vector_3 uncorrected_position = planner.adjusted_position();
-          DEBUG_POS("set_bed_level_equation_3pts", uncorrected_position);
-        }
-      #endif
+      matrix_3x3 inverse = matrix_3x3::transpose(planner.bed_level_matrix);
 
-      vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1);
-      vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2);
-      vector_3 pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3);
-      vector_3 planeNormal = vector_3::cross(pt1 - pt2, pt3 - pt2).get_normal();
+      //pos.debug("untilted_stepper_position offset");
+      //bed_level_matrix.debug("untilted_stepper_position");
+      //inverse.debug("in untilted_stepper_position");
 
-      if (planeNormal.z < 0) {
-        planeNormal.x = -planeNormal.x;
-        planeNormal.y = -planeNormal.y;
-        planeNormal.z = -planeNormal.z;
-      }
+      pos.apply_rotation(inverse);
 
-      planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
-      vector_3 corrected_position = planner.adjusted_position();
+      pos.x = LOGICAL_X_POSITION(pos.x + X_TILT_FULCRUM);
+      pos.y = LOGICAL_Y_POSITION(pos.y + Y_TILT_FULCRUM);
+      pos.z = LOGICAL_Z_POSITION(pos.z);
 
-      current_position[X_AXIS] = corrected_position.x;
-      current_position[Y_AXIS] = corrected_position.y;
-      current_position[Z_AXIS] = corrected_position.z;
+      //pos.debug("after rotation and reorientation");
 
-      #if ENABLED(DEBUG_LEVELING_FEATURE)
-        if (DEBUGGING(LEVELING)) DEBUG_POS("set_bed_level_equation_3pts", corrected_position);
-      #endif
-
-      SYNC_PLAN_POSITION_KINEMATIC();
+      return pos;
     }
 
-  #endif // !AUTO_BED_LEVELING_GRID
+  #endif // !DELTA
 
   #if ENABLED(DELTA)
 
 
     #endif // AUTO_BED_LEVELING_GRID
 
-    if (!dryrun) {
+    stepper.synchronize();
 
-      #if ENABLED(DEBUG_LEVELING_FEATURE) && DISABLED(DELTA)
-        if (DEBUGGING(LEVELING)) {
-          vector_3 corrected_position = planner.adjusted_position();
-          DEBUG_POS("BEFORE matrix.set_to_identity", corrected_position);
-          DEBUG_POS("BEFORE matrix.set_to_identity", current_position);
-        }
-      #endif
+    if (!dryrun) {
 
-      // make sure the bed_level_rotation_matrix is identity or the planner will get it wrong
+      // Reset the bed_level_matrix because leveling
+      // needs to be done without leveling enabled.
       planner.bed_level_matrix.set_to_identity();
 
-      #if ENABLED(DELTA)
-        reset_bed_level();
-      #else //!DELTA
-
-        //vector_3 corrected_position = planner.adjusted_position();
-        //corrected_position.debug("position before G29");
-        vector_3 uncorrected_position = planner.adjusted_position();
-        //uncorrected_position.debug("position during G29");
-        current_position[X_AXIS] = uncorrected_position.x;
-        current_position[Y_AXIS] = uncorrected_position.y;
-        current_position[Z_AXIS] = uncorrected_position.z;
+      //
+      // Re-orient the current position without leveling
+      // based on where the steppers are positioned.
+      //
+      #if ENABLED(DELTA) || ENABLED(SCARA)
 
-        #if ENABLED(DEBUG_LEVELING_FEATURE)
-          if (DEBUGGING(LEVELING)) DEBUG_POS("AFTER matrix.set_to_identity", uncorrected_position);
+        #if ENABLED(DELTA)
+          reset_bed_level();
         #endif
 
-        SYNC_PLAN_POSITION_KINEMATIC();
+        // For DELTA/SCARA we need to apply forward kinematics.
+        // This returns raw positions and we remap to the space.
+        set_cartesian_from_steppers();
+        LOOP_XYZ(i) current_position[i] = LOGICAL_POSITION(cartesian_position[i], i);
+
+      #else
+
+        // For cartesian/core the steppers are already mapped to
+        // the coordinate space by design.
+        LOOP_XYZ(i) current_position[i] = stepper.get_axis_position_mm((AxisEnum)i);
 
       #endif // !DELTA
-    }
 
-    stepper.synchronize();
+      // Inform the planner about the new coordinates
+      // (This is probably not needed here)
+      SYNC_PLAN_POSITION_KINEMATIC();
+    }
 
     setup_for_endstop_or_probe_move();
 
                                   LOGICAL_Y_POSITION(ABL_PROBE_PT_3_Y),
                                   stow_probe_after_each, verbose_level);
 
-      if (!dryrun) set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
+      if (!dryrun) {
+        vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1),
+                 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2),
+                 pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3);
+
+        vector_3 planeNormal = vector_3::cross(pt1 - pt2, pt3 - pt2).get_normal();
+
+        if (planeNormal.z < 0) {
+          planeNormal.x *= -1;
+          planeNormal.y *= -1;
+          planeNormal.z *= -1;
+        }
+        planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+      }
 
     #endif // !AUTO_BED_LEVELING_GRID
 
           }
         }
 
-        if (!dryrun) set_bed_level_equation_lsq(plane_equation_coefficients);
+        // Create the matrix but don't correct the position yet
+        if (!dryrun) {
+          planner.bed_level_matrix = matrix_3x3::create_look_at(
+            vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1)
+          );
+        }
 
         // Show the Topography map if enabled
         if (do_topography_map) {
             SERIAL_EOL;
           } // yy
           SERIAL_EOL;
+
           if (verbose_level > 3) {
             SERIAL_PROTOCOLLNPGM("\nCorrected Bed Height vs. Bed Topology:");
 
             SERIAL_EOL;
           }
         } //do_topography_map
+
       #endif //!DELTA
+
     #endif // AUTO_BED_LEVELING_GRID
 
     #if DISABLED(DELTA)
+
       if (verbose_level > 0)
         planner.bed_level_matrix.debug("\n\nBed Level Correction Matrix:");
 
       if (!dryrun) {
-        /**
-         * Correct the Z height difference from Z probe position and nozzle tip position.
-         * The Z height on homing is measured by Z probe, but the Z probe is quite far
-         * from the nozzle. When the bed is uneven, this height must be corrected.
-         */
-        float x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER,
-              y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER,
-              z_tmp = current_position[Z_AXIS],
-              stepper_z = stepper.get_axis_position_mm(Z_AXIS);  //get the real Z (since planner.adjusted_position is now correcting the plane)
+        //
+        // Correct the current XYZ position based on the tilted plane.
+        //
+
+        // Get the distance from the reference point to the current position
+        // The current XY is in sync with the planner/steppers at this point
+        // but the current Z is only known to the steppers.
+        float x_dist = RAW_CURRENT_POSITION(X_AXIS) - X_TILT_FULCRUM,
+              y_dist = RAW_CURRENT_POSITION(Y_AXIS) - Y_TILT_FULCRUM,
+              z_real = RAW_Z_POSITION(stepper.get_axis_position_mm(Z_AXIS));
 
         #if ENABLED(DEBUG_LEVELING_FEATURE)
           if (DEBUGGING(LEVELING)) {
-            SERIAL_ECHOPAIR("> BEFORE apply_rotation_xyz > stepper_z = ", stepper_z);
-            SERIAL_ECHOLNPAIR(" ... z_tmp  = ", z_tmp);
+            SERIAL_ECHOPAIR("BEFORE ROTATION ... x_dist:", x_dist);
+            SERIAL_ECHOPAIR("y_dist:", y_dist);
+            SERIAL_ECHOPAIR("z_real:", z_real);
           }
         #endif
 
-        // Apply the correction sending the Z probe offset
-        apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
+        // Apply the matrix to the distance from the reference point to XY,
+        // and from the homed Z to the current Z.
+        apply_rotation_xyz(planner.bed_level_matrix, x_dist, y_dist, z_real);
 
         #if ENABLED(DEBUG_LEVELING_FEATURE)
-          if (DEBUGGING(LEVELING))
-            SERIAL_ECHOLNPAIR("> AFTER apply_rotation_xyz > z_tmp  = ", z_tmp);
+          if (DEBUGGING(LEVELING)) {
+            SERIAL_ECHOPAIR("AFTER ROTATION ... x_dist:", x_dist);
+            SERIAL_ECHOPAIR("y_dist:", y_dist);
+            SERIAL_ECHOPAIR("z_real:", z_real);
+          }
         #endif
 
-        // Adjust the current Z and send it to the planner.
-        current_position[Z_AXIS] += z_tmp - stepper_z;
+        // Apply the rotated distance and Z to the current position
+        current_position[X_AXIS] = LOGICAL_X_POSITION(X_TILT_FULCRUM + x_dist);
+        current_position[Y_AXIS] = LOGICAL_Y_POSITION(Y_TILT_FULCRUM + y_dist);
+        current_position[Z_AXIS] = LOGICAL_Z_POSITION(z_real);
+
         SYNC_PLAN_POSITION_KINEMATIC();
 
         #if ENABLED(DEBUG_LEVELING_FEATURE)
-          if (DEBUGGING(LEVELING)) DEBUG_POS("> corrected Z in G29", current_position);
+          if (DEBUGGING(LEVELING)) DEBUG_POS("> corrected XYZ in G29", current_position);
         #endif
       }
+
     #endif // !DELTA
 
     #ifdef Z_PROBE_END_SCRIPT
       RAW_Z_POSITION(in_cartesian[Z_AXIS])
     };
 
-    delta[TOWER_1] = sqrt(delta_diagonal_rod_2_tower_1
+    delta[A_AXIS] = sqrt(delta_diagonal_rod_2_tower_1
                           - sq(delta_tower1_x - cartesian[X_AXIS])
                           - sq(delta_tower1_y - cartesian[Y_AXIS])
                          ) + cartesian[Z_AXIS];
-    delta[TOWER_2] = sqrt(delta_diagonal_rod_2_tower_2
+    delta[B_AXIS] = sqrt(delta_diagonal_rod_2_tower_2
                           - sq(delta_tower2_x - cartesian[X_AXIS])
                           - sq(delta_tower2_y - cartesian[Y_AXIS])
                          ) + cartesian[Z_AXIS];
-    delta[TOWER_3] = sqrt(delta_diagonal_rod_2_tower_3
+    delta[C_AXIS] = sqrt(delta_diagonal_rod_2_tower_3
                           - sq(delta_tower3_x - cartesian[X_AXIS])
                           - sq(delta_tower3_y - cartesian[Y_AXIS])
                          ) + cartesian[Z_AXIS];
     SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]);
     SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]);
 
-    SERIAL_ECHOPGM("delta a="); SERIAL_ECHO(delta[TOWER_1]);
-    SERIAL_ECHOPGM(" b="); SERIAL_ECHO(delta[TOWER_2]);
-    SERIAL_ECHOPGM(" c="); SERIAL_ECHOLN(delta[TOWER_3]);
+    SERIAL_ECHOPGM("delta a="); SERIAL_ECHO(delta[A_AXIS]);
+    SERIAL_ECHOPGM(" b="); SERIAL_ECHO(delta[B_AXIS]);
+    SERIAL_ECHOPGM(" c="); SERIAL_ECHOLN(delta[C_AXIS]);
     */
   }
 
       LOGICAL_Z_POSITION(0)
     };
     inverse_kinematics(cartesian);
-    float distance = delta[TOWER_3];
+    float distance = delta[A_AXIS];
     cartesian[Y_AXIS] = LOGICAL_Y_POSITION(DELTA_PRINTABLE_RADIUS);
     inverse_kinematics(cartesian);
-    return abs(distance - delta[TOWER_3]);
+    return abs(distance - delta[A_AXIS]);
   }
 
   void forward_kinematics_DELTA(float z1, float z2, float z3) {
     set_cartesian_from_steppers();
     current_position[axis] = LOGICAL_POSITION(cartesian_position[axis], axis);
   #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
-    vector_3 pos = planner.adjusted_position();
+    vector_3 pos = untilted_stepper_position();
     current_position[axis] = axis == X_AXIS ? pos.x : axis == Y_AXIS ? pos.y : pos.z;
   #else
     current_position[axis] = stepper.get_axis_position_mm(axis); // CORE handled transparently

Marlin/planner.cpp

   #endif
 }
 
+#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
+
+  void Planner::apply_leveling(
+    #if ENABLED(MESH_BED_LEVELING)
+      const float &x, const float &y
+    #else
+      float &x, float &y
+    #endif
+    , float &z
+  ) {
+    #if ENABLED(MESH_BED_LEVELING)
+
+      if (mbl.active())
+        z += mbl.get_z(RAW_X_POSITION(x), RAW_Y_POSITION(y));
+
+    #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
+
+      float tx = RAW_X_POSITION(x) - (X_TILT_FULCRUM),
+            ty = RAW_Y_POSITION(y) - (Y_TILT_FULCRUM),
+            tz = RAW_Z_POSITION(z);
+
+      apply_rotation_xyz(bed_level_matrix, tx, ty, tz);
+
+      x = LOGICAL_X_POSITION(tx + X_TILT_FULCRUM);
+      y = LOGICAL_Y_POSITION(ty + Y_TILT_FULCRUM);
+      z = LOGICAL_Z_POSITION(tz);
+
+    #endif
+  }
+
+#endif
+
 /**
  * Planner::buffer_line
  *
  *  extruder  - target extruder
  */
 
-#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
-  void Planner::buffer_line(float x, float y, float z, const float& e, float fr_mm_s, const uint8_t extruder)
-#else
-  void Planner::buffer_line(const float& x, const float& y, const float& z, const float& e, float fr_mm_s, const uint8_t extruder)
-#endif  // AUTO_BED_LEVELING_FEATURE
-{
+void Planner::buffer_line(
+  #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
+    float x, float y, float z
+  #else
+    const float& x, const float& y, const float& z
+  #endif
+  , const float& e, float fr_mm_s, const uint8_t extruder
+) {
   // Calculate the buffer head after we push this byte
   int next_buffer_head = next_block_index(block_buffer_head);
 
   // Rest here until there is room in the buffer.
   while (block_buffer_tail == next_buffer_head) idle();
 
-  #if ENABLED(MESH_BED_LEVELING)
-    if (mbl.active())
-      z += mbl.get_z(x - home_offset[X_AXIS], y - home_offset[Y_AXIS]);
-  #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
-    apply_rotation_xyz(bed_level_matrix, x, y, z);
+  #if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
+    apply_leveling(x, y, z);
   #endif
 
   // The target position of the tool in absolute steps
 
 } // buffer_line()
 
-#if ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(DELTA)
-
-  /**
-   * Get the XYZ position of the steppers as a vector_3.
-   *
-   * On CORE machines XYZ is derived from ABC.
-   */
-  vector_3 Planner::adjusted_position() {
-    vector_3 pos = vector_3(stepper.get_axis_position_mm(X_AXIS), stepper.get_axis_position_mm(Y_AXIS), stepper.get_axis_position_mm(Z_AXIS));
-
-    //pos.debug("in Planner::adjusted_position");
-    //bed_level_matrix.debug("in Planner::adjusted_position");
-
-    matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
-    //inverse.debug("in Planner::inverse");
-
-    pos.apply_rotation(inverse);
-    //pos.debug("after rotation");
-
-    return pos;
-  }
-
-#endif // AUTO_BED_LEVELING_FEATURE && !DELTA
-
 /**
  * Directly set the planner XYZ position (hence the stepper positions).
  *
  * On CORE machines stepper ABC will be translated from the given XYZ.
  */
-#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
-  void Planner::set_position_mm(float x, float y, float z, const float& e)
-#else
-  void Planner::set_position_mm(const float& x, const float& y, const float& z, const float& e)
-#endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING
-  {
-    #if ENABLED(MESH_BED_LEVELING)
-
-      if (mbl.active())
-        z += mbl.get_z(RAW_X_POSITION(x), RAW_Y_POSITION(y));
-
-    #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
-
-      apply_rotation_xyz(bed_level_matrix, x, y, z);
+void Planner::set_position_mm(
+  #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
+    float x, float y, float z
+  #else
+    const float& x, const float& y, const float& z
+  #endif
+  , const float& e
+) {
 
-    #endif
+  #if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
+    apply_leveling(x, y, z);
+  #endif
 
-    long nx = position[X_AXIS] = lround(x * axis_steps_per_mm[X_AXIS]),
-         ny = position[Y_AXIS] = lround(y * axis_steps_per_mm[Y_AXIS]),
-         nz = position[Z_AXIS] = lround(z * axis_steps_per_mm[Z_AXIS]),
-         ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]);
-    stepper.set_position(nx, ny, nz, ne);
-    previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
+  long nx = position[X_AXIS] = lround(x * axis_steps_per_mm[X_AXIS]),
+       ny = position[Y_AXIS] = lround(y * axis_steps_per_mm[Y_AXIS]),
+       nz = position[Z_AXIS] = lround(z * axis_steps_per_mm[Z_AXIS]),
+       ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]);
+  stepper.set_position(nx, ny, nz, ne);
+  previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
 
-    LOOP_XYZE(i) previous_speed[i] = 0.0;
-  }
+  LOOP_XYZE(i) previous_speed[i] = 0.0;
+}
 
 /**
  * Directly set the planner E position (hence the stepper E position).

Marlin/planner.h

 
     #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
 
-      #if ENABLED(AUTO_BED_LEVELING_FEATURE)
-        /**
-         * The corrected position, applying the bed level matrix
-         */
-        static vector_3 adjusted_position();
+      #if ENABLED(MESH_BED_LEVELING)
+        static void apply_leveling(const float &x, const float &y, float &z);
+      #else
+        static void apply_leveling(float &x, float &y, float &z);
       #endif
 
       /**