diff --git a/configs/sim/axis/axis_9axis_scurve.ini b/configs/sim/axis/axis_9axis_scurve.ini
index c91ab4a7165..a6abf8106d6 100644
--- a/configs/sim/axis/axis_9axis_scurve.ini
+++ b/configs/sim/axis/axis_9axis_scurve.ini
@@ -54,7 +54,6 @@ HALFILE = axis_9axis_scurve.hal
 # PLANNER_TYPE: Select the acceleration profile shape
 #   0 = Trapezoidal (traditional - constant acceleration)
 #   1 = S-curve (smoother - limited jerk/derivative of acceleration)
-#   2 = S-curve (smoother - but sharp in corners - to be deprecated)
 # Default: 0 (trapezoidal)
 #
 # NOTE: This parameter can be changed at RUNTIME via HAL pin:
diff --git a/configs/sim/axis/axis_mm_scurve.ini b/configs/sim/axis/axis_mm_scurve.ini
index ca58a2f469f..035f650de52 100644
--- a/configs/sim/axis/axis_mm_scurve.ini
+++ b/configs/sim/axis/axis_mm_scurve.ini
@@ -54,7 +54,6 @@ HALFILE = axis_mm_scurve.hal
 # PLANNER_TYPE: Select the acceleration profile shape
 #   0 = Trapezoidal (traditional - constant acceleration)
 #   1 = S-curve (smoother - limited jerk/derivative of acceleration)
-#   2 = S-curve (smoother - but sharp in corners - to be deprecated)
 # Default: 0 (trapezoidal)
 #
 # NOTE: This parameter can be changed at RUNTIME via HAL pin:
diff --git a/src/emc/ini/inihal.cc b/src/emc/ini/inihal.cc
index 93ebf6ceb9b..4825bb08bac 100644
--- a/src/emc/ini/inihal.cc
+++ b/src/emc/ini/inihal.cc
@@ -297,7 +297,12 @@ int check_ini_hal_items(int numjoints)
     if (CHANGED(traj_planner_type)) {
         if (debug) SHOW_CHANGE_INT(traj_planner_type)
         UPDATE(traj_planner_type);
-        if (0 != emcTrajPlannerType(NEW(traj_planner_type))) {
+        // Only 0 and 1 are supported, set to 0 if invalid
+        int planner_type = NEW(traj_planner_type);
+        if (planner_type != 0 && planner_type != 1) {
+            planner_type = 0;
+        }
+        if (0 != emcTrajPlannerType(planner_type)) {
             if (emc_debug & EMC_DEBUG_CONFIG) {
                 rcs_print("check_ini_hal_items:bad return value from emcTrajPlannerType\n");
             }
diff --git a/src/emc/ini/initraj.cc b/src/emc/ini/initraj.cc
index 76c5404f0e8..3cd2de52c2a 100644
--- a/src/emc/ini/initraj.cc
+++ b/src/emc/ini/initraj.cc
@@ -225,6 +225,10 @@ static int loadTraj(EmcIniFile *trajInifile)
         old_inihal_data.traj_default_jerk = jerk;
         planner_type = 0;  // Default: 0 = trapezoidal, 1 = S-curve
         trajInifile->Find(&planner_type, "PLANNER_TYPE", "TRAJ");
+        // Only 0 and 1 are supported, set to 0 if invalid
+        if (planner_type != 0 && planner_type != 1) {
+            planner_type = 0;
+        }
         if (0 != emcTrajPlannerType(planner_type)) {
             if (emc_debug & EMC_DEBUG_CONFIG) {
                 rcs_print("bad return value from emcTrajPlannerType\n");
diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c
index 9d4d30367e2..2d50b6db80f 100644
--- a/src/emc/motion/command.c
+++ b/src/emc/motion/command.c
@@ -1194,7 +1194,12 @@ void emcmotCommandHandler_locked(void *arg, long servo_period)
 		/* set the type of planner: 0 = trapezoidal, 1 = S-curve */
 		/* can do it at any time */
 		rtapi_print_msg(RTAPI_MSG_DBG, "SET_PLANNER_TYPE, type(%d)", emcmotCommand->planner_type);
-		emcmotStatus->planner_type = emcmotCommand->planner_type;
+		// Only 0 and 1 are supported, set to 0 if invalid
+		if (emcmotCommand->planner_type != 0 && emcmotCommand->planner_type != 1) {
+			emcmotStatus->planner_type = 0;
+		} else {
+			emcmotStatus->planner_type = emcmotCommand->planner_type;
+		}
 		break;
 				
 	case EMCMOT_PAUSE:
diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc
index f8c6cfb69ed..0df62f0d5e7 100644
--- a/src/emc/task/emccanon.cc
+++ b/src/emc/task/emccanon.cc
@@ -704,13 +704,16 @@ static double getStraightJerk(double x, double y, double z,
     }
 
     // Pure linear move:
+    // For jerk-limited motion: d = (1/6)*j*t³, so t = cbrt(6*d/j)
+    // We use t = cbrt(d/j) as a characteristic time (omitting the constant factor,
+    // which cancels out when we compute path jerk = dtot / tmax³)
     if (canon.cartesian_move && !canon.angular_move) {
-        tx = dx? (dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0;
-        ty = dy? (dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0;
-        tz = dz? (dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0;
-        tu = du? (du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0;
-        tv = dv? (dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0;
-        tw = dw? (dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0;
+        tx = dx? cbrt(dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0;
+        ty = dy? cbrt(dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0;
+        tz = dz? cbrt(dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0;
+        tu = du? cbrt(du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0;
+        tv = dv? cbrt(dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0;
+        tw = dw? cbrt(dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0;
             out.tmax = MAX3(tx, ty ,tz);
             out.tmax = MAX4(tu, tv, tw, out.tmax);
 
@@ -720,38 +723,38 @@ static double getStraightJerk(double x, double y, double z,
                 out.dtot = sqrt(du * du + dv * dv + dw * dw);
 
         if (out.tmax > 0.0) {
-            out.jerk = out.dtot / (out.tmax * out.tmax);
+            out.jerk = out.dtot / (out.tmax * out.tmax * out.tmax);
         }
     }
     // Pure angular move:
     else if (!canon.cartesian_move && canon.angular_move) {
-        ta = da? (da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0;
-        tb = db? (db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0;
-        tc = dc? (dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0;
+        ta = da? cbrt(da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0;
+        tb = db? cbrt(db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0;
+        tc = dc? cbrt(dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0;
             out.tmax = MAX3(ta, tb, tc);
 
         out.dtot = sqrt(da * da + db * db + dc * dc);
         if (out.tmax > 0.0) {
-            out.jerk = out.dtot / (out.tmax * out.tmax);
+            out.jerk = out.dtot / (out.tmax * out.tmax * out.tmax);
         }
     }
     // Combination angular and linear move:
     else if (canon.cartesian_move && canon.angular_move) {
-        tx = dx? (dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0;
-        ty = dy? (dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0;
-        tz = dz? (dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0;
-        ta = da? (da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0;
-        tb = db? (db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0;
-        tc = dc? (dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0;
-        tu = du? (du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0;
-        tv = dv? (dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0;
-        tw = dw? (dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0;
+        tx = dx? cbrt(dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0;
+        ty = dy? cbrt(dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0;
+        tz = dz? cbrt(dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0;
+        ta = da? cbrt(da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0;
+        tb = db? cbrt(db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0;
+        tc = dc? cbrt(dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0;
+        tu = du? cbrt(du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0;
+        tv = dv? cbrt(dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0;
+        tw = dw? cbrt(dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0;
             out.tmax = MAX9(tx, ty, tz,
                         ta, tb, tc,
                         tu, tv, tw);
 
         if(debug_velacc)
-            printf("getStraightJerk t^2 tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n",
+            printf("getStraightJerk t tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n",
                 tx, ty, tz, ta, tb, tc, tu, tv, tw);
 
         if(dx || dy || dz)
@@ -760,7 +763,7 @@ static double getStraightJerk(double x, double y, double z,
             out.dtot = sqrt(du * du + dv * dv + dw * dw);
 
         if (out.tmax > 0.0) {
-            out.jerk = out.dtot / (out.tmax * out.tmax);
+            out.jerk = out.dtot / (out.tmax * out.tmax * out.tmax);
         }
     }
     //if(debug_velacc)
diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c
index 06093a3bbac..3d7c6cdcee9 100644
--- a/src/emc/tp/blendmath.c
+++ b/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 */
diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h
index 8f2d699f5af..3d290ba1c1c 100644
--- a/src/emc/tp/blendmath.h
+++ b/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,
diff --git a/src/emc/tp/sp_scurve.c b/src/emc/tp/sp_scurve.c
index b7608b5c852..f2f1a7a3994 100644
--- a/src/emc/tp/sp_scurve.c
+++ b/src/emc/tp/sp_scurve.c
@@ -20,30 +20,6 @@
 #include <string.h>
 #endif
 
-unsigned getPhase(double v, double a, double j)  {
-    if (!v) return 0;
-    // Handle negative velocity
-    //double v = this->v;
-    //double a = this->a;
-    if (v < 0) {
-        v = -v;
-        a = -a;
-    }
-
-    if (0 < a) {
-    if (0 < j) return 1;
-    if (!j) return 2;
-    return 3;
-    }
-
-    if (!a) return 4;
-    if (j < 0) return 5;
-    if (!j) return 6;
-
-    return 7;
-}
-
-
 /*
 a cubic coefficient
 b quadratic coefficient
@@ -184,300 +160,6 @@ int solve_cubic(double a, double b, double c, double d, double res[3], int* len)
 }
 
 
-/*
- Continuous form
- PT = P0 + V0T + 1/2A0T2 + 1/6JT3
- VT = PT' = V0 + A0T + 1/2 JT2
- AT = VT' = PT'' = A0 + JT
-
- Discrete time form (let T be 1, then T^2 == 1, T^3 == 1)
- PT = PT + VT + 1/2AT + 1/6J
- VT = VT + AT + 1/2JT
- AT = AT + JT
- */
-int getTargetV(double distence, double v, double a, double period, double maxV, double maxA, double maxJ, double* req_v, double* req_a){
-  if(distence == 0) return 0;
-  double T1 = maxA / maxJ;
-  double V1 = maxJ * T1 * T1 / 2;
-  int phase = 0;
-  //double tT = 0;
-  
-  if(V1 >= maxV / 2){ // Handle case where jerk is small, causing T2 and T6 segments to not exist
-    T1 = sqrt(maxV / maxJ);
-  }
-
-  double S1 = maxJ * T1 * T1 * T1 / 6.0;
-  double S2 = S1;
-  double S3 = S2;
-  double V2 = V1;
-  double T2 = 0;
-  double T3 = 0;
-  double needTime = 0;
-
-  if (distence < 0) {   // Due to symmetry, only consider one case
-    distence = -distence;
-    v = -v;
-    a = -a;
-  }
-  
-  double calc_v = 0;
-  double calc_a = a;
-
-  if(distence <= S1){  
-    double t1;
-    //rtai
-    t1 = pow(6.0 * distence / maxJ, 1.0/3.0);
-    //uspace
-    //t1 = cbrt(6.0 * distence / maxJ) ;
-
-
-    //if(t1 -period > 0)
-    //  t1 = t1 - period; 
-    
-
-    calc_a = maxJ * t1;
-    calc_v = (calc_a * t1) / 2;
-    //tT = t1;
-    
-    phase = 1; 
-
-    //double p = calc_v * period;
-    //double cp = calc_v * t1 + maxJ * t1 * t1 * t1 / 6;
-    //double cpNext = calc_v * (t1 - period) + maxJ * (t1 - period) * (t1 - period) * (t1 - period) / 6;
-    //if(t1 - period > 0)
-    //  calc_v = (distence - cpNext) / period;
-    needTime = t1;
-    //printf("S1 | D: %.10f | CP: %f | CNP: %f | P: %f | T: %f | J: %f | V: %f | calc_a: %f | RV: %f | t1: %f \n",  distence, cp, cpNext, p, period, maxJ, calc_v, calc_a, v, t1);
-  }
-
-  if(phase == 0){
-    if(V1 < maxV / 2){ // T2 and T6 segments exist
-      T2 = (maxV / maxA) - T1;
-      S2 = S1 + V1 * T2 + maxJ * T1 * T2 * T2 / 2;
-      V2 = V1 + maxA * T2;
-      if(distence < S2){
-        double t2;
-        double A = maxJ * T1 / 2;
-        double B = V1;
-        double C = S1 - distence;
-        double dt = sqrt(B * B - 4 * A * C);
-        t2 = (-B + dt) / (2 * A);
-        //printf("In S2 segment ");
-        //t2 = t2 - period;
-        //if(t2 < 0){
-          //calc_v = V1;
-        //  calc_a = maxJ * t2;
-        //  calc_v = (calc_a * t2) / 2;
-        //}else{
-          //Vt = Vs + J * T1^2 - 0.5 * J * (t - 2 * T1) ^2
-          calc_a = maxA;
-          calc_v = V1 + maxA * t2;
-        //}
-        //tT = t2;
-        phase = 2;
-
-        //double p = calc_v * period;
-        //printf("P: %f, Ov: %f Nv: %f Oa: %f, Na: %f ",  p * 1000, v, calc_v, a, calc_a);
-        
-        //double p = calc_v * period;
-        //t2 = t2 - period;
-        // S2 = S1 + V1 * T2 + maxJ * T1 * T2 * T2 / 2;
-        //double cp = S1 + V1 * t2 + 0.5 * maxJ * T1 * t2 * t2;
-        //double cpNext = S1 + V1 * (t2 - period) + 0.5 * maxJ * T1 * (t2 - period) *  (t2 - period);
-        needTime = T1 + t2;
-        //printf("S2 | D: %.10f | CP: %f | CNP: %f | P: %f | T: %f | J: %f | V: %f | calc_a: %f | RV: %f | t2: %f \n",  distence, cp, cpNext, p, period, maxJ, calc_v, calc_a, v, T1 + t2);
-      }
-   
-    }
-  }
-
-  if(phase == 0){
-    S3 = S2 + V2 * T1 + S1 * 2;
-    T3 = T1;
-    if(distence <= S3){ // Solving cubic equation is expensive 
-      double A = - maxJ / 6;
-      double B = maxJ * T1 / 2;
-      double C = V2;
-      double D = S2 - distence;
-      double t3 = 0;//solute(A,B,C,D,T2);
-      //double x1, x2, x3;
-      //int n;
-      //x1 = x2 = x3 = 0;
-      //n = solve_cubic(A, B, C, D, &x1, &x2, &x3);
-      //double temp;
-      double xo[10];
-      int len;
-      int i = 0;
-      solve_cubic(A, B, C, D, xo, &len);
-      if(len > 0){
-        t3 = xo[0];
-        for (; i < len; i++)
-        {
-          if(i == 0)continue;
-          t3 = fmax(xo[i], t3);
-        }
-      }
-      //t3 = fmax(fmax(xo[0], x2), x3);
-      if(t3 < 0){
-        t3 = 0.001;
-      }
-      //if(x1>x2) temp=x1,x1=x2,x2=temp;
-      //if(x2>x3) temp=x2,x2=x3,x3=temp;
-      //if(x1>x2) temp=x1,x1=x2,x2=temp;
-      //if(x1 > 0) t3 = x1;
-      //else if(x2 > 0) t3 = x2;
-      //else if(x3 > 0) t3 = x3;
-      //else t3 = 0.001;
-      //printf("S3 t3: %f n: %d x1: %f x2: %f x3: %f  A: %f B: %f C: %f D: %f\n" , t3, n, x1, x2, x3, A, B, C, D);
-      //JT1 - J ( t - T1 )
-      //t3 = t3 - period;
-      //if(t3 <= 0 ){
-      //  calc_a = maxA;
-      //  calc_v = V1 + maxA *  (T2 + t3);
-     // }else{
-        calc_v = V2 + maxA * t3 - 0.5 * maxJ * t3 * t3;
-        calc_a = maxA - maxJ * t3;
-      //}
-      //tT = t3;
-
-      //double p = calc_v * period;
-      //t3 = t3 - period;
-      //double cp = 0;
-      //if(t3 < 0){
-
-      //}else{
-      //cp = S2 + calc_v * t3 + 0.5 * maxJ * T1 * t3* t3 - maxJ * t3 * t3 * t3 / 6;
-      //double cpNext = S2 + calc_v * (t3 - period) + 0.5 * maxJ * T1 * (t3 - period) * (t3 - period) - maxJ * (t3 - period) * (t3 - period) * (t3 - period) / 6;
-         //(cp - distence) / period;
-      //}
-      //double rp =
-      //printf("P: %f, Ov: %f Nv: %f Oa: %f, Na: %f ",  p * 1000, v, calc_v, a, calc_a);
-      needTime = T1 + T2 + t3;
-      //printf("S3 | D: %.10f | CP: %f | CNP: %f | P: %f | T: %f | J: %f | T1: %f | V: %f | calc_a: %f | RV: %f | t3: %f \n",  distence, cp, cpNext, p, period, maxJ, T1, calc_v, calc_a, v, T1 + T2 + t3);
-      phase = 3;
-    }
-  }
-  //printf("In constant velocity phase ");
-  // Need to enter S3 early by one period
-
-  //*req_v = calc_v;
-  if(phase == 0)
-    phase = 4;
-
-  // PT = P0 + V0 * T + 0.5 * A0 * T^2 + J * T^3 / 6
-  // VT = V0 + A0 * T + J * T^2 /2
-  // AT = A0 + J * T
-  // After discretization:
-  // PT = PT + VT + 0.5 * AT + J / 6
-  // VT = VT + AT + 0.5 * J * T
-  // AT = AT + J * T
-
-  if(phase == 4){//
-    if(v < maxV){ // Need to accelerate to target speed
-      double tt = fabs(a / maxJ);
-      double ve = v + (a - maxJ * period) * tt - maxJ * tt * tt / 2;
-      // a * t + 1/2 * j * t^2
-      if(ve >=  maxV ){
-        phase = 5;
-        calc_a = a - maxJ * period;
-      }
-      else{
-        phase = 7;
-        calc_a = a + maxJ * period;
-      }
-
-      if(calc_a >= maxA){
-        calc_a = maxA;
-        phase = 6;
-      }
-      //double p = 0;
-      // VT = V0 + A0 * T + J * T^2 /2
-      // AT = A0 + J * T
-      if(phase == 7){
-        //p = v * period + 0.5 * calc_a * period * period + maxJ * period * period* period / 6;
-        calc_v = v + calc_a * period + maxJ * period * period / 2;
-      }
-      else if(phase == 6){
-        if(a != maxA){
-          double J = (maxA - a) / period;
-          calc_v = v + maxA * period + J * period * period / 2;
-        }else
-          calc_v = v + calc_a * period;
-      }
-      else if(phase == 5){ // Need compensation; after discretization each segment is s = v * t, so distance may be more reliable
-        // V0 * T + 0.5 * A0 * T^2 + J * T^3 / 6
-        //p = v * period + 0.5 * calc_a * period * period - maxJ * period * period* period / 6;
-        calc_v = v + calc_a * period - maxJ * period * period / 2;
-      }
-      
-      if(calc_v >= maxV){
-        calc_v = maxV;
-      }
-      //printf("VE: %f, OV: %f NV: %f P: %f OA: %f, NA: %f ", ve, v, calc_v, p * 1000, a, calc_a);
-    }else if(v > maxV){// Need to decelerate to target speed; triangular acceleration algorithm used here; entered when velocity is modified
-      double tt = fabs(a) / maxJ;
-      double ve = v + a * tt + maxJ * tt * tt / 2;
-      if(ve <= maxV) {
-        phase = 8;
-        calc_a = a + maxJ * period;
-      }
-      else{
-        phase = 9;
-        calc_a = a - maxJ * period;
-      }
-
-      if(calc_a < -maxA){
-        calc_a = -maxA;
-      }
-        
-      calc_v = v + calc_a * period - maxJ * period * period / 2;
-      if(calc_v < maxV){
-        calc_v = maxV;
-      }
-    }else{ // Constant velocity motion, forced
-      phase = 4;
-      calc_v = maxV;
-    }
-  }else{ // This section is intended for optimizing the stopping segment, very important, may be used to smooth the curve
-    //if(a)
-
-    if(needTime == 0){ // Constant velocity or acceleration phase
-      // Decelerate a few periods early
-    }else if(needTime > T1 + T2 && needTime <= T1 + T2 +T3){ // Handle S3 segment
-
-    }else if(needTime > T1 && needTime <= T1 + T2){ // Handle S2 segment
-
-    }else if(needTime >= 0 && needTime <= T1){ // Handle S1 segment
-      
-    }
-
-
-  }
-
-
-  if(calc_v > maxV)
-    calc_v = maxV;
-  *req_a = calc_a;
-  //*req_a = (calc_v - v) / period;
-
-#define MAX_A_OVERRIDE 1.5
-  double ra = (calc_v - v) / period;
-  if(ra > maxA * MAX_A_OVERRIDE){ // This is not an ideal solution
-    *req_a = maxA* MAX_A_OVERRIDE;
-    calc_v = v + *req_a * period;
-  }else if(ra < -maxA * MAX_A_OVERRIDE){
-    *req_a = -maxA * MAX_A_OVERRIDE;
-    calc_v = v + *req_a * period;
-  }
-#undef MAX_A_OVERRIDE
-
-  *req_v = calc_v;
-  //if(phase == 1 || phase == 2 || phase == 3)
-  //  printf("S%d L: %.10f, ACC %f V: %f *req_v:  %f v: %f S3: %f A: %f CA: %f\n", phase, distence , *req_a, calc_v, *req_v, v, S3, (calc_v - v) / period, ov);
-  return phase;
-}
-
-
 // PT = P0 + V0 * T + 0.5 * A0 * T^2 + J * T^3 / 6
 // VT = V0 + A0 * T + J * T^2 /2
 // AT = A0 + J * T
@@ -778,66 +460,6 @@ tp->prograss = 0;
 return 0;
 }
 
-double getNextPoint(simple_tp_t *tp, int n, double T, double* req_v, double* req_a){
-  double cal_v;
-  double cal_a;
-  int phase = 0;
-  if(n <= tp->n0){                                           // S0 segment
-    cal_v = tp->vc + tp->jm * T * T * n * (n + 1) / 2;
-    cal_a = n * tp->jm * T;
-    phase = 0;
-  }else if(tp->n1 != 0 && n <= tp->n0 + tp->n1){              // S1 segment
-    cal_v = tp->v1 + (n - tp->n0) * tp->a1 * T;
-    cal_a = tp->a1;
-    phase = 1;
-  }else if(n <= tp->n0 + tp->n1 + tp->n2){                    // S2 segment
-    //Vm - ∑(k * J1 * Tp^2)
-    double x = tp->n0 + tp->n1 + tp->n2 - n;
-    cal_v = tp->v3 - fabs(tp->j2) * T * T * x * (x + 1) / 2;
-    cal_a = tp->a3 - x * fabs(tp->j2) * T;
-    phase = 2;
-  }else if(n <= tp->n0 + tp->n1 + tp->n2 + tp->n3){           // S3 segment
-    cal_v = tp->vm;
-    cal_a = 0;
-    phase = 3;
-  }else if(n <= tp->n0 + tp->n1 + tp->n2 + tp->n3 + tp->n4){  // S4 segment
-    double x = n - (tp->n0 + tp->n1 + tp->n2 + tp->n3) ;
-    cal_v = tp->vm - tp->jm * T * T * x * (x + 1) / 2;
-    cal_a = - x * tp->jm * T;
-    phase = 4;
-  }else if(tp->n5 != 0  &&n <= tp->n0 + tp->n1 + tp->n2 + tp->n3 + tp->n4 + tp->n5){
-    phase = 5;
-    double x = n - (tp->n0 + tp->n1 + tp->n2 + tp->n3 + tp->n4);
-    cal_v = tp->v5 + x * tp->a6 * T;
-    cal_a = tp->a6;
-  }else if(n <= tp->n0 + tp->n1 + tp->n2 + tp->n3 + tp->n4 + tp->n5 + tp->n6){
-    phase = 6;
-    double x = tp->n0 + tp->n1 + tp->n2 + tp->n3 + tp->n4 + tp->n5 + tp->n6 - n;
-    cal_v = tp->v7 + fabs(tp->j4) * T * T *  x * (x + 1) / 2.0;
-    cal_a = tp->a6 - x * fabs(tp->j4) * T;
-  }else{
-    if( tp->n0 + tp->n1 + tp->n2 + tp->n3 + tp->n4 + tp->n5 + tp->n6 == n + 1 ){
-      tp->curr_n++ ;
-      phase = 7;
-      cal_v = tp->ve;
-      cal_a = tp->a7;
-    }
-    return 0;
-  }
-  if(phase > 3 && tp->fix_verr == 0 && cal_v < tp->verr){
-    tp->fix_verr = 1;  // Insert additional residual velocity
-    cal_v = tp->verr;
-    //printf("ver: %f | acc: %f | n: %d | phase: %d | prograss: %.10f F\n", cal_v, cal_a, n, phase, tp->prograss);
-  }else{
-    tp->curr_n++ ;
-    //printf("ver: %f | acc: %f | n: %d | phase: %d | prograss: %.10f TN: %d\n", cal_v, cal_a, n, phase, tp->prograss, tp->n0 + tp->n1 + tp->n2 + tp->n3 + tp->n4 + tp->n5 + tp->n6);
-  }
-  tp->prograss += cal_v * T;
-  *req_v = cal_v;
-  *req_a = cal_a;
-  return 0;
-}
-
 
 // PT = P0 + V0 * T + 0.5 * A0 * T^2 + J * T^3 / 6
 // VT = V0 + A0 * T + J * T^2 /2
@@ -895,15 +517,6 @@ double nextSpeed(double v, double a, double t, double targetV, double maxA, doub
   return v;
 }
 
-double getStoppingDist(simple_tp_t *tp) {
-    double maxJ = tp->max_jerk ;
-    double v = tp->curr_vel;
-    double a = tp->curr_acc;
-    double maxA = tp->max_acc;
-    //double phase;
-    return stoppingDist(v, a, maxA, maxJ/*, &phase*/);
-}
-
 double stoppingDist(double v, double a, double maxA, double maxJ/*, int* phase*/) {
     // Already stopped
     //*phase = 0;
@@ -924,7 +537,7 @@ double stoppingDist(double v, double a, double maxA, double maxJ/*, int* phase*/
       // velocity => a * t + 1/2 * j * t^2
       double t = a / maxJ;
       d += sc_distance(t, v, a, -maxJ);
-      v += velocity(t, a, -maxJ);
+      v += delta_velocity(t, a, -maxJ);
       a = 0;
       //if(*phase == 0) *phase = 3;
     }
@@ -955,7 +568,7 @@ double stoppingDist(double v, double a, double maxA, double maxJ/*, int* phase*/
     if (maxDeccel < a) {
         double t = (a - maxDeccel) / maxJ;
         d += sc_distance(t, v, a, -maxJ);
-        v += velocity(t, a, -maxJ);
+        v += delta_velocity(t, a, -maxJ);
         a = maxDeccel;
     }
 
@@ -971,7 +584,7 @@ double stoppingDist(double v, double a, double maxA, double maxJ/*, int* phase*/
       // velocity => a * t + 1/2 * j * t^2
         double t = (v - deltaV) / -a;
         d += sc_distance(t, v, a, 0);
-        v += velocity(t, a, 0);
+        v += delta_velocity(t, a, 0);
         //if(*phase == 0) *phase = 6;
     }
 
@@ -983,78 +596,105 @@ double stoppingDist(double v, double a, double maxA, double maxJ/*, int* phase*/
 }
 
 double finishWithSpeedDist(double v, double ve, double a, double maxA, double maxJ/*, int* phase*/) {
-    // Already stopped
-    //*phase = 0;
-    if (fabs(v) < 0.0001) return 0;
-    // Handle negative velocity
+    // Handle negative velocity: transform to positive domain
     if (v < 0) {
-    v = -v;
-    a = -a;
-    ve = -ve;
+        v = -v;
+        a = -a;
+        ve = -ve;
+    }
+
+    // Velocity difference too small, no accel/decel needed
+    if (fabs(v - ve) < 0.0001 && fabs(a) < 0.0001) {
+        return 0;
     }
 
     double d = 0;
 
-    //if(ve > v)
-    //  return 0;
-    if(ve > v){
-      
+    // ========== Acceleration case (ve > v) ==========
+    if (ve > v) {
+        // Phase 1: If a < 0 (decelerating), first bring a to 0
+        if (a < 0) {
+            double t = -a / maxJ;
+            d += sc_distance(t, v, a, maxJ);
+            v += delta_velocity(t, a, maxJ);
+            a = 0;
+        }
+
+        // Compute required max acceleration
+        // Amax^2 = (ve - v) * maxJ + 0.5 * a * a
+        double sqrt_arg = (ve - v) * maxJ + 0.5 * a * a;
+        if (sqrt_arg < 0) sqrt_arg = 0;
+        double maxAccel = sqrt(sqrt_arg);
+        if (maxAccel > maxA) maxAccel = maxA;
+
+        // Phase 2: Increase a from current value to maxAccel
+        if (maxAccel > a) {
+            double t = (maxAccel - a) / maxJ;
+            d += sc_distance(t, v, a, maxJ);
+            v += delta_velocity(t, a, maxJ);
+            a = maxAccel;
+        }
+
+        // Compute velocity when entering decel phase
+        double deltaV = ve - 0.5 * a * a / maxJ;
+
+        // Phase 3: Constant acceleration phase (if needed)
+        if (v < deltaV && a > 0.0001) {
+            double t = (deltaV - v) / a;
+            d += sc_distance(t, v, a, 0);
+            v += delta_velocity(t, a, 0);
+        }
+
+        // Phase 4: Decrease a from maxAccel to 0, velocity reaches ve
+        if (a > 0.0001) {
+            double t = a / maxJ;
+            d += sc_distance(t, v, a, -maxJ);
+        }
+
+        return d;
     }
 
-    //double vs = v;
-    // Compute distance and velocity change to accel = 0
-    if (0 < a) {
-      // Compute distance to decrease accel to zero
-      // distance(double t, double v, double a, double j)
-      // distance => v * t + 1/2 * a * t^2 + 1/6 * j * t^3
-      // velocity => a * t + 1/2 * j * t^2
-      double t = a / maxJ;
-      d += sc_distance(t, v, a, -maxJ);
-      v += velocity(t, a, -maxJ);
-      a = 0;
-      //if(*phase == 0) *phase = 3;
+    // ========== Deceleration case (v > ve) ==========
+
+    // Phase 1: If a > 0 (accelerating), first bring a to 0
+    if (a > 0) {
+        double t = a / maxJ;
+        d += sc_distance(t, v, a, -maxJ);
+        v += delta_velocity(t, a, -maxJ);
+        a = 0;
     }
 
-    // Compute max deccel
-    // PT = P0 + V0 * T + 0.5 * A0 * T^2 + J * T^3 / 6
-    // VT = V0 + A0 * T + J * T^2 /2
-    // AT = A0 + J * T
-    // 
-    // Amax^2 = (Vs - Ve) * maxJ + 0.5 * a * a
-    double maxDeccel = -sqrt((v - ve) * maxJ + 0.5 * a * a);
+    // Compute required max deceleration
+    // Amax^2 = (v - ve) * maxJ + 0.5 * a * a
+    double sqrt_arg = (v - ve) * maxJ + 0.5 * a * a;
+    if (sqrt_arg < 0) sqrt_arg = 0;
+    double maxDeccel = -sqrt(sqrt_arg);
     if (maxDeccel < -maxA) maxDeccel = -maxA;
-    //double maxDeccel = -fabs(maxA);
 
-    // Compute distance and velocity change to max deccel
-    // a * t + 1/2 * j * t^2
+    // Phase 2: Decrease a from current value to maxDeccel
     if (maxDeccel < a) {
         double t = (a - maxDeccel) / maxJ;
         d += sc_distance(t, v, a, -maxJ);
-        v += velocity(t, a, -maxJ);
+        v += delta_velocity(t, a, -maxJ);
         a = maxDeccel;
     }
 
-    // Compute velocity change over remaining accel
+    // Compute velocity when entering decel end phase
     // VT = Ve + Amax^2 / (2 * J)
     double deltaV = ve + 0.5 * a * a / maxJ;
 
-    // Compute constant deccel period
-    // P = v * t + 1/2 * a * t^2 + 1/6 * j * t^3
-    // VT = V0 + A0 * T + J * T^2 /2
-    if (deltaV < v) {
-      // distance => v * t + 1/2 * a * t^2 + 1/6 * j * t^3
-      // velocity => a * t + 1/2 * j * t^2
-        double t = (v - deltaV) / -a;
+    // Phase 3: Constant deceleration phase (if needed)
+    if (deltaV < v && a < -0.0001) {
+        double t = (v - deltaV) / (-a);
         d += sc_distance(t, v, a, 0);
-        v += velocity(t, a, 0);
-        //if(*phase == 0) *phase = 6;
+        v += delta_velocity(t, a, 0);
     }
 
-    // Compute distance to ve vel
-    // distance => v * t + 1/2 * a * t^2 + 1/6 * j * t^3
-    // A = J * T
-    d += sc_distance(-a / maxJ, v, a, maxJ);
-    //if(*phase == 0) *phase = 7;
+    // Phase 4: Increase a from maxDeccel to 0, velocity reaches ve
+    if (a < -0.0001) {
+        double t = (-a) / maxJ;
+        d += sc_distance(t, v, a, maxJ);
+    }
 
     return d;
 }
@@ -1066,7 +706,7 @@ double finishWithSpeedDist(double v, double ve, double a, double maxA, double ma
 double nextAccel(double t, double targetV, double v, double a, double maxA,
                         double maxJ) {
   int increasing = v < targetV;
-  double deltaA = acceleration(t, maxJ);
+  double deltaA = delta_accel(t, maxJ);
 
   if (increasing && a < -deltaA)
     return a + deltaA; // negative accel, increasing speed
@@ -1116,7 +756,7 @@ double nextAccel(double t, double targetV, double v, double a, double maxA,
 double nextAccel(double t, double targetV, double v, double a, double maxA,
                         double maxJ) {
   double max_da, tiny_da, vel_err, acc_req;
-  max_da = acceleration(t, maxJ);
+  max_da = delta_accel(t, maxJ);
   tiny_da = max_da * t * 0.001;
   vel_err = targetV - v;
   if (vel_err > tiny_da){
@@ -1146,18 +786,27 @@ double nextAccel(double t, double targetV, double v, double a, double maxA,
 }
 
 double sc_distance(double t, double v, double a, double j) {
-  // v * t + 1/2 * a * t^2 + 1/6 * j * t^3
+    // P = v*t + (1/2)*a*t² + (1/6)*j*t³
   return t * (v + t * (0.5 * a + 1.0 / 6.0 * j * t));
 }
 
+/**
+ * Trapezoidal integration for displacement calculation
+ * Uses average jerk assumption for more stable integration
+ */
+double trapz_distance(double t, double v, double a, double j) {
+  // v * t + 1/2 * a * t^2 + 1/4 * j * t^3
+  return t * (v + t * (0.5 * a + 0.25 * j * t));
+}
 
-double velocity(double t, double a, double j) {
+
+double delta_velocity(double t, double a, double j) {
   // a * t + 1/2 * j * t^2
   return t * (a + 0.5 * j * t);
 }
 
 
-double acceleration(double t, double j) {return j * t;}
+double delta_accel(double t, double j) {return j * t;}
 
 #include "tp_types.h"
 
@@ -1350,4 +999,4 @@ double calcSCurveSpeedWithT(double amax, double jerk, double T) {
 
     // Use fma to optimize multiply-add operations, improving numerical stability
     return fma(jerk * T1, T1 + T2, 0.0);
-}
+}
\ No newline at end of file
diff --git a/src/emc/tp/sp_scurve.h b/src/emc/tp/sp_scurve.h
index 73844f1ecb8..20187db1b8c 100644
--- a/src/emc/tp/sp_scurve.h
+++ b/src/emc/tp/sp_scurve.h
@@ -1,13 +1,3 @@
-/*
- * Author: 杨阳
- * Date: 2020-06-05 22:32:44
- * LastEditors: 杨阳
- * LastEditTime: 2020-07-25 10:47:58
- * Contact: mika-net@outlook.com
- * Copyright (c) 2017-2020: 杨阳
- * FilePath: \pomelo\PROGRAM\linuxcnc\src\emc\tp\sp_scurve.h
- * 
- */ 
 /********************************************************************
 * Description: sp_scurve.h
 *   Discriminate-based trajectory planning
@@ -15,6 +5,7 @@
 *   Derived from a work by 杨阳
 *
 * Author: 杨阳
+* Contact: mika-net@outlook.com
 * License: GPL Version 2
 * System: Linux
 *    
@@ -30,20 +21,28 @@
 
 double nextAccel(double t, double targetV, double v, double a, double maxA, double maxJ);
 double sc_distance(double t, double v, double a, double j);
-double velocity(double t, double a, double j);
-double acceleration(double t, double j);
-unsigned getPhase(double v, double a, double j);
+double trapz_distance(double t, double v, double a, double j);
+double delta_velocity(double t, double a, double j);
+double delta_accel(double t, double j);
 double nextSpeed(double v, double a, double t, double targetV, double maxA, double maxJ, double* req_v, double* req_a, double* req_j);
-double getStoppingDist(simple_tp_t *tp) ;
 double stoppingDist(double v, double a, double maxA, double maxJ) ;
 double finishWithSpeedDist(double v, double ve, double a, double maxA, double maxJ) ;
-int getTargetV(double distence, double v, double a, double period, double maxV, double maxA, double maxJ, double* req_v, double* req_a);
 
 int getNext( simple_tp_t *tp, double Vs, double Ve, double period);
-double getNextPoint(simple_tp_t *tp, int n, double T, double* req_v, double* req_a);
 int findSCurveVSpeed(double distence,/* double maxV, */double maxA, double maxJ, double *req_v);
 int findSCurveVSpeedWithEndSpeed(double distence, double Ve, double maxA, double maxJ, double* req_v);
 double calcDecelerateTimes(double v, double amax, double jerk, double* t1, double* t2);
 double calcSCurveSpeedWithT(double amax, double jerk, double T);
 
+/**
+ * tpCalculateSCurveAccel return value definitions
+ *
+ * TP_SCURVE_ACCEL_ERROR - calculation failed (maxjerk invalid or less than/equal to 1)
+ * TP_SCURVE_ACCEL_ACCEL - acceleration or normal state (no deceleration needed)
+ * TP_SCURVE_ACCEL_DECEL - deceleration needed
+ */
+#define TP_SCURVE_ACCEL_ERROR  -5
+#define TP_SCURVE_ACCEL_ACCEL   0
+#define TP_SCURVE_ACCEL_DECEL   1
+
 #endif
\ No newline at end of file
diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c
index 84a334ba6e1..022c4b9b605 100644
--- a/src/emc/tp/tc.c
+++ b/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;
diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h
index 6e43217ef0f..3c1afa6dde2 100644
--- a/src/emc/tp/tc.h
+++ b/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);
 
diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h
index 492cf253b70..90c62041cd7 100644
--- a/src/emc/tp/tc_types.h
+++ b/src/emc/tp/tc_types.h
@@ -141,6 +141,7 @@ typedef struct {
     //S-curve jerk limiting
     double maxjerk;         // max jerk for S-curve motion
     double currentacc;      // current acceleration for S-curve planning
+    double currentjerk;     // current jerk for S-curve planning
     double initialvel;      // initial velocity when segment activated
     int accel_phase;        // current phase of S-curve acceleration
     double elapsed_time;    // time elapsed since segment activation
diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c
index d326ef77f8a..b9a15ba357f 100644
--- a/src/emc/tp/tp.c
+++ b/src/emc/tp/tp.c
@@ -353,29 +353,6 @@ STATIC inline double tpGetRealFinalVel(TP_STRUCT const * const tp,
     return fmin(tc->finalvel, v_target);
 }
 
-/**
- * Get acceleration for a tc based on the trajectory planner state.
- */
-STATIC inline double tpGetScaledAccel(TP_STRUCT const * const tp __attribute__((unused)),
-        TC_STRUCT const * const tc) {
-    double a_scale = tc->maxaccel;
-    /* Parabolic blending conditions: If the next segment or previous segment
-     * has a parabolic blend with this one, acceleration is scaled down by 1/2
-     * so that the sum of the two does not exceed the maximum.
-     */
-    if (tc->term_cond == TC_TERM_COND_PARABOLIC || tc->blend_prev) {
-        a_scale *= 0.5;
-    }
-    else {
-        a_scale *= 8.0/15.0;
-    }
-    if (tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_SPHERICAL) {
-        //Limit acceleration for cirular arcs to allow for normal acceleration
-        a_scale *= tc->acc_ratio_tan;
-    }
-    return a_scale;
-}
-
 /**
  * Convert the 2-part spindle position and sign to a signed double.
  */
@@ -1764,11 +1741,20 @@ STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * cons
     double acc_this = tcGetTangentialMaxAccel(tc);
 
     // Find the reachable velocity of tc, moving backwards in time
-    double vs_back = pmSqrt(pmSq(tc->finalvel) + 2.0 * acc_this * tc->target);
-    if(GET_TRAJ_PLANNER_TYPE() == 1 || GET_TRAJ_PLANNER_TYPE() == 2){
-        double vs_back2;
-        if(findSCurveVSpeedWithEndSpeed(tc->target * 2.0 , tc->finalvel, acc_this, emcmotStatus->jerk, &vs_back2) == 1)
-            vs_back = vs_back2;
+    double vs_back;
+    if(GET_TRAJ_PLANNER_TYPE() == 1){
+        // S-curve mode: use S-curve kinematics calculations
+        // Starting from finalvel, maximum starting speed achievable within distance tc->target
+        // During actual execution, tpCalculateSCurveAccel will adjust dynamically based on current state (including current acceleration)
+        // Use minimum of segment's max jerk and system max jerk to ensure limits are not exceeded
+        double maxjerk = fmin(tc->maxjerk, emcmotStatus->jerk);
+        if(findSCurveVSpeedWithEndSpeed(tc->target, tc->finalvel, acc_this, maxjerk, &vs_back) != 1){
+            // S-curve calculation failed, use conservative estimate (at least maintain finalvel)
+            vs_back = tc->finalvel;
+        }
+    } else {
+        // Trapezoidal acceleration/deceleration mode: use trapezoidal kinematics formula
+        vs_back = pmSqrt(pmSq(tc->finalvel) + 2.0 * acc_this * tc->target);
     }
     // Find the reachable velocity of prev1_tc, moving forwards in time
 
@@ -1890,7 +1876,7 @@ STATIC int tpRunOptimization(TP_STRUCT * const tp) {
             tp_debug_print("Segment %d, type %d not finalized, continuing\n",tc->id,tc->motion_type);
             // use worst-case final velocity that allows for up to 1/2 of a segment to be consumed.
 
-            if(GET_TRAJ_PLANNER_TYPE() == 1 || GET_TRAJ_PLANNER_TYPE() == 2)
+            if(GET_TRAJ_PLANNER_TYPE() == 1)
                 prev1_tc->finalvel = fmin(prev1_tc->maxvel, tpCalculateOptimizationSCurveInitialVel(tp,tc));
             else
                 prev1_tc->finalvel = fmin(prev1_tc->maxvel, tpCalculateOptimizationInitialVel(tp,tc));
@@ -2272,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);
 
@@ -2472,7 +2461,7 @@ STATIC int tpComputeBlendSCurveVelocity(
         double tblend_vel;
         /* Minimum value of cos(theta) to prevent numerical instability */
         const double min_cos_theta = cos(PM_PI / 2.0 - TP_MIN_ARC_ANGLE);
-        if (cos_theta > min_cos_theta && (maxjerk > 0 || maxjerk < 99999)) {
+        if (cos_theta > min_cos_theta) {
             //tblend_vel = 2.0 * pmSqrt(acc_this * tc->tolerance / cos_theta);
             //*v_blend_this = fmin(*v_blend_this, tblend_vel);
             //*v_blend_next = fmin(*v_blend_next, tblend_vel);
@@ -2506,7 +2495,7 @@ STATIC double estimateParabolicBlendPerformance(
     double target_vel_next = tpGetMaxTargetVel(tp, nexttc);
 
     double v_net = 0.0;
-    if(GET_TRAJ_PLANNER_TYPE() == 1 || GET_TRAJ_PLANNER_TYPE() == 2)
+    if(GET_TRAJ_PLANNER_TYPE() == 1)
         tpComputeBlendSCurveVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, &v_net);
     else
         tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, &v_net);
@@ -2711,54 +2700,51 @@ STATIC int tpCalculateRampAccel(TP_STRUCT const * const tp,
 /**
  * Calculate distance update from velocity and acceleration.
  */
-STATIC int tcUpdateDistFromSCurveAccel(TC_STRUCT *const tc, double acc, double jerk, double vel_desired, double perror, int reverse_run, int dec)
+STATIC int tcUpdateDistFromSCurveAccel(TC_STRUCT *const tc, double acc, double jerk, double vel_desired, double perror __attribute__((unused)), int reverse_run, int dec)
 {
-    // If the resulting velocity is less than zero, than we're done. This
-    // causes a small overshoot, but in practice it is very small.
-    //double v_next = vel_desired;//velocity(tc->cycle_time, acc, jerk); //tc->currentvel + acc * tc->cycle_time; double velocity(double t, double a, double j);
-    double v_next = vel_desired;//velocity(tc->cycle_time, acc, jerk);
+    double v_next = vel_desired;
+    double dx = tcGetDistanceToGo(tc, reverse_run);
 
-    // update position in this tc using trapezoidal integration
-    // Note that progress can be greater than the target after this step.
-    double dx = tcGetDistanceToGo(tc,reverse_run) ;
     if (v_next < 0.0) {
         v_next = 0.0;
-        //KLUDGE: the trapezoidal planner undershoots by half a cycle time, so
-        //forcing the endpoint here is necessary. However, velocity undershoot
-        //also occurs during pausing and stopping, which can happen far from
-        //the end. If we could "cruise" to the endpoint within a cycle at our
-        //current speed, then assume that we want to be at the end.
-        if (dx < (fabs(tc->currentvel) *  tc->cycle_time)) {
-            tc->progress = tcGetTarget(tc,reverse_run);
-        }
-    } else {
-        if(dx < 1e-6){
-            tc->progress = tcGetTarget(tc,reverse_run);
-        }else{
-            // sc_distance(double t, double v, double a, double j);
-            double displacement = sc_distance(tc->cycle_time, tc->currentvel, tc->currentacc, jerk); 
-            // Account for reverse run (flip sign if need be)
-            double disp_sign = reverse_run ? -1 : 1;
-
-            if(perror > 0 && tc->last_move_length >= perror && displacement <= perror){
-                tc->progress += (disp_sign * perror);
-                tc->last_move_length = perror;
-                //printf("TRY TO FIX ERROR: %.15f \n", perror);
-                return TP_ERR_OK;
-            }
-            tc->last_move_length = displacement;
+    }
 
-            tc->progress += (disp_sign * displacement);
-            
-            //Progress has to be within the allowable range
-            tc->progress = bisaturate(tc->progress, tcGetTarget(tc, TC_DIR_FORWARD), tcGetTarget(tc, TC_DIR_REVERSE));
+    if (dx < TP_POS_EPSILON) {
+        tc->progress = tcGetTarget(tc, reverse_run);
+        tc->last_move_length = dx;
+        if (dec) {
+            v_next = 0.0;
+            acc = 0.0;
+            jerk = 0.0;
         }
     }
-    tc->currentvel = v_next;// v_next;
-    tc->currentacc = acc;
+    else if (v_next < TP_VEL_EPSILON && tc->currentvel < TP_VEL_EPSILON && dx < 1e-4) {
+        tc->progress = tcGetTarget(tc, reverse_run);
+        tc->last_move_length = dx;
+        if (dec) {
+            v_next = 0.0;
+            acc = 0.0;
+            jerk = 0.0;
+        }
+    }
+    else {
+        // Use trapezoidal integration for displacement calculation
+        // displacement = (v_current + v_next) * dt / 2
+        double displacement = (tc->currentvel + v_next) * tc->cycle_time / 2.0;
 
-    // Check if we can make the desired velocity
-    tc->on_final_decel = dec;//(fabs(vel_desired - tc->currentvel) < TP_VEL_EPSILON) && (acc < 0.0); //dec && (acc < 0.0); 
+        double disp_sign = reverse_run ? -1 : 1;
+
+        tc->last_move_length = displacement;
+        tc->progress += (disp_sign * displacement);
+
+        // Progress has to be within the allowable range
+        tc->progress = bisaturate(tc->progress, tcGetTarget(tc, TC_DIR_FORWARD), tcGetTarget(tc, TC_DIR_REVERSE));
+    }
+
+    tc->currentvel = v_next;
+    tc->currentacc = acc;
+    tc->currentjerk = jerk;
+    tc->on_final_decel = dec;
 
     return TP_ERR_OK;
 }
@@ -2790,7 +2776,7 @@ int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_
         maxjerk = 1;
         //rtapi_print_msg(RTAPI_MSG_ERR,
         //        "ERROR!!! maxjerk Is less than 1\n");
-        return -5;
+        return TP_SCURVE_ACCEL_ERROR;
     }
 
     // Find maximum allowed velocity from feed and machine limits
@@ -2809,32 +2795,53 @@ int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_
         *acc = tc->currentacc;
         *vel_desired = tc->currentvel;
         *jerk =0;//(maxnewaccel - tc->currentacc) / dt;
-        return 0;
+        return TP_SCURVE_ACCEL_ACCEL;
     }
-    int res = 1;
+    int res = TP_SCURVE_ACCEL_DECEL;
 
 
     nextSpeed(tc->currentvel, tc->currentacc, tc->cycle_time, tc_target_vel, maxaccel, maxjerk, &req_v, &req_a, &req_j);
     double dlen1 = finishWithSpeedDist(tc->currentvel, tc_finalvel, tc->currentacc, maxaccel, maxjerk);
-    double dlen2 = finishWithSpeedDist(saturate(req_v, tc_target_vel), tc_finalvel, saturate(req_a, maxaccel), maxaccel, maxjerk);
-    double moveL = sc_distance(tc->cycle_time, tc->currentvel, tc->currentacc, req_j);
-    double error = fabs(dx) - dlen1; //误差
-    *pos_error = error;
 
+    // When velocity exceeds target, correct acceleration
+    // Since velocity is limited to target, acceleration should be 0 (maintain speed) or decel
+    double v_for_dlen2 = req_v;
+    double a_for_dlen2 = req_a;
+    if (req_v > tc_target_vel) {
+        v_for_dlen2 = tc_target_vel;
+        a_for_dlen2 = 0;
+    }
+    double dlen2 = finishWithSpeedDist(v_for_dlen2, tc_finalvel, a_for_dlen2, maxaccel, maxjerk);
+    double moveL = (tc->currentvel + req_v) * tc->cycle_time / 2.0;
+
+    // margin = remaining distance - decel distance
+    // margin > 0: can continue accelerating
+    // margin <= 0: must decelerate
+    double margin = dx - dlen1;
+    *pos_error = margin;
 
-    if(tc->currentvel < 1e-6 && dx > TP_POS_EPSILON && dx < 1e-4){
-        //nextSpeed(tc->currentvel, tc->currentacc, tc->cycle_time, tc_target_vel, maxaccel, maxjerk, &req_v, &req_a, &req_j); 
-        res = 1;
+    // Handle special case: position overshoot or reached endpoint
+    if (dx <= TP_POS_EPSILON) {
+        // Already reached or exceeded endpoint, must decelerate/stop
+        nextSpeed(tc->currentvel, tc->currentacc, tc->cycle_time, tc_finalvel, maxaccel, maxjerk, &req_v, &req_a, &req_j);
+        res = TP_SCURVE_ACCEL_DECEL;
+    }
+    else if(tc->currentvel < TP_VEL_EPSILON && dx > TP_POS_EPSILON && dx < 1e-4){
+        // Very low velocity and close to target
+        res = TP_SCURVE_ACCEL_DECEL;
     }else{
-        //fabs(fabs(pos_err) - decLen) <= 0.000001 || fabs(pos_err) - moveL * dir <= decLen2 || fabs(pos_err) <= decLen 
-        if (fabs(error) <= 0.0001 || dx - moveL <= dlen2 || dx < dlen1){
+        // Decel conditions:
+        // Condition 1: margin <= 0 - decel distance already >= remaining distance, must decelerate
+        // Condition 2: dx - moveL <= dlen2 - if we continue accel, next cycle remaining <= decel dist
+        int need_decel = (margin <= TP_POS_EPSILON) || (dx - moveL <= dlen2);
+
+        if (need_decel){
             nextSpeed(tc->currentvel, tc->currentacc, tc->cycle_time, tc_finalvel, maxaccel, maxjerk, &req_v, &req_a, &req_j);
         }else{
             if(tc_finalvel > TP_VEL_EPSILON || tc_target_vel > TP_VEL_EPSILON)
-            res = 0;
+            res = TP_SCURVE_ACCEL_ACCEL;
         }
     }
-    //}
 
     maxnewvel = req_v;
     maxnewacc = req_a;
@@ -2848,8 +2855,7 @@ int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_
     maxnewaccel = saturate(maxnewaccel, maxaccel);
     *acc = maxnewaccel;
     *vel_desired = maxnewvel;
-    *jerk =maxnewjerk;//(maxnewaccel - tc->currentacc) / dt;
-    //*vel_desired = tc->currentvel + velocity(dt, *acc, *jerk);
+    *jerk = maxnewjerk;
 
     return res;
 }
@@ -3470,7 +3476,7 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp,
 
     if(mode == NULL) planner_type = 0;
 
-    if(planner_type != 1 && planner_type != 2){
+    if(planner_type != 1){
         // If the slowdown is not too great, use velocity ramping instead of trapezoidal velocity
         // Also, don't ramp up for parabolic blends
         if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) {
@@ -3487,11 +3493,12 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp,
     }else{ 
         double jerk;
         double perror;
-        if(*mode == 1){      
+        if(*mode == 1){
             tc->cycle_time = tp->cycleTime;
             int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 1);
-            if(is_dec == -5){ //If the calculation fails, revert to T-shaped acceleration/deceleration.
-                *mode = -5;
+
+            if(is_dec == TP_SCURVE_ACCEL_ERROR){ //If the calculation fails, revert to T-shaped acceleration/deceleration.
+                *mode = TP_SCURVE_ACCEL_ERROR;
                 res_accel = 1;
                 acc=0, vel_desired=0;
                 if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) {
@@ -3507,8 +3514,8 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp,
             }
         }else{
             int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 0);
-            if(is_dec == -5){ //If the calculation fails, revert to T-shaped acceleration/deceleration.
-                *mode = -5;
+            if(is_dec == TP_SCURVE_ACCEL_ERROR){ //If the calculation fails, revert to T-shaped acceleration/deceleration.
+                *mode = TP_SCURVE_ACCEL_ERROR;
                 res_accel = 1;
                 acc=0, vel_desired=0;
                 if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) {
@@ -3739,8 +3746,21 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc,
     switch (tc->term_cond) {
         case TC_TERM_COND_TANGENT:
             nexttc->cycle_time = tp->cycleTime - tc->cycle_time;
-            nexttc->currentvel = tc->term_vel;
-            tp_debug_print("Doing tangent split\n");
+            // In S-curve mode, use actual current velocity instead of expected term_vel
+            // S-curve can't change velocity instantly, term_vel is just desired value
+            if (GET_TRAJ_PLANNER_TYPE() == 1) {
+                nexttc->currentvel = tc->currentvel;
+                // Inherit acceleration, but limit to nexttc's allowed range
+                // Important for line-to-arc transitions where arc has lower tangential accel
+                double maxacc_next = tcGetTangentialMaxAccel(nexttc);
+                nexttc->currentacc = saturate(tc->currentacc, maxacc_next);
+                tp_debug_print("Doing tangent split (S-curve): vel=%f, acc=%f (limited by %f)\n",
+                              nexttc->currentvel, nexttc->currentacc, maxacc_next);
+            } else {
+                // Trapezoidal: can use term_vel (assumes instant velocity change)
+                nexttc->currentvel = tc->term_vel;
+                tp_debug_print("Doing tangent split (trapezoidal): vel=%f\n", nexttc->currentvel);
+            }
             break;
         case TC_TERM_COND_PARABOLIC:
             break;
@@ -3799,7 +3819,7 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp,
     double target_vel_this = tpGetRealTargetVel(tp, tc);
     double target_vel_next = tpGetRealTargetVel(tp, nexttc);
 
-    if(mode != -5 && (GET_TRAJ_PLANNER_TYPE() == 1 || GET_TRAJ_PLANNER_TYPE() == 2))
+    if(mode != TP_SCURVE_ACCEL_ERROR && GET_TRAJ_PLANNER_TYPE() == 1)
         tpComputeBlendSCurveVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL);
     else
         tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL);