diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c
index b7deaf864aa..e8e402ed580 100644
--- a/src/emc/tp/blendmath.c
+++ b/src/emc/tp/blendmath.c
@@ -446,7 +446,13 @@ int calculateInscribedDiameter(PmCartesian const * const normal,
     PmCartesian planar_x,planar_y,planar_z;
 
     //Find perpendicular component of unit directions
-    // FIXME use plane project?
+    // FIXME Assumes normal is unit length
+    
+    /* This section projects the X / Y / Z unit vectors onto the plane
+     * containing the motions. The operation is done "backwards" here due to a
+     * quirk with posemath. 
+     *
+     */
     pmCartScalMult(normal, -normal->x, &planar_x);
     pmCartScalMult(normal, -normal->y, &planar_y);
     pmCartScalMult(normal, -normal->z, &planar_z);
@@ -647,9 +653,15 @@ int blendParamKinematics(BlendGeom3 * const geom,
     tp_debug_print("a_max = %f, a_n_max = %f\n", param->a_max,
             param->a_n_max);
 
-    // Find common velocity and acceleration
-    param->v_req = fmax(prev_tc->reqvel, tc->reqvel);
-    param->v_goal = param->v_req * maxFeedScale;
+    // Find the nominal velocity for the blend segment with no overrides
+    double v_req_prev = tcGetMaxTargetVel(prev_tc, 1.0);
+    double v_req_this = tcGetMaxTargetVel(tc, 1.0);
+    tp_debug_print("vr_prev = %f, vr_this = %f\n", v_req_prev, v_req_this);
+    param->v_req = fmax(v_req_prev, v_req_this);
+
+    // Find the worst-case velocity we should reach for either segment
+    param->v_goal = fmax(tcGetMaxTargetVel(prev_tc, maxFeedScale),
+            tcGetMaxTargetVel(tc, maxFeedScale));
 
     // Calculate the maximum planar velocity
     double v_planar_max;
@@ -693,7 +705,6 @@ int blendParamKinematics(BlendGeom3 * const geom,
     tp_debug_print("v_max = %f\n", v_max);
     param->v_goal = fmin(param->v_goal, v_max);
 
-    tp_debug_print("vr1 = %f, vr2 = %f\n", prev_tc->reqvel, tc->reqvel);
     tp_debug_print("v_goal = %f, max scale = %f\n", param->v_goal, maxFeedScale);
 
     return res_dia;
diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c
index eeee1d091f8..7181fb733bb 100644
--- a/src/emc/tp/tc.c
+++ b/src/emc/tp/tc.c
@@ -28,6 +28,31 @@
 #include "tp_debug.h"
 
 
+double tcGetMaxTargetVel(TC_STRUCT const * const tc,
+        double max_scale)
+{
+    double v_max_target;
+
+    switch (tc->synchronized) {
+        case TC_SYNC_NONE:
+            // Get maximum reachable velocity from max feed override
+            v_max_target = tc->reqvel * max_scale;
+            break;
+
+        case TC_SYNC_VELOCITY: //Fallthrough
+            max_scale = 1.0;
+        case TC_SYNC_POSITION:
+            // Assume no spindle override during blend target
+        default:
+            v_max_target = tc->maxvel;
+            break;
+    }
+
+    // Clip maximum velocity by the segment's own maximum velocity
+    return fmin(v_max_target, tc->maxvel);
+}
+
+
 int tcCircleStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out)
 {
     PmCartesian startpoint;
@@ -540,6 +565,7 @@ int tcSetupMotion(TC_STRUCT * const tc,
         double ini_maxvel,
         double acc)
 {
+    //FIXME assumes that state is already set up in TC_STRUCT, which depends on external order of function calls.
 
     tc->maxaccel = acc;
 
diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h
index 6f318b51a0b..c49012f3d19 100644
--- a/src/emc/tp/tc.h
+++ b/src/emc/tp/tc.h
@@ -22,6 +22,8 @@
 #include "tc_types.h"
 #include "tp_types.h"
 
+double tcGetMaxTargetVel(TC_STRUCT const * const tc,
+        double max_scale);
 int tcGetEndpoint(TC_STRUCT const * const tc, EmcPose * const out);
 int tcGetStartpoint(TC_STRUCT const * const tc, EmcPose * const out);
 int tcGetPos(TC_STRUCT const * const tc,  EmcPose * const out);
diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c
index b7a018ea35d..46c7fa41146 100644
--- a/src/emc/tp/tp.c
+++ b/src/emc/tp/tp.c
@@ -50,8 +50,19 @@ extern emcmot_debug_t *emcmotDebug;
 extern emcmot_config_t *emcmotConfig;
 
 /** static function primitives (ugly but less of a pain than moving code around)*/
-STATIC int tpComputeBlendVelocity(TP_STRUCT const * const tp, TC_STRUCT * const tc,
-        TC_STRUCT * const nexttc);
+STATIC int tpComputeBlendVelocity(
+        TC_STRUCT const *tc,
+        TC_STRUCT const *nexttc,
+        double v_target_this,
+        double v_target_next,
+        double *v_blend_this,
+        double *v_blend_next,
+        double *v_blend_net);
+
+STATIC double estimateParabolicBlendPerformance(
+        TP_STRUCT const *tp,
+        TC_STRUCT const *tc,
+        TC_STRUCT const *nexttc);
 
 STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc);
 
@@ -64,7 +75,7 @@ STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc
 
 STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc);
 
-STATIC int tpAdjustAccelForTangent(TP_STRUCT const * const,
+STATIC int tpAdjustAccelForTangent(
         TC_STRUCT * const tc,
         double normal_acc_scale);
 /**
@@ -91,7 +102,7 @@ STATIC int tcCheckLastParabolic(TC_STRUCT * const tc,
 /**
  * Returns true if there is motion along ABC or UVW axes, false otherwise.
  */
-STATIC int tpRotaryMotionCheck(TP_STRUCT const * const tp, TC_STRUCT const * const tc) {
+STATIC int tcRotaryMotionCheck(TC_STRUCT const * const tc) {
     switch (tc->motion_type) {
         //Note lack of break statements due to every path returning
         case TC_RIGIDTAP:
@@ -188,6 +199,9 @@ STATIC int tpGetMachineActiveLimit(double * const act_limit, PmCartesian const *
  */
 STATIC double tpGetFeedScale(TP_STRUCT const * const tp,
         TC_STRUCT const * const tc) {
+    if (!tc) {
+        return 0.0;
+    }
     //All reasons to disable feed override go here
     bool pausing = tp->pausing && (tc->synchronized == TC_SYNC_NONE || tc->synchronized == TC_SYNC_VELOCITY);
     bool aborting = tp->aborting;
@@ -215,6 +229,9 @@ STATIC double tpGetFeedScale(TP_STRUCT const * const tp,
 STATIC inline double tpGetRealTargetVel(TP_STRUCT const * const tp,
         TC_STRUCT const * const tc) {
 
+    if (!tc) {
+        return 0.0;
+    }
     // Start with the scaled target velocity based on the current feed scale
     double v_target = tc->synchronized ? tc->target_vel : tc->reqvel;
     /*tc_debug_print("Initial v_target = %f\n",v_target);*/
@@ -224,8 +241,20 @@ STATIC inline double tpGetRealTargetVel(TP_STRUCT const * const tp,
 }
 
 
+STATIC inline double getMaxFeedScale(TC_STRUCT const * tc)
+{
+    //All reasons to disable feed override go here
+    if (tc && tc->synchronized == TC_SYNC_POSITION ) {
+        return 1.0;
+    } else {
+        return emcmotConfig->maxFeedScale;
+    }
+}
+
+
 /**
  * Get the worst-case target velocity for a segment based on the trajectory planner state.
+ * Note that this factors in the user-specified velocity limit.
  */
 STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc)
 {
@@ -249,7 +278,7 @@ STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT con
         v_max_target = fmin(v_max_target,tp->vLimit);
     }
 
-    // Clip maximum velocity by the segment's own maximum velocity
+    // Apply maximum segment velocity limit (must always be respected)
     return fmin(v_max_target, tc->maxvel);
 }
 
@@ -283,16 +312,19 @@ STATIC inline double tpGetRealFinalVel(TP_STRUCT const * const tp,
 }
 
 /**
- * Get acceleration for a tc based on the trajectory planner state.
+ * Compute maximum acceleration a segment can have for a given terminal condition.
+ * Used (for example) to check if a different terminal condition would improve allowed acceleration.
  */
-STATIC inline double tpGetScaledAccel(TP_STRUCT const * const tp,
-        TC_STRUCT const * const tc) {
+STATIC inline double planMaxSegmentAccel(
+        TC_STRUCT const * const tc,
+        tc_term_cond_t term_cond)
+{
     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) {
+    if (term_cond == TC_TERM_COND_PARABOLIC) {
         a_scale *= 0.5;
     }
     if (tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_SPHERICAL) {
@@ -302,6 +334,21 @@ STATIC inline double tpGetScaledAccel(TP_STRUCT const * const tp,
     return a_scale;
 }
 
+
+/**
+ * Set up a spindle origin based on the current spindle COMMANDED direction and the given position.
+ *
+ * Convert the 2-part spindle position and sign to a signed double.
+ * The direction is stored as part of the origin to prevent discontinuous
+ * changes in displacement due to sign flips
+ */
+STATIC inline double tpGetScaledAccel(
+        TC_STRUCT const * const tc)
+{
+    // Backend to the planning function, but use the actual terminal condition.
+    return planMaxSegmentAccel(tc, (tc_term_cond_t)tc->term_cond);
+}
+
 /**
  * Convert the 2-part spindle position and sign to a signed double.
  */
@@ -649,9 +696,9 @@ int tpErrorCheck(TP_STRUCT const * const tp) {
  * This is used to estimate blend velocity, though by itself is not enough
  * (since requested velocity and max velocity could be lower).
  */
-STATIC double tpCalculateTriangleVel(TP_STRUCT const * const tp, TC_STRUCT * const tc) {
+STATIC double tpCalculateTriangleVel(TC_STRUCT const *tc) {
     //Compute peak velocity for blend calculations
-    double acc_scaled = tpGetScaledAccel(tp, tc);
+    double acc_scaled = tpGetScaledAccel(tc);
     double length = tc->target;
     if (!tc->finalized) {
         // blending may remove up to 1/2 of the segment
@@ -674,7 +721,7 @@ STATIC double tpCalculateTriangleVel(TP_STRUCT const * const tp, TC_STRUCT * con
  */
 STATIC double tpCalculateOptimizationInitialVel(TP_STRUCT const * const tp, TC_STRUCT * const tc)
 {
-    double acc_scaled = tpGetScaledAccel(tp, tc);
+    double acc_scaled = tpGetScaledAccel(tc);
     //FIXME this is defined in two places!
     double triangle_vel = pmSqrt( acc_scaled * tc->target * BLEND_DIST_FRACTION);
     double max_vel = tpGetMaxTargetVel(tp, tc);
@@ -689,22 +736,25 @@ STATIC double tpCalculateOptimizationInitialVel(TP_STRUCT const * const tp, TC_S
  * initialize a blend arc. This function does not handle connecting the
  * segments together, however.
  */
-STATIC int tpInitBlendArcFromPrev(TP_STRUCT const * const tp, TC_STRUCT const * const prev_line_tc,
-        TC_STRUCT* const blend_tc, double vel, double ini_maxvel, double acc) {
-
+STATIC int tpInitBlendArcFromPrev(TP_STRUCT const * const tp,
+        TC_STRUCT const * const prev_tc,
+        TC_STRUCT* const blend_tc,
+        double vel,
+        double ini_maxvel,
+        double acc) {
 
 #ifdef TP_SHOW_BLENDS
     int canon_motion_type = EMC_MOTION_TYPE_ARC;
 #else
-    int canon_motion_type = prev_line_tc->canon_motion_type;
+    int canon_motion_type = prev_tc->canon_motion_type;
 #endif
 
     tcInit(blend_tc,
             TC_SPHERICAL,
             canon_motion_type,
             tp->cycleTime,
-            prev_line_tc->enables,
-            prev_line_tc->atspeed);
+            prev_tc->enables,
+            false); // NOTE: blend arc never needs the atspeed flag, since the previous line will have it (and cannot be consumed).
 
     // Copy over state data from TP
     tcSetupState(blend_tc, tp);
@@ -716,7 +766,7 @@ STATIC int tpInitBlendArcFromPrev(TP_STRUCT const * const tp, TC_STRUCT const *
             acc);
 
     // Skip syncdio setup since this blend extends the previous line
-    blend_tc->syncdio = prev_line_tc->syncdio; //enqueue the list of DIOs that need toggling
+    blend_tc->syncdio = prev_tc->syncdio; //enqueue the list of DIOs that need toggling
 
     // find "helix" length for target
     double length;
@@ -753,6 +803,39 @@ STATIC int tcSetLineXYZ(TC_STRUCT * const tc, PmCartLine const * const line)
 }
 
 
+/**
+ * Force two segments to be treated as tangent based on the computed kink velocity.
+ *
+ * @warning for segments that are not close to tangent, the maximum
+ * acceleration will be drastically reduced to ensure that the transition is
+ * within velocity / acceleration limits.
+ */
+static void forceTangentSegments(TC_STRUCT *prev_tc, TC_STRUCT *tc)
+{
+    // Fall back to tangent, using kink_vel as final velocity
+    tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT);
+
+    // Finally, reduce acceleration proportionally to prevent violations during "kink"
+    const double kink_ratio = tpGetTangentKinkRatio();
+    tpAdjustAccelForTangent(tc, kink_ratio);
+    tpAdjustAccelForTangent(prev_tc, kink_ratio);
+}
+
+static inline int find_max_element(double arr[], int sz)
+{
+    if (sz < 1) {
+        return -1;
+    }
+    // Assumes at least one element
+    int max_idx = 0;
+    int idx;
+    for (idx = 0; idx < sz; ++idx) {
+        if (arr[idx] > arr[max_idx]) {
+            max_idx = idx;
+        }
+    }
+    return max_idx;
+}
 
 /**
  * Compare performance of blend arc and equivalent tangent speed.
@@ -761,36 +844,64 @@ STATIC int tcSetLineXYZ(TC_STRUCT * const tc, PmCartLine const * const line)
  * mostly useful for some odd arc-to-arc cases where the blend arc becomes very
  * short (and therefore slow).
  */
-STATIC int tpCheckTangentPerformance(TP_STRUCT const * const tp,
+STATIC tc_blend_type_t tpChooseBestBlend(TP_STRUCT const * const tp,
         TC_STRUCT * const prev_tc,
         TC_STRUCT * const tc,
         TC_STRUCT * const blend_tc)
 {
-    tcFinalizeLength(blend_tc);
-    if (blend_tc->maxvel < tc->kink_vel) {
-        tp_debug_print("segment maxvel %f is less than kink_vel %f, falling back to simple tangent\n",
-                blend_tc->maxvel, tc->kink_vel);
-
-        // Fall back to tangent, using kink_vel as final velocity
-        tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT);
-
-        // Finally, reduce acceleration proportionally to prevent violations during "kink"
-        const double kink_ratio = tpGetTangentKinkRatio();
-        tpAdjustAccelForTangent(tp, tc, kink_ratio);
-        tpAdjustAccelForTangent(tp, prev_tc, kink_ratio);
-        return TP_ERR_NO_ACTION;
-    } else {
-        tp_debug_print("segment maxvel %f is greater than kink_vel %f, using blend arc\n",
-                blend_tc->maxvel, tc->kink_vel);
-        // Since we get near-exact tangency with the blend arc, throw out the kink velocity since it's not relevant
-        tc->kink_vel = -1.0;
+    if (!tc || !prev_tc) {
+        return NO_BLEND;
     }
 
-    return TP_ERR_OK;
+    // Can't blend segments that are explicitly disallowed
+    switch  (prev_tc->term_cond)
+    {
+    case TC_TERM_COND_EXACT:
+    case TC_TERM_COND_STOP:
+        return NO_BLEND;
+    }
+
+    // Compute performance measures ("perf_xxx") for each method. This is
+    // basically the blend velocity. However, because parabolic blends require
+    // halving the acceleration of both blended segments, they in effect slow
+    // down the next and previous blends as well. We model this loss by scaling
+    // the blend velocity down to find an "equivalent" velocity.
+    double perf_parabolic = estimateParabolicBlendPerformance(tp, prev_tc, tc) / 2.0;
+    double perf_tangent = tc->kink_vel;
+    double perf_arc_blend = blend_tc ? blend_tc->maxvel : 0.0;
+
+    tp_debug_print("Blend performance: parabolic %f, tangent %f, arc_blend %f, ",
+                   perf_parabolic,
+                   perf_tangent,
+                   perf_arc_blend);
+
+    // KLUDGE Order the performance measurements so that they match the enum values
+    double perf[3] = {perf_parabolic, perf_tangent, perf_arc_blend};
+    tc_blend_type_t best_blend = find_max_element(perf, 3);
+
+    switch (best_blend) {
+        case PARABOLIC_BLEND: // parabolic
+            tp_debug_print("using parabolic blend\n");
+            tc->kink_vel = -1.0;
+            tcSetTermCond(prev_tc, TC_TERM_COND_PARABOLIC);
+            tcCheckLastParabolic(tc, prev_tc);
+            break;
+        case TANGENT_SEGMENTS_BLEND: // tangent
+            tp_debug_print("using parabolic blending\n");
+            forceTangentSegments(prev_tc, tc);
+            break;
+        case ARC_BLEND: // arc blend
+            tp_debug_print("using blend arc\n");
+            tc->kink_vel = -1.0;
+            break;
+        case NO_BLEND:
+            break;
+    }
+    return best_blend;
 }
 
 
-STATIC int tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc)
+STATIC tp_err_t tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc)
 {
     tp_debug_print("-- Starting LineArc blend arc --\n");
 
@@ -910,7 +1021,6 @@ STATIC int tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
     //set the max velocity to v_plan, since we'll violate constraints otherwise.
     tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req,
             param.v_plan, param.a_max);
-    blend_tc->target_vel = param.v_actual;
 
     int res_tangent = checkTangentAngle(&circ2_temp,
             &blend_tc->coords.arc.xyz,
@@ -924,15 +1034,11 @@ STATIC int tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
         return TP_ERR_FAIL;
     }
 
-    if (tpCheckTangentPerformance(tp, prev_tc, tc, blend_tc) == TP_ERR_NO_ACTION) {
+    if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) {
         return TP_ERR_NO_ACTION;
     }
 
     tp_debug_print("Passed all tests, updating segments\n");
-
-    //Cleanup any mess from parabolic
-    tc->blend_prev = 0;
-
     //TODO refactor to pass consume to connect function
     if (param.consume) {
         //Since we're consuming the previous segment, pop the last line off of the queue
@@ -952,7 +1058,7 @@ STATIC int tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
 }
 
 
-STATIC int tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc)
+STATIC tp_err_t tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc)
 {
 
     tp_debug_print("-- Starting ArcLine blend arc --\n");
@@ -1071,7 +1177,6 @@ STATIC int tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
     //set the max velocity to v_plan, since we'll violate constraints otherwise.
     tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req,
             param.v_plan, param.a_max);
-    blend_tc->target_vel = param.v_actual;
 
     int res_tangent = checkTangentAngle(&circ1_temp, &blend_tc->coords.arc.xyz, &geom, &param, tp->cycleTime, false);
     if (res_tangent) {
@@ -1079,7 +1184,7 @@ STATIC int tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
         return TP_ERR_FAIL;
     }
 
-    if (tpCheckTangentPerformance(tp, prev_tc, tc, blend_tc) == TP_ERR_NO_ACTION) {
+    if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) {
         return TP_ERR_NO_ACTION;
     }
 
@@ -1094,7 +1199,7 @@ STATIC int tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
     return TP_ERR_OK;
 }
 
-STATIC int tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc)
+STATIC tp_err_t tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc)
 {
 
     tp_debug_print("-- Starting ArcArc blend arc --\n");
@@ -1240,7 +1345,6 @@ STATIC int tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
     //set the max velocity to v_plan, since we'll violate constraints otherwise.
     tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req,
             param.v_plan, param.a_max);
-    blend_tc->target_vel = param.v_actual;
 
     int res_tangent1 = checkTangentAngle(&circ1_temp, &blend_tc->coords.arc.xyz, &geom, &param, tp->cycleTime, false);
     int res_tangent2 = checkTangentAngle(&circ2_temp, &blend_tc->coords.arc.xyz, &geom, &param, tp->cycleTime, true);
@@ -1249,7 +1353,7 @@ STATIC int tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
         return TP_ERR_FAIL;
     }
 
-    if (tpCheckTangentPerformance(tp, prev_tc, tc, blend_tc) == TP_ERR_NO_ACTION) {
+    if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) {
         return TP_ERR_NO_ACTION;
     }
 
@@ -1266,7 +1370,7 @@ STATIC int tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
 }
 
 
-STATIC int tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
+STATIC tp_err_t tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
         TC_STRUCT * const tc, TC_STRUCT * const blend_tc)
 {
 
@@ -1318,9 +1422,10 @@ STATIC int tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc
     //set the max velocity to v_plan, since we'll violate constraints otherwise.
     tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req,
             param.v_plan, param.a_max);
-    blend_tc->target_vel = param.v_actual;
 
-    if (tpCheckTangentPerformance(tp, prev_tc, tc, blend_tc) == TP_ERR_NO_ACTION) {
+    tp_debug_print("blend_tc target_vel = %g\n", blend_tc->target_vel);
+
+    if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) {
         return TP_ERR_NO_ACTION;
     }
 
@@ -1398,6 +1503,8 @@ STATIC int tpSetupSyncedIO(TP_STRUCT * const tp, TC_STRUCT * const tc) {
         tc->syncdio.anychanged = 0;
         return TP_ERR_NO_ACTION;
     }
+
+
 }
 
 
@@ -1461,7 +1568,7 @@ int tpAddRigidTap(TP_STRUCT * const tp, EmcPose end, double vel, double ini_maxv
     return retval;
 }
 
-STATIC blend_type_t tpCheckBlendArcType(TP_STRUCT const * const tp,
+STATIC blend_type_t tpCheckBlendArcType(
         TC_STRUCT const * const prev_tc,
         TC_STRUCT const * const tc) {
 
@@ -1477,7 +1584,7 @@ STATIC blend_type_t tpCheckBlendArcType(TP_STRUCT const * const tp,
     }
 
     //If we have any rotary axis motion, then don't create a blend arc
-    if (tpRotaryMotionCheck(tp, tc) || tpRotaryMotionCheck(tp, prev_tc)) {
+    if (tcRotaryMotionCheck(tc) || tcRotaryMotionCheck(prev_tc)) {
         tp_debug_print("One of the segments has rotary motion, aborting blend arc\n");
         return BLEND_NONE;
     }
@@ -1514,7 +1621,7 @@ STATIC blend_type_t tpCheckBlendArcType(TP_STRUCT const * const tp,
 STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT * const prev1_tc) {
     //Calculate the maximum starting velocity vs_back of segment tc, given the
     //trajectory parameters
-    double acc_this = tpGetScaledAccel(tp, tc);
+    double acc_this = tpGetScaledAccel(tc);
 
     // Find the reachable velocity of tc, moving backwards in time
     double vs_back = pmSqrt(pmSq(tc->finalvel) + 2.0 * acc_this * tc->target);
@@ -1522,7 +1629,8 @@ STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * cons
 
     double vf_limit_this = tc->maxvel;
     double vf_limit_prev = prev1_tc->maxvel;
-    if (prev1_tc->kink_vel >=0 ) {
+    if (prev1_tc->kink_vel >=0  && prev1_tc->term_cond == TC_TERM_COND_TANGENT) {
+        // Only care about kink_vel with tangent segments
         vf_limit_prev = fmin(vf_limit_prev, prev1_tc->kink_vel);
     }
     //Limit the PREVIOUS velocity by how much we can overshoot into
@@ -1532,14 +1640,18 @@ STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * cons
         //If we've hit the requested velocity, then prev_tc is definitely a "peak"
         vs_back = vf_limit;
         prev1_tc->optimization_state = TC_OPTIM_AT_MAX;
-        tp_debug_print("found peak due to v_limit\n");
+        tp_debug_print("found peak due to v_limit %f\n", vf_limit);
     }
 
     //Limit tc's target velocity to avoid creating "humps" in the velocity profile
     prev1_tc->finalvel = vs_back;
 
-    tp_info_print(" prev1_tc-> fv = %f, tc->fv = %f, capped target = %f\n",
-            prev1_tc->finalvel, tc->finalvel, tc->target_vel);
+    //Reduce max velocity to match sample rate
+    double sample_maxvel = tc->target / (tp->cycleTime * TP_MIN_SEGMENT_CYCLES);
+    tc->maxvel = fmin(tc->maxvel, sample_maxvel);
+
+    tp_info_print(" prev1_tc-> fv = %f, tc->fv = %f\n",
+            prev1_tc->finalvel, tc->finalvel);
 
     return TP_ERR_OK;
 }
@@ -1658,7 +1770,7 @@ STATIC double pmCartAbsMax(PmCartesian const * const v)
     return fmax(fmax(fabs(v->x),fabs(v->y)),fabs(v->z));
 }
 
-STATIC int tpAdjustAccelForTangent(TP_STRUCT const * const tp,
+STATIC int tpAdjustAccelForTangent(
         TC_STRUCT * const tc,
         double normal_acc_scale)
 {
@@ -1685,7 +1797,7 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp,
         return TP_ERR_FAIL;
     }
     //If we have ABCUVW movement, then don't check for tangency
-    if (tpRotaryMotionCheck(tp, tc) || tpRotaryMotionCheck(tp, prev_tc)) {
+    if (tcRotaryMotionCheck(tc) || tcRotaryMotionCheck(prev_tc)) {
         tp_debug_print("found rotary axis motion\n");
         return TP_ERR_FAIL;
     }
@@ -1725,8 +1837,10 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp,
 
     // Calculate instantaneous acceleration required for change in direction
     // from v1 to v2, assuming constant speed
-    double v_max1 = fmin(prev_tc->maxvel, prev_tc->reqvel * emcmotConfig->maxFeedScale);
-    double v_max2 = fmin(tc->maxvel, tc->reqvel * emcmotConfig->maxFeedScale);
+    double v_max1 = tcGetMaxTargetVel(prev_tc, getMaxFeedScale(prev_tc));
+    double v_max2 = tcGetMaxTargetVel(tc, getMaxFeedScale(tc));
+    // Note that this is a minimum since the velocity at the intersection must
+    // be the slower of the two segments not to violate constraints.
     double v_max = fmin(v_max1, v_max2);
     tp_debug_print("tangent v_max = %f\n",v_max);
 
@@ -1761,11 +1875,11 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp,
 
     const double kink_ratio = tpGetTangentKinkRatio();
     if (acc_scale_max < kink_ratio) {
-        tp_debug_print(" Kink acceleration within %g, treating as tangent\n", kink_ratio);
+        tp_debug_print(" Kink acceleration within %g, using tangent blend\n", kink_ratio);
         tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT);
         tc->kink_vel = v_max;
-        tpAdjustAccelForTangent(tp, tc, acc_scale_max);
-        tpAdjustAccelForTangent(tp, prev_tc, acc_scale_max);
+        tpAdjustAccelForTangent(tc, acc_scale_max);
+        tpAdjustAccelForTangent(prev_tc, acc_scale_max);
 
         return TP_ERR_OK;
     } else {
@@ -1779,6 +1893,40 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp,
 
 }
 
+static bool tpCreateBlendIfPossible(
+        TP_STRUCT *tp,
+        TC_STRUCT *prev_tc,
+        TC_STRUCT *tc,
+        TC_STRUCT *blend_tc)
+{
+    tp_err_t res_create = TP_ERR_FAIL;
+    blend_type_t blend_requested = tpCheckBlendArcType(prev_tc, tc);
+
+    switch (blend_requested) {
+        case BLEND_LINE_LINE:
+            res_create = tpCreateLineLineBlend(tp, prev_tc, tc, blend_tc);
+            break;
+        case BLEND_LINE_ARC:
+            res_create = tpCreateLineArcBlend(tp, prev_tc, tc, blend_tc);
+            break;
+        case BLEND_ARC_LINE:
+            res_create = tpCreateArcLineBlend(tp, prev_tc, tc, blend_tc);
+            break;
+        case BLEND_ARC_ARC:
+            res_create = tpCreateArcArcBlend(tp, prev_tc, tc, blend_tc);
+            break;
+        case BLEND_NONE:
+        default:
+            tp_debug_print("intersection type not recognized, aborting arc\n");
+            res_create = TP_ERR_FAIL;
+            break;
+    }
+
+    // Can always do this (blend_tc may not be used, in which case it has no effect)
+    tcCheckLastParabolic(blend_tc, prev_tc);
+    return res_create == TP_ERR_OK;
+}
+
 
 /**
  * Handle creating a blend arc when a new line segment is about to enter the queue.
@@ -1786,7 +1934,7 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp,
  * blend arc. Essentially all of the blend arc functions are called through
  * here to isolate the process.
  */
-STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) {
+STATIC tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) {
 
     tp_debug_print("*****************************************\n** Handle Blend Arc **\n");
 
@@ -1815,7 +1963,6 @@ STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) {
             tp_debug_print(" tpSetupTangent failed, aborting blend arc\n");
         case TP_ERR_OK:
             return res_tan;
-            break;
         case TP_ERR_NO_ACTION:
         default:
             //Continue with creation
@@ -1824,35 +1971,20 @@ STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) {
 
     TC_STRUCT blend_tc = {0};
 
-    blend_type_t type = tpCheckBlendArcType(tp, prev_tc, tc);
-    int res_create;
-    switch (type) { 
-        case BLEND_LINE_LINE:
-            res_create = tpCreateLineLineBlend(tp, prev_tc, tc, &blend_tc);
-            break;
-        case BLEND_LINE_ARC:
-            res_create = tpCreateLineArcBlend(tp, prev_tc, tc, &blend_tc);
-            break;
-        case BLEND_ARC_LINE:
-            res_create = tpCreateArcLineBlend(tp, prev_tc, tc, &blend_tc);
-            break;
-        case BLEND_ARC_ARC:
-            res_create = tpCreateArcArcBlend(tp, prev_tc, tc, &blend_tc);
-            break;
-        default:
-            tp_debug_print("intersection type not recognized, aborting arc\n");
-            res_create = TP_ERR_FAIL;
-            break;
-    }
+    tc_blend_type_t blend_used = NO_BLEND;
+
+    bool arc_blend_ok = tpCreateBlendIfPossible(tp, prev_tc, tc, &blend_tc);
 
-    if (res_create == TP_ERR_OK) {
+    if (arc_blend_ok) {
         //Need to do this here since the length changed
+        blend_used = ARC_BLEND;
         tpAddSegmentToQueue(tp, &blend_tc, false);
     } else {
-        return res_create;
+        // If blend arc creation failed early on, catch it here and find the best blend
+        blend_used = tpChooseBestBlend(tp, prev_tc, tc, NULL) ;
     }
 
-    return TP_ERR_OK;
+    return blend_used;
 }
 
 //TODO final setup steps as separate functions
@@ -1879,24 +2011,25 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double v
             enables,
             atspeed);
 
-    // Copy in motion parameters
-    tcSetupMotion(&tc,
-            vel,
-            ini_maxvel,
-            acc);
-
     // Setup any synced IO for this move
     tpSetupSyncedIO(tp, &tc);
 
     // Copy over state data from the trajectory planner
     tcSetupState(&tc, tp);
 
+    // Copy in motion parameters
+    tcSetupMotion(&tc,
+            vel,
+            ini_maxvel,
+            acc);
+
     // Setup line geometry
     pmLine9Init(&tc.coords.line,
             &tp->goalPos,
             &end);
     tc.target = pmLine9Target(&tc.coords.line);
     if (tc.target < TP_POS_EPSILON) {
+        rtapi_print_msg(RTAPI_MSG_DBG,"failed to create line id %d, zero-length segment\n",tp->nextId);
         return TP_ERR_ZERO_LENGTH;
     }
     tc.nominal_length = tc.target;
@@ -2021,30 +2154,29 @@ int tpAddCircle(TP_STRUCT * const tp,
  * If we are blending between tc and nexttc, then we need to figure out what a
  * safe blend velocity is based on the known trajectory parameters. This
  * function updates the TC_STRUCT data with a safe blend velocity.
+ *
+ * @note This function will compute the parabolic blend start / end velocities
+ * regardless of the current terminal condition (useful for planning).
  */
-STATIC int tpComputeBlendVelocity(TP_STRUCT const * const tp,
-        TC_STRUCT * const tc, TC_STRUCT * const nexttc)
+STATIC int tpComputeBlendVelocity(
+        TC_STRUCT const *tc,
+        TC_STRUCT const *nexttc,
+        double target_vel_this,
+        double target_vel_next,
+        double *v_blend_this,
+        double *v_blend_next,
+        double *v_blend_net)
 {
     /* Pre-checks for valid pointers */
-    if (!nexttc || !tc) {
+    if (!nexttc || !tc || !v_blend_this || !v_blend_next ) {
         return TP_ERR_FAIL;
     }
 
-    if (tc->term_cond != TC_TERM_COND_PARABOLIC) {
-        return TP_ERR_NO_ACTION;
-    }
-
-    double acc_this = tpGetScaledAccel(tp, tc);
-    double acc_next = tpGetScaledAccel(tp, nexttc);
-
-    // cap the blend velocity at the current requested speed (factoring in feed override)
-    double target_vel_this;
-    double target_vel_next;
-    target_vel_this = tpGetRealTargetVel(tp, tc);
-    target_vel_next = tpGetRealTargetVel(tp, nexttc);
+    double acc_this = tpGetScaledAccel(tc);
+    double acc_next = tpGetScaledAccel(nexttc);
 
-    double v_reachable_this = fmin(tpCalculateTriangleVel(tp,tc), target_vel_this);
-    double v_reachable_next = fmin(tpCalculateTriangleVel(tp,nexttc), target_vel_next);
+    double v_reachable_this = fmin(tpCalculateTriangleVel(tc), target_vel_this);
+    double v_reachable_next = fmin(tpCalculateTriangleVel(nexttc), target_vel_next);
 
     /* Compute the maximum allowed blend time for each segment.
      * This corresponds to the minimum acceleration that will just barely reach
@@ -2064,10 +2196,19 @@ STATIC int tpComputeBlendVelocity(TP_STRUCT const * const tp,
     double t_blend = fmin(t_max_reachable, t_max_blend);
 
     // Now, use this blend time to find the best acceleration / velocity for each segment
-    double v_blend_this = fmin(v_reachable_this, t_blend * acc_this);
-    double v_blend_next = fmin(v_reachable_next, t_blend * acc_next);
+    *v_blend_this = fmin(v_reachable_this, t_blend * acc_this);
+    *v_blend_next = fmin(v_reachable_next, t_blend * acc_next);
 
     double theta;
+
+    PmCartesian v1, v2;
+
+    tcGetEndAccelUnitVector(tc, &v1);
+    tcGetStartAccelUnitVector(nexttc, &v2);
+    findIntersectionAngle(&v1, &v2, &theta);
+
+    double cos_theta = cos(theta);
+
     if (tc->tolerance > 0) {
         /* see diagram blend.fig.  T (blend tolerance) is given, theta
          * is calculated from dot(s1, s2)
@@ -2084,25 +2225,45 @@ STATIC int tpComputeBlendVelocity(TP_STRUCT const * const tp,
          * equate the two expressions for d and solve for v
          */
         double tblend_vel;
-        PmCartesian v1, v2;
-
-        tcGetEndAccelUnitVector(tc, &v1);
-        tcGetStartAccelUnitVector(nexttc, &v2);
-        findIntersectionAngle(&v1, &v2, &theta);
         /* 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) {
-            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);
+        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);
         }
     }
+    if (v_blend_net) {
+        /*
+         * Find net velocity in the direction tangent to the blend.
+         * When theta ~ 0, net velocity in tangent direction is very small.
+         * When the segments are nearly tangent (theta ~ pi/2), the blend
+         * velocity is almost entirely in the tangent direction.
+         */
+        *v_blend_net = sin(theta) * (*v_blend_this + *v_blend_next) / 2.0;
+    }
 
-    tc->blend_vel = v_blend_this;
-    nexttc->blend_vel = v_blend_next;
     return TP_ERR_OK;
 }
 
+STATIC double estimateParabolicBlendPerformance(
+        TP_STRUCT const *tp,
+        TC_STRUCT const *tc,
+        TC_STRUCT const *nexttc)
+{
+    double v_this = 0.0, v_next = 0.0;
+
+    // Use maximum possible target velocity to get best-case performance
+    double target_vel_this = tpGetMaxTargetVel(tp, tc);
+    double target_vel_next = tpGetMaxTargetVel(tp, nexttc);
+
+    double v_net = 0.0;
+    tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, &v_net);
+
+    return v_net;
+}
+
+
 
 /**
  * Calculate distance update from velocity and acceleration.
@@ -2187,7 +2348,7 @@ void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const t
 
     /* Calculations for desired velocity based on trapezoidal profile */
     double dx = tc->target - tc->progress;
-    double maxaccel = tpGetScaledAccel(tp, tc);
+    double maxaccel = tpGetScaledAccel(tc);
 
     double discr_term1 = pmSq(tc_finalvel);
     double discr_term2 = maxaccel * (2.0 * dx - tc->currentvel * tc->cycle_time);
@@ -2265,7 +2426,7 @@ STATIC int tpCalculateRampAccel(TP_STRUCT const * const tp,
     double acc_final = dv / dt;
 
     // Saturate estimated acceleration against maximum allowed by segment
-    double acc_max = tpGetScaledAccel(tp, tc);
+    double acc_max = tpGetScaledAccel(tc);
 
     // Output acceleration and velocity for position update
     *acc = saturate(acc_final, acc_max);
@@ -2421,6 +2582,9 @@ STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const
 STATIC void tpUpdateBlend(TP_STRUCT * const tp, TC_STRUCT * const tc,
         TC_STRUCT * const nexttc) {
 
+    if (!nexttc) {
+        return;
+    }
     double save_vel = nexttc->target_vel;
 
     if (tpGetFeedScale(tp, nexttc) > TP_VEL_EPSILON) {
@@ -2440,7 +2604,6 @@ STATIC void tpUpdateBlend(TP_STRUCT * const tp, TC_STRUCT * const tc,
     tpUpdateCycle(tp, nexttc, NULL);
     //Restore the original target velocity
     nexttc->target_vel = save_vel;
-
 }
 
 
@@ -2521,7 +2684,7 @@ STATIC int tpCompleteSegment(TP_STRUCT * const tp,
  * Handle an abort command.
  * Based on the current motion state, handle the consequences of an abort command.
  */
-STATIC int tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc,
+STATIC tp_err_t tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc,
         TC_STRUCT * const nexttc) {
 
     if(!tp->aborting) {
@@ -2556,7 +2719,7 @@ STATIC int tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc,
  * has not reached the requested speed, or the spindle index has not been
  * detected.
  */
-STATIC int tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc)
+STATIC tp_err_t tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc)
 {
 
     // this is no longer the segment we were waiting_for_index for
@@ -2598,17 +2761,15 @@ STATIC int tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc)
             tp->spindle.revs = 0;
         }
     }
-
     return TP_ERR_OK;
 }
 
-
 /**
  * "Activate" a segment being read for the first time.
  * This function handles initial setup of a new segment read off of the queue
  * for the first time.
  */
-STATIC int tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) {
+STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) {
 
     //Check if already active
     if (!tc || tc->active) {
@@ -2692,7 +2853,7 @@ STATIC int tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) {
  * Run velocity mode synchronization.
  * Update requested velocity to follow the spindle's velocity (scaled by feed rate).
  */
-STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT const * nexttc) {
+STATIC void tpSyncVelocityMode(TC_STRUCT * const tc, TC_STRUCT * const nexttc) {
     double speed = emcmotStatus->spindleSpeedIn;
     double pos_error = fabs(speed) * tc->uu_per_rev;
     // Account for movement due to parabolic blending with next segment
@@ -2700,6 +2861,12 @@ STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_ST
         pos_error -= nexttc->progress;
     }
     tc->target_vel = pos_error;
+
+    if (nexttc && nexttc->synchronized) {
+        //If the next move is synchronized too, then match it's
+        //requested velocity to the current move
+        nexttc->target_vel = tc->target_vel;
+    }
 }
 
 
@@ -2756,7 +2923,7 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc,
         double errorvel;
         spindle_vel = (tp->spindle.revs - oldrevs) / tp->cycleTime;
         target_vel = spindle_vel * tc->uu_per_rev;
-        errorvel = pmSqrt(fabs(pos_error) * tpGetScaledAccel(tp,tc));
+        errorvel = pmSqrt(fabs(pos_error) * tpGetScaledAccel(tc));
         if(pos_error<0) {
             errorvel *= -1.0;
         }
@@ -2873,6 +3040,7 @@ STATIC int tpUpdateInitialStatus(TP_STRUCT const * const tp) {
     emcmotStatus->tcqlen = tcqLen(&tp->queue);
     // Set default value for requested speed
     emcmotStatus->requested_vel = 0.0;
+    emcmotStatus->current_vel = 0.0;
     return TP_ERR_OK;
 }
 
@@ -2962,7 +3130,7 @@ STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc,
 
     //If this is a valid acceleration, then we're done. If not, then we solve
     //for v_f and dt given the max acceleration allowed.
-    double a_max = tpGetScaledAccel(tp,tc);
+    double a_max = tpGetScaledAccel(tc);
 
     //If we exceed the maximum acceleration, then the dt estimate is too small.
     double a = a_f;
@@ -3004,6 +3172,7 @@ STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc,
         tcSetSplitCycle(tc, dt, v_f);
     } else {
         tc_debug_print(" dt = %f, not at end yet\n",dt);
+        return TP_ERR_NO_ACTION;
     }
     return TP_ERR_OK;
 }
@@ -3099,7 +3268,18 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp,
 
     /* Parabolic blending */
 
-    tpComputeBlendVelocity(tp, tc, nexttc);
+    double v_this = 0.0, v_next = 0.0;
+
+    // cap the blend velocity at the current requested speed (factoring in feed override)
+    double target_vel_this = tpGetRealTargetVel(tp, tc);
+    double target_vel_next = tpGetRealTargetVel(tp, nexttc);
+
+    tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL);
+    tc->blend_vel = v_this;
+    if (nexttc) {
+        nexttc->blend_vel = v_next;
+    }
+
     if (nexttc && tcIsBlending(tc)) {
         tpDoParabolicBlending(tp, tc, nexttc);
     } else {
@@ -3133,6 +3313,13 @@ int tpRunCycle(TP_STRUCT * const tp, long period)
     //Set GUI status to "zero" state
     tpUpdateInitialStatus(tp);
 
+#ifdef TC_DEBUG
+    //Hack debug output for timesteps
+    // NOTE: need to track every timestep, even those where the trajectory planner is idle
+    static double time_elapsed = 0;
+    time_elapsed+=tp->cycleTime;
+#endif
+
     //If we have a NULL pointer, then the queue must be empty, so we're done.
     if(!tc) {
         tpHandleEmptyQueue(tp);
@@ -3141,11 +3328,6 @@ int tpRunCycle(TP_STRUCT * const tp, long period)
 
     tc_debug_print("-------------------\n");
 
-#ifdef TC_DEBUG
-    //Hack debug output for timesteps
-    static double time_elapsed = 0;
-    time_elapsed+=tp->cycleTime;
-#endif
 
     /* If the queue empties enough, assume that the program is near the end.
      * This forces the last segment to be "finalized" to let the optimizer run.*/
@@ -3183,7 +3365,7 @@ int tpRunCycle(TP_STRUCT * const tp, long period)
             break;
         case TC_SYNC_VELOCITY:
             tp_debug_print("sync velocity\n");
-            tpSyncVelocityMode(tp, tc, nexttc);
+            tpSyncVelocityMode(tc, nexttc);
             break;
         case TC_SYNC_POSITION:
             tp_debug_print("sync position\n");
diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h
index cb272e251e3..fbeb45806fd 100644
--- a/src/emc/tp/tp_types.h
+++ b/src/emc/tp/tp_types.h
@@ -132,4 +132,17 @@ typedef struct {
 
 } TP_STRUCT;
 
+
+/**
+ * Describes blend modes used in the trajectory planner.
+ * @note these values are used as array indices, so make sure valid options
+ * start at 0 and increase by one.
+ */
+typedef enum {
+    NO_BLEND = -1,
+    PARABOLIC_BLEND,
+    TANGENT_SEGMENTS_BLEND,
+    ARC_BLEND
+} tc_blend_type_t;
+
 #endif				/* TP_TYPES_H */