From 227b96b28b847fa573970dac9d9543b81d68e5a6 Mon Sep 17 00:00:00 2001
From: RowanMeara <rrm89@cornell.edu>
Date: Sun, 12 Nov 2017 20:13:23 -0800
Subject: [PATCH] Fix M428

This fixes the old implementation of M428 which was broken, did not
match the website, and made no sense.
---
 Marlin/Marlin.h        |  1 +
 Marlin/Marlin_main.cpp | 38 +++++++++++++++++---------------------
 2 files changed, 18 insertions(+), 21 deletions(-)

diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index bcbf3181b..5e2dd347b 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -439,6 +439,7 @@ void do_blocking_move_to_xy(const float &x, const float &y, const float &fr_mm_s
       || 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
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 34a4016e9..a75cc78f2 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -9959,31 +9959,27 @@ void quickstop_stepper() {
    *       Use M206 to set these values directly.
    */
   inline void gcode_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) {
-      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