diff --git a/src/emc/motion-logger/motion-logger.c b/src/emc/motion-logger/motion-logger.c index 36b77e54528..a499764cbb3 100644 --- a/src/emc/motion-logger/motion-logger.c +++ b/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; diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 07f89dcb890..d9cded5f081 100644 --- a/src/emc/motion/command.c +++ b/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; diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index 30ce42a6315..fa4472f5f9c 100644 --- a/src/emc/motion/motion.c +++ b/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 */ diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 7f3572eda73..f77d0aab4d4 100644 --- a/src/emc/motion/motion.h +++ b/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 */ diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 049ae4d26d4..8080dd92ca1 100644 --- a/src/emc/task/taskintf.cc +++ b/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; diff --git a/tests/probe-queue-not-empty/README b/tests/probe-queue-not-empty/README new file mode 100644 index 00000000000..864928f02a2 --- /dev/null +++ b/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. diff --git a/tests/probe-queue-not-empty/checkresult b/tests/probe-queue-not-empty/checkresult new file mode 100755 index 00000000000..acfd40f1b32 --- /dev/null +++ b/tests/probe-queue-not-empty/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# Test passes if test-ui.py returns successfully +exit 0 diff --git a/tests/probe-queue-not-empty/postgui.hal b/tests/probe-queue-not-empty/postgui.hal new file mode 100644 index 00000000000..a0ab5529dec --- /dev/null +++ b/tests/probe-queue-not-empty/postgui.hal @@ -0,0 +1 @@ +net done-count <= motion.analog-out-00 => test-ui.done-count diff --git a/tests/probe-queue-not-empty/probe.hal b/tests/probe-queue-not-empty/probe.hal new file mode 100644 index 00000000000..17810d04c34 --- /dev/null +++ b/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 diff --git a/tests/probe-queue-not-empty/servo_delay.comp b/tests/probe-queue-not-empty/servo_delay.comp new file mode 100644 index 00000000000..df01bbb39d9 --- /dev/null +++ b/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) + ; +} diff --git a/tests/probe-queue-not-empty/simpockets.tbl b/tests/probe-queue-not-empty/simpockets.tbl new file mode 100644 index 00000000000..0bdf35afecc --- /dev/null +++ b/tests/probe-queue-not-empty/simpockets.tbl @@ -0,0 +1 @@ +T1 P1 Z0.0 D0.0 ; diff --git a/tests/probe-queue-not-empty/skip b/tests/probe-queue-not-empty/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/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" ] diff --git a/tests/probe-queue-not-empty/test-ui.py b/tests/probe-queue-not-empty/test-ui.py new file mode 100755 index 00000000000..10a9c9632ef --- /dev/null +++ b/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) diff --git a/tests/probe-queue-not-empty/test.ini b/tests/probe-queue-not-empty/test.ini new file mode 100644 index 00000000000..346951d7590 --- /dev/null +++ b/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 diff --git a/tests/probe-queue-not-empty/test.ngc b/tests/probe-queue-not-empty/test.ngc new file mode 100644 index 00000000000..eca2d6bd19f --- /dev/null +++ b/tests/probe-queue-not-empty/test.ngc @@ -0,0 +1,14 @@ +G20 G90 G94 +G0 Z0 +# = 0 +o100 while [# LT 100] + G0 Z-0.4 + G38.2 Z-1 F600 + # = [# + 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# +M2 diff --git a/tests/probe-queue-not-empty/test.sh b/tests/probe-queue-not-empty/test.sh new file mode 100755 index 00000000000..f9f8e26a874 --- /dev/null +++ b/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 diff --git a/tests/stale-motion-status/README b/tests/stale-motion-status/README new file mode 100644 index 00000000000..993a1643a0d --- /dev/null +++ b/tests/stale-motion-status/README @@ -0,0 +1,23 @@ +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. The wait starts once task is up: its startup reads the +status only once a second and gives up after ten tries. diff --git a/tests/stale-motion-status/late-controller/checkresult b/tests/stale-motion-status/late-controller/checkresult new file mode 100755 index 00000000000..acfd40f1b32 --- /dev/null +++ b/tests/stale-motion-status/late-controller/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# Test passes if test-ui.py returns successfully +exit 0 diff --git a/tests/stale-motion-status/late-controller/postgui.hal b/tests/stale-motion-status/late-controller/postgui.hal new file mode 120000 index 00000000000..d5ae19df5df --- /dev/null +++ b/tests/stale-motion-status/late-controller/postgui.hal @@ -0,0 +1 @@ +../postgui.hal \ No newline at end of file diff --git a/tests/stale-motion-status/late-controller/probe.hal b/tests/stale-motion-status/late-controller/probe.hal new file mode 100644 index 00000000000..9f2ad8d04c6 --- /dev/null +++ b/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 diff --git a/tests/stale-motion-status/late-controller/simpockets.tbl b/tests/stale-motion-status/late-controller/simpockets.tbl new file mode 120000 index 00000000000..84a56f68b93 --- /dev/null +++ b/tests/stale-motion-status/late-controller/simpockets.tbl @@ -0,0 +1 @@ +../simpockets.tbl \ No newline at end of file diff --git a/tests/stale-motion-status/late-controller/test-ui.py b/tests/stale-motion-status/late-controller/test-ui.py new file mode 120000 index 00000000000..15b94027b91 --- /dev/null +++ b/tests/stale-motion-status/late-controller/test-ui.py @@ -0,0 +1 @@ +../test-ui.py \ No newline at end of file diff --git a/tests/stale-motion-status/late-controller/test.ini b/tests/stale-motion-status/late-controller/test.ini new file mode 100644 index 00000000000..2e800bf4986 --- /dev/null +++ b/tests/stale-motion-status/late-controller/test.ini @@ -0,0 +1,90 @@ +[TEST] +PROBES = 4 +FEED = 6 + +[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 = 50000000 + +[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 diff --git a/tests/stale-motion-status/late-controller/test.ngc b/tests/stale-motion-status/late-controller/test.ngc new file mode 120000 index 00000000000..45526095ca3 --- /dev/null +++ b/tests/stale-motion-status/late-controller/test.ngc @@ -0,0 +1 @@ +../test.ngc \ No newline at end of file diff --git a/tests/stale-motion-status/late-controller/test.sh b/tests/stale-motion-status/late-controller/test.sh new file mode 100755 index 00000000000..1f7aa8c808a --- /dev/null +++ b/tests/stale-motion-status/late-controller/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash +set -e + +linuxcnc -r test.ini diff --git a/tests/stale-motion-status/postgui.hal b/tests/stale-motion-status/postgui.hal new file mode 100644 index 00000000000..b3e0f7b7e58 --- /dev/null +++ b/tests/stale-motion-status/postgui.hal @@ -0,0 +1 @@ +net finished <= motion.analog-out-00 => test-ui.finished diff --git a/tests/stale-motion-status/simpockets.tbl b/tests/stale-motion-status/simpockets.tbl new file mode 100644 index 00000000000..0bdf35afecc --- /dev/null +++ b/tests/stale-motion-status/simpockets.tbl @@ -0,0 +1 @@ +T1 P1 Z0.0 D0.0 ; diff --git a/tests/stale-motion-status/slow-controller/checkresult b/tests/stale-motion-status/slow-controller/checkresult new file mode 100755 index 00000000000..acfd40f1b32 --- /dev/null +++ b/tests/stale-motion-status/slow-controller/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# Test passes if test-ui.py returns successfully +exit 0 diff --git a/tests/stale-motion-status/slow-controller/postgui.hal b/tests/stale-motion-status/slow-controller/postgui.hal new file mode 120000 index 00000000000..d5ae19df5df --- /dev/null +++ b/tests/stale-motion-status/slow-controller/postgui.hal @@ -0,0 +1 @@ +../postgui.hal \ No newline at end of file diff --git a/tests/stale-motion-status/slow-controller/probe.hal b/tests/stale-motion-status/slow-controller/probe.hal new file mode 100644 index 00000000000..0c0e3c6ba81 --- /dev/null +++ b/tests/stale-motion-status/slow-controller/probe.hal @@ -0,0 +1,11 @@ +# 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 + +# Let the controller take most of the servo period, so that status reads +# by task often fail while it runs. test-ui.py sets slowkins.wait-ns once +# task is up: at startup task reads the status only once a second. +addf slowkins.arm servo-thread 1 diff --git a/tests/stale-motion-status/slow-controller/simpockets.tbl b/tests/stale-motion-status/slow-controller/simpockets.tbl new file mode 120000 index 00000000000..84a56f68b93 --- /dev/null +++ b/tests/stale-motion-status/slow-controller/simpockets.tbl @@ -0,0 +1 @@ +../simpockets.tbl \ No newline at end of file diff --git a/tests/stale-motion-status/slow-controller/skip b/tests/stale-motion-status/slow-controller/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/stale-motion-status/slow-controller/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" ] diff --git a/tests/stale-motion-status/slow-controller/slowkins.comp b/tests/stale-motion-status/slow-controller/slowkins.comp new file mode 100644 index 00000000000..0025d944fec --- /dev/null +++ b/tests/stale-motion-status/slow-controller/slowkins.comp @@ -0,0 +1,93 @@ +component slowkins "Identity kinematics for X Y Z that busy wait in kinematicsForward once per servo period"; +pin out bool dummy = 1; // halcompile requires at least one pin +function arm "Let the next kinematicsForward call busy wait"; +option singleton yes; +option extra_setup; +option period no; +license "GPL"; +author "Luca Toniolo"; +;; +#include + +static struct haldata { + hal_uint_t wait_ns; + int armed; +} *haldata; + +EXTRA_SETUP() { + (void)__comp_inst; + (void)prefix; + (void)extra_arg; + + if (comp_id < 0) + return -1; + haldata = hal_malloc(sizeof(struct haldata)); + if (!haldata) + return -1; + return hal_param_new_uint(comp_id, HAL_RW, &haldata->wait_ns, 0, + "slowkins.wait-ns"); +} + +/* motion calls kinematicsForward twice per period in free mode; one wait + per period keeps the controller inside the period */ +FUNCTION(arm) { + (void)__comp_inst; + haldata->armed = 1; +} + +KINS_NOT_SWITCHABLE +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsForward); + +KINEMATICS_TYPE kinematicsType() +{ + return KINEMATICS_IDENTITY; +} + +int kinematicsForward(const double *j, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)fflags; + (void)iflags; + if (haldata->armed) { + rtapi_sint end = rtapi_get_time() + (rtapi_sint)hal_get_uint(haldata->wait_ns); + haldata->armed = 0; + while (rtapi_get_time() < end) + ; + } + pos->tran.x = j[0]; + pos->tran.y = j[1]; + pos->tran.z = j[2]; + pos->a = pos->b = pos->c = 0; + pos->u = pos->v = pos->w = 0; + return 0; +} + +int kinematicsInverse(const EmcPose *pos, double *j, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)iflags; + (void)fflags; + j[0] = pos->tran.x; + j[1] = pos->tran.y; + j[2] = pos->tran.z; + return 0; +} + +int kinematicsJacobian(const double *j, const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)j; + (void)pos; + (void)iflags; + memset(jac, 0, sizeof(double) * EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS); + jac[0][0] = 1; + jac[1][1] = 1; + jac[2][2] = 1; + return 0; +} diff --git a/tests/stale-motion-status/slow-controller/test-ui.py b/tests/stale-motion-status/slow-controller/test-ui.py new file mode 120000 index 00000000000..15b94027b91 --- /dev/null +++ b/tests/stale-motion-status/slow-controller/test-ui.py @@ -0,0 +1 @@ +../test-ui.py \ No newline at end of file diff --git a/tests/stale-motion-status/slow-controller/test.ini b/tests/stale-motion-status/slow-controller/test.ini new file mode 100644 index 00000000000..d4644a2b791 --- /dev/null +++ b/tests/stale-motion-status/slow-controller/test.ini @@ -0,0 +1,90 @@ +[TEST] +PROBES = 300 +FEED = 300 + +[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 = slowkins +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 diff --git a/tests/stale-motion-status/slow-controller/test.ngc b/tests/stale-motion-status/slow-controller/test.ngc new file mode 120000 index 00000000000..45526095ca3 --- /dev/null +++ b/tests/stale-motion-status/slow-controller/test.ngc @@ -0,0 +1 @@ +../test.ngc \ No newline at end of file diff --git a/tests/stale-motion-status/slow-controller/test.sh b/tests/stale-motion-status/slow-controller/test.sh new file mode 100755 index 00000000000..14a77f5bc36 --- /dev/null +++ b/tests/stale-motion-status/slow-controller/test.sh @@ -0,0 +1,5 @@ +#!/bin/bash +set -e + +halcompile --install slowkins.comp >/dev/null +linuxcnc -r test.ini diff --git a/tests/stale-motion-status/test-ui.py b/tests/stale-motion-status/test-ui.py new file mode 100755 index 00000000000..7387363da4a --- /dev/null +++ b/tests/stale-motion-status/test-ui.py @@ -0,0 +1,99 @@ +#!/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("finished", hal.Type.REAL, hal.Dir.IN) +h.ready() +os.system("halcmd source ./postgui.hal") + +failures = [] + +def settle(what, done): + """wait_complete() must not return before the command took effect; + note it if it did, then wait for it so the rest still runs""" + s.poll() + if done(): + return + failures.append("wait_complete() returned before " + what) + start = time.time() + while not done(): + if time.time() - start > 60: + sys.stderr.write("timeout waiting for %s\n" % what) + sys.exit(1) + time.sleep(0.05) + s.poll() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.wait_complete(60) +# slow-controller: task is up, from now on the controller takes most of +# the servo period +if hal.component_exists("slowkins"): + hal.set_p("slowkins.wait-ns", "700000") +c.state(linuxcnc.STATE_ON) +c.wait_complete(60) +settle("the machine was 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) + start = time.time() + while True: + s.poll() + if s.homed[joint] and not any(j["homing"] for j in s.joint[:3]): + break + if time.time() - start > 60: + sys.stderr.write("timeout homing joint %d\n" % joint) + sys.exit(1) + time.sleep(0.05) + +c.mode(linuxcnc.MODE_AUTO) +c.wait_complete(60) +settle("auto mode was on", lambda: s.task_mode == linuxcnc.MODE_AUTO + and s.motion_mode == linuxcnc.TRAJ_MODE_COORD) + +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 > 60: + 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 > 600: + 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 failures + errors: + sys.stderr.write("error: %s\n" % msg) +if h["finished"] != 1: + sys.stderr.write("the program did not finish\n") +sys.exit(1 if failures or errors or h["finished"] != 1 else 0) diff --git a/tests/stale-motion-status/test.ngc b/tests/stale-motion-status/test.ngc new file mode 100644 index 00000000000..ceeea2d276e --- /dev/null +++ b/tests/stale-motion-status/test.ngc @@ -0,0 +1,16 @@ +G20 G90 G94 +G0 Z0 +# = 0 +o100 while [# LT #<_ini[test]probes>] + # = [-0.45 - 0.1 * [# MOD 2]] + M68 E1 Q# + G0 Z-0.4 + G38.2 Z-1 F#<_ini[test]feed> + o110 if [ABS[#5063 - #] GT 0.02] + (abort, probe # read Z#5063, it tripped at Z#) + o110 endif + # = [# + 1] +o100 endwhile +G0 Z0 +M68 E0 Q1 +M2