diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 8c9ce66..0ab0f5f 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -19,6 +19,9 @@ #ifdef SWITCHKINS_DEBUG #include // rtpreempt only, consolidate to stderr #endif +#include +#include +#include #include "posemath.h" #include "rtapi.h" @@ -67,6 +70,46 @@ extern struct emcmot_status_t *emcmotStatus; // etc. static double *pcmd_p[EMCMOT_MAX_AXIS]; +/* Optional simulation-only servo oracle. It is disabled unless the caller + * provides a file path, so normal LinuxCNC motion timing is unchanged. */ +static FILE *xyzac_trace_stream; +static void xyzac_trace_tick(void) { + const char *path; + int joint; + struct timespec now; + clock_gettime(CLOCK_MONOTONIC, &now); + if (xyzac_trace_stream == NULL) { + path = getenv("LINUXCNC_XYZAC_TRACE_FILE"); + if (path == NULL || path[0] == '\0') return; + xyzac_trace_stream = fopen(path, "w"); + if (xyzac_trace_stream == NULL) return; + setvbuf(xyzac_trace_stream, NULL, _IOLBF, 0); + } + fprintf(xyzac_trace_stream, + "{\"wall_ns\":%llu,\"motion_heartbeat\":%u,\"position\":[%.17g,%.17g,%.17g,%.17g,%.17g,%.17g,%.17g,%.17g,%.17g],\"actual_position\":[%.17g,%.17g,%.17g,%.17g,%.17g,%.17g,%.17g,%.17g,%.17g],\"velocity\":%.17g,\"acceleration\":%.17g,\"queue_depth\":%d,\"active_depth\":%d,\"motion_id\":%d,\"motion_type\":%d,\"paused\":%d,\"queue_full\":%d,\"joints\":[", + (unsigned long long)now.tv_sec * 1000000000ull + (unsigned long long)now.tv_nsec, + emcmotStatus->heartbeat, + emcmotStatus->carte_pos_cmd.tran.x, emcmotStatus->carte_pos_cmd.tran.y, emcmotStatus->carte_pos_cmd.tran.z, + emcmotStatus->carte_pos_cmd.a, emcmotStatus->carte_pos_cmd.b, emcmotStatus->carte_pos_cmd.c, + emcmotStatus->carte_pos_cmd.u, emcmotStatus->carte_pos_cmd.v, emcmotStatus->carte_pos_cmd.w, + emcmotStatus->carte_pos_fb.tran.x, emcmotStatus->carte_pos_fb.tran.y, emcmotStatus->carte_pos_fb.tran.z, + emcmotStatus->carte_pos_fb.a, emcmotStatus->carte_pos_fb.b, emcmotStatus->carte_pos_fb.c, + emcmotStatus->carte_pos_fb.u, emcmotStatus->carte_pos_fb.v, emcmotStatus->carte_pos_fb.w, + emcmotStatus->current_vel, emcmotStatus->acc, emcmotStatus->depth, + emcmotStatus->activeDepth, emcmotStatus->id, emcmotStatus->motionType, + emcmotStatus->paused, emcmotStatus->queueFull); + for (joint = 0; joint < EMCMOT_MAX_JOINTS; ++joint) { + if (joint != 0) fputc(',', xyzac_trace_stream); + fprintf(xyzac_trace_stream, "{\"cmd\":%.17g,\"fb\":%.17g,\"vel\":%.17g,\"acc\":%.17g,\"homed\":%d}", + emcmotStatus->joint_status[joint].pos_cmd, + emcmotStatus->joint_status[joint].pos_fb, + emcmotStatus->joint_status[joint].vel_cmd, + emcmotStatus->joint_status[joint].acc_cmd, + emcmotStatus->joint_status[joint].homed); + } + fputs("]}\n", xyzac_trace_stream); +} + /*********************************************************************** * LOCAL FUNCTION PROTOTYPES * ************************************************************************/ @@ -271,6 +314,7 @@ void emcmotController(void *arg, long period) update_status(); /* here ends the core of the controller */ emcmotStatus->heartbeat++; + xyzac_trace_tick(); /* set tail to head, to indicate work complete */ emcmotStatus->tail = emcmotStatus->head; /* end of controller function */