MarlinFirmware · DerAndere1 · Jan 4, 2025 · Jan 4, 2025 · Jan 25, 2025
diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
@@ -378,6 +378,9 @@
 //#define HOTEND_OFFSET_Y { 0.0, 5.00 }  // (mm) relative Y-offset for each nozzle
 //#define HOTEND_OFFSET_Z { 0.0, 0.00 }  // (mm) relative Z-offset for each nozzle
 
+// Enable and disable tool length compensation with G43 and G49, respectively. true: Enabled by default. false: Disabled by default.
+//#define DEFAULT_TOOL_LENGTH_COMPENSATION true
+
 // @section multi-material
 
 /**

diff --git a/Marlin/src/gcode/feature/fwretract/G10_G11.cpp b/Marlin/src/gcode/feature/fwretract/G10_G11.cpp
@@ -22,21 +22,149 @@
 
 #include "../../../inc/MarlinConfig.h"
 
-#if ENABLED(FWRETRACT)
+#if ANY(FWRETRACT, CNC_COORDINATE_SYSTEMS, HAS_TOOL_LENGTH_COMPENSATION)
+  #include "../../gcode.h"
+  #include "../../../module/motion.h"
+
+  #if ENABLED(FWRETRACT)
+    #include "../../../feature/fwretract.h"
+  #endif
 
-#include "../../../feature/fwretract.h"
-#include "../../gcode.h"
-#include "../../../module/motion.h"
 
 /**
- * G10 - Retract filament according to settings of M207
- *       TODO: Handle 'G10 P' for tool settings and 'G10 L' for workspace settings
+ * G10
+ *     S#                    - Retract filament according to settings of M207
+ *     L1 P# X# Y# Z#...     - Sets the tool length of the tool specified by the P parameter (0 to (TOOLS - 1))
+ *     L10 P# Z#             - Set the tool length of the tool specified by the P parameter (0 to (TOOLS - 1)) so that if the tool offset is reloaded, with the machine in its 
+ *                             current position and with the current G5x and G52/G92 offsets active, the current coordinates for the given axes 
+ *                             will become the given Z value.
+ *     L2/L20 P# X# Y# Z#... - Sets the origin of the coordinate system, specified by the P parameter (1 to 9)
+ *     L2/L20 P0 X# Y# Z#... - Sets the origin of the current coordinate system
+ *
+ *
+ * Beta L2/L20 implementation based on https://github.com/MarlinFirmware/Marlin/issues/14734
+ * and http://linuxcnc.org/docs/2.6/html/gcode/gcode.html#sec:G10-L1_
+ *
+ * !Beta - Use with caution.
  */
-void GcodeSuite::G10() { fwretract.retract(true E_OPTARG(parser.boolval('S'))); }
+
+void GcodeSuite::G10() {
+  #if ENABLED(FWRETRACT)
+    fwretract.retract(true E_OPTARG(parser.boolval('S')));
+  #endif
+
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    uint8_t index,
+            offset_type;
+
+    if (parser.seenval('P'))
+      index = parser.value_byte();
+    else
+      return;
+    if (parser.seenval('L'))
+      offset_type = parser.value_byte();
+    else
+      return;
+
+    #if HAS_HOTEND_OFFSET && HAS_TOOL_LENGTH_COMPENSATION
+      if ((offset_type == 1 || offset_type == 10) && WITHIN(index, 1, TOOLS)) {
+        switch (offset_type) {
+
+          default: break;                                              // Ignore unknown G10 Lx
+
+          case 1: // Sets the tool offset. Intended to work as in LinuxCNC.
+            LOOP_NUM_AXES(i) {
+              if (parser.seen(axis_codes[i])) {
+                const float axis_value = parser.value_axis_units((AxisEnum)i);
+                if (i == Z_AXIS) {
+                  hotend_offset[index][i] = -axis_value;
+                }
+                else {
+                  hotend_offset[index][i] = axis_value;
+                }
+              }
+            }
+            break;
+
+          // G10 L10 changes the tool table entry for tool P so that if the tool offset is reloaded, with the machine in its 
+          // current position and with the current G5x and G52/G92 offsets active, the current coordinates for the given axes 
+          // will become the given values. Intended to work as in LinuxCNC.
+          // e.g. G10 L10 P1 Z1.5 followed by G43 sets the current position for Z to be 1.5.
+          case 10:
+            LOOP_NUM_AXES(i) {
+              if (parser.seen(axis_codes[i])) {
+                const float axis_value = parser.value_axis_units((AxisEnum)i);
+                hotend_offset[index][i] = current_position[i] - axis_value;
+              }
+            }
+            break;
+        }
+      }
+    #endif
+    if (offset_type == 2 || offset_type == 20) {
+      const int8_t target_system = (index == 0) ? gcode.active_coordinate_system : (index - 1);  // P0 selects current coordinate system. P1 is G54, which is Marlin coordinate_system 0 
+      const int8_t current_system = gcode.active_coordinate_system; // Store current coord system
+      if (WITHIN(target_system, 0, MAX_COORDINATE_SYSTEMS - 1)) {
+        if (current_system != target_system) {
+          gcode.select_coordinate_system(target_system); // Select new coordinate system if needed
+          #if ENABLED(DEBUG_G10)
+            SERIAL_ECHOLNPGM("Switching to workspace ", target_system);
+            report_current_position();
+          #endif
+        }
+      }
+      switch (offset_type) {
+        default: break;                                              // Ignore unknown G10 Lx
+
+        // Sets the work offsets of the specified (P[1-9]) coordinate system, by subtracting the specified X, Y, Z... values
+        // from the current machine coordinate X, Y, Z... values (Works the same as LinuxCNC)
+        // eg: If G53's X=50, Y=100 then G10 P1 L2 X10 Y2 will set G54's X=40, Y=98
+        case 2:
+          LOOP_LOGICAL_AXES(i) {
+            if (parser.seenval(axis_codes[i])) {
+              if (TERN1(HAS_EXTRUDERS, i != E_AXIS)) {
+                const float axis_shift = parser.value_axis_units((AxisEnum)i);
+                workspace_offset[i] = -axis_shift;
+              }
+            }
+          }
+          break;
+
+        // Sets the work coordinates of the specified (P[1-9]) coordinate system, by matching the specified X, Y, Z... values
+        // Works similar to G92, except you can specifiy a coordinate system directly (Works the same as LinuxCNC)
+        // eg: G10 P2 L20 X10 Y50 will set G55's X=10, Y=50 no matter which coordinate system is currently selected
+        case 20:
+          LOOP_LOGICAL_AXES(i) {
+            if (parser.seenval(axis_codes[i])) {
+              if (TERN1(HAS_EXTRUDERS, i != E_AXIS)) {
+                const float axis_value = parser.value_axis_units((AxisEnum)i);
+                workspace_offset[i] = axis_value - current_position[i];
+              }
+            }
+          }
+          break;
+      }
+      if (WITHIN(active_coordinate_system, 0, MAX_COORDINATE_SYSTEMS - 1)) {
+        coordinate_system[active_coordinate_system] = workspace_offset;
+      }
+      #if ENABLED(DEBUG_G10)
+        SERIAL_ECHOLNPGM("New position for workspace ", target_system);
+        report_current_position();
+      #endif
+      if (current_system != target_system) {
+        gcode.select_coordinate_system(current_system);
+      }
+    }
+  #endif // ENABLED(CNC_COORDINATE_SYSTEMS)
+}
+
+#if ENABLED(FWRETRACT)
 
 /**
  * G11 - Recover filament according to settings of M208
  */
 void GcodeSuite::G11() { fwretract.retract(false); }
 
 #endif // FWRETRACT
+
+#endif   // ANY(FWRETRACT, CNC_COORDINATE_SYSTEMS, HAS_TOOL_LENGTH_COMPENSATION)
diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp
@@ -365,9 +365,11 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) {
         case 6: G6(); break;                                      // G6: Direct Stepper Move
       #endif
 
-      #if ENABLED(FWRETRACT)
+      #if ANY(FWRETRACT, CNC_COORDINATE_SYSTEMS, HAS_TOOL_LENGTH_COMPENSATION)
         case 10: G10(); break;                                    // G10: Retract / Swap Retract
-        case 11: G11(); break;                                    // G11: Recover / Swap Recover
+        #if ENABLED(FWRETRACT)
+          case 11: G11(); break;                                    // G11: Recover / Swap Recover
+        #endif
       #endif
 
       #if ENABLED(NOZZLE_CLEAN_FEATURE)
@@ -434,6 +436,11 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) {
         case 42: G42(); break;                                    // G42: Coordinated move to a mesh point
       #endif
 
+      #if HAS_TOOL_LENGTH_COMPENSATION
+        case 43: G43(); break;                                    // G43.4: Rotational Tool Center Point Control Mode
+        case 49: G49(); break;
+      #endif
+
       #if ENABLED(CNC_COORDINATE_SYSTEMS)
         case 53: G53(); break;                                    // G53: (prefix) Apply native workspace
         case 54: G54(); break;                                    // G54: Switch to Workspace 1

diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h
@@ -44,7 +44,7 @@
  * G3   - CCW ARC
  * G4   - Dwell S<seconds> or P<milliseconds>
  * G5   - Cubic B-spline with XYZE destination and IJPQ offsets
- * G10  - Retract filament according to settings of M207 (Requires FWRETRACT)
+ * G10  - Set coordinate system (Requires CNC_COORDINATE_SYSTEMS) and tool table (Requires DEFAULT_TOOL_CENTERPOINT_CONTROL). Retract filament according to settings of M207 (Requires FWRETRACT)
  * G11  - Retract recover filament according to settings of M208 (Requires FWRETRACT)
  * G12  - Clean tool (Requires NOZZLE_CLEAN_FEATURE)
  * G17  - Select Plane XY (Requires CNC_WORKSPACE_PLANES)
@@ -64,6 +64,8 @@
  * G35  - Read bed corners to help adjust bed screws: T<screw_thread> (Requires ASSISTED_TRAMMING)
  * G38  - Probe in any direction using the Z_MIN_PROBE (Requires G38_PROBE_TARGET)
  * G42  - Coordinated move to a mesh point (Requires MESH_BED_LEVELING, AUTO_BED_LEVELING_BLINEAR, or AUTO_BED_LEVELING_UBL)
+ * G43  - Tool length compensation, tool centerpoint control (Requires DEFAULT_TOOL_CENTERPOINT_CONTROL)
+ * G49  - Cancel tool length compensation (Cancel tool length compensation (Requires DEFAULT_TOOL_CENTERPOINT_CONTROL) 
  * G60  - Save current position. (Requires SAVED_POSITIONS)
  * G61  - Apply/restore saved coordinates. (Requires SAVED_POSITIONS)
  * G76  - Calibrate first layer temperature offsets. (Requires PTC_PROBE and PTC_BED)
@@ -531,8 +533,11 @@ class GcodeSuite {
     static void G6();
   #endif
 
-  #if ENABLED(FWRETRACT)
+#if ANY(FWRETRACT, CNC_COORDINATE_SYSTEMS, HAS_TOOL_LENGTH_COMPENSATION)
     static void G10();
+  #endif
+
+  #if ENABLED(FWRETRACT)
     static void G11();
   #endif
 
@@ -606,6 +611,11 @@ class GcodeSuite {
     static void G42();
   #endif
 
+  #if HAS_TOOL_LENGTH_COMPENSATION
+    static void G43();
+    static void G49();  
+  #endif
+
   #if ENABLED(CNC_COORDINATE_SYSTEMS)
     static void G53();
     static void G54();

diff --git a/Marlin/src/gcode/geometry/G43_G49.cpp b/Marlin/src/gcode/geometry/G43_G49.cpp
@@ -0,0 +1,99 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+
+/**
+ * @file G43_G49.cpp
+ * @author DerAndere
+ * @brief G-codes related to Tool Length Compensation and Rotational Tool Center Point Control
+ * 
+ * Copyright 2024 DerAndere
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_TOOL_LENGTH_COMPENSATION
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+
+/**
+ * G43: Enable Tool Length Compensation.
+ * 
+ * G43: Enable Simple Tool Length Compensation.
+ * G43 Tool Length Compensation can be canceled with G49.
+ *
+ * G43.4: Enable Rotational Tool Center Point Control Mode.
+ * G43.4 Rotational Tool Center Point Control Mode can be canceled with G49.
+ * 
+ * Only one can be active at any time.
+ */
+void GcodeSuite::G43() {
+
+  #if USE_GCODE_SUBCODES
+    const uint8_t subcode_G43 = parser.subcode;
+  #else
+    constexpr uint8_t subcode_G43 = 0;                           
+  #endif
+
+  switch (subcode_G43) {
+    default: return;                                              // Ignore unknown G43.x
+
+    case 0:                                                       // G43 - Simple Tool Length Compensation Mode.
+      #if HAS_TOOL_CENTERPOINT_CONTROL
+        tool_centerpoint_control = false;
+      #endif
+      simple_tool_length_compensation = true;
+      break;
+
+    #if HAS_TOOL_CENTERPOINT_CONTROL
+      case 4:                                                     // G43.4 - Rotational Tool Center Point Control Mode.
+        simple_tool_length_compensation = false;
+        tool_centerpoint_control = true;
+        break;
+    #endif
+  }
+
+  current_position += hotend_offset[active_extruder];
+  sync_plan_position();
+}
+
+
+/**
+ * G49: Cancel Tool Length Compensation
+ * 
+ * Cancels Simple Tool Length Compensation Mode and Rotational Tool Center Point Control Mode.
+ * 
+ * Simple Tool Length Compensation Mode can be enabled with G43
+ * Rotational Tool Center Point Control Mode can be enabled with G43.4
+ */
+void GcodeSuite::G49() {
+  simple_tool_length_compensation = false;
+  #if HAS_TOOL_CENTERPOINT_CONTROL
+    tool_centerpoint_control = false;
+  #endif
+
+  current_position -= hotend_offset[active_extruder];
+  sync_plan_position();
+}
+
+#endif
diff --git a/Marlin/src/inc/Conditionals-1-axes.h b/Marlin/src/inc/Conditionals-1-axes.h
@@ -215,6 +215,11 @@
   #undef HOTEND_OFFSET_Z
 #endif
 
+#if defined(DEFAULT_TOOL_LENGTH_COMPENSATION)
+  #define HAS_TOOL_LENGTH_COMPENSATION 1
+  #define HAS_HOTEND_OFFSET 1
+#endif
+
 /**
  * Number of Linear Axes (e.g., XYZIJKUVW)
  * All the logical axes except for the tool (E) axis

diff --git a/Marlin/src/inc/Conditionals-5-post.h b/Marlin/src/inc/Conditionals-5-post.h
@@ -3415,7 +3415,7 @@
 #endif
 
 // G92 shifts the workspace
-#if DISABLED(NO_WORKSPACE_OFFSETS)
+#if DISABLED(NO_WORKSPACE_OFFSETS) || HAS_TOOL_LENGTH_COMPENSATION
   #define HAS_WORKSPACE_OFFSET 1
 #endif
 #if DISABLED(NO_HOME_OFFSETS)

diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp
@@ -153,10 +153,18 @@ int16_t feedrate_percentage = 100;
 // Cartesian conversion result goes here:
 xyz_pos_t cartes;
 
+#if HAS_TOOL_LENGTH_COMPENSATION
+  bool simple_tool_length_compensation = DEFAULT_TOOL_LENGTH_COMPENSATION;
+#endif
+
 #if IS_KINEMATIC
 
   abce_pos_t delta;
 
+  #if HAS_TOOL_CENTERPOINT_CONTROL
+    bool tool_centerpoint_control = false;
+  #endif
+
   #if HAS_SCARA_OFFSET
     abc_pos_t scara_home_offset;
   #endif

diff --git a/Marlin/src/module/motion.h b/Marlin/src/module/motion.h
@@ -57,9 +57,16 @@ extern xyze_pos_t current_position,  // High-level current tool position
 // Scratch space for a cartesian result
 extern xyz_pos_t cartes;
 
+#if HAS_TOOL_LENGTH_COMPENSATION
+  extern bool simple_tool_length_compensation;
+#endif
+
 // Until kinematics.cpp is created, declare this here
 #if IS_KINEMATIC
   extern abce_pos_t delta;
+  #if HAS_TOOL_CENTERPOINT_CONTROL
+    extern bool tool_centerpoint_control;
+  #endif
 #endif
 
 #if HAS_ABL_NOT_UBL