From 5fefa200baf0243c1133c547c456fa66e241533e Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Wed, 1 Nov 2017 12:05:24 -0500
Subject: [PATCH 1/6] Tweaks to cubic_b_sline code style

---
 Marlin/planner_bezier.cpp | 32 ++++++++++++++++----------------
 1 file changed, 16 insertions(+), 16 deletions(-)

diff --git a/Marlin/planner_bezier.cpp b/Marlin/planner_bezier.cpp
index 71697e04a..d07863d13 100644
--- a/Marlin/planner_bezier.cpp
+++ b/Marlin/planner_bezier.cpp
@@ -107,10 +107,10 @@ inline static float dist1(float x1, float y1, float x2, float y2) { return FABS(
  */
 void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS], const float offset[4], float fr_mm_s, uint8_t extruder) {
   // Absolute first and second control points are recovered.
-  float first0 = position[X_AXIS] + offset[0];
-  float first1 = position[Y_AXIS] + offset[1];
-  float second0 = target[X_AXIS] + offset[2];
-  float second1 = target[Y_AXIS] + offset[3];
+  const float first0 = position[X_AXIS] + offset[0],
+              first1 = position[Y_AXIS] + offset[1],
+              second0 = target[X_AXIS] + offset[2],
+              second1 = target[Y_AXIS] + offset[3];
   float t = 0.0;
 
   float bez_target[4];
@@ -134,15 +134,15 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
     bool did_reduce = false;
     float new_t = t + step;
     NOMORE(new_t, 1.0);
-    float new_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], new_t);
-    float new_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], new_t);
+    float new_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], new_t),
+          new_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], new_t);
     for (;;) {
       if (new_t - t < (MIN_STEP)) break;
-      float candidate_t = 0.5 * (t + new_t);
-      float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t);
-      float candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t);
-      float interp_pos0 = 0.5 * (bez_target[X_AXIS] + new_pos0);
-      float interp_pos1 = 0.5 * (bez_target[Y_AXIS] + new_pos1);
+      const float candidate_t = 0.5 * (t + new_t),
+                  candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t),
+                  candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t),
+                  interp_pos0 = 0.5 * (bez_target[X_AXIS] + new_pos0),
+                  interp_pos1 = 0.5 * (bez_target[Y_AXIS] + new_pos1);
       if (dist1(candidate_pos0, candidate_pos1, interp_pos0, interp_pos1) <= (SIGMA)) break;
       new_t = candidate_t;
       new_pos0 = candidate_pos0;
@@ -153,12 +153,12 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
     // If we did not reduce the step, maybe we should enlarge it.
     if (!did_reduce) for (;;) {
       if (new_t - t > MAX_STEP) break;
-      float candidate_t = t + 2.0 * (new_t - t);
+      const float candidate_t = t + 2.0 * (new_t - t);
       if (candidate_t >= 1.0) break;
-      float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t);
-      float candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t);
-      float interp_pos0 = 0.5 * (bez_target[X_AXIS] + candidate_pos0);
-      float interp_pos1 = 0.5 * (bez_target[Y_AXIS] + candidate_pos1);
+      const float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t),
+                  candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t),
+                  interp_pos0 = 0.5 * (bez_target[X_AXIS] + candidate_pos0),
+                  interp_pos1 = 0.5 * (bez_target[Y_AXIS] + candidate_pos1);
       if (dist1(new_pos0, new_pos1, interp_pos0, interp_pos1) > (SIGMA)) break;
       new_t = candidate_t;
       new_pos0 = candidate_pos0;

From 64f077fd2af3e5a4a5d68dd52582fefc65827554 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Sat, 4 Nov 2017 14:59:42 -0500
Subject: [PATCH 2/6] Remove I2CPEM workspace offset

---
 Marlin/I2CPositionEncoder.h | 17 ++---------------
 Marlin/Marlin_main.cpp      |  5 -----
 2 files changed, 2 insertions(+), 20 deletions(-)

diff --git a/Marlin/I2CPositionEncoder.h b/Marlin/I2CPositionEncoder.h
index a582a87b6..838024185 100644
--- a/Marlin/I2CPositionEncoder.h
+++ b/Marlin/I2CPositionEncoder.h
@@ -127,10 +127,7 @@
               invert              = false,
               ec                  = true;
 
-    float     axisOffset          = 0;
-
-    int32_t   axisOffsetTicks     = 0,
-              zeroOffset          = 0,
+    int32_t   zeroOffset          = 0,
               lastPosition        = 0,
               position;
 
@@ -168,7 +165,7 @@
     }
 
     FORCE_INLINE float get_position_mm() { return mm_from_count(get_position()); }
-    FORCE_INLINE int32_t get_position() { return get_raw_count() - zeroOffset - axisOffsetTicks; }
+    FORCE_INLINE int32_t get_position() { return get_raw_count() - zeroOffset; }
 
     int32_t get_axis_error_steps(const bool report);
     float get_axis_error_mm(const bool report);
@@ -219,16 +216,6 @@
 
     FORCE_INLINE int get_stepper_ticks() { return stepperTicks; }
     FORCE_INLINE void set_stepper_ticks(const int ticks) { stepperTicks = ticks; }
-
-    FORCE_INLINE float get_axis_offset() { return axisOffset; }
-    FORCE_INLINE void set_axis_offset(const float newOffset) {
-      axisOffset = newOffset;
-      axisOffsetTicks = int32_t(axisOffset * get_encoder_ticks_mm());
-    }
-
-    FORCE_INLINE void set_current_position(const float newPositionMm) {
-      set_axis_offset(get_position_mm() - newPositionMm + axisOffset);
-    }
   };
 
   class I2CPositionEncodersMgr {
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 0187ead34..b3da91f74 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -6219,11 +6219,6 @@ inline void gcode_G92() {
           #if HAS_POSITION_SHIFT
             position_shift[i] += v - p; // Offset the coordinate space
             update_software_endstops((AxisEnum)i);
-
-            #if ENABLED(I2C_POSITION_ENCODERS)
-              I2CPEM.encoders[I2CPEM.idx_from_axis((AxisEnum)i)].set_axis_offset(position_shift[i]);
-            #endif
-
           #endif
         }
       #endif

From e642a64b688b819345e5c2e91f5ab80e453160a3 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Wed, 1 Nov 2017 14:51:57 -0500
Subject: [PATCH 3/6] Prettify some EEPROM code

---
 Marlin/configuration_store.cpp | 68 ++++++++++++++++++++++++++++++----
 1 file changed, 61 insertions(+), 7 deletions(-)

diff --git a/Marlin/configuration_store.cpp b/Marlin/configuration_store.cpp
index 6324c9e1c..1ab43adb8 100644
--- a/Marlin/configuration_store.cpp
+++ b/Marlin/configuration_store.cpp
@@ -723,12 +723,16 @@ void MarlinSettings::postprocess() {
       float dummy = 0;
       bool dummyb;
 
-      working_crc = 0; //clear before reading first "real data"
+      working_crc = 0;  // Init to 0. Accumulated by EEPROM_READ
 
       // Number of esteppers may change
       uint8_t esteppers;
       EEPROM_READ(esteppers);
 
+      //
+      // Planner Motion
+      //
+
       // Get only the number of E stepper parameters previously stored
       // Any steppers added later are set to their defaults
       const float def1[] = DEFAULT_AXIS_STEPS_PER_UNIT, def2[] = DEFAULT_MAX_FEEDRATE;
@@ -752,6 +756,10 @@ void MarlinSettings::postprocess() {
       EEPROM_READ(planner.min_segment_time_us);
       EEPROM_READ(planner.max_jerk);
 
+      //
+      // Home Offset (M206)
+      //
+
       #if !HAS_HOME_OFFSET
         float home_offset[XYZ];
       #endif
@@ -763,6 +771,10 @@ void MarlinSettings::postprocess() {
         home_offset[Z_AXIS] -= DELTA_HEIGHT;
       #endif
 
+      //
+      // Hotend Offsets, if any
+      //
+
       #if HOTENDS > 1
         // Skip hotend 0 which must be 0
         for (uint8_t e = 1; e < HOTENDS; e++)
@@ -846,6 +858,10 @@ void MarlinSettings::postprocess() {
           for (uint16_t q = grid_max_x * grid_max_y; q--;) EEPROM_READ(dummy);
         }
 
+      //
+      // Unified Bed Leveling active state
+      //
+
       #if ENABLED(AUTO_BED_LEVELING_UBL)
         EEPROM_READ(planner.leveling_active);
         EEPROM_READ(ubl.storage_slot);
@@ -855,6 +871,10 @@ void MarlinSettings::postprocess() {
         EEPROM_READ(dummyui8);
       #endif // AUTO_BED_LEVELING_UBL
 
+      //
+      // DELTA Geometry or Dual Endstops offsets
+      //
+
       #if ENABLED(DELTA)
         EEPROM_READ(delta_endstop_adj);         // 3 floats
         EEPROM_READ(delta_radius);              // 1 float
@@ -891,19 +911,26 @@ void MarlinSettings::postprocess() {
 
       #endif
 
+      //
+      // LCD Preheat settings
+      //
+
       #if DISABLED(ULTIPANEL)
         int lcd_preheat_hotend_temp[2], lcd_preheat_bed_temp[2], lcd_preheat_fan_speed[2];
       #endif
-
-      EEPROM_READ(lcd_preheat_hotend_temp);
-      EEPROM_READ(lcd_preheat_bed_temp);
-      EEPROM_READ(lcd_preheat_fan_speed);
+      EEPROM_READ(lcd_preheat_hotend_temp); // 2 floats
+      EEPROM_READ(lcd_preheat_bed_temp);    // 2 floats
+      EEPROM_READ(lcd_preheat_fan_speed);   // 2 floats
 
       //EEPROM_ASSERT(
       //  WITHIN(lcd_preheat_fan_speed, 0, 255),
       //  "lcd_preheat_fan_speed out of range"
       //);
 
+      //
+      // Hotend PID
+      //
+
       #if ENABLED(PIDTEMP)
         for (uint8_t e = 0; e < MAX_EXTRUDERS; e++) {
           EEPROM_READ(dummy); // Kp
@@ -927,11 +954,19 @@ void MarlinSettings::postprocess() {
         for (uint8_t q = MAX_EXTRUDERS * 4; q--;) EEPROM_READ(dummy);  // Kp, Ki, Kd, Kc
       #endif // !PIDTEMP
 
+      //
+      // PID Extrusion Scaling
+      //
+
       #if DISABLED(PID_EXTRUSION_SCALING)
         int lpq_len;
       #endif
       EEPROM_READ(lpq_len);
 
+      //
+      // Heated Bed PID
+      //
+
       #if ENABLED(PIDTEMPBED)
         EEPROM_READ(dummy); // bedKp
         if (dummy != DUMMY_PID_VALUE) {
@@ -943,11 +978,19 @@ void MarlinSettings::postprocess() {
         for (uint8_t q=3; q--;) EEPROM_READ(dummy); // bedKp, bedKi, bedKd
       #endif
 
+      //
+      // LCD Contrast
+      //
+
       #if !HAS_LCD_CONTRAST
         uint16_t lcd_contrast;
       #endif
       EEPROM_READ(lcd_contrast);
 
+      //
+      // Firmware Retraction
+      //
+
       #if ENABLED(FWRETRACT)
         EEPROM_READ(autoretract_enabled);
         EEPROM_READ(retract_length);
@@ -963,13 +1006,20 @@ void MarlinSettings::postprocess() {
         for (uint8_t q=8; q--;) EEPROM_READ(dummy);
       #endif
 
-      EEPROM_READ(volumetric_enabled);
+      //
+      // Volumetric & Filament Size
+      //
 
+      EEPROM_READ(volumetric_enabled);
       for (uint8_t q = 0; q < MAX_EXTRUDERS; q++) {
         EEPROM_READ(dummy);
         if (q < COUNT(filament_size)) filament_size[q] = dummy;
       }
 
+      //
+      // TMC2130 Stepper Current
+      //
+
       uint16_t val;
       #if ENABLED(HAVE_TMC2130)
         EEPROM_READ(val);
@@ -1017,7 +1067,7 @@ void MarlinSettings::postprocess() {
           stepperE4.setCurrent(val, R_SENSE, HOLD_MULTIPLIER);
         #endif
       #else
-        for (uint8_t q = 0; q < 11; q++) EEPROM_READ(val);
+        for (uint8_t q = 11; --q;) EEPROM_READ(val);
       #endif
 
       //
@@ -1032,6 +1082,10 @@ void MarlinSettings::postprocess() {
         EEPROM_READ(dummy);
       #endif
 
+      //
+      // Motor Current PWM
+      //
+
       #if HAS_MOTOR_CURRENT_PWM
         for (uint8_t q = 3; q--;) EEPROM_READ(stepper.motor_current_setting[q]);
       #else

From 91abf0708735e5e4ec873b9925eb26bdeb68e2fd Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Sat, 4 Nov 2017 18:09:01 -0500
Subject: [PATCH 4/6] Cleanup and fix G33

---
 Marlin/Marlin_main.cpp | 55 ++++++++++++++++++++----------------------
 1 file changed, 26 insertions(+), 29 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index b3da91f74..f8a928c3c 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -5498,16 +5498,18 @@ void home_all_axes() { gcode_G28(true); }
                     dy = (Y_PROBE_OFFSET_FROM_EXTRUDER);
       #endif
 
-          LOOP_CAL_ALL(axis) z_at_pt[axis] = 0.0;
+      LOOP_CAL_ALL(axis) z_at_pt[axis] = 0.0;
 
       if (!_0p_calibration) {
 
         if (!_7p_no_intermediates && !_7p_4_intermediates && !_7p_11_intermediates) { // probe the center
-          #if ENABLED(PROBE_MANUALLY)
-            z_at_pt[CEN] += lcd_probe_pt(0, 0);
-          #else
-            z_at_pt[CEN] += probe_pt(dx, dy, stow_after_each, 1, false);
-          #endif
+          z_at_pt[CEN] +=
+            #if ENABLED(PROBE_MANUALLY)
+              lcd_probe_pt(0, 0)
+            #else
+              probe_pt(dx, dy, stow_after_each, 1, false)
+            #endif
+          ;
         }
 
         if (_7p_calibration) { // probe extra center points
@@ -5516,11 +5518,13 @@ void home_all_axes() { gcode_G28(true); }
           I_LOOP_CAL_PT(axis, start, steps) {
             const float a = RADIANS(210 + (360 / NPP) *  (axis - 1)),
                         r = delta_calibration_radius * 0.1;
-            #if ENABLED(PROBE_MANUALLY)
-              z_at_pt[CEN] += lcd_probe_pt(cos(a) * r, sin(a) * r);
-            #else
-              z_at_pt[CEN] += probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1);
-            #endif
+            z_at_pt[CEN] +=
+              #if ENABLED(PROBE_MANUALLY)
+                lcd_probe_pt(cos(a) * r, sin(a) * r)
+              #else
+                probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1)
+              #endif
+            ;
           }
           z_at_pt[CEN] /= float(_7p_2_intermediates ? 7 : probe_points);
         }
@@ -5543,29 +5547,22 @@ void home_all_axes() { gcode_G28(true); }
               const float a = RADIANS(210 + (360 / NPP) *  (axis - 1)),
                           r = delta_calibration_radius * (1 + 0.1 * (zig_zag ? circle : - circle)),
                           interpol = fmod(axis, 1);
-              #if ENABLED(PROBE_MANUALLY)
-                 float z_temp = lcd_probe_pt(cos(a) * r, sin(a) * r);
-              #else
-                float z_temp = probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1);
-              #endif
+              const float z_temp =
+                #if ENABLED(PROBE_MANUALLY)
+                  lcd_probe_pt(cos(a) * r, sin(a) * r)
+                #else
+                  probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1)
+                #endif
+              ;
               // split probe point to neighbouring calibration points
-              z_at_pt[round(axis - interpol + NPP - 1) % NPP + 1] += z_temp * sq(cos(RADIANS(interpol * 90)));
-              z_at_pt[round(axis - interpol) % NPP + 1] += z_temp * sq(sin(RADIANS(interpol * 90)));
+              z_at_pt[uint8_t(round(axis - interpol + NPP - 1)) % NPP + 1] += z_temp * sq(cos(RADIANS(interpol * 90)));
+              z_at_pt[uint8_t(round(axis - interpol          )) % NPP + 1] += z_temp * sq(sin(RADIANS(interpol * 90)));
             }
             zig_zag = !zig_zag;
           }
           if (_7p_intermed_points)
-            LOOP_CAL_RAD(axis) {
-/*
-            // average intermediate points to towers and opposites - only required with _7P_STEP >= 2
-              for (int8_t i = 1; i < _7P_STEP; i++) {
-                const float interpol = i * (1.0 / _7P_STEP);
-                z_at_pt[axis] += (z_at_pt[(axis + NPP - i - 1) % NPP + 1]
-                                 + z_at_pt[axis + i]) * sq(cos(RADIANS(interpol * 90)));
-              }
-*/
+            LOOP_CAL_RAD(axis)
               z_at_pt[axis] /= _7P_STEP  / steps;
-            }
         }
 
 
@@ -10708,7 +10705,7 @@ inline void invalid_extruder_error(const uint8_t e) {
     #endif
   }
 
-  FORCE_INLINE void fanmux_init(void){
+  FORCE_INLINE void fanmux_init(void) {
     SET_OUTPUT(FANMUX0_PIN);
     #if PIN_EXISTS(FANMUX1)
       SET_OUTPUT(FANMUX1_PIN);

From bb111b928ba346c29c152ede0debe790259fa068 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Wed, 1 Nov 2017 12:39:10 -0500
Subject: [PATCH 5/6] Add CNC_COORDINATE_SYSTEMS to configs

---
 Marlin/Configuration_adv.h                                | 8 ++++++++
 .../AlephObjects/TAZ4/Configuration_adv.h                 | 8 ++++++++
 Marlin/example_configurations/Anet/A6/Configuration_adv.h | 8 ++++++++
 Marlin/example_configurations/Anet/A8/Configuration_adv.h | 8 ++++++++
 .../BQ/Hephestos/Configuration_adv.h                      | 8 ++++++++
 .../BQ/Hephestos_2/Configuration_adv.h                    | 8 ++++++++
 .../example_configurations/BQ/WITBOX/Configuration_adv.h  | 8 ++++++++
 .../example_configurations/Cartesio/Configuration_adv.h   | 8 ++++++++
 .../Creality/CR-10/Configuration_adv.h                    | 8 ++++++++
 Marlin/example_configurations/Felix/Configuration_adv.h   | 8 ++++++++
 .../FolgerTech/i3-2020/Configuration_adv.h                | 8 ++++++++
 .../Infitary/i3-M508/Configuration_adv.h                  | 8 ++++++++
 .../Malyan/M150/Configuration_adv.h                       | 8 ++++++++
 .../Micromake/C1/enhanced/Configuration_adv.h             | 8 ++++++++
 .../example_configurations/RigidBot/Configuration_adv.h   | 8 ++++++++
 Marlin/example_configurations/SCARA/Configuration_adv.h   | 8 ++++++++
 .../Sanguinololu/Configuration_adv.h                      | 8 ++++++++
 .../example_configurations/TinyBoy2/Configuration_adv.h   | 8 ++++++++
 .../Velleman/K8200/Configuration_adv.h                    | 8 ++++++++
 .../Velleman/K8400/Configuration_adv.h                    | 8 ++++++++
 .../delta/FLSUN/auto_calibrate/Configuration_adv.h        | 8 ++++++++
 .../delta/FLSUN/kossel_mini/Configuration_adv.h           | 8 ++++++++
 .../delta/generic/Configuration_adv.h                     | 8 ++++++++
 .../delta/kossel_mini/Configuration_adv.h                 | 8 ++++++++
 .../delta/kossel_pro/Configuration_adv.h                  | 8 ++++++++
 .../delta/kossel_xl/Configuration_adv.h                   | 8 ++++++++
 .../gCreate/gMax1.5+/Configuration_adv.h                  | 8 ++++++++
 Marlin/example_configurations/makibox/Configuration_adv.h | 8 ++++++++
 .../tvrrug/Round2/Configuration_adv.h                     | 8 ++++++++
 Marlin/example_configurations/wt150/Configuration_adv.h   | 8 ++++++++
 30 files changed, 240 insertions(+)

diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index e8ac2161c..fe07b2bf2 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -1267,6 +1267,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/AlephObjects/TAZ4/Configuration_adv.h b/Marlin/example_configurations/AlephObjects/TAZ4/Configuration_adv.h
index 280097349..36171003d 100644
--- a/Marlin/example_configurations/AlephObjects/TAZ4/Configuration_adv.h
+++ b/Marlin/example_configurations/AlephObjects/TAZ4/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/Anet/A6/Configuration_adv.h b/Marlin/example_configurations/Anet/A6/Configuration_adv.h
index bd3b9c293..a4c35a66f 100644
--- a/Marlin/example_configurations/Anet/A6/Configuration_adv.h
+++ b/Marlin/example_configurations/Anet/A6/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/Anet/A8/Configuration_adv.h b/Marlin/example_configurations/Anet/A8/Configuration_adv.h
index bb76e1fed..2de577699 100644
--- a/Marlin/example_configurations/Anet/A8/Configuration_adv.h
+++ b/Marlin/example_configurations/Anet/A8/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/BQ/Hephestos/Configuration_adv.h b/Marlin/example_configurations/BQ/Hephestos/Configuration_adv.h
index 306a47fa3..d82b60fc8 100644
--- a/Marlin/example_configurations/BQ/Hephestos/Configuration_adv.h
+++ b/Marlin/example_configurations/BQ/Hephestos/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/BQ/Hephestos_2/Configuration_adv.h b/Marlin/example_configurations/BQ/Hephestos_2/Configuration_adv.h
index 000e82765..2d86e657c 100644
--- a/Marlin/example_configurations/BQ/Hephestos_2/Configuration_adv.h
+++ b/Marlin/example_configurations/BQ/Hephestos_2/Configuration_adv.h
@@ -1307,6 +1307,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/BQ/WITBOX/Configuration_adv.h b/Marlin/example_configurations/BQ/WITBOX/Configuration_adv.h
index 306a47fa3..d82b60fc8 100644
--- a/Marlin/example_configurations/BQ/WITBOX/Configuration_adv.h
+++ b/Marlin/example_configurations/BQ/WITBOX/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/Cartesio/Configuration_adv.h b/Marlin/example_configurations/Cartesio/Configuration_adv.h
index bb87a192c..dd8298d02 100644
--- a/Marlin/example_configurations/Cartesio/Configuration_adv.h
+++ b/Marlin/example_configurations/Cartesio/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/Creality/CR-10/Configuration_adv.h b/Marlin/example_configurations/Creality/CR-10/Configuration_adv.h
index 541d222c1..095b0de87 100755
--- a/Marlin/example_configurations/Creality/CR-10/Configuration_adv.h
+++ b/Marlin/example_configurations/Creality/CR-10/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/Felix/Configuration_adv.h b/Marlin/example_configurations/Felix/Configuration_adv.h
index 71a7654be..0a20b546f 100644
--- a/Marlin/example_configurations/Felix/Configuration_adv.h
+++ b/Marlin/example_configurations/Felix/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/FolgerTech/i3-2020/Configuration_adv.h b/Marlin/example_configurations/FolgerTech/i3-2020/Configuration_adv.h
index 79549621e..e36ad371b 100644
--- a/Marlin/example_configurations/FolgerTech/i3-2020/Configuration_adv.h
+++ b/Marlin/example_configurations/FolgerTech/i3-2020/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/Infitary/i3-M508/Configuration_adv.h b/Marlin/example_configurations/Infitary/i3-M508/Configuration_adv.h
index 3f37385fc..c60b0c050 100644
--- a/Marlin/example_configurations/Infitary/i3-M508/Configuration_adv.h
+++ b/Marlin/example_configurations/Infitary/i3-M508/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/Malyan/M150/Configuration_adv.h b/Marlin/example_configurations/Malyan/M150/Configuration_adv.h
index b5717f5ff..72ec19dcf 100644
--- a/Marlin/example_configurations/Malyan/M150/Configuration_adv.h
+++ b/Marlin/example_configurations/Malyan/M150/Configuration_adv.h
@@ -1307,6 +1307,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/Micromake/C1/enhanced/Configuration_adv.h b/Marlin/example_configurations/Micromake/C1/enhanced/Configuration_adv.h
index 8f653b4da..00d61655d 100644
--- a/Marlin/example_configurations/Micromake/C1/enhanced/Configuration_adv.h
+++ b/Marlin/example_configurations/Micromake/C1/enhanced/Configuration_adv.h
@@ -1314,6 +1314,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/RigidBot/Configuration_adv.h b/Marlin/example_configurations/RigidBot/Configuration_adv.h
index e440a2843..3ddf3f5d8 100644
--- a/Marlin/example_configurations/RigidBot/Configuration_adv.h
+++ b/Marlin/example_configurations/RigidBot/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/SCARA/Configuration_adv.h b/Marlin/example_configurations/SCARA/Configuration_adv.h
index 1c4557dd6..347afe6a7 100644
--- a/Marlin/example_configurations/SCARA/Configuration_adv.h
+++ b/Marlin/example_configurations/SCARA/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/Sanguinololu/Configuration_adv.h b/Marlin/example_configurations/Sanguinololu/Configuration_adv.h
index 54d58047f..454fff266 100644
--- a/Marlin/example_configurations/Sanguinololu/Configuration_adv.h
+++ b/Marlin/example_configurations/Sanguinololu/Configuration_adv.h
@@ -1277,6 +1277,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/TinyBoy2/Configuration_adv.h b/Marlin/example_configurations/TinyBoy2/Configuration_adv.h
index fadb8a18b..5000605e7 100644
--- a/Marlin/example_configurations/TinyBoy2/Configuration_adv.h
+++ b/Marlin/example_configurations/TinyBoy2/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/Velleman/K8200/Configuration_adv.h b/Marlin/example_configurations/Velleman/K8200/Configuration_adv.h
index 66af68163..81ac9508a 100644
--- a/Marlin/example_configurations/Velleman/K8200/Configuration_adv.h
+++ b/Marlin/example_configurations/Velleman/K8200/Configuration_adv.h
@@ -1318,6 +1318,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/Velleman/K8400/Configuration_adv.h b/Marlin/example_configurations/Velleman/K8400/Configuration_adv.h
index 1a4927462..dc83cb32b 100644
--- a/Marlin/example_configurations/Velleman/K8400/Configuration_adv.h
+++ b/Marlin/example_configurations/Velleman/K8400/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/delta/FLSUN/auto_calibrate/Configuration_adv.h b/Marlin/example_configurations/delta/FLSUN/auto_calibrate/Configuration_adv.h
index 1ba0cda2c..a91a581e7 100644
--- a/Marlin/example_configurations/delta/FLSUN/auto_calibrate/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/FLSUN/auto_calibrate/Configuration_adv.h
@@ -1312,6 +1312,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/delta/FLSUN/kossel_mini/Configuration_adv.h b/Marlin/example_configurations/delta/FLSUN/kossel_mini/Configuration_adv.h
index 12d81207b..82103d0cd 100644
--- a/Marlin/example_configurations/delta/FLSUN/kossel_mini/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/FLSUN/kossel_mini/Configuration_adv.h
@@ -1312,6 +1312,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/delta/generic/Configuration_adv.h b/Marlin/example_configurations/delta/generic/Configuration_adv.h
index 12d81207b..82103d0cd 100644
--- a/Marlin/example_configurations/delta/generic/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/generic/Configuration_adv.h
@@ -1312,6 +1312,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h b/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h
index 12d81207b..82103d0cd 100644
--- a/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h
@@ -1312,6 +1312,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h b/Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h
index 665353df1..9aea471d4 100644
--- a/Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h
@@ -1317,6 +1317,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h b/Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h
index 48e7dd3c2..290dc4a87 100644
--- a/Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h
@@ -1312,6 +1312,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/gCreate/gMax1.5+/Configuration_adv.h b/Marlin/example_configurations/gCreate/gMax1.5+/Configuration_adv.h
index 676ecad1e..498f6ea4c 100644
--- a/Marlin/example_configurations/gCreate/gMax1.5+/Configuration_adv.h
+++ b/Marlin/example_configurations/gCreate/gMax1.5+/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/makibox/Configuration_adv.h b/Marlin/example_configurations/makibox/Configuration_adv.h
index 1a2ff80c6..1d1dbb7eb 100644
--- a/Marlin/example_configurations/makibox/Configuration_adv.h
+++ b/Marlin/example_configurations/makibox/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h b/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h
index a0dcc0705..cddb95044 100644
--- a/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h
+++ b/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h
@@ -1310,6 +1310,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */
diff --git a/Marlin/example_configurations/wt150/Configuration_adv.h b/Marlin/example_configurations/wt150/Configuration_adv.h
index 5c36387d6..3b3da8b6c 100644
--- a/Marlin/example_configurations/wt150/Configuration_adv.h
+++ b/Marlin/example_configurations/wt150/Configuration_adv.h
@@ -1297,6 +1297,14 @@
   //#define SPEED_POWER_MAX      100      // 0-100%
 #endif
 
+/**
+ * CNC Coordinate Systems
+ *
+ * Enables G53 and G54-G59.3 commands to select coordinate systems
+ * and G92.1 to reset the workspace to native machine space.
+ */
+//#define CNC_COORDINATE_SYSTEMS
+
 /**
  * M43 - display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
  */

From 8ab368559a665d97e7b90aac5093be69cb16179d Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Wed, 1 Nov 2017 13:08:46 -0500
Subject: [PATCH 6/6] Implement CNC_COORDINATE_SYSTEMS

---
 .travis.yml                    |   2 +-
 Marlin/Conditionals_post.h     |   2 +-
 Marlin/Marlin.h                |   8 ++
 Marlin/Marlin_main.cpp         | 154 +++++++++++++++++++++++++++------
 Marlin/SanityCheck.h           |   4 +
 Marlin/configuration_store.cpp |  40 ++++++++-
 Marlin/gcode.cpp               |  20 +++++
 Marlin/gcode.h                 |   5 ++
 8 files changed, 204 insertions(+), 31 deletions(-)

diff --git a/.travis.yml b/.travis.yml
index 4ed3a6484..ad6732017 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -86,7 +86,7 @@ script:
   - opt_set TEMP_SENSOR_0 -2
   - opt_set TEMP_SENSOR_1 1
   - opt_set TEMP_SENSOR_BED 1
-  - opt_enable PIDTEMPBED FIX_MOUNTED_PROBE Z_SAFE_HOMING ARC_P_CIRCLES CNC_WORKSPACE_PLANES
+  - opt_enable PIDTEMPBED FIX_MOUNTED_PROBE Z_SAFE_HOMING ARC_P_CIRCLES CNC_WORKSPACE_PLANES CNC_COORDINATE_SYSTEMS
   - opt_enable REPRAP_DISCOUNT_SMART_CONTROLLER SDSUPPORT EEPROM_SETTINGS
   - opt_enable BLINKM PCA9632 RGB_LED NEOPIXEL_LED
   - opt_enable AUTO_BED_LEVELING_LINEAR Z_MIN_PROBE_REPEATABILITY_TEST DEBUG_LEVELING_FEATURE
diff --git a/Marlin/Conditionals_post.h b/Marlin/Conditionals_post.h
index 29686a724..cbceb619c 100644
--- a/Marlin/Conditionals_post.h
+++ b/Marlin/Conditionals_post.h
@@ -1036,7 +1036,7 @@
   #define GRID_MAX_POINTS ((GRID_MAX_POINTS_X) * (GRID_MAX_POINTS_Y))
 
   // Add commands that need sub-codes to this list
-  #define USE_GCODE_SUBCODES ENABLED(G38_PROBE_TARGET)
+  #define USE_GCODE_SUBCODES ENABLED(G38_PROBE_TARGET) || ENABLED(CNC_COORDINATE_SYSTEMS)
 
   // MESH_BED_LEVELING overrides PROBE_MANUALLY
   #if ENABLED(MESH_BED_LEVELING)
diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index 39a62fc52..0487f189d 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -290,6 +290,14 @@ extern float soft_endstop_min[XYZ], soft_endstop_max[XYZ];
   void update_software_endstops(const AxisEnum axis);
 #endif
 
+#if ENABLED(CNC_COORDINATE_SYSTEMS)
+  #define MAX_COORDINATE_SYSTEMS 9
+  extern float coordinate_system[MAX_COORDINATE_SYSTEMS][XYZ];
+  bool select_coordinate_system(const int8_t _new);
+#endif
+
+void report_current_position();
+
 #if IS_KINEMATIC
   extern float delta[ABC];
   void inverse_kinematics(const float raw[XYZ]);
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index f8a928c3c..aed37731a 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -364,6 +364,11 @@
   #define LED_WHITE 0, 0, 0, 255
 #endif
 
+#if ENABLED(CNC_COORDINATE_SYSTEMS)
+  int8_t active_coordinate_system = -1; // machine space
+  float coordinate_system[MAX_COORDINATE_SYSTEMS][XYZ];
+#endif
+
 bool Running = true;
 
 uint8_t marlin_debug_flags = DEBUG_NONE;
@@ -745,6 +750,7 @@ void stop();
 
 void get_available_commands();
 void process_next_command();
+void process_parsed_command();
 void prepare_move_to_destination();
 
 void get_cartesian_from_steppers();
@@ -3672,6 +3678,73 @@ inline void gcode_G4() {
 
 #endif // CNC_WORKSPACE_PLANES
 
+#if ENABLED(CNC_COORDINATE_SYSTEMS)
+
+  /**
+   * Select a coordinate system and update the current position.
+   * System index -1 is used to specify machine-native.
+   */
+  bool select_coordinate_system(const int8_t _new) {
+    if (active_coordinate_system == _new) return false;
+    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]);
+    if (WITHIN(_new, 0, MAX_COORDINATE_SYSTEMS - 1))
+      COPY(new_offset, coordinate_system[_new]);
+    active_coordinate_system = _new;
+    bool didXYZ = false;
+    LOOP_XYZ(i) {
+      const float diff = new_offset[i] - old_offset[i];
+      if (diff) {
+        position_shift[i] += diff;
+        update_software_endstops((AxisEnum)i);
+        didXYZ = true;
+      }
+    }
+    if (didXYZ) SYNC_PLAN_POSITION_KINEMATIC();
+    return true;
+  }
+
+  /**
+   * In CNC G-code G53 is like a modifier
+   * It precedes a movement command (or other modifiers) on the same line.
+   * This is the first command to use parser.chain() to make this possible.
+   */
+  inline void gcode_G53() {
+    // If this command has more following...
+    if (parser.chain()) {
+      const int8_t _system = active_coordinate_system;
+      active_coordinate_system = -1;
+      process_parsed_command();
+      active_coordinate_system = _system;
+    }
+  }
+
+  /**
+   * G54-G59.3: Select a new workspace
+   *
+   * A workspace is an XYZ offset to the machine native space.
+   * All workspaces default to 0,0,0 at start, or with EEPROM
+   * support they may be restored from a previous session.
+   *
+   * G92 is used to set the current workspace's offset.
+   */
+  inline void gcode_G54_59(uint8_t subcode=0) {
+    const int8_t _space = parser.codenum - 54 + subcode;
+    if (select_coordinate_system(_space)) {
+      SERIAL_PROTOCOLLNPAIR("Select workspace ", _space);
+      report_current_position();
+    }
+  }
+  FORCE_INLINE void gcode_G54() { gcode_G54_59(); }
+  FORCE_INLINE void gcode_G55() { gcode_G54_59(); }
+  FORCE_INLINE void gcode_G56() { gcode_G54_59(); }
+  FORCE_INLINE void gcode_G57() { gcode_G54_59(); }
+  FORCE_INLINE void gcode_G58() { gcode_G54_59(); }
+  FORCE_INLINE void gcode_G59() { gcode_G54_59(parser.subcode); }
+
+#endif
+
 #if ENABLED(INCH_MODE_SUPPORT)
   /**
    * G20: Set input mode to inches
@@ -4153,13 +4226,12 @@ inline void gcode_G28(const bool always_home_all) {
 
   // Restore the active tool after homing
   #if HOTENDS > 1
-    tool_change(old_tool_index, 0,
-      #if ENABLED(PARKING_EXTRUDER)
-        false // fetch the previous toolhead
-      #else
-        true
-      #endif
-    );
+    #if ENABLED(PARKING_EXTRUDER)
+      #define NO_FETCH false // fetch the previous toolhead
+    #else
+      #define NO_FETCH true
+    #endif
+    tool_change(old_tool_index, 0, NO_FETCH);
   #endif
 
   lcd_refresh();
@@ -6201,7 +6273,30 @@ inline void gcode_G92() {
 
   if (!didE) stepper.synchronize();
 
-  LOOP_XYZE(i) {
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    switch (parser.subcode) {
+      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_software_endstops((AxisEnum)i);
+            }
+          }
+        #endif // Not SCARA
+        return;
+    }
+  #endif
+
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    #define IS_G92_0 (parser.subcode == 0)
+  #else
+    #define IS_G92_0 true
+  #endif
+
+  if (IS_G92_0) LOOP_XYZE(i) {
     if (parser.seenval(axis_codes[i])) {
       #if IS_SCARA
         if (i != E_AXIS) didXYZ = true;
@@ -6221,6 +6316,13 @@ inline void gcode_G92() {
       #endif
     }
   }
+
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    // Apply workspace offset to the active coordinate system
+    if (WITHIN(active_coordinate_system, 0, MAX_COORDINATE_SYSTEMS - 1))
+      COPY(coordinate_system[active_coordinate_system], position_shift);
+  #endif
+
   if (didXYZ)
     SYNC_PLAN_POSITION_KINEMATIC();
   else if (didE)
@@ -11194,26 +11296,11 @@ inline void gcode_T(const uint8_t tmp_extruder) {
 }
 
 /**
- * Process a single command and dispatch it to its handler
- * This is called from the main loop()
+ * Process the parsed command and dispatch it to its handler
  */
-void process_next_command() {
-  char * const current_command = command_queue[cmd_queue_index_r];
-
-  if (DEBUGGING(ECHO)) {
-    SERIAL_ECHO_START();
-    SERIAL_ECHOLN(current_command);
-    #if ENABLED(M100_FREE_MEMORY_WATCHER)
-      SERIAL_ECHOPAIR("slot:", cmd_queue_index_r);
-      M100_dump_routine("   Command Queue:", (const char*)command_queue, (const char*)(command_queue + sizeof(command_queue)));
-    #endif
-  }
-
+void process_parsed_command() {
   KEEPALIVE_STATE(IN_HANDLER);
 
-  // Parse the next command in the queue
-  parser.parse(current_command);
-
   // Handle a known G, M, or T
   switch (parser.command_letter) {
     case 'G': switch (parser.codenum) {
@@ -12053,6 +12140,23 @@ void process_next_command() {
   ok_to_send();
 }
 
+void process_next_command() {
+  char * const current_command = command_queue[cmd_queue_index_r];
+
+  if (DEBUGGING(ECHO)) {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOLN(current_command);
+    #if ENABLED(M100_FREE_MEMORY_WATCHER)
+      SERIAL_ECHOPAIR("slot:", cmd_queue_index_r);
+      M100_dump_routine("   Command Queue:", (const char*)command_queue, (const char*)(command_queue + sizeof(command_queue)));
+    #endif
+  }
+
+  // Parse the next command in the queue
+  parser.parse(current_command);
+  process_parsed_command();
+}
+
 /**
  * Send a "Resend: nnn" message to the host to
  * indicate that a command needs to be re-sent.
diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index 61b148206..74cb848c0 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -1483,3 +1483,7 @@ static_assert(COUNT(sanity_arr_3) <= XYZE_N, "DEFAULT_MAX_ACCELERATION has too m
     #endif
   #endif
 #endif // SPINDLE_LASER_ENABLE
+
+#if ENABLED(CNC_COORDINATE_SYSTEMS) && ENABLED(NO_WORKSPACE_OFFSETS)
+  #error "CNC_COORDINATE_SYSTEMS is incompatible with NO_WORKSPACE_OFFSETS."
+#endif
diff --git a/Marlin/configuration_store.cpp b/Marlin/configuration_store.cpp
index 1ab43adb8..fa8df06b5 100644
--- a/Marlin/configuration_store.cpp
+++ b/Marlin/configuration_store.cpp
@@ -36,13 +36,13 @@
  *
  */
 
-#define EEPROM_VERSION "V43"
+#define EEPROM_VERSION "V44"
 
 // Change EEPROM version if these are changed:
 #define EEPROM_OFFSET 100
 
 /**
- * V43 EEPROM Layout:
+ * V44 EEPROM Layout:
  *
  *  100  Version                                    (char x4)
  *  104  EEPROM CRC16                               (uint16_t)
@@ -162,8 +162,11 @@
  *  588  M907 Z    Stepper Z current                (uint32_t)
  *  592  M907 E    Stepper E current                (uint32_t)
  *
- *  596                                Minimum end-point
- * 1917 (596 + 36 + 9 + 288 + 988)     Maximum end-point
+ * CNC_COORDINATE_SYSTEMS                           108 bytes
+ *  596  G54-G59.3 coordinate_system                (float x 27)
+ *
+ *  704                                Minimum end-point
+ * 2025 (704 + 36 + 9 + 288 + 988)     Maximum end-point
  *
  * ========================================================================
  * meshes_begin (between max and min end-point, directly above)
@@ -209,6 +212,10 @@ MarlinSettings settings;
   float new_z_fade_height;
 #endif
 
+#if ENABLED(CNC_COORDINATE_SYSTEMS)
+  bool position_changed;
+#endif
+
 /**
 * Post-process after Retrieve or Reset
 */
@@ -253,6 +260,13 @@ void MarlinSettings::postprocess() {
   #if HAS_MOTOR_CURRENT_PWM
     stepper.refresh_motor_power();
   #endif
+
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    if (position_changed) {
+      report_current_position();
+      position_changed = false;
+    }
+  #endif
 }
 
 #if ENABLED(EEPROM_SETTINGS)
@@ -663,6 +677,13 @@ void MarlinSettings::postprocess() {
       for (uint8_t q = 3; q--;) EEPROM_WRITE(dummyui32);
     #endif
 
+    #if ENABLED(CNC_COORDINATE_SYSTEMS)
+      EEPROM_WRITE(coordinate_system); // 27 floats
+    #else
+      dummy = 0.0f;
+      for (uint8_t q = 27; q--;) EEPROM_WRITE(dummy);
+    #endif
+
     if (!eeprom_error) {
       const int eeprom_size = eeprom_index;
 
@@ -1093,6 +1114,17 @@ void MarlinSettings::postprocess() {
         for (uint8_t q = 3; q--;) EEPROM_READ(dummyui32);
       #endif
 
+      //
+      // CNC Coordinate System
+      //
+
+      #if ENABLED(CNC_COORDINATE_SYSTEMS)
+        position_changed = select_coordinate_system(-1); // Go back to machine space
+        EEPROM_READ(coordinate_system);                  // 27 floats
+      #else
+        for (uint8_t q = 27; q--;) EEPROM_READ(dummy);
+      #endif
+
       if (working_crc == stored_crc) {
         postprocess();
         #if ENABLED(EEPROM_CHITCHAT)
diff --git a/Marlin/gcode.cpp b/Marlin/gcode.cpp
index edeb00e22..f7516eb69 100644
--- a/Marlin/gcode.cpp
+++ b/Marlin/gcode.cpp
@@ -228,6 +228,26 @@ void GCodeParser::parse(char *p) {
   }
 }
 
+#if ENABLED(CNC_COORDINATE_SYSTEMS)
+
+  // Parse the next parameter as a new command
+  bool GCodeParser::chain() {
+    #if ENABLED(FASTER_GCODE_PARSER)
+      char *next_command = command_ptr;
+      if (next_command) {
+        while (*next_command && *next_command != ' ') ++next_command;
+        while (*next_command == ' ') ++next_command;
+        if (!*next_command) next_command = NULL;
+      }
+    #else
+      const char *next_command = command_args;
+    #endif
+    if (next_command) parse(next_command);
+    return !!next_command;
+  }
+
+#endif // CNC_COORDINATE_SYSTEMS
+
 void GCodeParser::unknown_command_error() {
   SERIAL_ECHO_START();
   SERIAL_ECHOPAIR(MSG_UNKNOWN_COMMAND, command_ptr);
diff --git a/Marlin/gcode.h b/Marlin/gcode.h
index 8c255ef7b..b415dcb11 100644
--- a/Marlin/gcode.h
+++ b/Marlin/gcode.h
@@ -169,6 +169,11 @@ public:
   // This uses 54 bytes of SRAM to speed up seen/value
   static void parse(char * p);
 
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    // Parse the next parameter as a new command
+    static bool chain();
+  #endif
+
   // The code value pointer was set
   FORCE_INLINE static bool has_value() { return value_ptr != NULL; }