motion: defer mode transitions instead of dropping them when not in position

[grandixximo]

Motion's set_operating_mode() honored a requested mode change (teleop, coord, free) only if motion was in position in the single servo cycle that processed the request; otherwise the request was silently erased. Task acknowledges EMC_TASK_SET_MODE as soon as it is read and flips mdiOrAuto immediately, so a dropped transition left task mode and traj mode permanently diverged: determineMode() reports MANUAL whenever traj is FREE/TELEOP, nothing re-issues the request, and no error is reported anywhere. The mode machine stays wedged until some unrelated mode command happens by.

This is the likely cause of the intermittent halui/mdi CI failures tracked in #4283 ("timeout waiting for task mode" / "halui to report mode"). Those are wedges, not slowness, so no timeout bump can fix them.

Fix: keep the request pending when not in position; the transition completes as soon as motion comes to rest, matching the documented EMCMOT_COORD contract ("can be done at any time") and the existing deferred-enable behavior.

Verification:

• deterministic repro: requesting MANUAL mid-MDI-move wedges task mode at AUTO on master; with this change the mode converges to MANUAL when the move ends
• halui (jogging, mdi), linuxcncrsh, mdi-queue and abort runtests pass
• 60/60 halui/mdi iterations under CPU and IO load

[BsAtHome]

So:

1. motion-command-handler sets the teleoperating/coordinating flags when the command arrives
2. cases:
   • motion-controller resets teleoperating flag when in motion but the mode change did not happen
   • motion-controller sets/resets coordinating flag (depending on previous state) when in motion but the mode change did not happen
3. command from task is lost and expected state change has evaporated

I guess that multiple rapid sent commands about mode changes would not matter as long as motion is in progress. Both flags would be overwritten each time when motion-command-handler runs.

There is a side-effect when going to teleop mode. The switch_to_teleop_mode() function (motion.c:169) is called from command.c:624 and will alter the joint's free_tp.enable setting unconditionally. That means changing from one to the other would alter the state before the actual change is in effect. How does this affect the system?

[grandixximo]

Good catch, I looked at that path in detail and ran some tests on it.

switch_to_teleop_mode() does three things at command time: refuses if non-identity kins and not all homed, clears free_tp.enable on all joints, and sets the teleoperating/coordinating request flags. Effects while the actual transition is still pending:

1. In COORD mode (coordinated move running): free_tp is not in the output path at all, pos_cmd comes from coord_tp via the cubics in get_pos_cmds(), so clearing free_tp.enable is a no-op and the running move is not disturbed. The request flags are only read by set_operating_mode(); motion_state, which selects the active planner, only flips at INPOS.

2. Via task this window barely overlaps real motion anyway: switching from MDI/AUTO to MANUAL aborts motion first (emctaskmain.cc, EMC_TASK_SET_MODE_TYPE), and jogs block mode changes entirely (jogging_is_active in emcTaskSetMode). So in practice the pending window overlaps at most the abort decel.

3. If a joint jog were somehow active anyway (only a direct EMCMOT_TELEOP writer could produce that): simple_tp with enable=0 ramps velocity to zero at the accel limit and holds position (pos_cmd = curr_pos while disabled), so a controlled stop, no position step. The joint comes to rest, INPOS asserts, the transition completes.

4. Homing: for non-identity kins the allhomed check refuses. For identity kins a TELEOP request mid-home would clear free_tp.enable and the home state machine aborts with "end of move" (homing.c). Note this behavior is identical before and after this PR: the command-time free_tp.enable = 0 and the flag writes are unchanged, the PR only changes whether the pending request survives until INPOS instead of being erased.

Tested on a sim config (trivkins):

• mid-MDI-move MANUAL request: move aborts (by design), clean stop, zero following error, mode converges to MANUAL at rest
• jog after the deferred transition works
• TELEOP => COORD re-entry and further MDI moves work
• rapid MANUAL-then-MDI during a move: newest request wins, MDI functional afterwards

So I think the early free_tp.enable = 0 is harmless in every path task can produce, and the one case where it bites (identity-kins homing interrupted by a mode request) predates this change.