Skip to content

Commit

Permalink
🐛 Fix Leveling apply/unapply (MarlinFirmware#24188)
Browse files Browse the repository at this point in the history 
Co-authored-by: Scott Lahteine <github@thinkyhead.com>
  • Loading branch information
@tombrazier @thinkyhead @LCh-77
2 people authored and LCh-77 committed May 28, 2022
1 parent 9a1ec7a commit 2675014
Show file tree
Hide file tree
Showing 3 changed files with 52 additions and 53 deletions.
14 changes: 4 additions & 10 deletions Marlin/src/feature/bedlevel/bedlevel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
53 changes: 27 additions & 26 deletions Marlin/src/gcode/bedlevel/abl/G29.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down Expand Up @@ -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')
Expand Down Expand Up @@ -731,7 +737,7 @@ G29_TYPE GcodeSuite::G29() {

#endif

abl.reenable = false;
abl.reenable = false; // Don't re-enable after modifying the mesh
idle_no_sleep();
TERN_(JYENHANCED, if (temp_val.cancel_lev) break; );

Expand Down Expand Up @@ -917,33 +923,28 @@ G29_TYPE GcodeSuite::G29() {
current_position = converted;

if (DEBUGGING(LEVELING)) DEBUG_POS("G29 corrected XYZ", current_position);
}

#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
abl.reenable = true;
}

if (!abl.dryrun) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("G29 uncorrected Z:", current_position.z);
// Auto Bed Leveling is complete! Enable if possible.
if (abl.reenable) {
planner.leveling_active = true;
sync_plan_position();
}

// 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));
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)

if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM(" corrected Z:", current_position.z);
}
// Auto Bed Leveling is complete! Enable if possible.
if (!abl.dryrun || abl.reenable) set_bed_leveling_enabled(true);

#endif // ABL_PLANAR
#endif

// 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
Expand Down
38 changes: 21 additions & 17 deletions Marlin/src/module/planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1598,30 +1598,34 @@ void Planner::check_axes_activity() {
}

void Planner::unapply_leveling(xyz_pos_t &raw) {
if (!leveling_active) return;

if (leveling_active) {

#if ABL_PLANAR

matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
#if ABL_PLANAR

xy_pos_t d = raw - level_fulcrum;
inverse.apply_rotation_xyz(d.x, d.y, raw.z);
raw = d + level_fulcrum;
matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);

#elif HAS_MESH
xy_pos_t d = raw - level_fulcrum;
inverse.apply_rotation_xyz(d.x, d.y, raw.z);
raw = d + level_fulcrum;

TERN_(MESH_BED_LEVELING, raw.z -= bedlevel.get_z_offset());
#elif HAS_MESH

#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
Expand Down

0 comments on commit 2675014

Please sign in to comment.