From e0deb75764938995e32bc589d646ec397c4a3019 Mon Sep 17 00:00:00 2001 From: tombrazier <68918209+tombrazier@users.noreply.github.com> Date: Thu, 19 May 2022 22:15:15 +0100 Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix=20Leveling=20apply/unapply?= =?UTF-8?q?=20(#24188)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Scott Lahteine --- Marlin/src/feature/bedlevel/bedlevel.cpp | 14 ++----- Marlin/src/gcode/bedlevel/abl/G29.cpp | 53 ++++++++++++------------ Marlin/src/module/planner.cpp | 38 +++++++++-------- 3 files changed, 52 insertions(+), 53 deletions(-) diff --git a/Marlin/src/feature/bedlevel/bedlevel.cpp b/Marlin/src/feature/bedlevel/bedlevel.cpp index f9d17a69a1..1ca9696a3a 100644 --- a/Marlin/src/feature/bedlevel/bedlevel.cpp +++ b/Marlin/src/feature/bedlevel/bedlevel.cpp @@ -74,16 +74,10 @@ void set_bed_leveling_enabled(const bool enable/*=true*/) { _report_leveling(); planner.synchronize(); - if (planner.leveling_active) { // leveling from on to off - // change unleveled current_position to physical current_position without moving steppers. - planner.apply_leveling(current_position); - planner.leveling_active = false; // disable only AFTER calling apply_leveling - } - else { // leveling from off to on - planner.leveling_active = true; // enable BEFORE calling unapply_leveling, otherwise ignored - // change physical current_position to unleveled current_position without moving steppers. - planner.unapply_leveling(current_position); - } + // Get the corrected leveled / unleveled position + planner.apply_modifiers(current_position); // Physical position with all modifiers + planner.leveling_active ^= true; // Toggle leveling between apply and unapply + planner.unapply_modifiers(current_position); // Logical position with modifiers removed sync_plan_position(); _report_leveling(); diff --git a/Marlin/src/gcode/bedlevel/abl/G29.cpp b/Marlin/src/gcode/bedlevel/abl/G29.cpp index d9b0179a63..0d247e4767 100644 --- a/Marlin/src/gcode/bedlevel/abl/G29.cpp +++ b/Marlin/src/gcode/bedlevel/abl/G29.cpp @@ -74,14 +74,18 @@ #endif #endif +static void pre_g29_return(const bool retry, const bool did) { + if (!retry) { + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE, false)); + } + if (did) { + TERN_(HAS_DWIN_E3V2_BASIC, DWIN_LevelingDone()); + TERN_(EXTENSIBLE_UI, ExtUI::onLevelingDone()); + } +} + #define G29_RETURN(retry, did) do{ \ - if (TERN(G29_RETRY_AND_RECOVER, !retry, true)) { \ - TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE, false)); \ - } \ - if (did) { \ - TERN_(HAS_DWIN_E3V2_BASIC, DWIN_LevelingDone()); \ - TERN_(EXTENSIBLE_UI, ExtUI::onLevelingDone()); \ - } \ + pre_g29_return(TERN0(G29_RETRY_AND_RECOVER, retry), did); \ return TERN_(G29_RETRY_AND_RECOVER, retry); \ }while(0) @@ -326,8 +330,10 @@ G29_TYPE GcodeSuite::G29() { bedlevel.z_values[i][j] = rz; bedlevel.refresh_bed_level(); TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(i, j, rz)); - set_bed_leveling_enabled(abl.reenable); - if (abl.reenable) report_current_position(); + if (abl.reenable) { + set_bed_leveling_enabled(true); + report_current_position(); + } } G29_RETURN(false, false); } // parser.seen_test('W') @@ -693,7 +699,7 @@ G29_TYPE GcodeSuite::G29() { #endif - abl.reenable = false; + abl.reenable = false; // Don't re-enable after modifying the mesh idle_no_sleep(); } // inner @@ -878,33 +884,28 @@ G29_TYPE GcodeSuite::G29() { current_position = converted; if (DEBUGGING(LEVELING)) DEBUG_POS("G29 corrected XYZ", current_position); + + abl.reenable = true; + } + + // Auto Bed Leveling is complete! Enable if possible. + if (abl.reenable) { + planner.leveling_active = true; + sync_plan_position(); } #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) - if (!abl.dryrun) { - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("G29 uncorrected Z:", current_position.z); + // Auto Bed Leveling is complete! Enable if possible. + if (!abl.dryrun || abl.reenable) set_bed_leveling_enabled(true); - // Unapply the offset because it is going to be immediately applied - // and cause compensation movement in Z - current_position.z -= bedlevel.get_z_correction(current_position) - TERN_(ENABLE_LEVELING_FADE_HEIGHT, * planner.fade_scaling_factor_for_z(current_position.z)); + #endif - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM(" corrected Z:", current_position.z); - } - - #endif // ABL_PLANAR - - // Auto Bed Leveling is complete! Enable if possible. - planner.leveling_active = !abl.dryrun || abl.reenable; } // !isnan(abl.measured_z) // Restore state after probing if (!faux) restore_feedrate_and_scaling(); - // Sync the planner from the current_position - if (planner.leveling_active) sync_plan_position(); - TERN_(HAS_BED_PROBE, probe.move_z_after_probing()); #ifdef Z_PROBE_END_SCRIPT diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index a686d8ebc3..dcec8e3a62 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -1592,30 +1592,34 @@ void Planner::check_axes_activity() { } void Planner::unapply_leveling(xyz_pos_t &raw) { + if (!leveling_active) return; - if (leveling_active) { + #if ABL_PLANAR - #if ABL_PLANAR + matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix); - matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix); + xy_pos_t d = raw - level_fulcrum; + inverse.apply_rotation_xyz(d.x, d.y, raw.z); + raw = d + level_fulcrum; - xy_pos_t d = raw - level_fulcrum; - inverse.apply_rotation_xyz(d.x, d.y, raw.z); - raw = d + level_fulcrum; + #elif HAS_MESH - #elif HAS_MESH - - TERN_(MESH_BED_LEVELING, raw.z -= bedlevel.get_z_offset()); - - #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) - const float fade_scaling_factor = fade_scaling_factor_for_z(raw.z); - if (fade_scaling_factor) raw.z -= fade_scaling_factor * bedlevel.get_z_correction(raw); - #else - raw.z -= bedlevel.get_z_correction(raw); - #endif + const float z_correction = bedlevel.get_z_correction(raw), + z_full_fade = DIFF_TERN(MESH_BED_LEVELING, raw.z, bedlevel.get_z_offset()), + z_no_fade = z_full_fade - z_correction; + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + if (!z_fade_height || z_no_fade <= 0.0f) // Not fading or at bed level? + raw.z = z_no_fade; // Unapply full mesh Z. + else if (z_full_fade >= z_fade_height) // Above the fade height? + raw.z = z_full_fade; // Nothing more to unapply. + else // Within the fade zone? + raw.z = z_no_fade / (1.0f - z_correction * inverse_z_fade_height); // Unapply the faded Z offset + #else + raw.z = z_no_fade; #endif - } + + #endif } #endif // HAS_LEVELING