From f1467ddc0e57cc678ee1aa95c750efe724e1afc0 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Sat, 11 Aug 2018 22:44:51 -0500
Subject: [PATCH] UBL/planner patch tweaks

---
 Marlin/planner.cpp | 28 +++++++++++++---------------
 1 file changed, 13 insertions(+), 15 deletions(-)

diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 142a7489f..14e578de8 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -2527,12 +2527,12 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
   #endif
   position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]);
   position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]);
-  #if !IS_KINEMATIC && ENABLED(AUTO_BED_LEVELING_UBL)
-    if (leveling_active)
-      position[C_AXIS] = LROUND((c + ubl.get_z_correction(a, b)) * axis_steps_per_mm[Z_AXIS]);
-    else
-  #endif
-  position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]);
+  position[C_AXIS] = LROUND(axis_steps_per_mm[C_AXIS] * (c +
+    #if !IS_KINEMATIC && ENABLED(AUTO_BED_LEVELING_UBL)
+      leveling_active ? ubl.get_z_correction(a, b) :
+    #endif
+    0
+  ));
   position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
   #if HAS_POSITION_FLOAT
     position_float[A_AXIS] = a;
@@ -2574,19 +2574,17 @@ void Planner::set_position_mm(const AxisEnum axis, const float &v) {
   #else
     const uint8_t axis_index = axis;
   #endif
-  #if ENABLED(AUTO_BED_LEVELING_UBL)
-    if (axis == Z_AXIS && leveling_active)
-      position[axis] = LROUND((v + ubl.get_z_correction(current_position[X_AXIS], current_position[Y_AXIS])) * axis_steps_per_mm[axis_index]);
-    else
-  #endif
-  position[axis] = LROUND(v * axis_steps_per_mm[axis_index]);
+  position[axis] = LROUND(axis_steps_per_mm[axis_index] * (v +
+    #if ENABLED(AUTO_BED_LEVELING_UBL)
+      axis == Z_AXIS && leveling_active ? ubl.get_z_correction(current_position[X_AXIS], current_position[Y_AXIS]) :
+    #endif
+    0
+  ));
   #if HAS_POSITION_FLOAT
     position_float[axis] = v;
   #endif
-  if (has_blocks_queued()) {
-    //previous_speed[axis] = 0.0;
+  if (has_blocks_queued())
     buffer_sync_block();
-  }
   else
     stepper.set_position(axis, position[axis]);
 }