bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

🐛 Fix Leveling apply/unapply (#24188)

Browse Source 
Co-authored-by: Scott Lahteine <github@thinkyhead.com>
This commit is contained in:
tombrazier
2022-05-19 22:15:15 +01:00
committed by Scott Lahteine
parent a0fe4f4895
commit e0deb75764
3 changed files with 52 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
Marlin/src/module/planner.cpp
View File

@ -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