From cb92d78a468762507f010d2fae2ec1f44e580941 Mon Sep 17 00:00:00 2001
From: InsanityAutomation <d.menzel@insanityautomation.com>
Date: Thu, 20 Aug 2020 21:53:14 -0400
Subject: [PATCH] Allow M206 offsets to apply to z safe home move

---
 Marlin/Configuration_adv.h                  |  3 +++
 Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp |  3 +++
 Marlin/src/gcode/calibrate/G28.cpp          | 13 +++++++------
 Marlin/src/gcode/geometry/M206_M428.cpp     |  6 ++++++
 Marlin/src/module/motion.h                  |  4 ----
 5 files changed, 19 insertions(+), 10 deletions(-)

diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index 724696868a..fb3558dfe9 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -3327,6 +3327,9 @@
 
   #define USER_DESC_3 "Standard"
   #define USER_GCODE_3 "M92E814\nM907E750\nM500"
+
+  #define USER_DESC_4 "Mosquito BMG-M"
+  #define USER_GCODE_4 "M92E814\nM206X5Y15\nM907E750\nM500"
 #endif
 
 /**
diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
index dc91b7d6b1..80799e340f 100644
--- a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
+++ b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
@@ -1555,6 +1555,9 @@
          * numbers for those locations should be 0.
          */
         #ifdef VALIDATE_MESH_TILT
+          #if ENABLED(Z_SAFE_HOMING)
+            constexpr xy_float_t safe_homing_xy = { Z_SAFE_HOMING_X_POINT , Z_SAFE_HOMING_Y_POINT };
+          #endif
           auto d_from = []{ DEBUG_ECHOPGM("D from "); };
           auto normed = [&](const xy_pos_t &pos, const float &zadd) {
             return normal.x * pos.x + normal.y * pos.y + zadd;
diff --git a/Marlin/src/gcode/calibrate/G28.cpp b/Marlin/src/gcode/calibrate/G28.cpp
index 7bc40b2127..121fc9604c 100644
--- a/Marlin/src/gcode/calibrate/G28.cpp
+++ b/Marlin/src/gcode/calibrate/G28.cpp
@@ -47,7 +47,7 @@
 
 #include "../../lcd/ultralcd.h"
 #if ENABLED(DWIN_CREALITY_LCD)
-  #include "../../lcd/dwin/e3v2/dwin.h"
+  #include "../../lcd/dwin/dwin.h"
 #endif
 
 #if HAS_L64XX                         // set L6470 absolute position registers to counts
@@ -118,15 +118,15 @@
     DEBUG_SECTION(log_G28, "home_z_safely", DEBUGGING(LEVELING));
 
     // Disallow Z homing if X or Y homing is needed
-    if (homing_needed_error(_BV(X_AXIS) | _BV(Y_AXIS))) return;
-
+    if (axis_unhomed_error(_BV(X_AXIS) | _BV(Y_AXIS))) return;
     sync_plan_position();
 
     /**
      * Move the Z probe (or just the nozzle) to the safe homing point
      * (Z is already at the right height)
      */
-    destination.set(safe_homing_xy, current_position.z);
+
+    destination.set((xy_float_t){ Z_SAFE_HOMING_X_POINT + home_offset[X_AXIS], Z_SAFE_HOMING_Y_POINT + home_offset[Y_AXIS] }, current_position.z);
 
     TERN_(HOMING_Z_WITH_PROBE, destination -= probe.offset_xy);
 
@@ -192,6 +192,7 @@
  *  X   Home to the X endstop
  *  Y   Home to the Y endstop
  *  Z   Home to the Z endstop
+ *
  */
 void GcodeSuite::G28() {
   DEBUG_SECTION(log_G28, "G28", DEBUGGING(LEVELING));
@@ -298,8 +299,8 @@ void GcodeSuite::G28() {
   #else // NOT DELTA
 
     const bool homeZ = parser.seen('Z'),
-               needX = homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(X_AXIS))),
-               needY = homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(Y_AXIS))),
+               needX = homeZ && TERN0(Z_SAFE_HOMING, axes_need_homing(_BV(X_AXIS))),
+               needY = homeZ && TERN0(Z_SAFE_HOMING, axes_need_homing(_BV(Y_AXIS))),
                homeX = needX || parser.seen('X'), homeY = needY || parser.seen('Y'),
                home_all = homeX == homeY && homeX == homeZ, // All or None
                doX = home_all || homeX, doY = home_all || homeY, doZ = home_all || homeZ;
diff --git a/Marlin/src/gcode/geometry/M206_M428.cpp b/Marlin/src/gcode/geometry/M206_M428.cpp
index a477a1a526..a797f28685 100644
--- a/Marlin/src/gcode/geometry/M206_M428.cpp
+++ b/Marlin/src/gcode/geometry/M206_M428.cpp
@@ -41,6 +41,12 @@ void GcodeSuite::M206() {
     if (parser.seen(XYZ_CHAR(i)))
       set_home_offset((AxisEnum)i, parser.value_linear_units());
 
+    if (!parser.seen_any()) {
+      SERIAL_ECHOLNPAIR("M206X : ", home_offset[X_AXIS]);
+      SERIAL_ECHOLNPAIR("M206Y : ", home_offset[Y_AXIS]);
+      SERIAL_ECHOLNPAIR("M206Z : ", home_offset[Z_AXIS]);
+    }
+
   #if ENABLED(MORGAN_SCARA)
     if (parser.seen('T')) set_home_offset(A_AXIS, parser.value_float()); // Theta
     if (parser.seen('P')) set_home_offset(B_AXIS, parser.value_float()); // Psi
diff --git a/Marlin/src/module/motion.h b/Marlin/src/module/motion.h
index 2b108b0531..236f7aae28 100644
--- a/Marlin/src/module/motion.h
+++ b/Marlin/src/module/motion.h
@@ -78,10 +78,6 @@ extern xyz_pos_t cartes;
   #define XY_PROBE_FEEDRATE_MM_S PLANNER_XY_FEEDRATE()
 #endif
 
-#if ENABLED(Z_SAFE_HOMING)
-  constexpr xy_float_t safe_homing_xy = { Z_SAFE_HOMING_X_POINT, Z_SAFE_HOMING_Y_POINT };
-#endif
-
 /**
  * Feed rates are often configured with mm/m
  * but the planner and stepper like mm/s units.