task: do not take a motion status from before a command as current (#3650)

src/emc/motion-logger/motion-logger.c

@@ -708,6 +708,8 @@ int main(int argc, char* argv[]) {
 
         update_joint_status();
 
+        /* there is no controller here, so count the command as acted on */
+        emcmotStatus->commandHeartbeat = emcmotStatus->heartbeat++;
         emcmotStatus->commandEcho = c->command;
         emcmotStatus->commandNumEcho = c->commandNum;
         emcmotStatus->commandStatus = EMCMOT_COMMAND_OK;

src/emc/motion/command.c

@@ -434,6 +434,7 @@ void emcmotCommandHandler_locked(void *arg, long servo_period)
 	emcmotInternal->head++;
 
 	/* got a new command-- echo command and number... */
+	emcmotStatus->commandHeartbeat = emcmotStatus->heartbeat;
 	emcmotStatus->commandEcho = emcmotCommand->command;
 	emcmotStatus->commandNumEcho = emcmotCommand->commandNum;
 

src/emc/motion/motion.c

@@ -869,6 +869,7 @@ static int init_comm_buffers(void)
     emcmotStatus->head = 0;
     emcmotStatus->commandEcho = 0;
     emcmotStatus->commandNumEcho = 0;
+    emcmotStatus->commandHeartbeat = 0;
     emcmotStatus->commandStatus = 0;
 
     /* init more stuff */

src/emc/motion/motion.h

@@ -584,9 +584,12 @@ Suggestion: Split this in to an Error and a Status flag register..
 
     typedef struct emcmot_status_t {
 	unsigned char head;	/* flag count for mutex detect */
-	/* these three are updated only when a new command is handled */
+	/* these four are updated only when a new command is handled */
 	cmd_code_t commandEcho;	/* echo of input command */
 	int commandNumEcho;	/* echo of input command number */
+	rtapi_u64 commandHeartbeat;	/* heartbeat when the command was echoed:
+				   the controller has acted on the command
+				   once heartbeat has moved past it */
 	cmd_status_t commandStatus;	/* result of most recent command */
 	/* these are config info, updated when a command changes them */
 	double feed_scale;	/* velocity scale factor for all motion but rapids */
@@ -638,7 +641,7 @@ Suggestion: Split this in to an Error and a Status flag register..
 /*! \todo FIXME - all structure members beyond this point are in limbo */
 
 	/* dynamic status-- changes every cycle */
-	uint64_t heartbeat;     /* Incremented every time the motion controller is done. */
+	rtapi_u64 heartbeat;     /* Incremented every time the motion controller is done. */
 	int config_num;		/* incremented whenever configuration
 				   changed. */
 	int id;			/* id for executing motion */

src/emc/task/taskintf.cc

@@ -2073,6 +2073,18 @@ int emcSpindleUpdate(EMC_SPINDLE_STAT stat[], int num_spindles){
     return 0;
 }
 
+// A failed read leaves stat as it was.  If that copy is older than the
+// last command sent, motion may already be running it, so do not report
+// it done: a probe just queued would look finished.
+static int emcMotionUpdateFailed(EMC_MOTION_STAT * stat)
+{
+    if (stat->status == RCS_STATUS::DONE &&
+        stat->echo_serial_number != emcmotCommand.commandNum) {
+        stat->status = RCS_STATUS::EXEC;
+    }
+    return -1;
+}
+
 int emcMotionUpdate(EMC_MOTION_STAT * stat)
 {
     int r1, r2, r3, r4;
@@ -2083,12 +2095,12 @@ int emcMotionUpdate(EMC_MOTION_STAT * stat)
 
     // read the emcmot status
     if (0 != usrmotReadEmcmotStatus(&emcmotStatus)) {
-	return -1;
+	return emcMotionUpdateFailed(stat);
     }
     new_config = 0;
     if (emcmotStatus.config_num != emcmotConfig.config_num) {
 	if (0 != usrmotReadEmcmotConfig(&emcmotConfig)) {
-	    return -1;
+	    return emcMotionUpdateFailed(stat);
 	}
 	new_config = 1;
     }
@@ -2171,6 +2183,11 @@ int emcMotionUpdate(EMC_MOTION_STAT * stat)
     } else if (stat->traj.status == RCS_STATUS::EXEC) {
 	exec = 1;
     }
+    // what the controller makes of the last command (queue depth, in
+    // position, enable) shows only once it has run after taking it
+    if (emcmotStatus.heartbeat <= emcmotStatus.commandHeartbeat) {
+        exec = 1;
+    }
 
     if (error) {
 	stat->status = RCS_STATUS::ERROR;

tests/probe-queue-not-empty/README

@@ -0,0 +1,15 @@
+Runs 100 G38.2 probes in AUTO mode and fails on "Queue is not empty
+after probing" (#3650).
+
+After a probe, read-ahead waits for the EMC_TASK_PLAN_SYNCH that follows
+it, then reads the next line, whose read_inputs() wants an empty motion
+queue.  Motion acknowledges the probe command in motion-command-handler
+but only updates the queue depth and in-position flag in
+motion-controller.  A status snapshot taken in between still shows motion
+idle, so the SYNCH completes at once, and if the next snapshot shows the
+probe in the queue, read_inputs() fails.
+
+That gap is normally a few microseconds wide.  probe.hal puts servo_delay
+between the two functions, a busy wait that varies on each servo period,
+so about half of the probes see it.  Task must not clear the read-ahead
+wait while the queue it is about to check is not empty.

tests/probe-queue-not-empty/checkresult

@@ -0,0 +1,3 @@
+#!/bin/sh
+# Test passes if test-ui.py returns successfully
+exit 0

tests/probe-queue-not-empty/postgui.hal

@@ -0,0 +1 @@
+net done-count <= motion.analog-out-00 => test-ui.done-count

tests/probe-queue-not-empty/probe.hal

@@ -0,0 +1,14 @@
+# the probe trips when Z goes below -0.5
+loadrt comp names=probe-comp
+addf probe-comp servo-thread
+net Zpos => probe-comp.in0
+setp probe-comp.in1 -0.5
+net probe probe-comp.out => motion.probe-input
+
+# Stall the servo thread for a varying time between the command handler
+# and the controller, as preemption or a latency spike would.  Motion
+# has then acknowledged a new command while its status still shows the
+# old queue depth.
+loadrt servo_delay
+addf servo-delay.0 servo-thread 2
+setp servo-delay.0.max-ns 200000

tests/probe-queue-not-empty/servo_delay.comp

@@ -0,0 +1,19 @@
+component servo_delay "Busy wait for a varying time on each run, to widen the gap between two functions";
+pin in uint max_ns = 0 "Upper bound of the wait in ns, 0 disables it";
+param rw uint step_ns = 7919 "Amount the wait grows by on each run, modulo max-ns";
+variable rtapi_uint wait_ns;
+option period no;
+function _;
+license "GPL";
+author "Luca Toniolo";
+;;
+FUNCTION(_) {
+    rtapi_sint end;
+
+    if (max_ns == 0)
+        return;
+    wait_ns = (wait_ns + step_ns) % max_ns;
+    end = rtapi_get_time() + (rtapi_sint)wait_ns;
+    while (rtapi_get_time() < end)
+        ;
+}

tests/probe-queue-not-empty/simpockets.tbl

@@ -0,0 +1 @@
+T1 P1 Z0.0 D0.0 ;

tests/probe-queue-not-empty/skip

@@ -0,0 +1,4 @@
+#!/bin/sh
+# Builds a realtime component with halcompile, which needs the build
+# tools present.  Skip when testing installed packages.
+[ -z "$SYSTEM_BUILD" ]

tests/probe-queue-not-empty/test-ui.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+
+import linuxcnc
+import hal
+
+import time
+import sys
+import os
+
+c = linuxcnc.command()
+s = linuxcnc.stat()
+e = linuxcnc.error_channel()
+
+h = hal.component("test-ui")
+h.newpin("done-count", hal.Type.REAL, hal.Dir.IN)
+h.ready()
+os.system("halcmd source ./postgui.hal")
+
+def wait_for(what, done):
+    start = time.time()
+    while True:
+        s.poll()
+        if done():
+            return
+        if time.time() - start > 10:
+            sys.stderr.write("timeout waiting for %s\n" % what)
+            sys.exit(1)
+        time.sleep(0.05)
+
+c.state(linuxcnc.STATE_ESTOP_RESET)
+c.wait_complete()
+c.state(linuxcnc.STATE_ON)
+c.wait_complete()
+wait_for("machine on", lambda: s.task_state == linuxcnc.STATE_ON)
+
+# one joint at a time: a home command while another joint homes is refused
+for joint in range(3):
+    c.home(joint)
+    wait_for("joint %d homed" % joint, lambda: s.homed[joint]
+             and not any(j["homing"] for j in s.joint[:3]))
+c.mode(linuxcnc.MODE_AUTO)
+c.wait_complete()
+
+c.program_open('test.ngc')
+c.auto(linuxcnc.AUTO_RUN, 0)
+
+start = time.time()
+while True:
+    s.poll()
+    if s.interp_state != linuxcnc.INTERP_IDLE:
+        break
+    if time.time() - start > 10:
+        sys.stderr.write("timeout waiting for the program to start\n")
+        sys.exit(1)
+    time.sleep(0.001)
+
+errors = []
+while True:
+    err = e.poll()
+    if err:
+        errors.append(err[1])
+    s.poll()
+    if s.interp_state == linuxcnc.INTERP_IDLE:
+        break
+    if time.time() - start > 60:
+        sys.stderr.write("timeout waiting for the program to finish\n")
+        sys.exit(1)
+    time.sleep(0.05)
+
+err = e.poll()
+while err:
+    errors.append(err[1])
+    err = e.poll()
+
+for msg in errors:
+    sys.stderr.write("error: %s\n" % msg)
+sys.stderr.write("probes done: %d\n" % h["done-count"])
+if errors or h["done-count"] != 100:
+    sys.exit(1)
+sys.exit(0)

tests/probe-queue-not-empty/test.ini

@@ -0,0 +1,86 @@
+[EMC]
+VERSION = 1.1
+DEBUG = 0x0
+
+[DISPLAY]
+DISPLAY = ./test-ui.py
+
+[TASK]
+TASK = milltask
+CYCLE_TIME = 0.001
+
+[RS274NGC]
+PARAMETER_FILE = sim.var
+
+[EMCMOT]
+EMCMOT = motmod
+COMM_TIMEOUT = 4.0
+BASE_PERIOD = 0
+SERVO_PERIOD = 1000000
+
+[EMCIO]
+TOOL_TABLE = simpockets.tbl
+
+[HAL]
+HALFILE = LIB:core_sim.hal
+HALFILE = probe.hal
+
+[TRAJ]
+NO_FORCE_HOMING = 1
+COORDINATES = X Y Z
+LINEAR_UNITS = inch
+ANGULAR_UNITS = degree
+DEFAULT_LINEAR_VELOCITY = 10
+MAX_LINEAR_VELOCITY = 100
+
+[KINS]
+KINEMATICS = trivkins
+JOINTS = 3
+
+[AXIS_X]
+MIN_LIMIT = -40.0
+MAX_LIMIT = 40.0
+MAX_VELOCITY = 100
+MAX_ACCELERATION = 10000.0
+
+[JOINT_0]
+TYPE = LINEAR
+HOME = 0.0
+MAX_VELOCITY = 100
+MAX_ACCELERATION = 10000.0
+MIN_LIMIT = -40.0
+MAX_LIMIT = 40.0
+FERROR = 0.050
+MIN_FERROR = 0.010
+
+[AXIS_Y]
+MIN_LIMIT = -40.0
+MAX_LIMIT = 40.0
+MAX_VELOCITY = 100
+MAX_ACCELERATION = 10000.0
+
+[JOINT_1]
+TYPE = LINEAR
+HOME = 0.0
+MAX_VELOCITY = 100
+MAX_ACCELERATION = 10000.0
+MIN_LIMIT = -40.0
+MAX_LIMIT = 40.0
+FERROR = 0.050
+MIN_FERROR = 0.010
+
+[AXIS_Z]
+MIN_LIMIT = -40.0
+MAX_LIMIT = 40.0
+MAX_VELOCITY = 100
+MAX_ACCELERATION = 10000.0
+
+[JOINT_2]
+TYPE = LINEAR
+HOME = 0.0
+MAX_VELOCITY = 100
+MAX_ACCELERATION = 10000.0
+MIN_LIMIT = -40.0
+MAX_LIMIT = 40.0
+FERROR = 0.050
+MIN_FERROR = 0.010

tests/probe-queue-not-empty/test.ngc

@@ -0,0 +1,14 @@
+G20 G90 G94
+G0 Z0
+#<n> = 0
+o100 while [#<n> LT 100]
+  G0 Z-0.4
+  G38.2 Z-1 F600
+  #<n> = [#<n> + 1]
+o100 endwhile
+o110 if [[#5070 NE 1] OR [ABS[#5063 + 0.5] GT 0.02]]
+  (abort, bad probe result Z#5063 success #5070)
+o110 endif
+G0 Z0
+M68 E0 Q#<n>
+M2

tests/probe-queue-not-empty/test.sh

@@ -0,0 +1,5 @@
+#!/bin/bash
+set -e
+
+halcompile --install servo_delay.comp >/dev/null
+linuxcnc -r test.ini

tests/stale-motion-status/README

@@ -0,0 +1,22 @@
+Task must not take a motion status from before a command as current.
+
+Motion acknowledges a command in motion-command-handler and acts on it
+later, in motion-controller: it queues the move, enables the machine,
+switches mode.  A status read in between, or a failed read that leaves
+task with its copy from before the command, shows motion idle.  Task
+then finishes the command too soon: wait_complete() returns before the
+machine is on or in auto mode, and the line after a G38 move takes the
+result of the probe before, or fails with "Queue is not empty after
+probing" (#3650).
+
+Both tests check that the machine is on and in auto mode once
+wait_complete() returns, then probe at alternating heights and fail if
+a probe reads the height of the one before.
+
+late-controller runs motion-controller before motion-command-handler,
+so a whole servo period passes between the two.  The period is 50 ms,
+longer than the var file write task does when it synchs after a probe.
+
+slow-controller uses kinematics that busy wait, so motion-controller
+takes most of the servo period and status reads by task often fail
+while it runs.

tests/stale-motion-status/late-controller/checkresult

@@ -0,0 +1,3 @@
+#!/bin/sh
+# Test passes if test-ui.py returns successfully
+exit 0

tests/stale-motion-status/late-controller/postgui.hal

@@ -0,0 +1 @@
+../postgui.hal

tests/stale-motion-status/late-controller/probe.hal

@@ -0,0 +1,12 @@
+# the probe trips when Z goes below the height set with M68 E1
+loadrt comp names=probe-comp
+addf probe-comp servo-thread
+net Zpos => probe-comp.in0
+net probe-height motion.analog-out-01 => probe-comp.in1
+net probe probe-comp.out => motion.probe-input
+
+# Run the controller before the command handler, so that motion
+# acknowledges a command a whole servo period before the controller
+# acts on it.
+delf motion-command-handler servo-thread
+addf motion-command-handler servo-thread 2

tests/stale-motion-status/late-controller/simpockets.tbl

@@ -0,0 +1 @@
+../simpockets.tbl

tests/stale-motion-status/late-controller/test-ui.py

@@ -0,0 +1 @@
+../test-ui.py