From 267094af33f3c5ec70c816f44f78e85b9776868d Mon Sep 17 00:00:00 2001
From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com>
Date: Mon, 4 May 2026 09:50:32 +0800
Subject: [PATCH 1/2] interp: make #5401-#5409 the applied tool length offset

Parameters #5401-#5409 now reflect the tool length offset actually
applied to motion, matching the long-standing documentation ("Set by
G43") and the equivalent Fanuc #5081-#5088 / Haas #5081-#5086
semantics.

Previously they were rewritten on M6, G10 L1/L10/L11, and startup
from the loaded tool's stored offsets, while G43Hn (n != loaded tool),
G43.1, and G43.2 never updated them. Reading #5401-#5409 therefore
did not tell the program what offset was in effect.

Now:
  - convert_tool_length_offset writes the applied tool_offset for
    G43, G43Hn (any n), G43.1, G43.2, and G49 (zeroed).
  - set_tool_parameters / default_tool_parameters / init_tool_parameters
    no longer touch #5401-#5409. M6 changes the loaded tool but
    does not by itself apply its offset.
  - convert_setup_tool (G10 L1/L10/L11) no longer touches them either.
  - #5400 (toolno) and #5410-#5413 (diameter, angles, orientation)
    still update on M6 / G10 / startup as before.

Doc note in overview.adoc updated to match.

Fixes #2994
---
 docs/src/gcode/overview.adoc       |  3 ++-
 src/emc/rs274ngc/interp_convert.cc | 29 +++++++++++++++++--------
 src/emc/rs274ngc/rs274ngc_pre.cc   | 35 +++++++-----------------------
 3 files changed, 30 insertions(+), 37 deletions(-)

diff --git a/docs/src/gcode/overview.adoc b/docs/src/gcode/overview.adoc
index 6a6cca6ec28..261aa434502 100644
--- a/docs/src/gcode/overview.adoc
+++ b/docs/src/gcode/overview.adoc
@@ -339,7 +339,8 @@ example '##2'  means the value of the parameter whose index is the
   Persistent.
 * '5399' - Result of M66 - Check or wait for input. Volatile.
 * '5400' - Tool Number. Volatile.
-* '5401-5409' - Tool Offsets for X, Y, Z, A, B, C, U, V & W. Set by G43. Volatile.
+* '5401-5409' - Currently applied tool length offset for X, Y, Z, A, B, C,
+  U, V & W. Set by `G43`/`G43.1`/`G43.2`, cleared by `G49`. Volatile.
 * '5410' - Tool Diameter. Volatile.
 * '5411' - Tool Front Angle. Volatile.
 * '5412' - Tool Back Angle. Volatile.
diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc
index 59171b7306e..75004557bf4 100644
--- a/src/emc/rs274ngc/interp_convert.cc
+++ b/src/emc/rs274ngc/interp_convert.cc
@@ -4661,15 +4661,9 @@ int Interp::convert_setup_tool(block_pointer block, setup_pointer settings) {
         }
     }
 
-    settings->parameters[5401] = settings->tool_table[0].offset.tran.x;
-    settings->parameters[5402] = settings->tool_table[0].offset.tran.y;
-    settings->parameters[5403] = settings->tool_table[0].offset.tran.z;
-    settings->parameters[5404] = settings->tool_table[0].offset.a;
-    settings->parameters[5405] = settings->tool_table[0].offset.b;
-    settings->parameters[5406] = settings->tool_table[0].offset.c;
-    settings->parameters[5407] = settings->tool_table[0].offset.u;
-    settings->parameters[5408] = settings->tool_table[0].offset.v;
-    settings->parameters[5409] = settings->tool_table[0].offset.w;
+    // #5401-#5409 reflect the applied tool length offset (G43-family).
+    // G10 L1/L10/L11 modify the tool table but do not apply offsets to
+    // motion, so do not update them here.  See #2994.
     settings->parameters[5410] = settings->tool_table[0].diameter;
     settings->parameters[5411] = settings->tool_table[0].frontangle;
     settings->parameters[5412] = settings->tool_table[0].backangle;
@@ -6307,6 +6301,23 @@ int Interp::convert_tool_length_offset(int g_code,       //!< g_code being execu
   settings->w_current += settings->tool_offset.w - tool_offset.w;
 
   settings->tool_offset = tool_offset;
+
+  // Update parameters #5401-#5409 to reflect the actually applied tool
+  // length offset (covers G43Hn with n != loaded tool, G43.1 dynamic
+  // offsets, and G43.2 additive offsets). Without this, params lag the
+  // applied offset and only refresh on M6 / G10 L1.  See issue #2994.
+  // tool_offset here is in program units; params follow the user-unit
+  // convention used elsewhere when populating #5401-#5409.
+  settings->parameters[5401] = PROGRAM_TO_USER_LEN(tool_offset.tran.x);
+  settings->parameters[5402] = PROGRAM_TO_USER_LEN(tool_offset.tran.y);
+  settings->parameters[5403] = PROGRAM_TO_USER_LEN(tool_offset.tran.z);
+  settings->parameters[5404] = PROGRAM_TO_USER_ANG(tool_offset.a);
+  settings->parameters[5405] = PROGRAM_TO_USER_ANG(tool_offset.b);
+  settings->parameters[5406] = PROGRAM_TO_USER_ANG(tool_offset.c);
+  settings->parameters[5407] = PROGRAM_TO_USER_LEN(tool_offset.u);
+  settings->parameters[5408] = PROGRAM_TO_USER_LEN(tool_offset.v);
+  settings->parameters[5409] = PROGRAM_TO_USER_LEN(tool_offset.w);
+
   return INTERP_OK;
 }
 
diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc
index e2f798f161f..47740b2f332 100644
--- a/src/emc/rs274ngc/rs274ngc_pre.cc
+++ b/src/emc/rs274ngc/rs274ngc_pre.cc
@@ -2527,15 +2527,9 @@ int Interp::init_tool_parameters()
   if (_setup.random_toolchanger) {
      // random_toolchanger: tool at startup expected
     _setup.parameters[5400] = _setup.tool_table[0].toolno;
-    _setup.parameters[5401] = _setup.tool_table[0].offset.tran.x;
-    _setup.parameters[5402] = _setup.tool_table[0].offset.tran.y;
-    _setup.parameters[5403] = _setup.tool_table[0].offset.tran.z;
-    _setup.parameters[5404] = _setup.tool_table[0].offset.a;
-    _setup.parameters[5405] = _setup.tool_table[0].offset.b;
-    _setup.parameters[5406] = _setup.tool_table[0].offset.c;
-    _setup.parameters[5407] = _setup.tool_table[0].offset.u;
-    _setup.parameters[5408] = _setup.tool_table[0].offset.v;
-    _setup.parameters[5409] = _setup.tool_table[0].offset.w;
+    // #5401-#5409 reflect the applied tool length offset (Fanuc #5081-#5088
+    // semantic). Written only by G43/G43.1/G43.2/G49.  At startup no G43
+    // has been issued, so leave them at their default zero.  See #2994.
     _setup.parameters[5410] = _setup.tool_table[0].diameter;
     _setup.parameters[5411] = _setup.tool_table[0].frontangle;
     _setup.parameters[5412] = _setup.tool_table[0].backangle;
@@ -2552,15 +2546,8 @@ int Interp::init_tool_parameters()
 int Interp::default_tool_parameters()
 {
   _setup.parameters[5400] =  0; // toolno
-  _setup.parameters[5401] =  0; // x offset
-  _setup.parameters[5402] =  0; // y offset RESERVED
-  _setup.parameters[5403] =  0; // z offset
-  _setup.parameters[5404] =  0; // a offset RESERVED
-  _setup.parameters[5405] =  0; // b offset RESERVED
-  _setup.parameters[5406] =  0; // c offset RESERVED
-  _setup.parameters[5407] =  0; // u offset RESERVED
-  _setup.parameters[5408] =  0; // v offset RESERVED
-  _setup.parameters[5409] =  0; // w offset RESERVED
+  // #5401-#5409 reflect the applied tool length offset (G43-family).
+  // Not touched here; managed by convert_tool_length_offset.  See #2994.
   _setup.parameters[5410] =  0; // diameter
   _setup.parameters[5411] =  0; // frontangle
   _setup.parameters[5412] =  0; // backangle
@@ -2586,15 +2573,9 @@ int Interp::set_tool_parameters()
            _setup.tool_table[0].comment);
 #endif //}
   _setup.parameters[5400] = _setup.tool_table[0].toolno;
-  _setup.parameters[5401] = _setup.tool_table[0].offset.tran.x;
-  _setup.parameters[5402] = _setup.tool_table[0].offset.tran.y;
-  _setup.parameters[5403] = _setup.tool_table[0].offset.tran.z;
-  _setup.parameters[5404] = _setup.tool_table[0].offset.a;
-  _setup.parameters[5405] = _setup.tool_table[0].offset.b;
-  _setup.parameters[5406] = _setup.tool_table[0].offset.c;
-  _setup.parameters[5407] = _setup.tool_table[0].offset.u;
-  _setup.parameters[5408] = _setup.tool_table[0].offset.v;
-  _setup.parameters[5409] = _setup.tool_table[0].offset.w;
+  // #5401-#5409 reflect the applied tool length offset (G43-family) and
+  // are deliberately not updated by M6: M6 changes the loaded tool but
+  // does not by itself apply its offset to motion.  See #2994.
   _setup.parameters[5410] = _setup.tool_table[0].diameter;
   _setup.parameters[5411] = _setup.tool_table[0].frontangle;
   _setup.parameters[5412] = _setup.tool_table[0].backangle;

From 49ddfe633b1bfed7c70a0f7d8d7a1148ea02c456 Mon Sep 17 00:00:00 2001
From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com>
Date: Mon, 4 May 2026 12:31:37 +0800
Subject: [PATCH 2/2] tests: update tool offset assertions for #5401-#5409
 applied semantics

Eight tests in tests/interp/g10/, tests/t0/ and tests/toolchanger/
asserted the old behavior of #5401-#5409 (rewritten by M6 / G10 /
init from the loaded tool's stored offsets). Under the new applied
semantics they need to check the offset only after G43 (or check
that #5401-#5409 are unchanged when only M6 / G10 has run).

- tests/interp/g10/g10-l1-l10: add G43 after each G10 modification so
  the existing "should be ..." assertions still hold under the
  applied semantic. Add a final section explicitly probing that M6
  alone and G10 alone do not change the params.
- tests/interp/g10/g10-l11: add G43 after M6 and after each G10 L11
  before the (debug,#5403) probe.
- tests/t0/{nonrandom,random-with-t0,random-without-t0}: regenerate
  expected-gcode-output. The test sequences are unchanged; the values
  they introspect now reflect the applied offset.
- tests/toolchanger/m61: change the post-M6 / post-M61 #5403
  assertion to expect the previously-applied offset, then add a new
  assertion after the G43 to confirm the new offset is now applied.
- tests/toolchanger/toolno-pocket-differ/{nonrandom,random}:
  regenerate expected-gcode-output for the same reason as t0/.
---
 tests/interp/g10/g10-l1-l10/expected          |  40 ++++++-
 tests/interp/g10/g10-l1-l10/test.ngc          |  37 +++++-
 tests/interp/g10/g10-l11/expected             |  14 ++-
 tests/interp/g10/g10-l11/test.ngc             |   4 +
 tests/t0/nonrandom/expected-gcode-output      |  54 ++++-----
 tests/t0/random-with-t0/expected-gcode-output |  66 +++++------
 .../random-without-t0/expected-gcode-output   | 108 +++++++++---------
 tests/toolchanger/m61/test-ui.py              |  15 ++-
 .../nonrandom/expected-gcode-output           |  18 +--
 .../random/expected-gcode-output              |  18 +--
 10 files changed, 225 insertions(+), 149 deletions(-)

diff --git a/tests/interp/g10/g10-l1-l10/expected b/tests/interp/g10/g10-l1-l10/expected
index 0cad3409669..cbc131cb226 100644
--- a/tests/interp/g10/g10-l1-l10/expected
+++ b/tests/interp/g10/g10-l1-l10/expected
@@ -23,23 +23,29 @@
  N..... MESSAGE(" should be 1 0 0: 1.000000 0.000000 0.000000")
  N..... SET_TOOL_TABLE_ENTRY(1, 1, 1.0000 2.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, 1.0000 2.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... USE_TOOL_LENGTH_OFFSET(1.0000 2.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE(" should be 1 2 0: 1.000000 2.000000 0.000000")
  N..... SET_TOOL_TABLE_ENTRY(1, 1, 1.0000 2.0000 3.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, 1.0000 2.0000 3.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... USE_TOOL_LENGTH_OFFSET(1.0000 2.0000 3.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE(" should be 1 2 3: 1.000000 2.000000 3.000000")
  N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE(" part 2")
  N..... SET_TOOL_TABLE_ENTRY(1, 1, 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE(" should be 0 0 0: 0.000000 0.000000 0.000000")
  N..... SET_TOOL_TABLE_ENTRY(1, 1, 1.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, 1.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... USE_TOOL_LENGTH_OFFSET(1.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE(" should be 1 0 0: 1.000000 0.000000 0.000000")
  N..... SET_TOOL_TABLE_ENTRY(1, 1, 1.0000 2.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, 1.0000 2.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... USE_TOOL_LENGTH_OFFSET(1.0000 2.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE(" should be 1 2 0: 1.000000 2.000000 0.000000")
  N..... SET_TOOL_TABLE_ENTRY(1, 1, 1.0000 2.0000 3.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, 1.0000 2.0000 3.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... USE_TOOL_LENGTH_OFFSET(1.0000 2.0000 3.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE(" should be 1 2 3: 1.000000 2.000000 3.000000")
  N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE("G10 L10: set tool offsets relative to position")
@@ -58,23 +64,29 @@
  N..... MESSAGE(" should be -.9 0 0: -0.900000 0.000000 0.000000")
  N..... SET_TOOL_TABLE_ENTRY(1, 1, -0.9000 -1.8000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, -0.9000 -1.8000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... USE_TOOL_LENGTH_OFFSET(-0.9000 -1.8000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE(" should be -.9 -1.8 0: -0.900000 -1.800000 0.000000")
  N..... SET_TOOL_TABLE_ENTRY(1, 1, -0.9000 -1.8000 -2.7000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, -0.9000 -1.8000 -2.7000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... USE_TOOL_LENGTH_OFFSET(-0.9000 -1.8000 -2.7000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE(" should be -.9 -1.8 -2.7: -0.900000 -1.800000 -2.700000")
  N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE(" part 2")
  N..... SET_TOOL_TABLE_ENTRY(1, 1, 0.1000 0.2000 0.3000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, 0.1000 0.2000 0.3000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... USE_TOOL_LENGTH_OFFSET(0.1000 0.2000 0.3000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE(" should be .1 .2 .3: 0.100000 0.200000 0.300000")
  N..... SET_TOOL_TABLE_ENTRY(1, 1, -0.9000 0.2000 0.3000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, -0.9000 0.2000 0.3000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... USE_TOOL_LENGTH_OFFSET(-0.9000 0.2000 0.3000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE(" should be -.9 0.2 .3: -0.900000 0.200000 0.300000")
  N..... SET_TOOL_TABLE_ENTRY(1, 1, -0.9000 -1.8000 0.3000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, -0.9000 -1.8000 0.3000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... USE_TOOL_LENGTH_OFFSET(-0.9000 -1.8000 0.3000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE(" should be -.9 -1.8 .3: -0.900000 -1.800000 0.300000")
  N..... SET_TOOL_TABLE_ENTRY(1, 1, -0.9000 -1.8000 -2.7000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, -0.9000 -1.8000 -2.7000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... USE_TOOL_LENGTH_OFFSET(-0.9000 -1.8000 -2.7000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE(" should be -.9 -1.8 -2.7: -0.900000 -1.800000 -2.700000")
  N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE("G10 L10: set tool offsets relative to position + 45 deg. rotation")
@@ -90,14 +102,13 @@
  N..... MESSAGE(" should be 0 0 0: 0.000000 0.000000 0.000000")
  N..... SET_TOOL_TABLE_ENTRY(1, 1, -0.7071 -0.7071 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, -0.7071 -0.7071 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
- N..... MESSAGE(" should be -.707 -.707 0: -0.707107 -0.707107 0.000000")
  N..... USE_TOOL_LENGTH_OFFSET(-0.7071 -0.7071 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
+ N..... MESSAGE(" should be -.707 -.707 0: -0.707107 -0.707107 0.000000")
  N..... MESSAGE(" should be 1 0: 1.000000 0.000000")
  N..... SET_TOOL_TABLE_ENTRY(1, 1, 0.0000 -1.4142 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, 0.0000 -1.4142 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
- N..... MESSAGE(" should be 0 -1.414 0: 0.000000 -1.414214 0.000000")
- N..... MESSAGE(" should be 1 0: 1.000000 0.000000")
  N..... USE_TOOL_LENGTH_OFFSET(0.0000 -1.4142 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
+ N..... MESSAGE(" should be 0 -1.414 0: 0.000000 -1.414214 0.000000")
  N..... MESSAGE(" should be 1 1: 1.000000 1.000000")
  N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE("G10 L10: set tool offsets relative to position with rotation")
@@ -115,11 +126,30 @@
  N..... SET_TOOL_TABLE_ENTRY(0, 1, -0.7071 -0.7071 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(1, 1, 0.0000 -1.4142 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, 0.0000 -1.4142 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
- N..... MESSAGE(" should be 0 -1.414 0: 0.000000 -1.414214 0.000000")
- N..... MESSAGE(" should be 1 0: 0.000000 0.000000")
+ N..... MESSAGE(" G10 alone does not apply offset, should still be 0 0 0: 0.000000 0.000000 0.000000")
  N..... USE_TOOL_LENGTH_OFFSET(0.0000 -1.4142 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
+ N..... MESSAGE(" should be 0 -1.414 0: 0.000000 -1.414214 0.000000")
  N..... MESSAGE(" should be 1 1: 1.000000 1.000000")
  N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
+ N..... MESSAGE("Model B contract: M6 alone and G10 alone do not change tool offset params")
+ N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
+ N..... SET_TOOL_TABLE_ENTRY(1, 1, 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... SET_TOOL_TABLE_ENTRY(0, 1, 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... SELECT_TOOL(1)
+ N..... STOP_SPINDLE_TURNING(0)
+ N..... CHANGE_TOOL()
+ N..... MESSAGE(" after M6 alone, should be 0 0 0: 0.000000 0.000000 0.000000")
+ N..... SET_TOOL_TABLE_ENTRY(1, 1, 5.0000 6.0000 7.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... SET_TOOL_TABLE_ENTRY(0, 1, 5.0000 6.0000 7.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... MESSAGE(" after G10 alone, should still be 0 0 0: 0.000000 0.000000 0.000000")
+ N..... USE_TOOL_LENGTH_OFFSET(5.0000 6.0000 7.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
+ N..... MESSAGE(" after G43, should be 5 6 7: 5.000000 6.000000 7.000000")
+ N..... SET_TOOL_TABLE_ENTRY(1, 1, 8.0000 6.0000 7.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... SET_TOOL_TABLE_ENTRY(0, 1, 8.0000 6.0000 7.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... MESSAGE(" after G10 modifying loaded tool, should still be 5 6 7: 5.000000 6.000000 7.000000")
+ N..... USE_TOOL_LENGTH_OFFSET(8.0000 6.0000 7.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
+ N..... MESSAGE(" after G43, should be 8 6 7: 8.000000 6.000000 7.000000")
+ N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000)
  N..... SET_XY_ROTATION(45.0000)
  N..... SET_FEED_MODE(0, 0)
diff --git a/tests/interp/g10/g10-l1-l10/test.ngc b/tests/interp/g10/g10-l1-l10/test.ngc
index 78ef3332f4f..bd7fb9dde00 100644
--- a/tests/interp/g10/g10-l1-l10/test.ngc
+++ b/tests/interp/g10/g10-l1-l10/test.ngc
@@ -12,18 +12,24 @@ g10 l1 p1 x1
 g43
 (debug, should be 1 0 0: #5401 #5402 #5403)
 g10 l1 p1 y2
+g43
 (debug, should be 1 2 0: #5401 #5402 #5403)
 g10 l1 p1 z3
+g43
 (debug, should be 1 2 3: #5401 #5402 #5403)
 g49
 (debug,    part 2)
 g10 l1 p1 x0 y0 z0
+g43
 (debug, should be 0 0 0: #5401 #5402 #5403)
 g10 l1 p1 x1
+g43
 (debug, should be 1 0 0: #5401 #5402 #5403)
 g10 l1 p1 y2
+g43
 (debug, should be 1 2 0: #5401 #5402 #5403)
 g10 l1 p1 z3
+g43
 (debug, should be 1 2 3: #5401 #5402 #5403)
 g49
 
@@ -38,18 +44,24 @@ g10 l10 p1 x1
 g43
 (debug, should be -.9 0 0: #5401 #5402 #5403)
 g10 l10 p1 y2
+g43
 (debug, should be -.9 -1.8 0: #5401 #5402 #5403)
 g10 l10 p1 z3
+g43
 (debug, should be -.9 -1.8 -2.7: #5401 #5402 #5403)
 g49
 (debug,    part 2)
 g10 l10 p1 x0 y0 z0
+g43
 (debug, should be .1 .2 .3: #5401 #5402 #5403)
 g10 l10 p1 x1
+g43
 (debug, should be -.9 0.2 .3: #5401 #5402 #5403)
 g10 l10 p1 y2
+g43
 (debug, should be -.9 -1.8 .3: #5401 #5402 #5403)
 g10 l10 p1 y2 z3
+g43
 (debug, should be -.9 -1.8 -2.7: #5401 #5402 #5403)
 g49
 
@@ -61,13 +73,12 @@ g10 l10 p1 x0 y0 z0
 t1 m6 g43
 (debug, should be 0 0 0: #5401 #5402 #5403)
 g10 l10 p1 x1
-(debug, should be -.707 -.707 0: #5401 #5402 #5403)
 g43
+(debug, should be -.707 -.707 0: #5401 #5402 #5403)
 (debug, should be 1 0: #5420 #5421)
 g10 l10 p1 y1
-(debug, should be 0 -1.414 0: #5401 #5402 #5403)
-(debug, should be 1 0: #5420 #5421)
 g43
+(debug, should be 0 -1.414 0: #5401 #5402 #5403)
 (debug, should be 1 1: #5420 #5421)
 g49
 
@@ -80,9 +91,25 @@ t1 m6 g43
 (debug, should be 0 0 0: #5401 #5402 #5403)
 g10 l10 p1 x1
 g10 l10 p1 y1
-(debug, should be 0 -1.414 0: #5401 #5402 #5403)
-(debug, should be 1 0: #5420 #5421)
+(debug, G10 alone does not apply offset, should still be 0 0 0: #5401 #5402 #5403)
 g43
+(debug, should be 0 -1.414 0: #5401 #5402 #5403)
 (debug, should be 1 1: #5420 #5421)
 g49
+
+
+(debug,Model B contract: M6 alone and G10 alone do not change tool offset params)
+g49
+g10 l1 p1 x0 y0 z0
+t1 m6
+(debug, after M6 alone, should be 0 0 0: #5401 #5402 #5403)
+g10 l1 p1 x5 y6 z7
+(debug, after G10 alone, should still be 0 0 0: #5401 #5402 #5403)
+g43
+(debug, after G43, should be 5 6 7: #5401 #5402 #5403)
+g10 l1 p1 x8
+(debug, after G10 modifying loaded tool, should still be 5 6 7: #5401 #5402 #5403)
+g43
+(debug, after G43, should be 8 6 7: #5401 #5402 #5403)
+g49
 m2
diff --git a/tests/interp/g10/g10-l11/expected b/tests/interp/g10/g10-l11/expected
index 867b9539c30..7907f263f28 100644
--- a/tests/interp/g10/g10-l11/expected
+++ b/tests/interp/g10/g10-l11/expected
@@ -22,20 +22,22 @@
  N..... SELECT_TOOL(1)
  N..... STOP_SPINDLE_TURNING(0)
  N..... CHANGE_TOOL()
+ N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 -3.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE("-3.000000")
  N..... SET_G5X_OFFSET(2, 4.0000, 5.0000, -6.0000, 0.0000, 0.0000, 0.0000)
  N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000)
  N..... SET_XY_ROTATION(20.0000)
- N..... STRAIGHT_TRAVERSE(-5.4689, -3.3304, -24.0000, 0.0000, 0.0000, 0.0000)
+ N..... STRAIGHT_TRAVERSE(-5.4689, -3.3304, -21.0000, 0.0000, 0.0000, 0.0000)
  N..... SET_TOOL_TABLE_ENTRY(1, 1, 0.0000 0.0000 -4.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, 0.0000 0.0000 -4.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 -4.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE("-4.000000")
  N..... SET_G5X_OFFSET(3, 7.0000, 8.0000, -9.0000, 0.0000, 0.0000, 0.0000)
  N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000)
  N..... SET_XY_ROTATION(30.0000)
- N..... SET_G92_OFFSET(-43.0622, -47.4282, -76.0000, 0.0000, 0.0000, 0.0000)
+ N..... SET_G92_OFFSET(-43.0622, -47.4282, -72.0000, 0.0000, 0.0000, 0.0000)
  N..... SET_G5X_OFFSET(1, 1.0000, 2.0000, -3.0000, 0.0000, 0.0000, 0.0000)
- N..... SET_G92_OFFSET(-43.0622, -47.4282, -76.0000, 0.0000, 0.0000, 0.0000)
+ N..... SET_G92_OFFSET(-43.0622, -47.4282, -72.0000, 0.0000, 0.0000, 0.0000)
  N..... SET_XY_ROTATION(10.0000)
  N..... STRAIGHT_TRAVERSE(41.7301, 45.6322, 49.0000, 0.0000, 0.0000, 0.0000)
  N..... SET_TOOL_TABLE_ENTRY(1, 1, 0.0000 0.0000 -3.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
@@ -43,13 +45,15 @@
  N..... SELECT_TOOL(1)
  N..... STOP_SPINDLE_TURNING(0)
  N..... CHANGE_TOOL()
+ N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 -3.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE("-3.000000")
  N..... SET_G5X_OFFSET(2, 4.0000, 5.0000, -6.0000, 0.0000, 0.0000, 0.0000)
- N..... SET_G92_OFFSET(-43.0622, -47.4282, -76.0000, 0.0000, 0.0000, 0.0000)
+ N..... SET_G92_OFFSET(-43.0622, -47.4282, -72.0000, 0.0000, 0.0000, 0.0000)
  N..... SET_XY_ROTATION(20.0000)
- N..... STRAIGHT_TRAVERSE(37.5933, 44.0978, 52.0000, 0.0000, 0.0000, 0.0000)
+ N..... STRAIGHT_TRAVERSE(37.5933, 44.0978, 51.0000, 0.0000, 0.0000, 0.0000)
  N..... SET_TOOL_TABLE_ENTRY(1, 1, 0.0000 0.0000 -4.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
  N..... SET_TOOL_TABLE_ENTRY(0, 1, 0.0000 0.0000 -4.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000, 0.0000, 0.0000, 0)
+ N..... USE_TOOL_LENGTH_OFFSET(0.0000 0.0000 -4.0000, 0.0000 0.0000 0.0000, 0.0000 0.0000 0.0000)
  N..... MESSAGE("-4.000000")
  N..... SET_G5X_OFFSET(1, 1.0000, 2.0000, -3.0000, 0.0000, 0.0000, 0.0000)
  N..... SET_XY_ROTATION(10.0000)
diff --git a/tests/interp/g10/g10-l11/test.ngc b/tests/interp/g10/g10-l11/test.ngc
index be03aeca10c..e2cba9ad9b1 100644
--- a/tests/interp/g10/g10-l11/test.ngc
+++ b/tests/interp/g10/g10-l11/test.ngc
@@ -15,10 +15,12 @@ g54
 g0 g53 z-30
 g10 l11 p1 z0 ; length should be -3
 t1 m6
+g43
 (debug,#5403)
 g55
 g0 g53 z-30
 g10 l11 p1 z1 ; length should be -4
+g43
 (debug,#5403)
 
 g56
@@ -28,10 +30,12 @@ g54
 g0 g53 z-30
 g10 l11 p1 z0 ; ... so the length should still be -3
 t1 m6
+g43
 (debug,#5403)
 g55
 g0 g53 z-30
 g10 l11 p1 z1 ; ... so the length should still be -4
+g43
 (debug,#5403)
 
 m2
diff --git a/tests/t0/nonrandom/expected-gcode-output b/tests/t0/nonrandom/expected-gcode-output
index 9d94ac4e0a1..a05b5207e69 100644
--- a/tests/t0/nonrandom/expected-gcode-output
+++ b/tests/t0/nonrandom/expected-gcode-output
@@ -17,7 +17,7 @@ X = 0.000000
 Y = 0.000000
 Z = -1.000000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = 1.000000
 
 sequence number 3.000000
 X = 0.000000
@@ -38,14 +38,14 @@ X = 0.000000
 Y = 0.000000
 Z = -0.100000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = 0.100000
 
 sequence number 6.000000
 X = 0.000000
 Y = 0.000000
 Z = -0.100000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = 0.100000
 
 sequence number 7.000000
 X = 0.000000
@@ -66,14 +66,14 @@ X = 0.000000
 Y = 0.000000
 Z = 0.300000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = -0.300000
 
 sequence number 10.000000
 X = 0.000000
 Y = 0.000000
 Z = 0.300000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = -0.300000
 
 sequence number 11.000000
 X = 0.000000
@@ -115,14 +115,14 @@ X = 0.000000
 Y = 0.000000
 Z = -4.500000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = 4.500000
 
 sequence number 16.000000
 X = 0.000000
 Y = 0.000000
 Z = -4.500000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = 4.500000
 
 sequence number 17.000000
 X = 0.000000
@@ -136,7 +136,7 @@ X = 0.000000
 Y = 0.000000
 Z = 0.000000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 0.000000
 
 sequence number 101.000000
 X = 0.000000
@@ -157,14 +157,14 @@ X = 0.000000
 Y = 0.000000
 Z = 1.100000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = -1.100000
 
 sequence number 104.000000
 X = 0.000000
 Y = 0.000000
 Z = 1.100000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = -1.100000
 
 sequence number 105.000000
 X = 0.000000
@@ -185,14 +185,14 @@ X = 0.000000
 Y = 0.000000
 Z = -1.300000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 1.300000
 
 sequence number 108.000000
 X = 0.000000
 Y = 0.000000
 Z = -1.300000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 1.300000
 
 sequence number 109.000000
 X = 0.000000
@@ -213,14 +213,14 @@ X = 0.000000
 Y = 0.000000
 Z = 1.500000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = -1.500000
 
 sequence number 112.000000
 X = 0.000000
 Y = 0.000000
 Z = 1.500000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = -1.500000
 
 sequence number 113.000000
 X = 0.000000
@@ -234,7 +234,7 @@ X = 0.000000
 Y = 0.000000
 Z = -2.000000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.000000
 
 sequence number 115.000000
 X = 0.000000
@@ -248,7 +248,7 @@ X = 0.000000
 Y = 0.000000
 Z = 1.500000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -1.500000
 
 sequence number 117.000000
 X = 0.000000
@@ -290,14 +290,14 @@ X = 0.000000
 Y = 0.000000
 Z = -2.500000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.500000
 
 sequence number 122.000000
 X = 0.000000
 Y = 0.000000
 Z = -2.500000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.500000
 
 sequence number 123.000000
 X = 0.000000
@@ -311,7 +311,7 @@ X = 0.000000
 Y = 0.000000
 Z = 1.700000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = -1.700000
 
 sequence number 201.000000
 X = 0.000000
@@ -332,14 +332,14 @@ X = 0.000000
 Y = 0.000000
 Z = 2.000000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = -2.000000
 
 sequence number 204.000000
 X = 0.000000
 Y = 0.000000
 Z = 2.000000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = -2.000000
 
 sequence number 205.000000
 X = 0.000000
@@ -360,14 +360,14 @@ X = 0.000000
 Y = 0.000000
 Z = -2.200000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = 2.200000
 
 sequence number 208.000000
 X = 0.000000
 Y = 0.000000
 Z = -2.200000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = 2.200000
 
 sequence number 209.000000
 X = 0.000000
@@ -388,14 +388,14 @@ X = 0.000000
 Y = 0.000000
 Z = 2.400000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = -2.400000
 
 sequence number 212.000000
 X = 0.000000
 Y = 0.000000
 Z = 2.400000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = -2.400000
 
 sequence number 213.000000
 X = 0.000000
@@ -437,14 +437,14 @@ X = 0.000000
 Y = 0.000000
 Z = -4.400000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = 4.400000
 
 sequence number 218.000000
 X = 0.000000
 Y = 0.000000
 Z = -4.400000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = 4.400000
 
 sequence number 219.000000
 X = 0.000000
diff --git a/tests/t0/random-with-t0/expected-gcode-output b/tests/t0/random-with-t0/expected-gcode-output
index d23b22c9706..927161c9389 100644
--- a/tests/t0/random-with-t0/expected-gcode-output
+++ b/tests/t0/random-with-t0/expected-gcode-output
@@ -17,7 +17,7 @@ X = 0.000000
 Y = 0.000000
 Z = -1.000000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = 1.000000
 
 sequence number 3.000000
 X = 0.000000
@@ -38,14 +38,14 @@ X = 0.000000
 Y = 0.000000
 Z = -0.100000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = 0.100000
 
 sequence number 6.000000
 X = 0.000000
 Y = 0.000000
 Z = -0.100000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = 0.100000
 
 sequence number 7.000000
 X = 0.000000
@@ -66,14 +66,14 @@ X = 0.000000
 Y = 0.000000
 Z = 0.300000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = -0.300000
 
 sequence number 10.000000
 X = 0.000000
 Y = 0.000000
 Z = 0.300000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = -0.300000
 
 sequence number 11.000000
 X = 0.000000
@@ -115,14 +115,14 @@ X = 0.000000
 Y = 0.000000
 Z = -4.500000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = 4.500000
 
 sequence number 16.000000
 X = 0.000000
 Y = 0.000000
 Z = -4.500000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = 4.500000
 
 sequence number 17.000000
 X = 0.000000
@@ -136,7 +136,7 @@ X = 0.000000
 Y = 0.000000
 Z = 0.000000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 0.000000
 
 sequence number 101.000000
 X = 0.000000
@@ -157,98 +157,98 @@ X = 0.000000
 Y = 0.000000
 Z = 1.100000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = -1.100000
 
 sequence number 104.000000
 X = 0.000000
 Y = 0.000000
 Z = 1.100000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = -1.100000
 
 sequence number 105.000000
 X = 0.000000
 Y = 0.000000
 Z = 0.000000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 0.000000
 
 sequence number 106.000000
 X = 0.000000
 Y = 0.000000
 Z = 0.000000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 0.000000
 
 sequence number 107.000000
 X = 0.000000
 Y = 0.000000
 Z = -1.300000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 1.300000
 
 sequence number 108.000000
 X = 0.000000
 Y = 0.000000
 Z = -1.300000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 1.300000
 
 sequence number 109.000000
 X = 0.000000
 Y = 0.000000
 Z = 0.000000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 0.000000
 
 sequence number 110.000000
 X = 0.000000
 Y = 0.000000
 Z = 0.000000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 0.000000
 
 sequence number 111.000000
 X = 0.000000
 Y = 0.000000
 Z = 1.500000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = -1.500000
 
 sequence number 112.000000
 X = 0.000000
 Y = 0.000000
 Z = 1.500000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = -1.500000
 
 sequence number 113.000000
 X = 0.000000
 Y = 0.000000
 Z = 0.000000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 0.000000
 
 sequence number 114.000000
 X = 0.000000
 Y = 0.000000
 Z = 0.000000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 0.000000
 
 sequence number 115.000000
 X = 0.000000
 Y = 0.000000
 Z = 0.000000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 0.000000
 
 sequence number 116.000000
 X = 0.000000
 Y = 0.000000
 Z = 1.500000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -1.500000
 
 sequence number 117.000000
 X = 0.000000
@@ -290,21 +290,21 @@ X = 0.000000
 Y = 0.000000
 Z = -2.500000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.500000
 
 sequence number 122.000000
 X = 0.000000
 Y = 0.000000
 Z = -2.500000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.500000
 
 sequence number 123.000000
 X = 0.000000
 Y = 0.000000
 Z = 0.000000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 0.000000
 
 sequence number 200.000000
 X = 0.000000
@@ -332,14 +332,14 @@ X = 0.000000
 Y = 0.000000
 Z = 2.000000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = -2.000000
 
 sequence number 204.000000
 X = 0.000000
 Y = 0.000000
 Z = 2.000000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = -2.000000
 
 sequence number 205.000000
 X = 0.000000
@@ -360,14 +360,14 @@ X = 0.000000
 Y = 0.000000
 Z = -2.200000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = 2.200000
 
 sequence number 208.000000
 X = 0.000000
 Y = 0.000000
 Z = -2.200000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = 2.200000
 
 sequence number 209.000000
 X = 0.000000
@@ -388,14 +388,14 @@ X = 0.000000
 Y = 0.000000
 Z = 2.400000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = -2.400000
 
 sequence number 212.000000
 X = 0.000000
 Y = 0.000000
 Z = 2.400000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = -2.400000
 
 sequence number 213.000000
 X = 0.000000
@@ -437,14 +437,14 @@ X = 0.000000
 Y = 0.000000
 Z = -4.400000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = 4.400000
 
 sequence number 218.000000
 X = 0.000000
 Y = 0.000000
 Z = -4.400000
 toolno = 0.000000
-tlo_z = 0.000000
+tlo_z = 4.400000
 
 sequence number 219.000000
 X = 0.000000
diff --git a/tests/t0/random-without-t0/expected-gcode-output b/tests/t0/random-without-t0/expected-gcode-output
index 71fa114294d..5e63053dd0d 100644
--- a/tests/t0/random-without-t0/expected-gcode-output
+++ b/tests/t0/random-without-t0/expected-gcode-output
@@ -17,126 +17,126 @@ X = 0.000000
 Y = 0.000000
 Z = -1.000000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = 1.000000
 
 sequence number 3.000000
 X = 0.000000
 Y = 0.000000
 Z = -1.000000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = 1.000000
 
 sequence number 4.000000
 X = 0.000000
 Y = 0.000000
 Z = -1.000000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = 1.000000
 
 sequence number 5.000000
 X = 0.000000
 Y = 0.000000
 Z = -0.100000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = 0.100000
 
 sequence number 6.000000
 X = 0.000000
 Y = 0.000000
 Z = -0.100000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = 0.100000
 
 sequence number 7.000000
 X = 0.000000
 Y = 0.000000
 Z = -0.100000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = 0.100000
 
 sequence number 8.000000
 X = 0.000000
 Y = 0.000000
 Z = -0.100000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = 0.100000
 
 sequence number 9.000000
 X = 0.000000
 Y = 0.000000
 Z = 0.300000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = -0.300000
 
 sequence number 10.000000
 X = 0.000000
 Y = 0.000000
 Z = 0.300000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = -0.300000
 
 sequence number 11.000000
 X = 0.000000
 Y = 0.000000
 Z = 0.300000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = -0.300000
 
 sequence number 11.500000
 X = 0.000000
 Y = 0.000000
 Z = 0.300000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = -0.300000
 
 sequence number 12.000000
 X = 0.000000
 Y = 0.000000
 Z = 5.000000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = -0.300000
 
 sequence number 13.000000
 X = 0.000000
 Y = 0.000000
 Z = 0.300000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = -0.300000
 
 sequence number 14.000000
 X = 0.000000
 Y = 0.000000
 Z = 0.300000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = -0.300000
 
 sequence number 15.000000
 X = 0.000000
 Y = 0.000000
 Z = -4.200000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = 4.200000
 
 sequence number 16.000000
 X = 0.000000
 Y = 0.000000
 Z = -4.200000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = 4.200000
 
 sequence number 17.000000
 X = 0.000000
 Y = 0.000000
 Z = -4.200000
 toolno = -1.000000
-tlo_z = 0.000000
+tlo_z = 4.200000
 
 sequence number 100.000000
 X = 0.000000
 Y = 0.000000
 Z = -4.200000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 4.200000
 
 sequence number 101.000000
 X = 0.000000
@@ -157,98 +157,98 @@ X = 0.000000
 Y = 0.000000
 Z = 1.100000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = -1.100000
 
 sequence number 104.000000
 X = 0.000000
 Y = 0.000000
 Z = 1.100000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = -1.100000
 
 sequence number 105.000000
 X = 0.000000
 Y = 0.000000
 Z = 1.100000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = -1.100000
 
 sequence number 106.000000
 X = 0.000000
 Y = 0.000000
 Z = 1.100000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = -1.100000
 
 sequence number 107.000000
 X = 0.000000
 Y = 0.000000
 Z = -1.300000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 1.300000
 
 sequence number 108.000000
 X = 0.000000
 Y = 0.000000
 Z = -1.300000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 1.300000
 
 sequence number 109.000000
 X = 0.000000
 Y = 0.000000
 Z = -1.300000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 1.300000
 
 sequence number 110.000000
 X = 0.000000
 Y = 0.000000
 Z = -1.300000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = 1.300000
 
 sequence number 111.000000
 X = 0.000000
 Y = 0.000000
 Z = 1.500000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = -1.500000
 
 sequence number 112.000000
 X = 0.000000
 Y = 0.000000
 Z = 1.500000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = -1.500000
 
 sequence number 113.000000
 X = 0.000000
 Y = 0.000000
 Z = 1.500000
 toolno = 50.000000
-tlo_z = 2.000000
+tlo_z = -1.500000
 
 sequence number 114.000000
 X = 0.000000
 Y = 0.000000
 Z = 1.500000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -1.500000
 
 sequence number 115.000000
 X = 0.000000
 Y = 0.000000
 Z = 1.500000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -1.500000
 
 sequence number 116.000000
 X = 0.000000
 Y = 0.000000
 Z = 1.500000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -1.500000
 
 sequence number 117.000000
 X = 0.000000
@@ -290,28 +290,28 @@ X = 0.000000
 Y = 0.000000
 Z = -2.500000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.500000
 
 sequence number 122.000000
 X = 0.000000
 Y = 0.000000
 Z = -2.500000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.500000
 
 sequence number 123.000000
 X = 0.000000
 Y = 0.000000
 Z = -2.500000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.500000
 
 sequence number 200.000000
 X = 0.000000
 Y = 0.000000
 Z = -2.500000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.500000
 
 sequence number 201.000000
 X = 0.000000
@@ -332,124 +332,124 @@ X = 0.000000
 Y = 0.000000
 Z = 2.000000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -2.000000
 
 sequence number 204.000000
 X = 0.000000
 Y = 0.000000
 Z = 2.000000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -2.000000
 
 sequence number 205.000000
 X = 0.000000
 Y = 0.000000
 Z = 2.000000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -2.000000
 
 sequence number 206.000000
 X = 0.000000
 Y = 0.000000
 Z = 2.000000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -2.000000
 
 sequence number 207.000000
 X = 0.000000
 Y = 0.000000
 Z = -2.200000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.200000
 
 sequence number 208.000000
 X = 0.000000
 Y = 0.000000
 Z = -2.200000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.200000
 
 sequence number 209.000000
 X = 0.000000
 Y = 0.000000
 Z = -2.200000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.200000
 
 sequence number 210.000000
 X = 0.000000
 Y = 0.000000
 Z = -2.200000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.200000
 
 sequence number 211.000000
 X = 0.000000
 Y = 0.000000
 Z = 2.400000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -2.400000
 
 sequence number 212.000000
 X = 0.000000
 Y = 0.000000
 Z = 2.400000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -2.400000
 
 sequence number 213.000000
 X = 0.000000
 Y = 0.000000
 Z = 2.400000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -2.400000
 
 sequence number 213.500000
 X = 0.000000
 Y = 0.000000
 Z = 2.400000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -2.400000
 
 sequence number 214.000000
 X = 0.000000
 Y = 0.000000
 Z = 7.000000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -2.400000
 
 sequence number 215.000000
 X = 0.000000
 Y = 0.000000
 Z = 2.400000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -2.400000
 
 sequence number 216.000000
 X = 0.000000
 Y = 0.000000
 Z = 2.400000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = -2.400000
 
 sequence number 217.000000
 X = 0.000000
 Y = 0.000000
 Z = -2.000000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.000000
 
 sequence number 218.000000
 X = 0.000000
 Y = 0.000000
 Z = -2.000000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.000000
 
 sequence number 219.000000
 X = 0.000000
 Y = 0.000000
 Z = -2.000000
 toolno = 50.000000
-tlo_z = -1.700000
+tlo_z = 2.000000
 
diff --git a/tests/toolchanger/m61/test-ui.py b/tests/toolchanger/m61/test-ui.py
index ca29beb5d6e..83110f6ed26 100755
--- a/tests/toolchanger/m61/test-ui.py
+++ b/tests/toolchanger/m61/test-ui.py
@@ -175,9 +175,11 @@ def do_tool_change_handshake(tool_number, pocket_number):
 print("*** tool change complete")
 verify_tool_number(1)
 
+# M6 changes the loaded tool but does not by itself apply its offset
+# to motion, so #5401-#5409 (applied tool length offset) stay at 0.
 verify_interp_param(5401, 0)      # tlo x
 verify_interp_param(5402, 0)      # tlo y
-verify_interp_param(5403, 1)      # tlo z
+verify_interp_param(5403, 0)      # tlo z (not applied yet)
 verify_interp_param(5404, 0)      # tlo a
 verify_interp_param(5405, 0)      # tlo b
 verify_interp_param(5406, 0)      # tlo c
@@ -202,6 +204,9 @@ def do_tool_change_handshake(tool_number, pocket_number):
 c.mdi('g43')
 c.wait_complete()
 
+# After G43 the tool length offset is applied; #5403 now reflects it.
+verify_interp_param(5403, 1)      # tlo z (now applied)
+
 verify_interp_param(5420, 0)      # current x
 verify_interp_param(5421, 0)      # current y
 verify_interp_param(5422, -1)     # current z
@@ -239,9 +244,12 @@ def do_tool_change_handshake(tool_number, pocket_number):
 
 verify_tool_number(10)
 
+# M61 changes the loaded tool but does not by itself apply its offset
+# to motion. The previous offset from T1 (z=1) is still applied here,
+# until the next G43.
 verify_interp_param(5401, 0)      # tlo x
 verify_interp_param(5402, 0)      # tlo y
-verify_interp_param(5403, 3)      # tlo z
+verify_interp_param(5403, 1)      # tlo z (still T1's offset, not yet reapplied)
 verify_interp_param(5404, 0)      # tlo a
 verify_interp_param(5405, 0)      # tlo b
 verify_interp_param(5406, 0)      # tlo c
@@ -266,6 +274,9 @@ def do_tool_change_handshake(tool_number, pocket_number):
 c.mdi('g43')
 c.wait_complete()
 
+# After G43 the new tool's offset is applied; #5403 now reflects T10.
+verify_interp_param(5403, 3)      # tlo z (now applied)
+
 verify_interp_param(5420, 0)      # current x
 verify_interp_param(5421, 0)      # current y
 verify_interp_param(5422, -3)     # current z
diff --git a/tests/toolchanger/toolno-pocket-differ/nonrandom/expected-gcode-output b/tests/toolchanger/toolno-pocket-differ/nonrandom/expected-gcode-output
index edc35011d66..5e5cb8a0426 100644
--- a/tests/toolchanger/toolno-pocket-differ/nonrandom/expected-gcode-output
+++ b/tests/toolchanger/toolno-pocket-differ/nonrandom/expected-gcode-output
@@ -10,7 +10,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.333000
 toolno = 1.000000
-tlo_z = 1.000000
+tlo_z = 0.000000
 
 sequence number 2.000000
 X = 0.111000
@@ -24,7 +24,7 @@ X = 0.111000
 Y = 0.222000
 Z = -0.667000
 toolno = 1.000000
-tlo_z = 0.233000
+tlo_z = 1.000000
 
 sequence number 4.000000
 X = 0.111000
@@ -66,7 +66,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.100000
 toolno = 10.000000
-tlo_z = 0.183000
+tlo_z = 0.233000
 
 sequence number 10.000000
 X = 0.111000
@@ -94,7 +94,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.150000
 toolno = 10.000000
-tlo_z = 0.229500
+tlo_z = 0.183000
 
 sequence number 14.000000
 X = 0.111000
@@ -122,7 +122,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.103500
 toolno = 99999.000000
-tlo_z = 0.229000
+tlo_z = 0.229500
 
 sequence number 18.000000
 X = 0.111000
@@ -164,7 +164,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.104000
 toolno = 99999.000000
-tlo_z = -0.067000
+tlo_z = 0.229000
 
 sequence number 24.000000
 X = 0.111000
@@ -178,7 +178,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.400000
 toolno = 1.000000
-tlo_z = 0.033000
+tlo_z = -0.067000
 
 sequence number 26.000000
 X = 0.111000
@@ -192,7 +192,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.300000
 toolno = 10.000000
-tlo_z = -0.017000
+tlo_z = 0.033000
 
 sequence number 28.000000
 X = 0.111000
@@ -206,7 +206,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.350000
 toolno = 99999.000000
-tlo_z = -0.067000
+tlo_z = -0.017000
 
 sequence number 30.000000
 X = 0.111000
diff --git a/tests/toolchanger/toolno-pocket-differ/random/expected-gcode-output b/tests/toolchanger/toolno-pocket-differ/random/expected-gcode-output
index 30edd89efac..d5986569f15 100644
--- a/tests/toolchanger/toolno-pocket-differ/random/expected-gcode-output
+++ b/tests/toolchanger/toolno-pocket-differ/random/expected-gcode-output
@@ -10,7 +10,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.333000
 toolno = 1.000000
-tlo_z = 1.000000
+tlo_z = 0.000000
 
 sequence number 2.000000
 X = 0.111000
@@ -24,7 +24,7 @@ X = 0.111000
 Y = 0.222000
 Z = -0.667000
 toolno = 1.000000
-tlo_z = 0.233000
+tlo_z = 1.000000
 
 sequence number 4.000000
 X = 0.111000
@@ -66,7 +66,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.100000
 toolno = 10.000000
-tlo_z = 0.183000
+tlo_z = 0.233000
 
 sequence number 10.000000
 X = 0.111000
@@ -94,7 +94,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.150000
 toolno = 10.000000
-tlo_z = 0.229500
+tlo_z = 0.183000
 
 sequence number 14.000000
 X = 0.111000
@@ -122,7 +122,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.103500
 toolno = 99999.000000
-tlo_z = 0.229000
+tlo_z = 0.229500
 
 sequence number 18.000000
 X = 0.111000
@@ -164,7 +164,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.104000
 toolno = 99999.000000
-tlo_z = -0.067000
+tlo_z = 0.229000
 
 sequence number 24.000000
 X = 0.111000
@@ -178,7 +178,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.400000
 toolno = 1.000000
-tlo_z = 0.033000
+tlo_z = -0.067000
 
 sequence number 26.000000
 X = 0.111000
@@ -192,7 +192,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.300000
 toolno = 10.000000
-tlo_z = -0.017000
+tlo_z = 0.033000
 
 sequence number 28.000000
 X = 0.111000
@@ -206,7 +206,7 @@ X = 0.111000
 Y = 0.222000
 Z = 0.350000
 toolno = 99999.000000
-tlo_z = -0.067000
+tlo_z = -0.017000
 
 sequence number 30.000000
 X = 0.111000