From 6f156d01d9b8903a6fb20546859447330203e158 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Mon, 16 Apr 2018 03:35:12 -0500 Subject: [PATCH] Update position for UBL non-segmented activate/deactivate --- Marlin/Marlin_main.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 2be15edb9..34879ddf8 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -2461,7 +2461,21 @@ void clean_up_after_endstop_or_probe_move() { planner.unapply_leveling(current_position); } #else - planner.leveling_active = enable; // just flip the bit, current_position will be wrong until next move. + // UBL equivalents for apply/unapply_leveling + #if ENABLED(SKEW_CORRECTION) + float pos[XYZ] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] }; + planner.skew(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS]); + #else + const float (&pos)[XYZE] = current_position; + #endif + if (planner.leveling_active) { + current_position[Z_AXIS] += ubl.get_z_correction(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS]); + planner.leveling_active = false; + } + else { + planner.leveling_active = true; + current_position[Z_AXIS] -= ubl.get_z_correction(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS]); + } #endif #else // ABL