70 lines
3.6 KiB
Diff
70 lines
3.6 KiB
Diff
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 <stdio.h> // rtpreempt only, consolidate to stderr
|
|
#endif
|
|
+#include <stdio.h>
|
|
+#include <stdlib.h>
|
|
+#include <time.h>
|
|
|
|
#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 */
|