diff --git a/Marlin/src/gcode/geometry/M206_M428.cpp b/Marlin/src/gcode/geometry/M206_M428.cpp index 1de0f004e4..4d15fb56b6 100644 --- a/Marlin/src/gcode/geometry/M206_M428.cpp +++ b/Marlin/src/gcode/geometry/M206_M428.cpp @@ -47,7 +47,6 @@ void GcodeSuite::M206() { if (parser.seen('P')) set_home_offset(B_AXIS, parser.value_linear_units()); // Psi #endif - SYNC_PLAN_POSITION_KINEMATIC(); report_current_position(); } @@ -63,32 +62,27 @@ void GcodeSuite::M206() { * Use M206 to set these values directly. */ void GcodeSuite::M428() { - bool err = false; + if (axis_unhomed_error()) return; + + float diff[XYZ]; LOOP_XYZ(i) { - if (axis_homed[i]) { - const float base = (current_position[i] > (soft_endstop_min[i] + soft_endstop_max[i]) * 0.5) ? base_home_pos((AxisEnum)i) : 0, - diff = base - current_position[i]; - if (WITHIN(diff, -20, 20)) { - set_home_offset((AxisEnum)i, diff); - } - else { - SERIAL_ERROR_START(); - SERIAL_ERRORLNPGM(MSG_ERR_M428_TOO_FAR); - LCD_ALERTMESSAGEPGM("Err: Too far!"); - BUZZ(200, 40); - err = true; - break; - } + diff[i] = base_home_pos((AxisEnum)i) - current_position[i]; + if (!WITHIN(diff[i], -20, 20) && home_dir((AxisEnum)i) > 0) + diff[i] = -current_position[i]; + if (!WITHIN(diff[i], -20, 20)) { + SERIAL_ERROR_START(); + SERIAL_ERRORLNPGM(MSG_ERR_M428_TOO_FAR); + LCD_ALERTMESSAGEPGM("Err: Too far!"); + BUZZ(200, 40); + return; } } - if (!err) { - SYNC_PLAN_POSITION_KINEMATIC(); - report_current_position(); - LCD_MESSAGEPGM(MSG_HOME_OFFSETS_APPLIED); - BUZZ(100, 659); - BUZZ(100, 698); - } + LOOP_XYZ(i) set_home_offset((AxisEnum)i, diff[i]); + report_current_position(); + LCD_MESSAGEPGM(MSG_HOME_OFFSETS_APPLIED); + BUZZ(100, 659); + BUZZ(100, 698); } #endif // HAS_M206_COMMAND diff --git a/Marlin/src/module/motion.h b/Marlin/src/module/motion.h index 826091c8ca..7edbd29b3d 100644 --- a/Marlin/src/module/motion.h +++ b/Marlin/src/module/motion.h @@ -173,6 +173,7 @@ void clean_up_after_endstop_or_probe_move(); || ENABLED(NOZZLE_CLEAN_FEATURE) \ || ENABLED(NOZZLE_PARK_FEATURE) \ || (ENABLED(ADVANCED_PAUSE_FEATURE) && ENABLED(HOME_BEFORE_FILAMENT_CHANGE)) \ + || HAS_M206_COMMAND \ ) || ENABLED(NO_MOTION_BEFORE_HOMING) #if HAS_AXIS_UNHOMED_ERR