MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
Marlin/src/feature/bedlevel/bedlevel.cpp
View File

@ -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();

53
Marlin/src/gcode/bedlevel/abl/G29.cpp
View File

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

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