From 105d20c2e8165fad280ef698cd792c42bbcc3c49 Mon Sep 17 00:00:00 2001 From: Thomas Moore Date: Fri, 10 Aug 2018 14:00:45 -0400 Subject: [PATCH] Account for UBL correction when setting the planner position (#11511) --- Marlin/planner.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index b17d4bab6..142a7489f 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -2525,9 +2525,14 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c #if ENABLED(DISTINCT_E_FACTORS) last_extruder = active_extruder; #endif - position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]), - position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]), - position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]), + 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[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]); #if HAS_POSITION_FLOAT position_float[A_AXIS] = a; @@ -2569,6 +2574,11 @@ 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]); #if HAS_POSITION_FLOAT position_float[axis] = v;