fix velocity spikes at segment junctions under feed override

Two root causes addressed:

1. Backward-pass missing kink constraint (motion_planning_9d.cc):
   The backward pass computed prev_tc's final_vel without applying
   tc->kink_vel (the junction kink at the prev_tc→tc boundary).
   Only prev_tc->kink_vel (its own entry kink) was applied, leaving
   the predecessor free to exit faster than the downstream junction
   allows.  Add tc->kink_vel as Constraint 4 in
   tpComputeOptimalVelocity_9D so the predecessor's exit is always
   capped to the physical junction limit.

2. Stale-feed profile v0 mismatch at handoff (tp.c, tc_types.h):
   When feed override changes after a profile is written but before
   RT reaches that segment, profile.v[0] reflects the old feed while
   the actual junction velocity reflects the new feed.  tpUpdateCycle
   samples the profile at t=0, snapping currentvel to the stale v0.
   Fix: stamp each profile with written_at_feed (the committed feed
   at write time).  At split-cycle handoff, when the feed drift
   exceeds 5%, clamp nexttc->currentvel to the physical junction_vel
   and correct progress/position_base proportionally.  Userspace
   re-converges a fresh profile within 1-2 cycles.

Author: Luca Toniolo

src/emc/motion_planning/motion_planning_9d.cc

@@ -1116,10 +1116,12 @@ double tpComputeOptimalVelocity_9D(TC_STRUCT const * const tc,
     // All inputs are in physical mm/s (feed-scaled). jerkLimitedMaxEntryVelocity
     // is nonlinear (braking distance ∝ v²), so computing in scaled space avoids
     // the systematic gap that appears when the forward pass re-checks at feed≠1.
-    // Use trapezoidal for v_f≈0 (tail/STOP segments) for fast velocity recovery
-    // in the backward chain.
+    // Always use jerk-limited formula, including v_f=0 (STOP/EXACT segments).
+    // Trapezoidal (sqrt(2*a*d)) ignores jerk and overestimates the safe approach
+    // velocity for short STOP segments — the predecessor gets too-high final_vel,
+    // which the forward pass then delivers to the STOP segment causing overshoot.
     double vs_back;
-    if (jrk_this > 0.0 && v_f_this > 1e-6) {
+    if (jrk_this > 0.0) {
         vs_back = jerkLimitedMaxEntryVelocity(v_f_this, tc->target, acc_this, jrk_this);
     } else {
         vs_back = sqrt(v_f_this * v_f_this + 2.0 * acc_this * tc->target);
@@ -1128,13 +1130,19 @@ double tpComputeOptimalVelocity_9D(TC_STRUCT const * const tc,
     // Constraint 2: Current segment velocity limit (scaled to physical mm/s)
     double vf_limit_this = tcGetPlanMaxTargetVel(tc, feed_tc);
 
-    // Constraint 3: Previous segment velocity limit (with kink)
-    // kink_vel is an absolute physical limit — applyKinkVelLimit caps correctly
+    // Constraint 3: Previous segment velocity limit (with kink at its own entry)
+    // prev_tc->kink_vel is the kink at the junction before prev_tc.
     double v_max_prev = tcGetPlanMaxTargetVel(prev_tc, feed_prev);
     double vf_limit_prev = applyKinkVelLimit(prev_tc, v_max_prev);
 
+    // Constraint 4: Junction kink at the prev_tc→tc boundary.
+    // tc->kink_vel is the physical kink limit for *entering* tc, which equals
+    // the *exit* velocity of prev_tc.  The backward pass must enforce this here
+    // so prev_tc's final_vel never exceeds the junction constraint ahead of it.
+    double vf_kink_tc = (tc->kink_vel > 0.0) ? tc->kink_vel : 1e9;
+
     // Return minimum of all constraints
-    double v_optimal = std::min({vs_back, vf_limit_this, vf_limit_prev});
+    double v_optimal = std::min({vs_back, vf_limit_this, vf_limit_prev, vf_kink_tc});
 
     return v_optimal;
 }
@@ -1554,6 +1562,7 @@ static bool computeAndStoreProfile(TC_STRUCT *tc, const RuckigProfileParams &p)
         tc->shared_9d.profile.computed_vLimit = p.vLimit;
         tc->shared_9d.profile.computed_desired_fvel = p.desired_fvel;
         tc->shared_9d.profile.locked = (result == ruckig::Result::Working) ? 1 : 0;
+        tc->shared_9d.profile.written_at_feed  = g_feed_mgr.committed_feed;
         copyRuckigProfile(traj, &tc->shared_9d.profile);
         return true;
     } else {
@@ -2289,7 +2298,6 @@ static int replanForward(TP_STRUCT *tp, double v0_override, double budget_sec)
                     scaled_v_entry = fmin(scaled_v_entry, prev_tc->kink_vel);
             }
         }
-
         // Target exit velocity from backward pass
         double v_exit = (tc->term_cond == TC_TERM_COND_TANGENT)
             ? readFinalVelCapped(tc) : 0.0;
@@ -2441,7 +2449,12 @@ static int replanForward(TP_STRUCT *tp, double v0_override, double budget_sec)
         bool bidir_restore_fired = false;
         if (pre_bidir_v_entry - scaled_v_entry > 0.1 && i > 0) {
             TC_STRUCT *prev_tc_fix = tcqItem_user(queue, i - 1);
-            if (prev_tc_fix && (prev_tc_fix->active || prev_was_corridor)) {
+            // Don't restore for STOP/EXACT: the machine must stop regardless.
+            // Restoring a high v_entry and then setting v_exit=min_exit>0 produces
+            // a profile that doesn't stop at a STOP segment — causing overshoot.
+            // Accept the v0-gap spike instead; it's less bad than position overshoot.
+            if (prev_tc_fix && (prev_tc_fix->active || prev_was_corridor) &&
+                tc->term_cond == TC_TERM_COND_TANGENT) {
                 scaled_v_entry = pre_bidir_v_entry;
                 // Override exit with physics-based minimum from braking.
                 double min_exit = jerkLimitedMinExitVelocity(

src/emc/tp/tc_types.h

@@ -210,6 +210,7 @@ typedef struct {
     volatile int generation;                // Incremented each time profile is rewritten (stopwatch reset detection)
     double duration;                        // Total profile duration (seconds)
     double computed_feed_scale;             // Feed scale when profile was computed
+    double written_at_feed;                 // committed_feed when profile was written (planner-side)
     double computed_vel_limit;              // Velocity limit when profile was computed (for vLimit change detection)
     double computed_vLimit;                 // tp->vLimit when profile was computed (absolute cap, not scaled by feed)
     double computed_desired_fvel;            // Desired final velocity when profile was computed (for convergence detection)

src/emc/tp/tp.c

@@ -4917,7 +4917,6 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc,
                 double maxacc_next = tcGetTangentialMaxAccel(nexttc);
                 nexttc->currentacc = saturate(tc->currentacc, maxacc_next);
 
-
             } else {
                 // Trapezoidal: can use term_vel (assumes instant velocity change)
                 nexttc->currentvel = tc->term_vel;
@@ -5127,44 +5126,41 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc,
         }
     }
 
-    // Correct profile v0 mismatch at split junction.
+    // Stale-feed v0 clamp at split junction.
     // When feed override changes between profile computation and junction
-    // arrival, the profile's v[0] doesn't match the actual junction velocity.
-    // tpUpdateCycle samples the profile and overwrites currentvel with the
-    // profile velocity, creating a discontinuity. Correct by shifting
-    // velocity and position by the mismatch delta. Both progress and
-    // position_base are shifted equally so subsequent cycle displacements
-    // (which depend on delta-progress) are unaffected.
-    // Correct positive v0 mismatch (junction_vel > profile_v0), capped at
-    // one jerk step (maxjerk * dt²) so the velocity correction on cycle 2
-    // (when the profile takes over) stays within the segment's jerk limit.
-    // Negative corrections (junction_vel < profile_v0) are NOT applied:
-    // they shift position_base negative, which causes the segment to land
-    // short of target and trigger a 10-cycle stuck detection stall — worse
-    // than the velocity step itself for small mismatches, and ineffective
-    // for large ones (e.g. feed hold recovery with 40 mm/s gap).
-    // v0_correction DISABLED for testing — OLD code had none.
-    // When enabled, this shifts currentvel/progress/position_base to partially
-    // absorb junction velocity mismatch, but the position_base discontinuity
-    // itself may cause jerk spikes visible in halscope.
-#if 0
+    // arrival, profile.v[0] may not match the actual junction velocity.
+    // tpUpdateCycle samples the profile at t=0 and overwrites currentvel
+    // with profile.v[0], creating a discontinuity.
+    //
+    // Gate: only apply when the profile's written_at_feed differs from the
+    // current canonical_feed_scale by more than 5%.  This avoids perturbing
+    // normal (feed-consistent) profile handoffs.
+    //
+    // Correction: clamp nexttc->currentvel to junction_vel and shift
+    // progress/position_base by the proportional first-cycle displacement
+    // correction so the path position is consistent with the clamped velocity.
+    // Userspace will write a fresh profile at the new feed within 1-2 cycles.
     if (GET_TRAJ_PLANNER_TYPE() == 2 &&
         __atomic_load_n(&nexttc->shared_9d.profile.valid, __ATOMIC_ACQUIRE)) {
-        double profile_v0 = nexttc->shared_9d.profile.v[0];
-        double vel_mismatch = junction_vel - profile_v0;
-        if (vel_mismatch > 0.01) {
-            double dt = tp->cycleTime;
-            double max_pos_corr = nexttc->maxjerk * dt * dt;
-            if (vel_mismatch > max_pos_corr)
-                vel_mismatch = max_pos_corr;
-            nexttc->currentvel += vel_mismatch;
-            // Correct split-cycle displacement to reflect actual velocity
-            double pos_correction = vel_mismatch * nexttc_remain_time;
-            nexttc->progress += pos_correction;
-            nexttc->position_base += pos_correction;
+        double _wf = nexttc->shared_9d.profile.written_at_feed;
+        double _cf = nexttc->shared_9d.canonical_feed_scale;
+        if (_cf > 0.001 && _wf > 0.001 && fabs(_wf - _cf) / _cf > 0.05) {
+            double profile_v0 = nexttc->shared_9d.profile.v[0];
+            double vel_mismatch = junction_vel - profile_v0;
+            if (fabs(vel_mismatch) > 0.5) {
+                // Correct currentvel to physical junction velocity
+                nexttc->currentvel = junction_vel;
+                // Correct first-cycle displacement: was profile_v0*remain_time,
+                // should be junction_vel*remain_time.
+                double pos_correction = vel_mismatch * nexttc_remain_time;
+                nexttc->progress += pos_correction;
+                nexttc->position_base += pos_correction;
+                // Clamp progress to [0, target] to be safe
+                if (nexttc->progress < 0.0) nexttc->progress = 0.0;
+                if (nexttc->progress > nexttc->target) nexttc->progress = nexttc->target;
+            }
         }
     }
-#endif
 
     // Post-correct elapsed_time for Ruckig split cycle.
     // tpUpdateCycle sampled at remain_time then advanced by remain_time internally