From 47abd6ac66394591b21446de951bc304f1a77edb Mon Sep 17 00:00:00 2001
From: is-primary-dev <215415441+is-primary-dev@users.noreply.github.com>
Date: Sun, 12 Apr 2026 23:23:14 -0700
Subject: [PATCH] motion: axis_sync_teleop_tp_to_carte_pos resets
 velocity/acceleration
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Without this, residual teleop_tp.curr_vel from an aborted jog can
survive the FREE -> TELEOP transition. axis_sync_teleop_tp_to_carte_pos
is called in two places: enabling motion from DISABLED, and entering
teleop mode (e.g. after homing finishes). Both semantically represent
the joint being at rest at the current cartesian position — leaving
the trajectory planner's velocity and acceleration at stale non-zero
values causes the first simple_tp_update_normal call after the sync
to integrate one cycle of motion, drifting curr_pos away from the
synced position.

Reproduction on a XYZZ machine where Z homes at MAX_LIMIT:
  1. Home all
  2. Jog Z in the minus direction
  3. Release the jog key (axis_jog_abort(immediate=0) runs, leaving
     teleop_tp.curr_vel partially decayed but non-zero)
  4. Re-home all (homing runs in FREE mode, does not touch teleop_tp)
  5. Motion mode transitions FREE -> TELEOP on homing completion
  6. axis_sync_teleop_tp_to_carte_pos sets curr_pos to the current
     (correct) cartesian position but leaves curr_vel non-zero
  7. The first axis_calc_motion -> simple_tp_update_normal call
     integrates one cycle of motion, drifting curr_pos away
  8. Result: homed Z lands offset from HOME by distance equal to the
     area under the velocity decay curve, not at HOME exactly

Two-line fix matching the function's documented intent.
---
 src/emc/motion/axis.c | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/src/emc/motion/axis.c b/src/emc/motion/axis.c
index d68220d6c09..25b74309be3 100644
--- a/src/emc/motion/axis.c
+++ b/src/emc/motion/axis.c
@@ -440,10 +440,22 @@ void axis_sync_teleop_tp_to_carte_pos(int extfactor, double *pcmd_p[])
 {
     int n;
     // expect extfactor =  -1 || 0 || +1
+    //
+    // This function initializes the teleop trajectory planner to a REST
+    // state at the current cartesian position. Both callers (enabling
+    // motion, entering teleop mode) expect the joint to be stationary
+    // at the synced position. Without resetting curr_vel/curr_acc, stale
+    // values from a previous motion (e.g. a jog that was aborted with
+    // immediate=0, leaving a non-zero ramp-down velocity) would survive
+    // a mode transition and cause the trajectory to drift away from the
+    // synced curr_pos the next time simple_tp_update integrates one
+    // cycle of motion.
     for (n = 0; n < EMCMOT_MAX_AXIS; n++) {
         axis_array[n].teleop_tp.curr_pos = *pcmd_p[n]
                             + extfactor * axis_array[n].ext_offset_tp.curr_pos;
         axis_array[n].teleop_tp.pos_cmd = axis_array[n].teleop_tp.curr_pos;
+        axis_array[n].teleop_tp.curr_vel = 0.0;
+        axis_array[n].teleop_tp.curr_acc = 0.0;
     }
 }