From 04abd8d45901199e03ded423478885b659eb65e6 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Mon, 13 Nov 2017 01:34:19 -0600
Subject: [PATCH] Improve and clarify M428 code

---
 Marlin/src/gcode/geometry/M206_M428.cpp | 40 +++++++++++--------------
 Marlin/src/module/motion.h              |  1 +
 2 files changed, 18 insertions(+), 23 deletions(-)

diff --git a/Marlin/src/gcode/geometry/M206_M428.cpp b/Marlin/src/gcode/geometry/M206_M428.cpp
index 1de0f004e..4d15fb56b 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 826091c8c..7edbd29b3 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