From d065d378222480a248cfe7c14ed22c316276a8d2 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Wed, 29 Apr 2015 18:26:16 -0700
Subject: [PATCH] Add M428 to set home_offset logically

---
 Marlin/Marlin_main.cpp         | 30 ++++++++++++++++++++++++++++++
 Marlin/configuration_store.cpp |  2 +-
 Marlin/language.h              |  1 +
 Marlin/ultralcd.cpp            | 15 +++++----------
 4 files changed, 37 insertions(+), 11 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 996bd60b91..6e41e13350 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -189,6 +189,7 @@
  * M410 - Quickstop. Abort all the planned moves
  * M420 - Enable/Disable Mesh Leveling (with current values) S1=enable S0=disable
  * M421 - Set a single Z coordinate in the Mesh Leveling grid. X<mm> Y<mm> Z<mm>
+ * M428 - Set the home_offset logically based on the current_position
  * M500 - Store parameters in EEPROM
  * M501 - Read parameters from EEPROM (if you need reset them after you changed them temporarily).
  * M502 - Revert to the default "factory settings". You still need to store them in EEPROM afterwards if you want to.
@@ -4504,6 +4505,31 @@ inline void gcode_M410() { quickStop(); }
 
 #endif
 
+/**
+ * M428: Set home_offset based on the distance between the
+ *       current_position and the nearest "reference position."
+ *       If an axis is past center the endstop position
+ *       is the reference-point. Otherwise it uses 0. This allows
+ *       the Z offset to be set near the bed when using a max endstop.
+ *
+ *       Use M206 to set these values directly.
+ */
+inline void gcode_M428() {
+  for (int8_t i = X_AXIS; i <= Z_AXIS; i++) {
+    float base = (current_position[i] > (min_pos[i] + max_pos[i]) / 2) ? base_home_pos(i) : 0,
+          diff = current_position[i] - base;
+    if (diff > -20 && diff < 20) {
+      home_offset[i] -= diff;
+      current_position[i] = base;
+    }
+    else {
+      SERIAL_ERROR_START;
+      SERIAL_ERRORLNPGM(MSG_ERR_M428_TOO_FAR);
+    }
+  }
+  sync_plan_position();
+}
+
 /**
  * M500: Store settings in EEPROM
  */
@@ -5353,6 +5379,10 @@ void process_commands() {
           break;
       #endif
 
+      case 428: // M428 Apply current_position to home_offset
+        gcode_M428();
+        break;
+
       case 500: // M500 Store settings in EEPROM
         gcode_M500();
         break;
diff --git a/Marlin/configuration_store.cpp b/Marlin/configuration_store.cpp
index 782b5d320a..362749a10f 100644
--- a/Marlin/configuration_store.cpp
+++ b/Marlin/configuration_store.cpp
@@ -676,7 +676,7 @@ void Config_PrintSettings(bool forReplay) {
       SERIAL_ECHOLNPGM("Mesh bed leveling:");
       CONFIG_ECHO_START;
     }
-    SERIAL_ECHOPAIR("  M420 S", (int32_t)mbl.active);
+    SERIAL_ECHOPAIR("  M420 S", (unsigned long)mbl.active);
     SERIAL_ECHOPAIR(" X", MESH_NUM_X_POINTS);
     SERIAL_ECHOPAIR(" Y", MESH_NUM_Y_POINTS);
     SERIAL_EOL;
diff --git a/Marlin/language.h b/Marlin/language.h
index 252ee595ab..31239658eb 100644
--- a/Marlin/language.h
+++ b/Marlin/language.h
@@ -162,6 +162,7 @@
 #define MSG_ERR_MATERIAL_INDEX              "M145 S<index> out of range (0-1)"
 #define MSG_ERR_M421_REQUIRES_XYZ           "M421 requires XYZ parameters"
 #define MSG_ERR_MESH_INDEX_OOB              "Mesh XY index is out of bounds"
+#define MSG_ERR_M428_TOO_FAR                "Too far from home or origin position"
 #define MSG_M119_REPORT                     "Reporting endstop status"
 #define MSG_ENDSTOP_HIT                     "TRIGGERED"
 #define MSG_ENDSTOP_OPEN                    "open"
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 4dd32eb0de..57e5113617 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -438,17 +438,12 @@ static void lcd_main_menu() {
   }
 #endif
 
+/**
+ * Set the home offset based on the current_position
+ */
 void lcd_set_home_offsets() {
-  for (int8_t i=0; i < NUM_AXIS; i++) {
-    if (i != E_AXIS) {
-      home_offset[i] -= current_position[i];
-      current_position[i] = 0.0;
-    }
-  }
-  plan_set_position(0.0, 0.0, 0.0, current_position[E_AXIS]);
-
-  // Audio feedback
-  enqueuecommands_P(PSTR("M300 S659 P200\nM300 S698 P200"));
+  // Command with Audio feedback
+  enqueuecommands_P(PSTR("M428\nM300 S659 P200\nM300 S698 P200"));
   lcd_return_to_status();
 }