Backport #4167: expose absolute machine position as #5021-#5029 (2.9)

docs/src/gcode/g-code.adoc

@@ -1314,28 +1314,25 @@ G43
 
 [[gcode:probe-machine-coordinates]]
 Probe results in #5061 to #5069 are expressed in the current work
-coordinate system, so they include the active coordinate system origin,
-any G92/G52 offset, and the applied tool length offset.
-To recover the absolute machine coordinate of a probe, add those offsets
-back.
-The tool length offset currently applied to motion is reported by
-parameters #5401 to #5409 (set by the G43 family, zeroed by G49), so the
-conversion works whether or not a tool offset is active and you do not
-need to cancel it with G49 first.
+coordinate system, not in machine coordinates. To convert one, add the
+offset between the two frames. That offset is the difference between the
+current position reported in both frames at once: #5021 to #5029 hold the
+absolute machine position and #5420 to #5428 hold the work position.
+Because the offset is the same everywhere, it can be read at any later
+position and still applies to the earlier probe result.
 
 .Reading the machine coordinates of a probe
 [source,{ngc}]
 ----
 G38.2 Z-100 F100
-#<zmachine> = [#5063 + #[5203 + #5220 * 20] + #5213 * #5210 + #5403]
+#<zmachine> = [#5063 + #5023 - #5422]
 ----
 
-The added terms are the current coordinate system Z origin
-(#5223 for G54, #5243 for G55, and so on, selected by #5220), the
-G92/G52 Z offset applied only when active (#5213 multiplied by the
-G92 flag #5210), and the applied tool length offset #5403.
-The same pattern applies to the other axes using their respective
-parameters.
+#5063 is the probed Z in the work frame, and #5023 - #5422 is the
+machine-to-work Z offset (current machine Z minus current work Z). The
+same pattern applies to the other axes using their respective parameters.
+This holds whichever coordinate system, G92/G52 or tool length offset is
+active, as long as no coordinate-system rotation is in effect.
 
 A comment of the form '(PROBEOPEN filename.txt)' will open
 'filename.txt' and store the 9-number coordinate consisting of

docs/src/gcode/overview.adoc

@@ -304,6 +304,10 @@ example '##2'  means the value of the parameter whose index is the
 
 * '31-5000' - G-code user parameters. These parameters are global in the G
   code file, and available for general use. Volatile.
+* '5021-5029' - Current absolute machine position (G53) for X, Y, Z, A, B,
+  C, U, V & W, in the current program units. Includes no coordinate system,
+  G92/G52 or tool length offset. Same as `#<_abs_x>` through `#<_abs_w>`.
+  Read-only, volatile.
 * '5061-5069' - Coordinates of a <<gcode:g38,G38>> probe result (X, Y,
   Z, A, B, C, U, V & W).  Coordinates are in the coordinate system in
   which the G38 took place.  Volatile.
@@ -591,6 +595,9 @@ can be added easily without changes to the source code.
 * '#<_abs_a>' - Return current absolute A coordinate (G53) including no offsets.
 * '#<_abs_b>' - Return current absolute B coordinate (G53) including no offsets.
 * '#<_abs_c>' - Return current absolute C coordinate (G53) including no offsets.
+* '#<_abs_u>' - Return current absolute U coordinate (G53) including no offsets.
+* '#<_abs_v>' - Return current absolute V coordinate (G53) including no offsets.
+* '#<_abs_w>' - Return current absolute W coordinate (G53) including no offsets.
 * '#<_current_tool>' - Return number of the current tool in spindle. Same as #5400.
 * '#<_current_pocket>' - Return the tooldata index for the current tool.
 * '#<_selected_tool>' - Return number of the selected tool post a T code. Default -1.

src/emc/rs274ngc/interp_array.cc

@@ -237,6 +237,7 @@ const int Interp::_required_parameters[] = {
 };
 
 const int Interp::_readonly_parameters[] = {
+ 5021, 5022, 5023, 5024, 5025, 5026, 5027, 5028, 5029, // machine X Y ... W
  5400, // tool toolno
  5401, // tool x offset
  5402, // tool y offset

src/emc/rs274ngc/interp_convert.cc

@@ -2420,6 +2420,39 @@ int Interp::convert_g(block_pointer block,       //!< pointer to a block of RS27
   return INTERP_OK;
 }
 
+/*! get_abs_position
+
+Returned Value: none
+
+Side effects:
+   abs_pos[0..8] is filled with the current absolute machine position
+   (G53 frame) for X, Y, Z, A, B, C, U, V, W.
+
+Called by: read (to fill #5021-#5029) and the #<_abs_*> named parameters.
+
+The values are in the same units and frame as the #<_abs_*> named
+parameters: the controlled point mapped through the active G92/G52,
+coordinate system rotation, G5x and tool length offset, i.e. the
+offsetless machine coordinate.
+
+*/
+
+void Interp::get_abs_position(setup_pointer s, double abs_pos[9])
+{
+    double x = s->current_x + s->axis_offset_x;
+    double y = s->current_y + s->axis_offset_y;
+    rotate(&x, &y, s->rotation_xy);
+    abs_pos[0] = x + s->origin_offset_x + s->tool_offset.tran.x;
+    abs_pos[1] = y + s->origin_offset_y + s->tool_offset.tran.y;
+    abs_pos[2] = s->current_z + s->axis_offset_z + s->origin_offset_z + s->tool_offset.tran.z;
+    abs_pos[3] = s->AA_current + s->AA_axis_offset + s->AA_origin_offset + s->tool_offset.a;
+    abs_pos[4] = s->BB_current + s->BB_axis_offset + s->BB_origin_offset + s->tool_offset.b;
+    abs_pos[5] = s->CC_current + s->CC_axis_offset + s->CC_origin_offset + s->tool_offset.c;
+    abs_pos[6] = s->u_current + s->u_axis_offset + s->u_origin_offset + s->tool_offset.u;
+    abs_pos[7] = s->v_current + s->v_axis_offset + s->v_origin_offset + s->tool_offset.v;
+    abs_pos[8] = s->w_current + s->w_axis_offset + s->w_origin_offset + s->tool_offset.w;
+}
+
 /*! convert_savehome
 
 Returned Value: int

src/emc/rs274ngc/interp_namedparams.cc

@@ -101,6 +101,9 @@ enum predefined_named_parameters {
     NP_ABS_A,
     NP_ABS_B,
     NP_ABS_C,
+    NP_ABS_U,
+    NP_ABS_V,
+    NP_ABS_W,
     NP_VALUE,
     NP_CALL_LEVEL,
     NP_REMAP_LEVEL,
@@ -761,6 +764,21 @@ int Interp::lookup_named_param(const char *nameBuf,
                  _setup.CC_origin_offset + _setup.tool_offset.c;
 	break;
 
+    case NP_ABS_U:  // abs position
+	*value = _setup.u_current + _setup.u_axis_offset +
+                 _setup.u_origin_offset + _setup.tool_offset.u;
+	break;
+
+    case NP_ABS_V:  // abs position
+	*value = _setup.v_current + _setup.v_axis_offset +
+                 _setup.v_origin_offset + _setup.tool_offset.v;
+	break;
+
+    case NP_ABS_W:  // abs position
+	*value = _setup.w_current + _setup.w_axis_offset +
+                 _setup.w_origin_offset + _setup.tool_offset.w;
+	break;
+
 	// o-word subs may optionally have an
 	// expression after endsub and return
 	// this 'function return value' is accessible as '_value'
@@ -943,6 +961,9 @@ int Interp::init_named_parameters()
   init_readonly_param("_abs_a", NP_ABS_A, PA_USE_LOOKUP);
   init_readonly_param("_abs_b", NP_ABS_B, PA_USE_LOOKUP);
   init_readonly_param("_abs_c", NP_ABS_C, PA_USE_LOOKUP);
+  init_readonly_param("_abs_u", NP_ABS_U, PA_USE_LOOKUP);
+  init_readonly_param("_abs_v", NP_ABS_V, PA_USE_LOOKUP);
+  init_readonly_param("_abs_w", NP_ABS_W, PA_USE_LOOKUP);
 
   // last (optional) endsub/return value
   init_readonly_param("_value", NP_VALUE, PA_USE_LOOKUP);

src/emc/rs274ngc/rs274ngc_interp.hh

@@ -523,6 +523,7 @@ int read_dollar(char *line, int *counter, block_pointer block,
  int read_z(char *line, int *counter, block_pointer block,
                   double *parameters);
  int refresh_actual_position(setup_pointer settings);
+ void get_abs_position(setup_pointer settings, double abs_pos[9]);
  void rotate(double *x, double *y, double t);
  int set_probe_data(setup_pointer settings);
  int write_g_codes(block_pointer block, setup_pointer settings);

src/emc/rs274ngc/rs274ngc_pre.cc

@@ -1613,6 +1613,18 @@ int Interp::_read(const char *command)  //!< may be NULL or a string to read
   _setup.parameters[5427] = _setup.v_current;
   _setup.parameters[5428] = _setup.w_current;
 
+  double abs_pos[9];
+  get_abs_position(&_setup, abs_pos);
+  _setup.parameters[5021] = abs_pos[0];
+  _setup.parameters[5022] = abs_pos[1];
+  _setup.parameters[5023] = abs_pos[2];
+  _setup.parameters[5024] = abs_pos[3];
+  _setup.parameters[5025] = abs_pos[4];
+  _setup.parameters[5026] = abs_pos[5];
+  _setup.parameters[5027] = abs_pos[6];
+  _setup.parameters[5028] = abs_pos[7];
+  _setup.parameters[5029] = abs_pos[8];
+
   if(_setup.file_pointer)
   {
       EXECUTING_BLOCK(_setup).offset = ftell(_setup.file_pointer);

tests/interp/machine-coord-params/expected

@@ -0,0 +1,13 @@
+ N..... MESSAGE("0.000000 0.000000 0.000000")
+ N..... MESSAGE("1.000000 2.000000 3.000000")
+ N..... MESSAGE("0.000000 0.000000 0.000000")
+ N..... MESSAGE("1.000000 2.000000 3.000000")
+ N..... MESSAGE("0.000000 0.000000 0.000000")
+ N..... MESSAGE("1.000000 2.000000 3.000000")
+ N..... MESSAGE("0.000000 0.000000 0.000000")
+ N..... MESSAGE("1.000000 2.000000 3.000000")
+ N..... MESSAGE("0.000000 0.000000 0.000000")
+ N..... MESSAGE("1.000000 2.000000 3.000000")
+ N..... MESSAGE("1.000000 2.000000 3.000000")
+ N..... MESSAGE("1.000000 1.000000")
+ N..... MESSAGE("3.000000 3.000000")

tests/interp/machine-coord-params/test.ngc

@@ -0,0 +1,48 @@
+; Verify #5021-#5029 (current absolute machine position) report the
+; commanded G53 location regardless of the active coordinate system,
+; G92/G52, coordinate rotation or tool length offset, and that the
+; numbered set matches the #<_abs_*> named parameters.
+
+o<coords> sub
+g53 g0 x0 y0 z0
+(debug,#5021 #5022 #5023)
+g53 g0 x1 y2 z3
+(debug,#5021 #5022 #5023)
+o<coords> endsub
+
+o<suite> sub
+g54
+g92.2
+g10 l2 p1 x0 y0 z0 r0
+o<coords> call
+
+g10 l2 p1 x3 y7 z0 r0
+o<coords> call
+
+g10 l2 p1 x0 y0 z0 r45
+o<coords> call
+
+g10 l2 p1 x3 y7 z0 r45
+o<coords> call
+
+g10 l2 p1 x0 y0 z0 r0
+g92 x3 y7
+o<coords> call
+g92.2
+o<suite> endsub
+
+g21
+o<suite> call
+
+; tool length offset must not change the machine position
+g43.1 z5
+g53 g0 x1 y2 z3
+(debug,#5021 #5022 #5023)
+g49
+
+; numbered set agrees with the named set
+g53 g0 x1 y2 z3
+(debug,#5021 #<_abs_x>)
+(debug,#5023 #<_abs_z>)
+
+m2

tests/interp/machine-coord-params/test.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+rs274 -g test.ngc -t test.tbl | awk '/MESSAGE/ {$1=""; print}' | sed 's/-0\.0000/0.0000/g'
+exit "${PIPESTATUS[0]}"

tests/interp/machine-coord-params/test.tbl

@@ -0,0 +1 @@
+t1 p1 x0 y0 z0

tests/remap/introspect/expected

@@ -30,8 +30,8 @@ speed= 3000.0
 global parameter set in test.ngc: 47.11
 parameter set via test.ini: 3.14159
 locals:  ['a_new_local']
-globals:  ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo']
-params():  ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo']
+globals:  ['_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo']
+params():  ['a_new_local', '_a', '_a_global_set_in_test_dot_ngc', '_a_new_global', '_abs_a', '_abs_b', '_abs_c', '_abs_u', '_abs_v', '_abs_w', '_abs_x', '_abs_y', '_abs_z', '_absolute', '_adaptive_feed', '_b', '_c', '_call_level', '_ccomp', '_coord_system', '_current_pocket', '_current_tool', '_feed', '_feed_hold', '_feed_override', '_flood', '_ijk_absolute_mode', '_imperial', '_incremental', '_ini[example]variable', '_inverse_time', '_lathe_diameter_mode', '_lathe_radius_mode', '_line', '_metric', '_metric_machine', '_mist', '_motion_mode', '_plane', '_remap_level', '_retract_old_z', '_retract_r_plane', '_rpm', '_selected_pocket', '_selected_tool', '_speed_override', '_spindle_css_mode', '_spindle_cw', '_spindle_on', '_spindle_rpm_mode', '_task', '_tool_offset', '_u', '_units_per_minute', '_units_per_rev', '_v', '_value', '_value_returned', '_vmajor', '_vminor', '_w', '_x', '_y', '_z', 'foo']
    15 N..... MESSAGE(" after introspect: return value=2.718280 call_level= 0.000000")
    16 N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000)
    17 N..... SET_XY_ROTATION(0.0000)