Unify arc jerk limiting for S-curve planner

- Rename tcUpdateCircleAccRatio() to tcUpdateArcLimits() and extend to handle both TC_CIRCULAR and TC_SPHERICAL arcs
- Add comprehensive jerk-based velocity limiting for S-curve planner (planner_type 1) with three constraints:
  * Steady-state rotational jerk (v³/R²) with entry/exit transition budget
  * Normal jerk from tangential acceleration coupling (3·v·a_t/R)
  * Entry/exit transition jerk at arc boundaries
- Apply jerk constraints to blend arcs in blendComputeParameters()
- Remove obsolete pmCircleActualMaxVel() function and PmCircleLimits struct
- Consolidate all arc limit calculations (acceleration and jerk) in unified tcUpdateArcLimits()
- Add debug output for jerk limiting analysis

This ensures consistent jerk limiting for both programmed arcs (G2/G3) and blend arcs at segment corners.

Author: Luca Toniolo

src/emc/tp/blendmath.c

@@ -1156,7 +1156,7 @@ int blendComputeParameters(BlendParameters * const param)
 
     // Find maximum velocity allowed by accel and radius
     double v_normal;
-    
+
     if(GET_TRAJ_PLANNER_TYPE() == 1){
         v_normal = findSCurveVPeak(param->a_n_max, emcmotStatus->jerk, R_geom);
     }else{
@@ -1179,7 +1179,38 @@ int blendComputeParameters(BlendParameters * const param)
     double s_blend = t_blend * param->v_plan;
     double R_blend = fmin(s_blend / param->phi, R_geom);   //Clamp by limiting radius
 
-    param->R_plan = fmax(pmSq(param->v_plan) / param->a_n_max, R_blend);
+    // Calculate minimum radius needed to keep jerk within limits at v_plan
+    // For circular motion: j = v³/R² (worst case at corner transitions)
+    // Therefore: R_min = v^(3/2) / sqrt(j_max)
+    double R_jerk_min = 0.0;
+    if (GET_TRAJ_PLANNER_TYPE() == 1 && emcmotStatus->jerk > TP_POS_EPSILON) {
+        // R_min = v^(3/2) / sqrt(j)
+        double v_32 = pmSqrt(param->v_plan) * param->v_plan;  // v^(3/2)
+        double j_sqrt = pmSqrt(emcmotStatus->jerk);
+        R_jerk_min = v_32 / j_sqrt;
+
+        tp_debug_print("R_jerk_min = %f (for v=%f, j=%f)\n",
+                       R_jerk_min, param->v_plan, emcmotStatus->jerk);
+    }
+
+    // Calculate radius from acceleration constraint
+    double R_accel = pmSq(param->v_plan) / param->a_n_max;
+
+    // Apply jerk constraint - use the larger of R_blend and R_jerk_min
+    double R_min = fmax(R_blend, R_jerk_min);
+
+    // Final radius must satisfy both acceleration and minimum (blend/jerk) constraints
+    param->R_plan = fmax(R_accel, R_min);
+
+    // Calculate arc length
+    param->s_arc = param->R_plan * param->phi;
+
+    tp_debug_print("R_accel=%f, R_blend=%f, R_jerk_min=%f, R_plan=%f\n",
+                   R_accel, R_blend, R_jerk_min, param->R_plan);
+
+    // Note: Velocity jerk limiting for blend arcs is now handled uniformly
+    // in tcUpdateArcLimits() during segment finalization, along with TC_CIRCULAR arcs.
+
     param->d_plan = param->R_plan / tan(param->theta);
 
     tp_debug_print("v_plan = %f\n", param->v_plan);
@@ -1662,43 +1693,6 @@ double pmCartAbsMax(PmCartesian const * const v)
 }
 
 
-PmCircleLimits pmCircleActualMaxVel(PmCircle const * circle,
-        double v_max,
-        double a_max)
-{
-    double a_n_max_cutoff = BLEND_ACC_RATIO_NORMAL * a_max;
-
-    double eff_radius = pmCircleEffectiveMinRadius(circle);
-    // Find the acceleration necessary to reach the maximum velocity
-    double a_n_vmax = pmSq(v_max) / fmax(eff_radius, DOUBLE_FUZZ);
-    // Find the maximum velocity that still obeys our desired tangential / total acceleration ratio
-    double v_max_cutoff = pmSqrt(a_n_max_cutoff * eff_radius);
-
-    double v_max_actual = v_max;
-    double acc_ratio_tan = BLEND_ACC_RATIO_TANGENTIAL;
-
-    if (a_n_vmax > a_n_max_cutoff) {
-        v_max_actual = v_max_cutoff;
-    } else {
-        acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n_vmax / a_max));
-    }
-
-    tp_debug_json_start(pmCircleActualMaxVel);
-    tp_debug_json_double(eff_radius);
-    tp_debug_json_double(v_max);
-    tp_debug_json_double(v_max_cutoff);
-    tp_debug_json_double(a_n_max_cutoff);
-    tp_debug_json_double(a_n_vmax);
-    tp_debug_json_double(acc_ratio_tan);
-    tp_debug_json_end();
-
-    PmCircleLimits limits = {
-        v_max_actual,
-        acc_ratio_tan
-    };
-
-    return limits;
-}
 
 
 /** @section spiralfuncs Functions to approximate spiral arc length */

src/emc/tp/blendmath.h

@@ -243,15 +243,6 @@ int blendPoints3Print(BlendPoints3 const * const points);
 
 double pmCartAbsMax(PmCartesian const * const v);
 
-typedef struct {
-    double v_max;
-    double acc_ratio;
-} PmCircleLimits;
-
-PmCircleLimits pmCircleActualMaxVel(const PmCircle *circle,
-        double v_max_nominal,
-        double a_max_nominal);
-
 int findSpiralArcLengthFit(PmCircle const * const circle,
         SpiralArcLengthFit * const fit);
 int pmCircleAngleFromProgress(PmCircle const * const circle,

src/emc/tp/tc.c

@@ -23,10 +23,18 @@
 #include "tp_types.h"
 #include "spherical_arc.h"
 #include "motion_types.h"
+#include "motion.h"
 
 //Debug output
 #include "tp_debug.h"
 
+// For jerk-limited arc velocity (planner_type 1)
+extern emcmot_status_t *emcmotStatus;
+
+#ifndef GET_TRAJ_PLANNER_TYPE
+#define GET_TRAJ_PLANNER_TYPE() (emcmotStatus->planner_type)
+#endif
+
 
 double tcGetMaxTargetVel(TC_STRUCT const * const tc,
         double max_scale)
@@ -748,18 +756,111 @@ double pmCircle9Target(PmCircle9 const * const circ9)
     return helical_length;
 }
 
-int tcUpdateCircleAccRatio(TC_STRUCT * tc)
+/**
+ * Apply acceleration and jerk limits to circular/spherical arc segments.
+ *
+ * For any arc (TC_CIRCULAR or TC_SPHERICAL), this function:
+ * 1. Limits velocity based on centripetal acceleration budget
+ * 2. For planner_type 1 (S-curve), applies three jerk constraints:
+ *    - Steady-state rotational jerk: v³/R²
+ *    - Normal jerk from tangential acceleration coupling: 3·v·a_t/R
+ *    - Entry/exit transition jerk at arc boundaries
+ * 3. Calculates the tangential acceleration ratio for the arc
+ *
+ * This unified approach ensures consistent jerk limiting for both
+ * programmed arcs (G2/G3) and blend arcs at segment corners.
+ */
+int tcUpdateArcLimits(TC_STRUCT * tc)
 {
-    if (tc->motion_type == TC_CIRCULAR) {
-        PmCircleLimits limits = pmCircleActualMaxVel(&tc->coords.circle.xyz,
-                             tc->maxvel,
-                             tcGetOverallMaxAccel(tc));
-        tc->maxvel = limits.v_max;
-        tc->acc_ratio_tan = limits.acc_ratio;
-        return 0;
+    double radius, angle;
+
+    // Extract radius and angle based on motion type
+    switch (tc->motion_type) {
+        case TC_CIRCULAR:
+            radius = pmCircleEffectiveMinRadius(&tc->coords.circle.xyz);
+            angle = tc->coords.circle.xyz.angle;
+            break;
+        case TC_SPHERICAL:
+            radius = tc->coords.arc.xyz.radius;
+            angle = tc->coords.arc.xyz.angle;
+            break;
+        default:
+            return 1; // Not an arc, nothing to do
+    }
+
+    if (radius < DOUBLE_FUZZ || angle < TP_ANGLE_EPSILON) {
+        return 1; // Degenerate arc
     }
-    // TODO handle blend arc here too?
-    return 1; //nothing to do, but not an error
+
+    double a_max = tcGetOverallMaxAccel(tc);
+    double a_n_max_cutoff = BLEND_ACC_RATIO_NORMAL * a_max;
+
+    // Find the acceleration necessary to reach the maximum velocity
+    double a_n_vmax = pmSq(tc->maxvel) / radius;
+
+    // Find the maximum velocity that still obeys our desired normal/total acceleration ratio
+    double v_max_cutoff = pmSqrt(a_n_max_cutoff * radius);
+
+    double v_max_actual = tc->maxvel;
+    double acc_ratio_tan = BLEND_ACC_RATIO_TANGENTIAL;
+
+    if (a_n_vmax > a_n_max_cutoff) {
+        v_max_actual = v_max_cutoff;
+    } else {
+        acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n_vmax / a_max));
+    }
+
+    // Jerk-based velocity limiting for S-curve planner (planner_type 1)
+    if (GET_TRAJ_PLANNER_TYPE() == 1 && emcmotStatus->jerk > TP_POS_EPSILON &&
+        tc->cycle_time > TP_TIME_EPSILON) {
+
+        double jerk = emcmotStatus->jerk;
+        double R_sq = pmSq(radius);
+
+        // Constraint 1: Steady-state rotational jerk + entry/exit transitions
+        // The jerk budget is shared between steady-state (v³/R²) and transitions.
+        // Solving: v³ ≤ R² × j × φ / (2 + φ)
+        // The (2 + φ) term: 2 for two transitions, φ for steady-state budget
+        double v_max_jerk_steady = cbrt(R_sq * jerk * angle / (2.0 + angle));
+
+        // Constraint 2: Normal jerk from tangential acceleration coupling
+        // During S-curve ramps on arc: j_n = 3·v·a_t/R
+        // Using BLEND_ACC_RATIO_TANGENTIAL as max tangential accel ratio
+        double a_t_max = BLEND_ACC_RATIO_TANGENTIAL * a_max;
+        double v_max_jerk_tan = jerk * radius / (3.0 * a_t_max);
+
+        // Constraint 3: Entry/exit transition jerk (centripetal accel ramp)
+        // At line-arc boundary, centripetal accel changes from 0 to v²/R
+        // j_entry = (v²/R) / cycle_time ≤ j_max
+        double v_max_jerk_entry = pmSqrt(jerk * radius * tc->cycle_time);
+
+        double v_max_jerk = fmin(fmin(v_max_jerk_steady, v_max_jerk_tan), v_max_jerk_entry);
+
+        tp_debug_print("tcUpdateArcLimits: type=%d R=%f phi=%f j=%f\n",
+                       tc->motion_type, radius, angle, jerk);
+        tp_debug_print("  v_jerk: steady=%f tan=%f entry=%f => min=%f\n",
+                       v_max_jerk_steady, v_max_jerk_tan, v_max_jerk_entry, v_max_jerk);
+
+        if (v_max_jerk < v_max_actual) {
+            tp_debug_print("  Limiting v_max from %f to %f for jerk\n",
+                           v_max_actual, v_max_jerk);
+            v_max_actual = v_max_jerk;
+
+            // Recalculate acc_ratio_tan for jerk-limited velocity
+            double a_n_at_jerk_vel = pmSq(v_max_actual) / radius;
+            if (a_n_at_jerk_vel < a_max) {
+                acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n_at_jerk_vel / a_max));
+            }
+        }
+    }
+
+    tc->maxvel = v_max_actual;
+    tc->acc_ratio_tan = acc_ratio_tan;
+
+    tp_debug_print("tcUpdateArcLimits: final v_max=%f acc_ratio_tan=%f\n",
+                   tc->maxvel, tc->acc_ratio_tan);
+
+    return 0;
 }
 
 /**
@@ -785,7 +886,7 @@ int tcFinalizeLength(TC_STRUCT * const tc)
 
     tcClampVelocityByLength(tc);
 
-    tcUpdateCircleAccRatio(tc);
+    tcUpdateArcLimits(tc);
 
     tc->finalized = 1;
     return TP_ERR_OK;

src/emc/tp/tc.h

@@ -105,7 +105,7 @@ int tcSetupMotion(TC_STRUCT * const tc,
 
 int tcSetupState(TC_STRUCT * const tc, TP_STRUCT const * const tp);
 
-int tcUpdateCircleAccRatio(TC_STRUCT * tc);
+int tcUpdateArcLimits(TC_STRUCT * tc);
 
 int tcFinalizeLength(TC_STRUCT * const tc);
 

src/emc/tp/tp.c

@@ -2258,6 +2258,9 @@ int tpAddCircle(TP_STRUCT * const tp,
     //Reduce max velocity to match sample rate
     tcClampVelocityByLength(&tc);
 
+    // Apply acceleration and jerk limits for circular motion
+    tcUpdateArcLimits(&tc);
+
     TC_STRUCT *prev_tc;
     prev_tc = tcqLast(&tp->queue);