motion: report the spindle synch of the move in progress

docs/src/config/python-interface.adoc

@@ -345,6 +345,22 @@ see <<python:reading-ini-values,ReadingINI file values>> for an example.
   returns the current spindle status,
   see <<sec:the-spindle-dictionary, The spindle dictionary>>.
 
+*spindle_sync_mode*:: '(returns integer)' -
+  how the motion in progress follows the spindle. One of:
+  * SPINDLE_SYNC_NONE - not synchronized, or no motion in progress.
+  * SPINDLE_SYNC_VELOCITY - feed per revolution, G95.
+  * SPINDLE_SYNC_POSITION - locked to the spindle position, G33, G33.1 and G76.
+
+*spindle_sync_index_pulse_angle_offset*:: '(returns float)' -
+  for a synchronized motion in progress, the angle in degrees past the
+  spindle index pulse at which the synch started: the D word of G33 or G76.
+  0 when the motion in progress is not synchronized.
+
+*spindle_sync_pitch*:: '(returns float)' -
+  machine units the motion in progress advances per spindle revolution
+  (the K word of G33, the F word under G95). 0 when the motion in progress
+  is not synchronized.
+
 *spindles*:: '(returns integer)' -
   number of spindles. Reflects `[TRAJ]SPINDLES` INI value.
 
@@ -1078,6 +1094,10 @@ inifile = linuxcnc.ini(stat.ini_filename)
 
 == The `linuxcnc.positionlogger` type
 
+`linuxcnc.positionlogger(stat, color0, ..., color5, geometry[, is_xyuv], sync=False)`
+makes a logger; `sync=True` also keeps the spindle synch of each point, see
+`sync_points()`.
+
 Some usage hints can be gleaned from
 `src/emc/usr_intf/gremlin/gremlin.py`.
 
@@ -1111,4 +1131,20 @@ Some usage hints can be gleaned from
 `last([int])`::
   Return the most recent point on the plot or None
 
+`sync_points()`::
+  For a logger made with the keyword argument `sync=True`, return
+  `(bytes, npts)`: the spindle synch each point of `points()` was taken under,
+  same indices, copied under the same lock. Each entry is a C `logger_sync`,
+  12 bytes -- uint8 mode (`stat.spindle_sync_mode`), 3 pad bytes, float32
+  pitch (`stat.spindle_sync_pitch`), float32 index_pulse_angle_offset
+  (`stat.spindle_sync_index_pulse_angle_offset`); numpy
+  `[('mode', 'u1'), ('pad', 'u1', 3), ('pitch', 'f4'),
+  ('index_pulse_angle_offset', 'f4')]`. An entry describes the line that
+  ends at its point, so the point before the first entry of a synched run is
+  where that synch started. Such a logger
+  also starts a new point whenever the synch changes, so a synched move
+  colinear with the move before it is not merged into it. A logger made
+  without `sync=True` (the default) returns None and logs exactly as before;
+  the extra buffer costs 12 bytes a point, 1.2 MB at the 100000 point limit.
+
 // vim: set syntax=asciidoc:

include/motion_types.h

@@ -22,6 +22,12 @@
 #define EMC_MOTION_TYPE_PROBING 5
 #define EMC_MOTION_TYPE_INDEXROTARY 6
 
+/* spindle synch of the move in progress, as stat.spindle_sync_mode;
+   the same values as the planner's TC_SYNC_* */
+#define EMC_SPINDLE_SYNC_NONE     0
+#define EMC_SPINDLE_SYNC_VELOCITY 1	/* G95, feed per revolution */
+#define EMC_SPINDLE_SYNC_POSITION 2	/* G33, G33.1, G76 */
+
 /* modes of EMC_TRAJ_SET_SO_ENABLE, passed on as EMCMOT_SS_ENABLE */
 #define EMC_SO_OVERRIDE_OFF  0
 #define EMC_SO_OVERRIDE_ON   1

src/emc/motion/motion.h

@@ -667,6 +667,11 @@ Suggestion: Split this in to an Error and a Status flag register..
 	EmcPose dtg;
 	double current_vel;
 	double requested_vel;
+	/* spindle synch of the move in progress, 0 when there is none */
+	int sync_mode;		/* TC_SYNC_*: 0 none, 1 velocity (G95), 2 position (G33/G76) */
+	double sync_uu_per_rev;	/* units advanced per spindle revolution */
+	double sync_index_pulse_angle_offset;	/* degrees past the spindle index pulse
+						   the synch started at (D word) */
 
 	/* S-curve motion state - for accurate jerk output */
 	double current_acc;     /* current path acceleration */

src/emc/nml_intf/emc.cc

@@ -1720,6 +1720,9 @@ void EMC_TRAJ_STAT::update(CMS * cms)
     cms->update(switchkins_seq);
     cms->update(switchkins_changed);
     cms->update(switchkins_flags, SWITCHKINS_MAX_TYPES);
+    cms->update(spindle_sync_mode);
+    cms->update(spindle_sync_pitch);
+    cms->update(spindle_sync_index_pulse_angle_offset);
 }
 
 /*

src/emc/nml_intf/emc_nml.hh

@@ -1043,6 +1043,12 @@ class EMC_TRAJ_STAT:public EMC_TRAJ_STAT_MSG {
                              // (KINSTYPE_*), as the module declares it;
                              // -1 for a type the module does not provide
 
+    // spindle synch of the move in progress, 0 when there is none
+    int spindle_sync_mode;       // EMC_SPINDLE_SYNC_NONE/_VELOCITY/_POSITION
+    double spindle_sync_pitch;   // machine units per spindle revolution
+    // degrees past the spindle index pulse the synch started at (G33/G76 D)
+    double spindle_sync_index_pulse_angle_offset;
+
     StateTag tag;
 };
 

src/emc/nml_intf/emcops.cc

@@ -106,6 +106,9 @@ EMC_TRAJ_STAT::EMC_TRAJ_STAT()
     switchkins_type(0),
     switchkins_seq(0),
     switchkins_changed(false),
+    spindle_sync_mode(0),
+    spindle_sync_pitch(0.0),
+    spindle_sync_index_pulse_angle_offset(0.0),
     tag()
 {
     // -1 = nothing known yet; a zero here would read as "types exist

src/emc/task/taskintf.cc

@@ -1678,6 +1678,10 @@ int emcTrajUpdate(EMC_TRAJ_STAT * stat)
     stat->distance_to_go = emcmotStatus.distance_to_go;
     stat->dtg = emcmotStatus.dtg;
     stat->current_vel = emcmotStatus.current_vel;
+    stat->spindle_sync_mode = emcmotStatus.sync_mode;
+    stat->spindle_sync_pitch = emcmotStatus.sync_uu_per_rev;
+    stat->spindle_sync_index_pulse_angle_offset =
+        emcmotStatus.sync_index_pulse_angle_offset;
     if (EMC_DEBUG_MOTION_TIME & emc_debug) {
 	if (stat->id != last_id) {
 	    if (last_id != last_id_printed) {

src/emc/tp/tc_types.h

@@ -16,6 +16,7 @@
 #include <posemath.h>
 #include <emcpos.h>
 #include <emcmotcfg.h>
+#include <motion_types.h>
 
 #include "spherical_arc.h"
 #include "../nml_intf/state_tag.h"
@@ -36,10 +37,11 @@ typedef enum {
     TC_SPHERICAL = 4
 } tc_motion_type_t;
 
+/* reported as is in stat.spindle_sync_mode */
 typedef enum {
-    TC_SYNC_NONE = 0,
-    TC_SYNC_VELOCITY,
-    TC_SYNC_POSITION
+    TC_SYNC_NONE = EMC_SPINDLE_SYNC_NONE,
+    TC_SYNC_VELOCITY = EMC_SPINDLE_SYNC_VELOCITY,
+    TC_SYNC_POSITION = EMC_SPINDLE_SYNC_POSITION
 } tc_spindle_sync_t;
 
 typedef enum {

src/emc/tp/tp.c

@@ -3217,6 +3217,9 @@ STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const
         emcmotStatus->requested_vel = 0;
         emcmotStatus->current_vel = 0;
         emcmotStatus->spindleSync = 0;
+        emcmotStatus->sync_mode = TC_SYNC_NONE;
+        emcmotStatus->sync_uu_per_rev = 0;
+        emcmotStatus->sync_index_pulse_angle_offset = 0;
 
         // Clear S-curve motion state
         emcmotStatus->current_acc = 0;
@@ -3245,6 +3248,14 @@ STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const
     tp->execId = tc->id;
     emcmotStatus->requested_vel = tc->reqvel;
     emcmotStatus->current_vel = tc->currentvel;
+    emcmotStatus->sync_mode = tc->synchronized;
+    if (tc->synchronized != TC_SYNC_NONE) {
+        emcmotStatus->sync_uu_per_rev = tc->uu_per_rev;
+        emcmotStatus->sync_index_pulse_angle_offset = tc->angle_offset * 360.0;
+    } else {
+        emcmotStatus->sync_uu_per_rev = 0;
+        emcmotStatus->sync_index_pulse_angle_offset = 0;
+    }
 
     // Output accurate S-curve motion state (for accurate jerk calculation)
     emcmotStatus->current_acc = tc->currentacc;

src/emc/usr_intf/axis/extensions/emcmodule.cc

@@ -1198,6 +1198,17 @@ static PyMemberDef Stat_members[] = {
         "currently taking place."},
     { "distance_to_go", T_DOUBLE, O(motion.traj.distance_to_go), READONLY, NULL},
     { "current_vel", T_DOUBLE, O(motion.traj.current_vel), READONLY, NULL},
+    { "spindle_sync_mode", T_INT, O(motion.traj.spindle_sync_mode), READONLY,
+        "How the motion in progress follows the spindle: SPINDLE_SYNC_NONE,\n"
+        "SPINDLE_SYNC_VELOCITY (G95 feed per revolution) or SPINDLE_SYNC_POSITION\n"
+        "(G33, G33.1, G76)."},
+    { "spindle_sync_pitch", T_DOUBLE, O(motion.traj.spindle_sync_pitch), READONLY,
+        "Machine units the motion in progress advances per spindle revolution,\n"
+        "0 when it is not synchronized."},
+    { "spindle_sync_index_pulse_angle_offset", T_DOUBLE,
+        O(motion.traj.spindle_sync_index_pulse_angle_offset), READONLY,
+        "Degrees past the spindle index pulse the synchronized motion in\n"
+        "progress started at (the D word of G33 or G76), 0 when there is none."},
     { "feed_override_enabled", T_BOOL, O(motion.traj.feed_override_enabled), READONLY, NULL},
     { "adaptive_feed_enabled", T_BOOL, O(motion.traj.adaptive_feed_enabled), READONLY, NULL},
     { "feed_hold_enabled", T_BOOL, O(motion.traj.feed_hold_enabled), READONLY, NULL},
@@ -2882,12 +2893,23 @@ struct logger_point {
     struct color c2;
 };
 
+// The spindle synch a logged point was taken under, kept beside p (same
+// index) when the logger is made with sync=True; see Logger_get_sync_points.
+struct logger_sync {
+    unsigned char mode;         // stat.spindle_sync_mode
+    unsigned char pad[3];
+    float pitch;                // stat.spindle_sync_pitch
+    float index_pulse_angle_offset; // stat.spindle_sync_index_pulse_angle_offset
+};
+static_assert(sizeof(struct logger_sync) == 12, "logger_sync layout is documented");
+
 #define NUMCOLORS (6)
 #define MAX_POINTS (100000)
 typedef struct {
     PyObject_HEAD
     int npts, mpts, lpts;
     struct logger_point *p;
+    struct logger_sync *sp;     // NULL unless made with sync=True
     struct color colors[NUMCOLORS];
     bool exit, clear, changed;
     char *geometry;
@@ -2916,10 +2938,27 @@ static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
 static void LOCK() { pthread_mutex_lock(&mutex); }
 static void UNLOCK() { pthread_mutex_unlock(&mutex); }
 
-static int Logger_init(pyPositionLogger *self, PyObject *a, PyObject * /*k*/) {
+static int Logger_init(pyPositionLogger *self, PyObject *a, PyObject *k) {
     char *geometry;
     struct color *c = self->colors;
+    // The positional arguments stay as they were; sync is keyword only.
+    bool sync = false;
+    if(k) {
+        PyObject *key, *value;
+        Py_ssize_t pos = 0;
+        while(PyDict_Next(k, &pos, &key, &value)) {
+            if(!PyUnicode_Check(key) || PyUnicode_CompareWithASCIIString(key, "sync")) {
+                PyErr_Format(PyExc_TypeError,
+                        "positionlogger() got an unexpected keyword argument '%S'", key);
+                return -1;
+            }
+            int t = PyObject_IsTrue(value);
+            if(t < 0) return -1;
+            sync = t;
+        }
+    }
     self->p = (logger_point*)malloc(sizeof(self->p[0])); // Will be realloc'ed
+    self->sp = sync ? (logger_sync*)malloc(sizeof(self->sp[0])) : NULL;
     self->npts = self->mpts = 0;
     self->exit = self->clear = 0;
     self->changed = 1;
@@ -2945,6 +2984,7 @@ static int Logger_init(pyPositionLogger *self, PyObject *a, PyObject * /*k*/) {
 
 static void Logger_dealloc(pyPositionLogger *s) {
     free(s->p);
+    free(s->sp);
     Py_XDECREF(s->st);
     free(s->geometry);
     PyObject_Del(s);
@@ -3023,6 +3063,22 @@ static PyObject *Logger_start(pyPositionLogger *s, PyObject *o) {
             struct logger_point *op = &s->p[s->npts-1];
             struct logger_point *oop = &s->p[s->npts-2];
             bool add_point = s->npts < 2 || c != op->c;
+            struct logger_sync sy = {};
+            if(s->sp) {
+                sy.mode = status->motion.traj.spindle_sync_mode;
+                sy.pitch = status->motion.traj.spindle_sync_pitch;
+                sy.index_pulse_angle_offset =
+                    status->motion.traj.spindle_sync_index_pulse_angle_offset;
+                // A synch starting or changing starts a point of its own,
+                // so the start of a synched move, where its phase is set,
+                // is not merged into a colinear move before it.
+                if(s->npts) {
+                    const struct logger_sync &osy = s->sp[s->npts-1];
+                    add_point = add_point || sy.mode != osy.mode
+                        || sy.pitch != osy.pitch
+                        || sy.index_pulse_angle_offset != osy.index_pulse_angle_offset;
+                }
+            }
             double x, y, z, rx, ry, rz;
             if(s->is_xyuv) {
                 x = status->motion.traj.position.tran.x - status->task.toolOffset.tran.x,
@@ -3076,11 +3132,17 @@ static PyObject *Logger_start(pyPositionLogger *s, PyObject *o) {
                         s->npts -= adjust;
                         memmove(s->p, s->p + adjust,
                                 sizeof(struct logger_point) * s->npts);
+                        if(s->sp)
+                            memmove(s->sp, s->sp + adjust,
+                                    sizeof(struct logger_sync) * s->npts);
                     } else {
                         s->mpts = 2 * s->mpts + 2;
                         s->changed = 1;
                         s->p = (struct logger_point*) realloc(s->p,
                                     sizeof(struct logger_point) * s->mpts);
+                        if(s->sp)
+                            s->sp = (struct logger_sync*) realloc(s->sp,
+                                        sizeof(struct logger_sync) * s->mpts);
                     }
                     UNLOCK();
                     op = &s->p[s->npts-1];
@@ -3099,12 +3161,14 @@ static PyObject *Logger_start(pyPositionLogger *s, PyObject *o) {
                     np.rx = rx; np.ry = ry; np.rz = rz;
                     np.c = np.c2 = c;
                     }
+                    if(s->sp) s->sp[s->npts] = s->sp[s->npts+1] = sy;
                     s->npts += 2;
                 } else {
                     struct logger_point &np = s->p[s->npts];
                     np.x = x; np.y = y; np.z = z;
                     np.rx = rx; np.ry = ry; np.rz = rz;
                     np.c = np.c2 = c;
+                    if(s->sp) s->sp[s->npts] = sy;
                     s->npts++;
                 }
             } else {
@@ -3200,6 +3264,28 @@ static PyObject *Logger_get_points(pyPositionLogger *s, PyObject * /*o*/) {
     return Py_BuildValue("Nii", buf, npts, is_xyuv);
 }
 
+// The spindle synch of each point points() returns, same indices, for a
+// logger made with sync=True; None otherwise. Returns (bytes, npts); each
+// entry is a `struct logger_sync`, 12 bytes: uint8 mode, 3 pad bytes,
+// float32 pitch, float32 index_pulse_angle_offset (numpy: [('mode','u1'),
+// ('pad','u1',3), ('pitch','f4'), ('index_pulse_angle_offset','f4')]). An
+// entry is the state the point was taken under, so it describes the line
+// that ends at that point. Does not advance lpts: points() does that.
+static PyObject *Logger_get_sync_points(pyPositionLogger *s, PyObject * /*o*/) {
+    LOCK();
+    if(!s->sp) {
+        UNLOCK();
+        Py_RETURN_NONE;
+    }
+    int npts = s->npts;
+    if(npts < 0) npts = 0;
+    Py_ssize_t nbytes = (Py_ssize_t)npts * (Py_ssize_t)sizeof(struct logger_sync);
+    PyObject *buf = PyBytes_FromStringAndSize((const char*)s->sp, nbytes);
+    UNLOCK();
+    if(!buf) return NULL;
+    return Py_BuildValue("Ni", buf, npts);
+}
+
 static PyMemberDef Logger_members[] = {
     {(char*)"npts", T_INT, offsetof(pyPositionLogger, npts), READONLY, NULL},
     {},
@@ -3217,6 +3303,11 @@ static PyMethodDef Logger_methods[] = {
     {"points", (PyCFunction)Logger_get_points, METH_NOARGS,
         "Return (bytes, npts, is_xyuv): a copy of the logged point buffer for "
         "VBO upload by the core renderer"},
+    {"sync_points", (PyCFunction)Logger_get_sync_points, METH_NOARGS,
+        "Return (bytes, npts): the spindle synch of each point of points(), "
+        "12 bytes each (uint8 mode, 3 pad, float32 pitch, "
+        "float32 index_pulse_angle_offset), "
+        "or None unless the logger was made with sync=True"},
     {"set_depth", (PyCFunction)Logger_set_depth, METH_VARARGS,
         "set the Z and W depths for foam cutter"},
     {"set_colors", (PyCFunction)Logger_set_colors, METH_VARARGS,
@@ -3418,6 +3509,10 @@ PyMODINIT_FUNC PyInit_linuxcnc(void)
     ENUMX(4, EMC_MOTION_TYPE_PROBING);
     ENUMX(4, EMC_MOTION_TYPE_INDEXROTARY);
 
+    ENUMX(4, EMC_SPINDLE_SYNC_NONE);
+    ENUMX(4, EMC_SPINDLE_SYNC_VELOCITY);
+    ENUMX(4, EMC_SPINDLE_SYNC_POSITION);
+
     ENUM(KINEMATICS_IDENTITY);
     ENUM(KINEMATICS_FORWARD_ONLY);
     ENUM(KINEMATICS_INVERSE_ONLY);

tests/motion/spindle-sync-status/expected

@@ -0,0 +1 @@
+Completed successfully

tests/motion/spindle-sync-status/sync.ngc

@@ -0,0 +1,13 @@
+(One move of each kind of spindle synch, for stat.spindle_sync_* and the)
+(position logger's sync_points.)
+g21 g18 g90 g8
+s600 m3
+g4 p1 (let the simulated spindle reach speed)
+g94 g1 x10 z5 f500
+g95 g1 z0 f0.2
+g94 g0 x10 z5
+g33 z-10 k1.5 d90
+g0 x10 z5
+g76 p2 z-10 i-1 j0.4 k0.6 r1 q29.5 h0 e1 l3 d45
+m5
+m2