Add accurate S-curve jerk output for joint motion commands

Add infrastructure to output precise jerk values from the trajectory planner
to joint commands during S-curve (planner_type=1) coordinated motion.

Previously, joint jerk values were derived from cubic interpolation, which
introduces numerical delays and inaccuracies. This change passes the exact
path jerk and direction vector from the TP through emcmotStatus, allowing
control.c to compute per-joint jerk as: joint_jerk = path_jerk * direction.

Changes:
- motion.h: Add current_acc, current_jerk, current_dir fields to emcmot_status_t
- tc.c/tc.h: Add tcGetCurrentTangentUnitVector() for direction at current progress
- tp.c: Output S-curve motion state in tpUpdateMovementStatus()
- control.c: Override joint jerk values for XYZ joints in S-curve mode

This improves jerk accuracy for Cartesian machines using S-curve planning,
which is important for servo tuning and motion smoothness analysis.

ABC-UVW to come in future

Author: Luca Toniolo

src/emc/motion/control.c

@@ -1396,6 +1396,24 @@ static void get_pos_cmds(long period)
 	    /* interpolate to get new position and velocity */
 		joint->pos_cmd = cubicInterpolate(&(joint->cubic), 0, &(joint->vel_cmd), &(joint->acc_cmd),  &(joint->jerk_cmd));
 	}
+
+	/* Use accurate jerk values from TP output (for Cartesian machines only)
+	 * For standard XYZ machines, joint[0-2] correspond to X, Y, Z axes
+	 * TP outputs: current_jerk (path jerk) and current_dir (direction unit vector)
+	 * Per-axis jerk = path_jerk * direction_component
+	 */
+	if (emcmotStatus->planner_type == 1) {
+	    // S-curve mode: use accurate jerk values
+	    double path_jerk = emcmotStatus->current_jerk;
+	    PmCartesian dir = emcmotStatus->current_dir;
+
+	    // For the first 3 joints (assuming X, Y, Z), use accurate jerk
+	    if (NO_OF_KINS_JOINTS >= 1) joints[0].jerk_cmd = path_jerk * dir.x;
+	    if (NO_OF_KINS_JOINTS >= 2) joints[1].jerk_cmd = path_jerk * dir.y;
+	    if (NO_OF_KINS_JOINTS >= 3) joints[2].jerk_cmd = path_jerk * dir.z;
+	    // Rotary axes (A, B, C) keep the cubic interpolator values for now
+	}
+
 	/* report motion status */
 	SET_MOTION_INPOS_FLAG(0);
 	if (tpIsDone(&emcmotInternal->coord_tp)) {

src/emc/motion/motion.h

@@ -648,6 +648,14 @@ Suggestion: Split this in to an Error and a Status flag register..
 	double current_vel;
 	double requested_vel;
 
+	/* S-curve motion state - for accurate jerk output */
+	double current_acc;     /* current path acceleration */
+	double current_jerk;    /* current path jerk (accurate value from TP) */
+	double decel_dist;      /* S-curve deceleration distance (dlen1) for debugging */
+	double tc_finalvel;     /* S-curve segment final velocity for debugging */
+	double tc_maxaccel;     /* S-curve segment max tangential accel for debugging */
+	PmCartesian current_dir; /* current motion direction unit vector */
+
 	unsigned int tcqlen;
 	EmcPose tool_offset;
 	int atspeed_next_feed;  /* at next feed move, wait for spindle to be at speed  */

src/emc/tp/tc.c

@@ -326,7 +326,35 @@ int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou
     return 0;
 }
 
+/**
+ * Calculate the unit tangent vector at the current progress of a move.
+ * For linear moves, this is constant. For circular moves, it varies with progress.
+ */
+int tcGetCurrentTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) {
 
+    switch (tc->motion_type) {
+        case TC_LINEAR:
+            *out = tc->coords.line.xyz.uVec;
+            break;
+        case TC_RIGIDTAP:
+            *out = tc->coords.rigidtap.xyz.uVec;
+            break;
+        case TC_CIRCULAR:
+            {
+                // Calculate current angle based on progress
+                double current_angle = 0.0;
+                if (tc->target > 0.0) {
+                    current_angle = (tc->progress / tc->target) * tc->coords.circle.xyz.angle;
+                }
+                pmCircleTangentVector(&tc->coords.circle.xyz, current_angle, out);
+            }
+            break;
+        default:
+            rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n", tc->motion_type);
+            return -1;
+    }
+    return 0;
+}
 
 /**
  * Calculate the distance left in the trajectory segment in the indicated

src/emc/tp/tc.h

@@ -39,6 +39,7 @@ int tcGetEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out)
 int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out);
 int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out);
 int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out);
+int tcGetCurrentTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out);
 
 double tcGetDistanceToGo(TC_STRUCT const * const tc, int direction);
 double tcGetTarget(TC_STRUCT const * const tc, int direction);

src/emc/tp/tp.c

@@ -2980,6 +2980,13 @@ STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const
         emcmotStatus->current_vel = 0;
         emcmotStatus->spindleSync = 0;
 
+        // Clear S-curve motion state
+        emcmotStatus->current_acc = 0;
+        emcmotStatus->current_jerk = 0;
+        emcmotStatus->current_dir.x = 0;
+        emcmotStatus->current_dir.y = 0;
+        emcmotStatus->current_dir.z = 0;
+
         emcPoseZero(&emcmotStatus->dtg);
 
         tp->motionType = 0;
@@ -3001,6 +3008,21 @@ STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const
     emcmotStatus->requested_vel = tc->reqvel;
     emcmotStatus->current_vel = tc->currentvel;
 
+    // Output accurate S-curve motion state (for accurate jerk calculation)
+    emcmotStatus->current_acc = tc->currentacc;
+    emcmotStatus->current_jerk = tc->currentjerk;
+
+    // Get current motion direction unit vector (precise tangent at current progress)
+    PmCartesian dir;
+    if (tcGetCurrentTangentUnitVector(tc, &dir) == 0) {
+        emcmotStatus->current_dir = dir;
+    } else {
+        // If direction unavailable, use zero vector
+        emcmotStatus->current_dir.x = 0;
+        emcmotStatus->current_dir.y = 0;
+        emcmotStatus->current_dir.z = 0;
+    }
+
     emcPoseSub(&tc_pos, &tp->currentPos, &emcmotStatus->dtg);
     return TP_ERR_OK;
 }