Add headings and other geometric pins in motion using State_tags

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

@@ -263,6 +263,49 @@ 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.
+
+* `motion.interp.arc-center-y` (float, out) +
+  The absolute Y-coordinate of the center point for the current arc.
+
+* `motion.interp.normal-heading` (float, out) +
+  the heading from the current point back to the arc circlefor the current arc.
+
+ * `motion.interp.iscircle` (bit, out) +
+  True if the current arc is a full circle.
+
+[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,15 @@ 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_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

@@ -26,14 +26,15 @@
 #include <posemath.h>
 #include <kinematics.h>  //for kinematicsSwitchable()
 #include <motion_types.h>
-
 #include "../tp/tp.h"
 #include "simple_tp.h"
 #include "motion.h"
 #include "mot_priv.h"
 #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,20 @@ 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;
 
+    /* Update the HAL Output Pins from the active tag  */
+    /* 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_straight_heading) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_STRAIGHT_HEADING];
+
+    /* 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:
@@ -2033,10 +2048,8 @@ static void output_to_hal(void)
 	/* point to joint struct */
 	joint = &joints[joint_num];
 	joint_data = &(emcmot_hal_data->joint[joint_num]);
-
 	/* apply backlash and motor offset to output */
-	joint->motor_pos_cmd =
-	    joint->pos_cmd + joint->backlash_filt + joint->motor_offset;
+	joint->motor_pos_cmd = joint->pos_cmd + joint->backlash_filt + joint->motor_offset;
 	/* point to HAL data */
 	/* write to HAL pins */
 	*(joint_data->motor_offset) = joint->motor_offset;
@@ -2131,6 +2144,9 @@ static void update_status(void)
 		// and the state machine is still active. The homing status deassertion
 		// must be delayed until the state machine is done.
 		joint_status->homing = get_homing(joint_num);
+    /* check to see if we should pause in order to implement
+       single emcmotStatus->stepping */
+
 	}
 	joint_status->homed  = get_homed(joint_num);
 	joint_status->pos_cmd = joint->pos_cmd;
@@ -2211,6 +2227,78 @@ 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 */
+	if (emcmot_hal_data) {			
+		// Line and Motion Type
+		if (emcmot_hal_data->interp_line_number) {
+			*(emcmot_hal_data->interp_line_number) = (int)emcmotStatus->tag.fields[GM_FIELD_LINE_NUMBER];
+		}
+
+		// Performance Metadata
+		if (emcmot_hal_data->interp_feedrate) {
+			*(emcmot_hal_data->interp_feedrate) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_FEED];
+		}
+
+		// Geometric Metadata
+
+		if (emcmot_hal_data->interp_arc_radius) {
+			*(emcmot_hal_data->interp_arc_radius) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_RADIUS];
+		}
+		if (emcmot_hal_data->interp_arc_center_x) {
+			*(emcmot_hal_data->interp_arc_center_x) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_X];
+		}
+		if (emcmot_hal_data->interp_arc_center_y) {
+			*(emcmot_hal_data->interp_arc_center_y) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_Y];
+		}		
+		// 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: STATIC HEADING --- */
+			// For linear moves, the heading doesn't change during the segment.
+			if (emcmot_hal_data->interp_straight_heading) {
+				*(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 --- */
+			double cx = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_X];
+			double cy = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_Y];
+			*(emcmot_hal_data->iscircle) = (hal_bit_t)((emcmotStatus->tag.packed_flags & (1UL << GM_FLAG_IS_CIRCLE)) != 0);
+			// Current RT feedback position
+			double dx = emcmotStatus->carte_pos_fb.tran.x - cx;
+			double dy = emcmotStatus->carte_pos_fb.tran.y - cy;
+			double angle_rad = atan2(dy, dx);
+			// Normal heading is tool-to-centre (opposite of radial)
+			double normal_heading_deg = (angle_rad * (180.0 / M_PI)) + 180.0;
+			while (normal_heading_deg < 0) normal_heading_deg += 360.0;
+			while (normal_heading_deg >= 360.0) normal_heading_deg -= 360.0;
+			*(emcmot_hal_data->interp_normal_heading) = normal_heading_deg;
+
+			double heading_deg;
+
+			// G3 (CCW): Heading is Radial Angle + 90 degrees
+			if (motion_type == 30) {
+				heading_deg = (angle_rad + (M_PI / 2.0)) * (180.0 / M_PI);
+			}
+			// G2 (CW): Heading is Radial Angle - 90 degrees
+			else {
+				heading_deg = (angle_rad - (M_PI / 2.0)) * (180.0 / M_PI);
+			}
+			// 0-360 Normalization
+			while (heading_deg < 0) heading_deg += 360.0;
+			while (heading_deg >= 360.0) heading_deg -= 360.0;
+
+			// Now assign the calculated value
+			if (emcmot_hal_data->interp_straight_heading)
+				*(emcmot_hal_data->interp_straight_heading) = heading_deg;		
+			// Performance Metadata
+			if (emcmot_hal_data->interp_feedrate) {
+				*(emcmot_hal_data->interp_feedrate) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_FEED];
+			}
+		}
+	}
 #ifdef WATCH_FLAGS
     /*! \todo FIXME - this is for debugging */
     if ( old_motion_flag != emcmotStatus->motionFlag ) {

src/emc/motion/mot_priv.h

@@ -199,6 +199,18 @@ 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_straight_heading;
+    hal_float_t *interp_normal_heading;  
+    hal_bit_t   *iscircle;
 } emcmot_hal_data_t;
 
 /***********************************************************************

src/emc/motion/motion.c

@@ -551,6 +551,21 @@ 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_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"));
+  //CALL_CHECK(hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->jog_is_active), mot_comp_id, "motion.jog-is-active"));
+
+
     /* export debug parameters */
     /* these can be used to view any internal variable, simply change a line
        in control.c:output_to_hal() and recompile */
@@ -563,6 +578,7 @@ static int init_hal_io(void)
     CALL_CHECK(hal_param_s32_newf(HAL_RO, &(emcmot_hal_data->debug_s32_0), mot_comp_id, "motion.debug-s32-0"));
     CALL_CHECK(hal_param_s32_newf(HAL_RO, &(emcmot_hal_data->debug_s32_1), mot_comp_id, "motion.debug-s32-1"));
 
+
     // FIXME - debug only, remove later
     // export HAL parameters for some trajectory planner internal variables
     // so they can be scoped

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 MAX_FIELDS GM_FIELD_FLOAT_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_FEEDRATE,
+    GM_FIELD_FLOAT_ARC_RADIUS,
+    GM_FIELD_FLOAT_ARC_CENTER_X,
+    GM_FIELD_FLOAT_ARC_CENTER_Y,
+    GM_FIELD_FLOAT_STRAIGHT_HEADING,
+    GM_FIELD_FLOAT_NORMAL_HEADING,
     GM_FIELD_FLOAT_MAX_FIELDS
 } StateFieldFloat;
 
@@ -118,7 +125,9 @@ struct state_tag_t {
     float fields_float[GM_FIELD_FLOAT_MAX_FIELDS];
 
     // Any G / M code states that doesn't pack nicely into a single bit
-    // These are an array mostly because it's easier to pass an
+    // These are an array mostly because In function ‘output_to_hal’,
+
+    //it's easier to pass an
     // arbitrary-length array through NML than individual fields
     int fields[GM_FIELD_MAX_FIELDS];