Merge pull request #3995 from rodw-au/state-tags-arcs

Add headings and other geometric pins in motion using State_tags

Author: andypugh

docs/src/man/man9/motion.9.adoc

@@ -263,6 +263,52 @@ Note: feed-inhibit applies to G-code commands -- not jogs.
 *motion.tp-reverse* OUT BIT::
   Trajectory planning is reversed (reverse run)
 
+=== Interpreter Metadata Pins
+
+These pins provide geometric intent and interpreter state for the segment of motion currently being executed. Unlike standard position feedback, these values are synchronized with the trajectory planner's execution point.
+
+==== Interpreter Status Pins
+
+* `motion.interp.line-number` (s32, out) +
+  The current G-code line number being executed.
+  
+* `motion.interp.motion-type` (s32, out) +
+  The type of motion currently in progress:
+** 0: None
+** 1: Rapid (G0)
+** 2: Feed (G1)
+** 3: Arc (G2, G3)
+** 4: Tool Change/Other
+
+* `motion.interp.feedrate` (float, out) +
+  The interpreted feedrate for the current segment in units per minute.
+
+==== Interpreter Geometric Pins
+
+* `motion.interp.heading` (float, out) +
+  The XY plane heading of the current linear move in degrees. Measured counter-clockwise from the +X axis (0 to 360). This could be used to control a tangential knife.
+
+* `motion.interp.arc-radius` (float, out) +
+  The radius of the current circular move. This value is 0.0 during linear moves. This could be used to modify plasma cutting parameters based on the arc radius and when hole cutting.
+
+* `motion.interp.arc-center-x` (float, out) +
+  The absolute X-coordinate of the center point for the current arc. 0 if in YZ plane
+
+* `motion.interp.arc-center-y` (float, out) +
+  The absolute Y-coordinate of the center point for the current arc. 0 if in XZ plane
+
+* `motion.interp.arc-center-z` (float, out) +
+  The absolute Y-coordinate of the center point for the current arc if in XZ or YZ plane.
+
+* `motion.interp.normal-heading` (float, out) +
+  the heading from the current point back to the arc centre for the current arc.
+  
+ * `motion.interp.iscircle` (bit, out) +
+  True if the current arc is a full circle and not a helix.
+  
+[NOTE]
+These pins represent the *commanded geometric intent* from the interpreter and are updated in real-time as each motion segment is consumed by the trajectory planner.
+
 == AXIS PINS
 
 (*L* is the axis letter, one of: *x y z a b c u v w*)

src/emc/motion/command.c

@@ -530,6 +530,16 @@ void emcmotCommandHandler_locked(void *arg, long servo_period)
 		SET_JOINT_FAULT_FLAG(joint, 0);
 	    }
 	    emcmotStatus->paused = 0;
+	    // Clear pins on abort so tests see a clean state
+		if (emcmot_hal_data) {
+				*(emcmot_hal_data->interp_arc_radius) = 0.0;
+				*(emcmot_hal_data->interp_arc_center_x) = 0.0;
+				*(emcmot_hal_data->interp_arc_center_y) = 0.0;
+				*(emcmot_hal_data->interp_arc_center_z) = 0.0;
+				*(emcmot_hal_data->interp_straight_heading) = 0.0;
+				*(emcmot_hal_data->interp_normal_heading) = 0.0;
+				*(emcmot_hal_data->iscircle) = 0.0;
+		}
 	    break;
 
 	case EMCMOT_JOG_ABORT:

src/emc/motion/control.c

@@ -34,6 +34,7 @@
 #include "config.h"
 #include "homing.h"
 #include "axis.h"
+#include "state_tag.h"
 
 // Mark strings for translation, but defer translation to userspace
 #define _(s) (s)
@@ -1888,6 +1889,13 @@ static void output_to_hal(void)
     *(emcmot_hal_data->coord_error) = GET_MOTION_ERROR_FLAG();
     *(emcmot_hal_data->on_soft_limit) = emcmotStatus->on_soft_limit;
 
+    /* Performance Metadata */
+    *(emcmot_hal_data->interp_feedrate)         = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_FEED];
+
+    /* Line and Motion Type (Casting to int for s32 HAL pins) */
+    *(emcmot_hal_data->interp_line_number)      = (int)emcmotStatus->tag.fields[GM_FIELD_LINE_NUMBER];
+    *(emcmot_hal_data->interp_motion_type)      = (int)emcmotStatus->tag.fields[GM_FIELD_MOTION_MODE];
+    *(emcmot_hal_data->iscircle)                = (hal_bit_t)((emcmotStatus->tag.packed_flags & (1UL << GM_FLAG_IS_CIRCLE)) != 0);
     switch (emcmotStatus->motionType) {
         case EMC_MOTION_TYPE_FEED: //fall thru
         case EMC_MOTION_TYPE_ARC:
@@ -2211,6 +2219,74 @@ static void update_status(void)
       emcmotStatus->stepping = 0;
       emcmotStatus->paused = 1;
     }
+    // State Tags handling
+    // Get the current executing trajectory component (the "Source of Truth")
+    /* Update the HAL Output Pins from the active tag */
+    // Line and Motion Type
+    *(emcmot_hal_data->interp_line_number) = (int)emcmotStatus->tag.fields[GM_FIELD_LINE_NUMBER];
+
+    // Performance Metadata
+    *(emcmot_hal_data->interp_feedrate) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_FEED];
+
+    // Geometric Metadata
+    *(emcmot_hal_data->interp_arc_radius) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_RADIUS];
+    *(emcmot_hal_data->interp_arc_center_x) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_X];
+    *(emcmot_hal_data->interp_arc_center_y) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_Y];
+    *(emcmot_hal_data->interp_arc_center_z) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_Z];
+
+    // Get the current motion type from the tag (1=G1, 2=G2, 3=G3)
+    int motion_type = (int)emcmotStatus->tag.fields[GM_FIELD_MOTION_MODE];
+    if (motion_type == 10 || motion_type == 0) {
+        /* --- G1/G0 STATIC HEADING --- */
+        // For linear moves, the heading doesn't change during the segment.
+        *(emcmot_hal_data->interp_straight_heading) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_STRAIGHT_HEADING];
+    }
+    else if (motion_type == 20 || motion_type == 30) {
+        /* --- G2/G3: DYNAMIC ARC HEADING --- */
+
+        // 1. Get Static Center from Tag
+        double cx = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_X];
+        double cy = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_Y];
+        double cz = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_Z];
+
+        // 2. Get Real-Time Feedback Deltas
+        double dx = emcmotStatus->carte_pos_fb.tran.x - cx;
+        double dy = emcmotStatus->carte_pos_fb.tran.y - cy;
+        double dz = emcmotStatus->carte_pos_fb.tran.z - cz;
+
+        // 3. Determine Plane and Radial Angle
+        int plane = emcmotStatus->tag.fields[GM_FIELD_PLANE];
+        double angle_rad = 0.0; // Initialize to prevent "uninitialized" error
+
+        if (plane == 170) {      // XY: X is Horizontal, Y is Verradiustical
+            angle_rad = atan2(dy, dx);
+        }
+        else if (plane == 180) { // XZ: Z is Horizontal, X is Vertical
+            angle_rad = atan2(dx, dz);
+        }
+        else if (plane == 190) { // YZ: Y is Horizontal, Z is Vertical
+            angle_rad = atan2(dz, dy);
+        }
+        // Optional: add an else here for a default plane if 170/180/190 aren't found
+
+        // 4. Calculate Normal Heading (Tool-to-Center)
+        double normal_deg = (angle_rad * (180.0 / M_PI)) + 180.0;
+        while (normal_deg < 0) normal_deg += 360.0;
+        while (normal_deg >= 360.0) normal_deg -= 360.0;
+        *(emcmot_hal_data->interp_normal_heading) = normal_deg;
+
+        // 5. Calculate Tangent Heading (Direction of Travel)
+        double tangent_rad = (motion_type == 30) ? (angle_rad + (M_PI / 2.0)) : (angle_rad - (M_PI / 2.0));
+        double heading_deg = tangent_rad * (180.0 / M_PI);
+
+        // 6. Final Normalization and Assignment
+        while (heading_deg < 0) heading_deg += 360.0;
+        while (heading_deg >= 360.0) heading_deg -= 360.0;
+
+        if (emcmot_hal_data->interp_straight_heading) {
+        *(emcmot_hal_data->interp_straight_heading) = heading_deg;
+        }
+    }
 #ifdef WATCH_FLAGS
     /*! \todo FIXME - this is for debugging */
     if ( old_motion_flag != emcmotStatus->motionFlag ) {

src/emc/motion/mot_priv.h

@@ -199,6 +199,19 @@ typedef struct {
     hal_float_t *feed_mm_per_second; /* feed mm per second*/
 
     hal_float_t *switchkins_type;
+    /* Interp State Pins */
+    hal_s32_t   *interp_line_number;
+    hal_s32_t   *interp_motion_type;
+    hal_float_t *interp_feedrate;
+
+    /* New Geometric Metadata Pins */
+    hal_float_t *interp_arc_radius;
+    hal_float_t *interp_arc_center_x;
+    hal_float_t *interp_arc_center_y;
+    hal_float_t *interp_arc_center_z;
+    hal_float_t *interp_straight_heading;
+    hal_float_t *interp_normal_heading;
+    hal_bit_t   *iscircle;
 } emcmot_hal_data_t;
 
 /***********************************************************************

src/emc/motion/motion.c

@@ -551,6 +551,20 @@ static int init_hal_io(void)
     CALL_CHECK(hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->program_line), mot_comp_id, "motion.program-line"));
     CALL_CHECK(hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->jog_is_active), mot_comp_id, "motion.jog-is-active"));
 
+    /* Standard Interp State Pins */
+    CALL_CHECK(hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->interp_line_number), mot_comp_id, "motion.interp.line-number"));
+    CALL_CHECK(hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->interp_motion_type), mot_comp_id, "motion.interp.motion-type"));
+    CALL_CHECK(hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->interp_feedrate), mot_comp_id, "motion.interp.feedrate"));
+
+    /* New Geometric Metadata Pins */
+    CALL_CHECK(hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->interp_arc_radius), mot_comp_id, "motion.interp.arc-radius"));
+    CALL_CHECK(hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->interp_arc_center_x), mot_comp_id, "motion.interp.arc-center-x"));
+    CALL_CHECK(hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->interp_arc_center_y), mot_comp_id, "motion.interp.arc-center-y"));
+    CALL_CHECK(hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->interp_arc_center_z), mot_comp_id, "motion.interp.arc-center-z"));
+    CALL_CHECK(hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->interp_straight_heading), mot_comp_id, "motion.interp.heading"));
+    CALL_CHECK(hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->interp_normal_heading), mot_comp_id, "motion.interp.normal-heading"));
+    CALL_CHECK(hal_pin_bit_newf(HAL_OUT, &emcmot_hal_data->iscircle, mot_comp_id, "motion.interp.iscircle"));
+
     /* export debug parameters */
     /* these can be used to view any internal variable, simply change a line
        in control.c:output_to_hal() and recompile */

src/emc/motion/state_tag.h

@@ -57,6 +57,7 @@ typedef enum {
     GM_FLAG_IN_REMAP,
     GM_FLAG_IN_SUB,
     GM_FLAG_EXTERNAL_FILE,
+    GM_FLAG_IS_CIRCLE,
     GM_FLAG_MAX_FLAGS
 } StateFlag;
 
@@ -67,7 +68,7 @@ typedef enum {
  * WARNING:
  *
  * 1) Since these are used as array indices, they have to start at 0,
- * be monotonic, and the MAX_FIELDS enum MUST be last in the list.
+ * be monotonic, and the GM_FIELD_MAX_FIELDS enum MUST be last in the list.
  *
  * 2) If your application needs to pass state tags through NML, then
  * you MUST update the corresponding cms->update function for state
@@ -98,6 +99,12 @@ typedef enum {
     GM_FIELD_FLOAT_SPEED,
     GM_FIELD_FLOAT_PATH_TOLERANCE,
     GM_FIELD_FLOAT_NAIVE_CAM_TOLERANCE,
+    GM_FIELD_FLOAT_ARC_RADIUS,
+    GM_FIELD_FLOAT_ARC_CENTER_X,
+    GM_FIELD_FLOAT_ARC_CENTER_Y,
+    GM_FIELD_FLOAT_ARC_CENTER_Z,
+    GM_FIELD_FLOAT_STRAIGHT_HEADING,
+    GM_FIELD_FLOAT_NORMAL_HEADING,
     GM_FIELD_FLOAT_MAX_FIELDS
 } StateFieldFloat;