Fix planner leveling and rename arguments

Use lx, ly, lz for “logical” positions

Marlin/planner.cpp

@@ -523,30 +523,44 @@ void Planner::check_axes_activity() {
 
 #if PLANNER_LEVELING
 
-  void Planner::apply_leveling(
+  void Planner::apply_leveling(float &lx, float &ly, float &lz) {
     #if ENABLED(MESH_BED_LEVELING)
-      const float &x, const float &y
-    #else
-      float &x, float &y
+
+      if (mbl.active())
+        lz += mbl.get_z(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
+
+    #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+      float dx = RAW_X_POSITION(lx) - (X_TILT_FULCRUM),
+            dy = RAW_Y_POSITION(ly) - (Y_TILT_FULCRUM),
+            dz = RAW_Z_POSITION(lz);
+
+      apply_rotation_xyz(bed_level_matrix, dx, dy, dz);
+
+      lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
+      ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
+      lz = LOGICAL_Z_POSITION(dz);
+
     #endif
-    , float &z
-  ) {
+  }
+
+  void Planner::unapply_leveling(float &lx, float &ly, float &lz) {
     #if ENABLED(MESH_BED_LEVELING)
 
       if (mbl.active())
-        z += mbl.get_z(RAW_X_POSITION(x), RAW_Y_POSITION(y));
+        lz -= mbl.get_z(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
+
+    #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
 
-    #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
+      matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
 
-      float tx = RAW_X_POSITION(x) - (X_TILT_FULCRUM),
-            ty = RAW_Y_POSITION(y) - (Y_TILT_FULCRUM),
-            tz = RAW_Z_POSITION(z);
+      float dx = lx - (X_TILT_FULCRUM), dy = ly - (Y_TILT_FULCRUM), dz = lz;
 
-      apply_rotation_xyz(bed_level_matrix, tx, ty, tz);
+      apply_rotation_xyz(inverse, dx, dy, dz);
 
-      x = LOGICAL_X_POSITION(tx + X_TILT_FULCRUM);
-      y = LOGICAL_Y_POSITION(ty + Y_TILT_FULCRUM);
-      z = LOGICAL_Z_POSITION(tz);
+      lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
+      ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
+      lz = LOGICAL_Z_POSITION(dz);
 
     #endif
   }
@@ -562,15 +576,7 @@ void Planner::check_axes_activity() {
  *  fr_mm_s   - (target) speed of the move
  *  extruder  - target extruder
  */
-
-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
-) {
+void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, 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);
 
@@ -578,17 +584,17 @@ void Planner::buffer_line(
   // Rest here until there is room in the buffer.
   while (block_buffer_tail == next_buffer_head) idle();
 
-  #if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
-    apply_leveling(x, y, z);
+  #if PLANNER_LEVELING
+    apply_leveling(lx, ly, lz);
   #endif
 
   // The target position of the tool in absolute steps
   // Calculate target position in absolute steps
   //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
   long target[NUM_AXIS] = {
-    lround(x * axis_steps_per_mm[X_AXIS]),
-    lround(y * axis_steps_per_mm[Y_AXIS]),
-    lround(z * axis_steps_per_mm[Z_AXIS]),
+    lround(lx * axis_steps_per_mm[X_AXIS]),
+    lround(ly * axis_steps_per_mm[Y_AXIS]),
+    lround(lz * axis_steps_per_mm[Z_AXIS]),
     lround(e * axis_steps_per_mm[E_AXIS])
   };
 
@@ -598,11 +604,22 @@ void Planner::buffer_line(
 
   /*
   SERIAL_ECHO_START;
-  SERIAL_ECHOPAIR("Planner X:", x);
+  SERIAL_ECHOPGM("Planner ", x);
+  #if IS_KINEMATIC
+    SERIAL_ECHOPAIR("A:", x);
+    SERIAL_ECHOPAIR(" (", dx);
+    SERIAL_ECHOPAIR(") B:", y);
+  #else
+    SERIAL_ECHOPAIR("X:", x);
     SERIAL_ECHOPAIR(" (", dx);
     SERIAL_ECHOPAIR(") Y:", y);
+  #endif
   SERIAL_ECHOPAIR(" (", dy);
+  #elif ENABLED(DELTA)
+    SERIAL_ECHOPAIR(") C:", z);
+  #else
     SERIAL_ECHOPAIR(") Z:", z);
+  #endif
   SERIAL_ECHOPAIR(" (", dz);
   SERIAL_ECHOLNPGM(")");
   //*/
@@ -671,7 +688,7 @@ void Planner::buffer_line(
   // For a mixing extruder, get a magnified step_event_count for each
   #if ENABLED(MIXING_EXTRUDER)
     for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
-      block->mix_event_count[i] = (mixing_factor[i] < 0.0001) ? 0 : block->step_event_count / mixing_factor[i];
+      block->mix_event_count[i] = UNEAR_ZERO(mixing_factor[i]) ? 0 : block->step_event_count / mixing_factor[i];
   #endif
 
   #if FAN_COUNT > 0
@@ -1152,22 +1169,15 @@ void Planner::buffer_line(
  *
  * On CORE machines stepper ABC will be translated from the given XYZ.
  */
-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
-) {
+void Planner::set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
 
-  #if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
-    apply_leveling(x, y, z);
+  #if PLANNER_LEVELING
+    apply_leveling(lx, ly, lz);
   #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]),
+  long nx = position[X_AXIS] = lround(lx * axis_steps_per_mm[X_AXIS]),
+       ny = position[Y_AXIS] = lround(ly * axis_steps_per_mm[Y_AXIS]),
+       nz = position[Z_AXIS] = lround(lz * 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.

Marlin/planner.h

@@ -202,11 +202,24 @@ class Planner {
     static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); }
 
     #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
-
-      #if ENABLED(MESH_BED_LEVELING)
-        static void apply_leveling(const float &x, const float &y, float &z);
+      #define ARG_X float lx
+      #define ARG_Y float ly
+      #define ARG_Z float lz
     #else
-        static void apply_leveling(float &x, float &y, float &z);
+      #define ARG_X const float &lx
+      #define ARG_Y const float &ly
+      #define ARG_Z const float &lz
+    #endif
+
+    #if PLANNER_LEVELING
+
+      /**
+       * Apply leveling to transform a cartesian position
+       * as it will be given to the planner and steppers.
+       */
+      static void apply_leveling(float &lx, float &ly, float &lz);
+      static void unapply_leveling(float &lx, float &ly, float &lz);
+
     #endif
 
     /**
@@ -216,7 +229,7 @@ class Planner {
      *  fr_mm_s   - (target) speed of the move (mm/s)
      *  extruder  - target extruder
      */
-      static void buffer_line(float x, float y, float z, const float& e, float fr_mm_s, const uint8_t extruder);
+    static void buffer_line(ARG_X, ARG_Y, ARG_Z, const float& e, float fr_mm_s, const uint8_t extruder);
 
     /**
      * Set the planner.position and individual stepper positions.
@@ -227,14 +240,7 @@ class Planner {
      *
      * Clears previous speed values.
      */
-      static void set_position_mm(float x, float y, float z, const float& e);
-
-    #else
-
-      static void buffer_line(const float& x, const float& y, const float& z, const float& e, float fr_mm_s, const uint8_t extruder);
-      static void set_position_mm(const float& x, const float& y, const float& z, const float& e);
-
-    #endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING
+    static void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float& e);
 
     /**
      * Set the E position (mm) of the planner (and the E stepper)