From 081e4506ca5356501415bfaf066d982947449623 Mon Sep 17 00:00:00 2001
From: Luu Lac <45380455+shitcreek@users.noreply.github.com>
Date: Wed, 28 Aug 2019 04:20:28 -0500
Subject: [PATCH] Fix G53 as prefix, G28 with CNC_COORDINATE_SYSTEMS (#15069)

---
 Marlin/src/gcode/calibrate/G28.cpp    |  1 +
 Marlin/src/gcode/geometry/G53-G59.cpp | 33 ++++++++++++++++-----------
 Marlin/src/gcode/geometry/G92.cpp     |  9 +++-----
 Marlin/src/module/motion.cpp          |  5 ----
 4 files changed, 24 insertions(+), 24 deletions(-)

diff --git a/Marlin/src/gcode/calibrate/G28.cpp b/Marlin/src/gcode/calibrate/G28.cpp
index d936809051..420eb65c9f 100644
--- a/Marlin/src/gcode/calibrate/G28.cpp
+++ b/Marlin/src/gcode/calibrate/G28.cpp
@@ -453,6 +453,7 @@ void GcodeSuite::G28(const bool always_home_all) {
   ui.refresh();
 
   report_current_position();
+
   #if ENABLED(NANODLP_Z_SYNC)
     #if ENABLED(NANODLP_ALL_AXIS)
       #define _HOME_SYNC true       // For any axis, output sync text.
diff --git a/Marlin/src/gcode/geometry/G53-G59.cpp b/Marlin/src/gcode/geometry/G53-G59.cpp
index 813c026e75..88914ccffd 100644
--- a/Marlin/src/gcode/geometry/G53-G59.cpp
+++ b/Marlin/src/gcode/geometry/G53-G59.cpp
@@ -27,23 +27,21 @@
 
 #include "../../module/stepper.h"
 
+//#define DEBUG_M53
+
 /**
  * Select a coordinate system and update the workspace offset.
  * System index -1 is used to specify machine-native.
  */
 bool GcodeSuite::select_coordinate_system(const int8_t _new) {
   if (active_coordinate_system == _new) return false;
-  planner.synchronize();
-  float old_offset[XYZ] = { 0 }, new_offset[XYZ] = { 0 };
-  if (WITHIN(active_coordinate_system, 0, MAX_COORDINATE_SYSTEMS - 1))
-    COPY(old_offset, coordinate_system[active_coordinate_system]);
+  active_coordinate_system = _new;
+  float new_offset[XYZ] = { 0 };
   if (WITHIN(_new, 0, MAX_COORDINATE_SYSTEMS - 1))
     COPY(new_offset, coordinate_system[_new]);
-  active_coordinate_system = _new;
   LOOP_XYZ(i) {
-    const float diff = new_offset[i] - old_offset[i];
-    if (diff) {
-      position_shift[i] += diff;
+    if (position_shift[i] != new_offset[i]) {
+      position_shift[i] = new_offset[i];
       update_workspace_offset((AxisEnum)i);
     }
   }
@@ -60,11 +58,20 @@ bool GcodeSuite::select_coordinate_system(const int8_t _new) {
  * Marlin also uses G53 on a line by itself to go back to native space.
  */
 void GcodeSuite::G53() {
-  const int8_t _system = active_coordinate_system;
-  active_coordinate_system = -1;
-  if (parser.chain()) { // If this command has more following...
-    process_parsed_command();
-    active_coordinate_system = _system;
+  const int8_t old_system = active_coordinate_system;
+  select_coordinate_system(-1);   // Always remove workspace offsets
+  #ifdef DEBUG_M53
+    SERIAL_ECHOLNPGM("Go to native space");
+    report_current_position();
+  #endif
+
+  if (parser.chain()) {       // Command to chain?
+    process_parsed_command(); // ...process the chained command
+    select_coordinate_system(old_system);
+    #ifdef DEBUG_M53
+      SERIAL_ECHOLNPAIR("Go back to workspace ", old_system);
+      report_current_position();
+    #endif
   }
 }
 
diff --git a/Marlin/src/gcode/geometry/G92.cpp b/Marlin/src/gcode/geometry/G92.cpp
index 9304274520..0ecf43e580 100644
--- a/Marlin/src/gcode/geometry/G92.cpp
+++ b/Marlin/src/gcode/geometry/G92.cpp
@@ -52,12 +52,9 @@ void GcodeSuite::G92() {
       case 1: {
         // Zero the G92 values and restore current position
         #if !IS_SCARA
-          LOOP_XYZ(i) {
-            const float v = position_shift[i];
-            if (v) {
-              position_shift[i] = 0;
-              update_workspace_offset((AxisEnum)i);
-            }
+          LOOP_XYZ(i) if (position_shift[i]) {
+            position_shift[i] = 0;
+            update_workspace_offset((AxisEnum)i);
           }
         #endif // Not SCARA
       } return;
diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp
index a31f1c08e5..c5854d1f15 100644
--- a/Marlin/src/module/motion.cpp
+++ b/Marlin/src/module/motion.cpp
@@ -1325,11 +1325,6 @@ void set_axis_is_at_home(const AxisEnum axis) {
   SBI(axis_known_position, axis);
   SBI(axis_homed, axis);
 
-  #if HAS_POSITION_SHIFT
-    position_shift[axis] = 0;
-    update_workspace_offset(axis);
-  #endif
-
   #if ENABLED(DUAL_X_CARRIAGE)
     if (axis == X_AXIS && (active_extruder == 1 || dual_x_carriage_mode == DXC_DUPLICATION_MODE)) {
       current_position[X_AXIS] = x_home_pos(active_extruder);