接入 LinuxCNC TP 运行反馈
This commit is contained in:
@@ -1,4 +1,5 @@
|
||||
#include <math.h>
|
||||
#include <stdarg.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
@@ -91,6 +92,76 @@ static int pose_near(const EmcPose *actual, const EmcPose *expected)
|
||||
fabs(actual->tran.z - expected->tran.z) < 1e-9;
|
||||
}
|
||||
|
||||
static const char *find_json_key(const char *cursor, const char *key)
|
||||
{
|
||||
if (!cursor || !key) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
char pattern[64];
|
||||
snprintf(pattern, sizeof(pattern), "\"%s\"", key);
|
||||
const char *match = strstr(cursor, pattern);
|
||||
if (!match) {
|
||||
return NULL;
|
||||
}
|
||||
match = strchr(match + strlen(pattern), ':');
|
||||
return match ? match + 1 : NULL;
|
||||
}
|
||||
|
||||
static double read_json_number_in_object(const char *object_start, const char *object_end, const char *key, double fallback)
|
||||
{
|
||||
const char *value = find_json_key(object_start, key);
|
||||
if (!value || value >= object_end) {
|
||||
return fallback;
|
||||
}
|
||||
char *end = NULL;
|
||||
const double number = strtod(value, &end);
|
||||
if (end == value || end > object_end || !isfinite(number)) {
|
||||
return fallback;
|
||||
}
|
||||
return number;
|
||||
}
|
||||
|
||||
static int read_json_string_in_object(
|
||||
const char *object_start,
|
||||
const char *object_end,
|
||||
const char *key,
|
||||
char *out,
|
||||
size_t out_size)
|
||||
{
|
||||
const char *value = find_json_key(object_start, key);
|
||||
if (!value || value >= object_end || out_size == 0) {
|
||||
return 0;
|
||||
}
|
||||
while (value < object_end && (*value == ' ' || *value == '\t' || *value == '\n' || *value == '\r')) {
|
||||
++value;
|
||||
}
|
||||
if (value >= object_end || *value != '"') {
|
||||
return 0;
|
||||
}
|
||||
++value;
|
||||
size_t index = 0;
|
||||
while (value < object_end && *value && *value != '"' && index + 1 < out_size) {
|
||||
out[index++] = *value++;
|
||||
}
|
||||
out[index] = '\0';
|
||||
return index > 0;
|
||||
}
|
||||
|
||||
static void appendf(char *out, size_t size, size_t *offset, const char *format, ...)
|
||||
{
|
||||
if (*offset >= size) {
|
||||
return;
|
||||
}
|
||||
va_list args;
|
||||
va_start(args, format);
|
||||
const int written = vsnprintf(out + *offset, size - *offset, format, args);
|
||||
va_end(args);
|
||||
if (written > 0) {
|
||||
*offset += (size_t)written;
|
||||
}
|
||||
}
|
||||
|
||||
static void append(char *out, size_t size, size_t *offset, const char *text)
|
||||
{
|
||||
if (*offset >= size) {
|
||||
@@ -102,6 +173,594 @@ static void append(char *out, size_t size, size_t *offset, const char *text)
|
||||
}
|
||||
}
|
||||
|
||||
typedef struct {
|
||||
int index;
|
||||
int line;
|
||||
char type[32];
|
||||
EmcPose end;
|
||||
EmcPose start;
|
||||
double feed_rate;
|
||||
int plane;
|
||||
double center_first;
|
||||
double center_second;
|
||||
int rotation;
|
||||
double axis_end_point;
|
||||
int zero_length;
|
||||
int accepted;
|
||||
int emitted;
|
||||
} LctpMotionEvent;
|
||||
|
||||
typedef struct {
|
||||
double cycle_time;
|
||||
double max_velocity;
|
||||
double max_acceleration;
|
||||
double max_jerk;
|
||||
double rapid_scale;
|
||||
double feed_scale;
|
||||
double tolerance;
|
||||
int queue_size;
|
||||
int max_cycles;
|
||||
int sample_stride;
|
||||
} LctpTimingOptions;
|
||||
|
||||
static LctpTimingOptions read_timing_options(const char *json)
|
||||
{
|
||||
LctpTimingOptions options;
|
||||
options.cycle_time = read_json_number_in_object(json, json + strlen(json), "cycleTime", 0.001);
|
||||
options.max_velocity = read_json_number_in_object(json, json + strlen(json), "maxVelocity", 35.0);
|
||||
options.max_acceleration = read_json_number_in_object(json, json + strlen(json), "maxAcceleration", 500.0);
|
||||
options.max_jerk = read_json_number_in_object(json, json + strlen(json), "maxJerk", 1000.0);
|
||||
options.rapid_scale = read_json_number_in_object(json, json + strlen(json), "rapidScale", 1.0);
|
||||
options.feed_scale = read_json_number_in_object(json, json + strlen(json), "feedScale", 1.0);
|
||||
options.tolerance = read_json_number_in_object(json, json + strlen(json), "tolerance", 0.0);
|
||||
options.queue_size = (int)read_json_number_in_object(json, json + strlen(json), "queueSize", TP_DEFAULT_QUEUE_SIZE);
|
||||
options.max_cycles = (int)read_json_number_in_object(json, json + strlen(json), "maxCycles", 1000000);
|
||||
options.sample_stride = (int)read_json_number_in_object(json, json + strlen(json), "sampleStride", 10.0);
|
||||
|
||||
if (!isfinite(options.cycle_time) || options.cycle_time <= 0.0) options.cycle_time = 0.001;
|
||||
if (!isfinite(options.max_velocity) || options.max_velocity <= 0.0) options.max_velocity = 35.0;
|
||||
if (!isfinite(options.max_acceleration) || options.max_acceleration <= 0.0) options.max_acceleration = 500.0;
|
||||
if (!isfinite(options.max_jerk) || options.max_jerk <= 0.0) options.max_jerk = 1000.0;
|
||||
if (!isfinite(options.rapid_scale) || options.rapid_scale <= 0.0) options.rapid_scale = 1.0;
|
||||
if (!isfinite(options.feed_scale) || options.feed_scale <= 0.0) options.feed_scale = 1.0;
|
||||
if (!isfinite(options.tolerance) || options.tolerance < 0.0) options.tolerance = 0.0;
|
||||
if (options.queue_size <= 0) options.queue_size = TP_DEFAULT_QUEUE_SIZE;
|
||||
if (options.max_cycles <= 0) options.max_cycles = 1000000;
|
||||
if (options.sample_stride <= 0) options.sample_stride = 10;
|
||||
return options;
|
||||
}
|
||||
|
||||
static int parse_motion_events(const char *json, LctpMotionEvent *events, int max_events)
|
||||
{
|
||||
const char *motion = find_json_key(json, "motion");
|
||||
if (!motion) {
|
||||
return 0;
|
||||
}
|
||||
const char *cursor = strchr(motion, '[');
|
||||
if (!cursor) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int count = 0;
|
||||
EmcPose previous;
|
||||
memset(&previous, 0, sizeof(previous));
|
||||
while (count < max_events) {
|
||||
const char *object_start = strchr(cursor, '{');
|
||||
if (!object_start) {
|
||||
break;
|
||||
}
|
||||
const char *object_end = strchr(object_start, '}');
|
||||
if (!object_end) {
|
||||
break;
|
||||
}
|
||||
|
||||
LctpMotionEvent *event = &events[count];
|
||||
memset(event, 0, sizeof(*event));
|
||||
event->index = count;
|
||||
event->line = (int)read_json_number_in_object(object_start, object_end, "line", -1.0);
|
||||
event->feed_rate = read_json_number_in_object(object_start, object_end, "feedRate", 0.0);
|
||||
event->plane = (int)read_json_number_in_object(object_start, object_end, "plane", 170.0);
|
||||
event->center_first = read_json_number_in_object(object_start, object_end, "centerFirst", 0.0);
|
||||
event->center_second = read_json_number_in_object(object_start, object_end, "centerSecond", 0.0);
|
||||
event->rotation = (int)read_json_number_in_object(object_start, object_end, "rotation", 0.0);
|
||||
event->axis_end_point = read_json_number_in_object(object_start, object_end, "axisEndPoint", 0.0);
|
||||
event->start = previous;
|
||||
read_json_string_in_object(object_start, object_end, "type", event->type, sizeof(event->type));
|
||||
event->end.tran.x = read_json_number_in_object(object_start, object_end, "x", 0.0);
|
||||
event->end.tran.y = read_json_number_in_object(object_start, object_end, "y", 0.0);
|
||||
event->end.tran.z = read_json_number_in_object(object_start, object_end, "z", 0.0);
|
||||
event->end.a = read_json_number_in_object(object_start, object_end, "a", 0.0);
|
||||
event->end.b = read_json_number_in_object(object_start, object_end, "b", 0.0);
|
||||
event->end.c = read_json_number_in_object(object_start, object_end, "c", 0.0);
|
||||
event->end.u = read_json_number_in_object(object_start, object_end, "u", 0.0);
|
||||
event->end.v = read_json_number_in_object(object_start, object_end, "v", 0.0);
|
||||
event->end.w = read_json_number_in_object(object_start, object_end, "w", 0.0);
|
||||
event->zero_length =
|
||||
fabs(event->end.tran.x - event->start.tran.x) < TP_POS_EPSILON &&
|
||||
fabs(event->end.tran.y - event->start.tran.y) < TP_POS_EPSILON &&
|
||||
fabs(event->end.tran.z - event->start.tran.z) < TP_POS_EPSILON &&
|
||||
fabs(event->end.a - event->start.a) < TP_POS_EPSILON &&
|
||||
fabs(event->end.b - event->start.b) < TP_POS_EPSILON &&
|
||||
fabs(event->end.c - event->start.c) < TP_POS_EPSILON &&
|
||||
fabs(event->end.u - event->start.u) < TP_POS_EPSILON &&
|
||||
fabs(event->end.v - event->start.v) < TP_POS_EPSILON &&
|
||||
fabs(event->end.w - event->start.w) < TP_POS_EPSILON;
|
||||
previous = event->end;
|
||||
count += 1;
|
||||
cursor = object_end + 1;
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
static struct state_tag_t tag_for_event(const LctpMotionEvent *event)
|
||||
{
|
||||
struct state_tag_t tag;
|
||||
memset(&tag, 0, sizeof(tag));
|
||||
tag.fields[GM_FIELD_LINE_NUMBER] = event->index + 1;
|
||||
tag.fields_float[GM_FIELD_FLOAT_LINE_NUMBER] = (float)event->line;
|
||||
tag.fields_float[GM_FIELD_FLOAT_FEED] = (float)event->feed_rate;
|
||||
return tag;
|
||||
}
|
||||
|
||||
static double event_velocity(const LctpMotionEvent *event, const LctpTimingOptions *options)
|
||||
{
|
||||
if (strcmp(event->type, "STRAIGHT_TRAVERSE") == 0) {
|
||||
return options->max_velocity * options->rapid_scale;
|
||||
}
|
||||
const double feed_units_per_sec = event->feed_rate > 0.0 ? event->feed_rate / 60.0 : options->max_velocity;
|
||||
const double scaled = feed_units_per_sec * options->feed_scale;
|
||||
return fmin(fmax(scaled, 0.000001), options->max_velocity);
|
||||
}
|
||||
|
||||
static PmCartesian arc_center_for_event(const LctpMotionEvent *event)
|
||||
{
|
||||
PmCartesian center;
|
||||
memset(¢er, 0, sizeof(center));
|
||||
if (event->plane == 180) {
|
||||
center.x = event->center_first;
|
||||
center.z = event->center_second;
|
||||
center.y = event->start.tran.y;
|
||||
} else if (event->plane == 190) {
|
||||
center.y = event->center_first;
|
||||
center.z = event->center_second;
|
||||
center.x = event->start.tran.x;
|
||||
} else {
|
||||
center.x = event->center_first;
|
||||
center.y = event->center_second;
|
||||
center.z = event->start.tran.z;
|
||||
}
|
||||
return center;
|
||||
}
|
||||
|
||||
static PmCartesian arc_normal_for_event(const LctpMotionEvent *event)
|
||||
{
|
||||
PmCartesian normal;
|
||||
memset(&normal, 0, sizeof(normal));
|
||||
if (event->plane == 180) {
|
||||
normal.y = 1.0;
|
||||
} else if (event->plane == 190) {
|
||||
normal.x = 1.0;
|
||||
} else {
|
||||
normal.z = 1.0;
|
||||
}
|
||||
return normal;
|
||||
}
|
||||
|
||||
static int add_motion_event_to_tp(
|
||||
TP_STRUCT *tp,
|
||||
LctpMotionEvent *event,
|
||||
const LctpTimingOptions *options)
|
||||
{
|
||||
const double vel = event_velocity(event, options);
|
||||
struct state_tag_t tag = tag_for_event(event);
|
||||
if (strcmp(event->type, "ARC_FEED") == 0) {
|
||||
const PmCartesian center = arc_center_for_event(event);
|
||||
const PmCartesian normal = arc_normal_for_event(event);
|
||||
return tpAddCircle(
|
||||
tp,
|
||||
event->end,
|
||||
center,
|
||||
normal,
|
||||
event->rotation,
|
||||
EMC_MOTION_TYPE_ARC,
|
||||
vel,
|
||||
options->max_velocity,
|
||||
options->max_acceleration,
|
||||
options->max_jerk,
|
||||
status.enables_new,
|
||||
0,
|
||||
tag);
|
||||
}
|
||||
|
||||
const int motion_type = strcmp(event->type, "STRAIGHT_TRAVERSE") == 0
|
||||
? EMC_MOTION_TYPE_TRAVERSE
|
||||
: EMC_MOTION_TYPE_FEED;
|
||||
return tpAddLine(
|
||||
tp,
|
||||
event->end,
|
||||
motion_type,
|
||||
vel,
|
||||
options->max_velocity,
|
||||
options->max_acceleration,
|
||||
options->max_jerk,
|
||||
status.enables_new,
|
||||
0,
|
||||
-1,
|
||||
tag);
|
||||
}
|
||||
|
||||
static void append_zero_length_segment(
|
||||
char *out,
|
||||
size_t out_size,
|
||||
size_t *offset,
|
||||
LctpMotionEvent *event,
|
||||
int *emitted_segments,
|
||||
int cycles,
|
||||
const LctpTimingOptions *options)
|
||||
{
|
||||
appendf(out, out_size, offset,
|
||||
"%s{\"index\":%d,\"line\":%d,\"type\":\"%s\","
|
||||
"\"zeroLength\":true,\"tpCycleRc\":0,\"tpGetPos\":0,\"cycles\":0,"
|
||||
"\"durationSeconds\":0,\"elapsedSeconds\":%.12g,"
|
||||
"\"queueDepth\":0,\"activeDepth\":0,\"velocity\":0,"
|
||||
"\"x\":%.12g,\"y\":%.12g,\"z\":%.12g,\"a\":%.12g,\"b\":%.12g,\"c\":%.12g}",
|
||||
*emitted_segments == 0 ? "" : ",",
|
||||
event->index,
|
||||
event->line,
|
||||
event->type,
|
||||
cycles * options->cycle_time,
|
||||
event->end.tran.x,
|
||||
event->end.tran.y,
|
||||
event->end.tran.z,
|
||||
event->end.a,
|
||||
event->end.b,
|
||||
event->end.c);
|
||||
event->emitted = 1;
|
||||
*emitted_segments += 1;
|
||||
}
|
||||
|
||||
static int enqueue_next_nonzero_event(
|
||||
TP_STRUCT *tp,
|
||||
LctpMotionEvent *events,
|
||||
int event_count,
|
||||
int *next_event,
|
||||
const LctpTimingOptions *options,
|
||||
int *accepted_events,
|
||||
int *add_failures,
|
||||
char *out,
|
||||
size_t out_size,
|
||||
size_t *offset,
|
||||
int *emitted_segments,
|
||||
int cycles)
|
||||
{
|
||||
while (*next_event < event_count) {
|
||||
LctpMotionEvent *event = &events[*next_event];
|
||||
*next_event += 1;
|
||||
if (event->zero_length) {
|
||||
append_zero_length_segment(out, out_size, offset, event, emitted_segments, cycles, options);
|
||||
continue;
|
||||
}
|
||||
|
||||
const int add_rc = add_motion_event_to_tp(tp, event, options);
|
||||
if (add_rc == TP_ERR_OK) {
|
||||
event->accepted = 1;
|
||||
*accepted_events += 1;
|
||||
} else if (add_rc == TP_ERR_ZERO_LENGTH) {
|
||||
event->zero_length = 1;
|
||||
append_zero_length_segment(out, out_size, offset, event, emitted_segments, cycles, options);
|
||||
} else {
|
||||
*add_failures += 1;
|
||||
}
|
||||
return add_rc;
|
||||
}
|
||||
return TP_ERR_NO_ACTION;
|
||||
}
|
||||
|
||||
static void append_runtime_sample(
|
||||
char *samples,
|
||||
size_t samples_size,
|
||||
size_t *samples_offset,
|
||||
int *sample_count,
|
||||
int cycles,
|
||||
int motion_index,
|
||||
const LctpMotionEvent *event,
|
||||
const LctpTimingOptions *options,
|
||||
const TP_STRUCT *tp)
|
||||
{
|
||||
EmcPose pos;
|
||||
memset(&pos, 0, sizeof(pos));
|
||||
tpGetPos(tp, &pos);
|
||||
appendf(samples, samples_size, samples_offset,
|
||||
"%s{\"sampleIndex\":%d,\"cycle\":%d,\"timeSeconds\":%.12g,"
|
||||
"\"motionIndex\":%d,\"line\":%d,\"type\":\"%s\","
|
||||
"\"x\":%.12g,\"y\":%.12g,\"z\":%.12g,\"a\":%.12g,\"b\":%.12g,\"c\":%.12g,"
|
||||
"\"u\":%.12g,\"v\":%.12g,\"w\":%.12g,"
|
||||
"\"currentVelocity\":%.12g,\"requestedVelocity\":%.12g,"
|
||||
"\"distanceToGo\":%.12g,"
|
||||
"\"dtgX\":%.12g,\"dtgY\":%.12g,\"dtgZ\":%.12g,"
|
||||
"\"queueDepth\":%d,\"activeDepth\":%d}",
|
||||
*sample_count == 0 ? "" : ",",
|
||||
*sample_count,
|
||||
cycles,
|
||||
cycles * options->cycle_time,
|
||||
motion_index,
|
||||
event ? event->line : -1,
|
||||
event ? event->type : "-",
|
||||
pos.tran.x,
|
||||
pos.tran.y,
|
||||
pos.tran.z,
|
||||
pos.a,
|
||||
pos.b,
|
||||
pos.c,
|
||||
pos.u,
|
||||
pos.v,
|
||||
pos.w,
|
||||
status.current_vel,
|
||||
status.requested_vel,
|
||||
status.distance_to_go,
|
||||
status.dtg.tran.x,
|
||||
status.dtg.tran.y,
|
||||
status.dtg.tran.z,
|
||||
tpQueueDepth((TP_STRUCT *)tp),
|
||||
tpActiveDepth((TP_STRUCT *)tp));
|
||||
*sample_count += 1;
|
||||
}
|
||||
|
||||
EMSCRIPTEN_KEEPALIVE
|
||||
char *lctp_run_canonical_motion_timing(const char *json)
|
||||
{
|
||||
const size_t out_size = 4 * 1024 * 1024;
|
||||
char *out = (char *)malloc(out_size);
|
||||
if (!out) {
|
||||
return NULL;
|
||||
}
|
||||
out[0] = '\0';
|
||||
size_t offset = 0;
|
||||
char *samples = (char *)malloc(out_size);
|
||||
if (!samples) {
|
||||
free(out);
|
||||
return NULL;
|
||||
}
|
||||
samples[0] = '\0';
|
||||
size_t samples_offset = 0;
|
||||
int sample_count = 0;
|
||||
|
||||
if (!json) {
|
||||
append(out, out_size, &offset, "{\"ok\":false,\"error\":\"missing canonical motion JSON\"}\n");
|
||||
free(samples);
|
||||
return out;
|
||||
}
|
||||
|
||||
enum { MAX_EVENTS = 4096 };
|
||||
LctpMotionEvent *events = (LctpMotionEvent *)calloc(MAX_EVENTS, sizeof(LctpMotionEvent));
|
||||
if (!events) {
|
||||
append(out, out_size, &offset, "{\"ok\":false,\"error\":\"failed to allocate motion events\"}\n");
|
||||
free(samples);
|
||||
return out;
|
||||
}
|
||||
|
||||
const LctpTimingOptions options = read_timing_options(json);
|
||||
const int event_count = parse_motion_events(json, events, MAX_EVENTS);
|
||||
if (event_count <= 0) {
|
||||
free(events);
|
||||
append(out, out_size, &offset, "{\"ok\":false,\"error\":\"no canonical motion events\"}\n");
|
||||
free(samples);
|
||||
return out;
|
||||
}
|
||||
|
||||
init_motion_state();
|
||||
status.net_feed_scale = options.feed_scale;
|
||||
status.feed_scale = options.feed_scale;
|
||||
status.rapid_scale = options.rapid_scale;
|
||||
|
||||
TP_STRUCT tp;
|
||||
memset(&tp, 0, sizeof(tp));
|
||||
|
||||
EmcPose start;
|
||||
memset(&start, 0, sizeof(start));
|
||||
const int create_rc = tpCreate(&tp, options.queue_size, 100);
|
||||
const int set_cycle_rc = tpSetCycleTime(&tp, options.cycle_time);
|
||||
const int set_pos_rc = tpSetPos(&tp, &start);
|
||||
const int set_vmax_rc = tpSetVmax(&tp, options.max_velocity, options.max_velocity);
|
||||
const int set_vlimit_rc = tpSetVlimit(&tp, options.max_velocity);
|
||||
const int set_amax_rc = tpSetAmax(&tp, options.max_acceleration);
|
||||
const int set_term_rc = tpSetTermCond(
|
||||
&tp,
|
||||
options.tolerance > 0.0 ? TC_TERM_COND_PARABOLIC : TC_TERM_COND_STOP,
|
||||
options.tolerance);
|
||||
|
||||
int add_failures = 0;
|
||||
int next_event = 0;
|
||||
int accepted_events = 0;
|
||||
appendf(out, out_size, &offset,
|
||||
"{\"apiName\":\"linuxcnc_tp_queue_timing\","
|
||||
"\"semanticBoundary\":\"linuxcnc_tp_queue_runtime_timing_from_canonical_motion\","
|
||||
"\"ok\":true,"
|
||||
"\"tpCreate\":%d,"
|
||||
"\"tpSetCycleTime\":%d,"
|
||||
"\"tpSetPos\":%d,"
|
||||
"\"tpSetVmax\":%d,"
|
||||
"\"tpSetVlimit\":%d,"
|
||||
"\"tpSetAmax\":%d,"
|
||||
"\"tpSetTermCond\":%d,"
|
||||
"\"cycleTime\":%.12g,"
|
||||
"\"motionCount\":%d,"
|
||||
"\"segments\":[",
|
||||
create_rc,
|
||||
set_cycle_rc,
|
||||
set_pos_rc,
|
||||
set_vmax_rc,
|
||||
set_vlimit_rc,
|
||||
set_amax_rc,
|
||||
set_term_rc,
|
||||
options.cycle_time,
|
||||
event_count);
|
||||
|
||||
int cycles = 0;
|
||||
int cycle_rc = 0;
|
||||
int current_segment = -1;
|
||||
int emitted_segments = 0;
|
||||
int segment_start_cycle = 0;
|
||||
int segment_last_cycle = 0;
|
||||
while (next_event < event_count && tpQueueDepth(&tp) < options.queue_size) {
|
||||
enqueue_next_nonzero_event(
|
||||
&tp,
|
||||
events,
|
||||
event_count,
|
||||
&next_event,
|
||||
&options,
|
||||
&accepted_events,
|
||||
&add_failures,
|
||||
out,
|
||||
out_size,
|
||||
&offset,
|
||||
&emitted_segments,
|
||||
cycles);
|
||||
}
|
||||
while (cycles < options.max_cycles && (!tpIsDone(&tp) || next_event < event_count)) {
|
||||
while (next_event < event_count && tpQueueDepth(&tp) < options.queue_size) {
|
||||
enqueue_next_nonzero_event(
|
||||
&tp,
|
||||
events,
|
||||
event_count,
|
||||
&next_event,
|
||||
&options,
|
||||
&accepted_events,
|
||||
&add_failures,
|
||||
out,
|
||||
out_size,
|
||||
&offset,
|
||||
&emitted_segments,
|
||||
cycles);
|
||||
}
|
||||
|
||||
cycle_rc = tpRunCycle(&tp, (long)llround(options.cycle_time * 1000000000.0));
|
||||
cycles += 1;
|
||||
const struct state_tag_t exec_tag = tpGetExecTag(&tp);
|
||||
const int exec_segment = exec_tag.fields[GM_FIELD_LINE_NUMBER] - 1;
|
||||
if (exec_segment >= 0 && exec_segment < event_count) {
|
||||
if (
|
||||
sample_count == 0 ||
|
||||
cycles % options.sample_stride == 0 ||
|
||||
exec_segment != current_segment
|
||||
) {
|
||||
append_runtime_sample(
|
||||
samples,
|
||||
out_size,
|
||||
&samples_offset,
|
||||
&sample_count,
|
||||
cycles,
|
||||
exec_segment,
|
||||
&events[exec_segment],
|
||||
&options,
|
||||
&tp);
|
||||
}
|
||||
if (current_segment >= 0 && exec_segment != current_segment) {
|
||||
LctpMotionEvent *completed = &events[current_segment];
|
||||
EmcPose pos;
|
||||
memset(&pos, 0, sizeof(pos));
|
||||
const int get_pos_rc = tpGetPos(&tp, &pos);
|
||||
const double vel = event_velocity(completed, &options);
|
||||
appendf(out, out_size, &offset,
|
||||
"%s{\"index\":%d,\"line\":%d,\"type\":\"%s\","
|
||||
"\"tpCycleRc\":%d,\"tpGetPos\":%d,\"cycles\":%d,"
|
||||
"\"durationSeconds\":%.12g,\"elapsedSeconds\":%.12g,"
|
||||
"\"queueDepth\":%d,\"activeDepth\":%d,\"velocity\":%.12g,"
|
||||
"\"x\":%.12g,\"y\":%.12g,\"z\":%.12g,\"a\":%.12g,\"b\":%.12g,\"c\":%.12g}",
|
||||
emitted_segments == 0 ? "" : ",",
|
||||
current_segment,
|
||||
completed->line,
|
||||
completed->type,
|
||||
cycle_rc,
|
||||
get_pos_rc,
|
||||
segment_last_cycle - segment_start_cycle + 1,
|
||||
(segment_last_cycle - segment_start_cycle + 1) * options.cycle_time,
|
||||
segment_last_cycle * options.cycle_time,
|
||||
tpQueueDepth(&tp),
|
||||
tpActiveDepth(&tp),
|
||||
vel,
|
||||
pos.tran.x,
|
||||
pos.tran.y,
|
||||
pos.tran.z,
|
||||
pos.a,
|
||||
pos.b,
|
||||
pos.c);
|
||||
emitted_segments += 1;
|
||||
segment_start_cycle = cycles;
|
||||
}
|
||||
if (exec_segment != current_segment) {
|
||||
current_segment = exec_segment;
|
||||
segment_start_cycle = cycles;
|
||||
}
|
||||
segment_last_cycle = cycles;
|
||||
}
|
||||
if (cycle_rc < 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (current_segment >= 0) {
|
||||
append_runtime_sample(
|
||||
samples,
|
||||
out_size,
|
||||
&samples_offset,
|
||||
&sample_count,
|
||||
cycles,
|
||||
current_segment,
|
||||
&events[current_segment],
|
||||
&options,
|
||||
&tp);
|
||||
LctpMotionEvent *completed = &events[current_segment];
|
||||
EmcPose pos;
|
||||
memset(&pos, 0, sizeof(pos));
|
||||
const int get_pos_rc = tpGetPos(&tp, &pos);
|
||||
const double vel = event_velocity(completed, &options);
|
||||
appendf(out, out_size, &offset,
|
||||
"%s{\"index\":%d,\"line\":%d,\"type\":\"%s\","
|
||||
"\"tpCycleRc\":%d,\"tpGetPos\":%d,\"cycles\":%d,"
|
||||
"\"durationSeconds\":%.12g,\"elapsedSeconds\":%.12g,"
|
||||
"\"queueDepth\":%d,\"activeDepth\":%d,\"velocity\":%.12g,"
|
||||
"\"x\":%.12g,\"y\":%.12g,\"z\":%.12g,\"a\":%.12g,\"b\":%.12g,\"c\":%.12g}",
|
||||
emitted_segments == 0 ? "" : ",",
|
||||
current_segment,
|
||||
completed->line,
|
||||
completed->type,
|
||||
cycle_rc,
|
||||
get_pos_rc,
|
||||
segment_last_cycle - segment_start_cycle + 1,
|
||||
(segment_last_cycle - segment_start_cycle + 1) * options.cycle_time,
|
||||
segment_last_cycle * options.cycle_time,
|
||||
tpQueueDepth(&tp),
|
||||
tpActiveDepth(&tp),
|
||||
vel,
|
||||
pos.tran.x,
|
||||
pos.tran.y,
|
||||
pos.tran.z,
|
||||
pos.a,
|
||||
pos.b,
|
||||
pos.c);
|
||||
emitted_segments += 1;
|
||||
}
|
||||
|
||||
appendf(out, out_size, &offset,
|
||||
"],\"samples\":[%s],\"sampleCount\":%d,"
|
||||
"\"acceptedMotionCount\":%d,\"emittedMotionCount\":%d,\"totalCycles\":%d,"
|
||||
"\"totalSeconds\":%.12g,\"addFailures\":%d,\"tpDone\":%d,\"finalQueueDepth\":%d}\n",
|
||||
samples,
|
||||
sample_count,
|
||||
accepted_events,
|
||||
emitted_segments,
|
||||
cycles,
|
||||
cycles * options.cycle_time,
|
||||
add_failures,
|
||||
tpIsDone(&tp),
|
||||
tpQueueDepth(&tp));
|
||||
|
||||
free(events);
|
||||
free(samples);
|
||||
return out;
|
||||
}
|
||||
|
||||
static void append_linear_probe(char *out, size_t size, size_t *offset)
|
||||
{
|
||||
init_motion_state();
|
||||
|
||||
228
wasm-port/runtime/sdk/src/linuxcnc-tp.js
Normal file
228
wasm-port/runtime/sdk/src/linuxcnc-tp.js
Normal file
@@ -0,0 +1,228 @@
|
||||
import createLinuxCncTpModule from "../../../build/wasm/tp/linuxcnc_tp.js";
|
||||
|
||||
const SEMANTIC_BOUNDARY = "linuxcnc_tp_queue_runtime_timing_from_canonical_motion";
|
||||
|
||||
function allocCString(mod, value) {
|
||||
const bytes = mod.lengthBytesUTF8(value) + 1;
|
||||
const ptr = mod._malloc(bytes);
|
||||
mod.stringToUTF8(value, ptr, bytes);
|
||||
return ptr;
|
||||
}
|
||||
|
||||
function callTiming(mod, payload) {
|
||||
const inputPtr = allocCString(mod, JSON.stringify(payload));
|
||||
let resultPtr = 0;
|
||||
try {
|
||||
resultPtr = mod._lctp_run_canonical_motion_timing(inputPtr);
|
||||
if (!resultPtr) {
|
||||
throw new Error("lctp_run_canonical_motion_timing returned null");
|
||||
}
|
||||
const text = mod.UTF8ToString(resultPtr);
|
||||
const result = JSON.parse(text);
|
||||
if (result.ok !== true) {
|
||||
throw new Error(result.error || "LinuxCNC TP queue timing failed");
|
||||
}
|
||||
return result;
|
||||
} finally {
|
||||
if (resultPtr) mod._lctp_free_string(resultPtr);
|
||||
mod._free(inputPtr);
|
||||
}
|
||||
}
|
||||
|
||||
export async function createLinuxCncTpSdk(moduleOptions = {}) {
|
||||
const mod = await createLinuxCncTpModule(moduleOptions);
|
||||
|
||||
return {
|
||||
apiName: "linuxcnc-tp-wasm-sdk",
|
||||
semanticBoundary: SEMANTIC_BOUNDARY,
|
||||
module: mod,
|
||||
|
||||
readiness() {
|
||||
return {
|
||||
apiName: "linuxcnc-tp-wasm-sdk-readiness",
|
||||
loaded: true,
|
||||
semanticBoundary: SEMANTIC_BOUNDARY,
|
||||
runCanonicalMotionTimingReady: typeof mod._lctp_run_canonical_motion_timing === "function",
|
||||
};
|
||||
},
|
||||
|
||||
runCanonicalMotionTiming({ motion = [], options = {} } = {}) {
|
||||
const result = callTiming(mod, {
|
||||
...options,
|
||||
motion: motion.map((event, index) => ({
|
||||
index,
|
||||
line: Number.isFinite(Number(event.line)) ? Number(event.line) : -1,
|
||||
type: event.type,
|
||||
feedRate: Number.isFinite(Number(event.feedRate)) ? Number(event.feedRate) : 0,
|
||||
x: numberOrZero(event.axes?.x),
|
||||
y: numberOrZero(event.axes?.y),
|
||||
z: numberOrZero(event.axes?.z),
|
||||
a: numberOrZero(event.axes?.a),
|
||||
b: numberOrZero(event.axes?.b),
|
||||
c: numberOrZero(event.axes?.c),
|
||||
u: numberOrZero(event.axes?.u),
|
||||
v: numberOrZero(event.axes?.v),
|
||||
w: numberOrZero(event.axes?.w),
|
||||
plane: numberOrZero(event.axes?.arc?.plane),
|
||||
centerFirst: numberOrZero(event.axes?.arc?.centerFirst),
|
||||
centerSecond: numberOrZero(event.axes?.arc?.centerSecond),
|
||||
rotation: numberOrZero(event.axes?.arc?.rotation),
|
||||
axisEndPoint: numberOrZero(event.axes?.arc?.axisEndPoint),
|
||||
})),
|
||||
});
|
||||
return normalizeTimingResult(result, motion);
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
function normalizeTimingResult(result, motion) {
|
||||
const rawSegmentsByIndex = new Map((result.segments || []).map((segment) => [segment.index, segment]));
|
||||
let elapsedSeconds = 0;
|
||||
const segments = motion.map((event, index) => {
|
||||
const raw = rawSegmentsByIndex.get(index);
|
||||
const durationSeconds = Math.max(Number(raw?.durationSeconds) || 0, 0);
|
||||
elapsedSeconds += durationSeconds;
|
||||
return {
|
||||
index,
|
||||
line: event.line ?? null,
|
||||
type: event.type,
|
||||
motionClass: event.type === "STRAIGHT_TRAVERSE" ? "rapid" : "feed",
|
||||
durationSeconds,
|
||||
elapsedSeconds,
|
||||
currentVelocity: Number(raw?.velocity) || 0,
|
||||
velocityMmPerMin: (Number(raw?.velocity) || 0) * 60,
|
||||
queueDepth: Number(raw?.queueDepth) || 0,
|
||||
activeDepth: Number(raw?.activeDepth) || 0,
|
||||
cycles: Number(raw?.cycles) || 0,
|
||||
axes: event.axes || {},
|
||||
runtimeAxes: axesFromRuntimeRecord(raw, event.axes || {}),
|
||||
tp: raw || null,
|
||||
};
|
||||
});
|
||||
const rawSamples = Array.isArray(result.samples) && result.samples.length > 0
|
||||
? result.samples
|
||||
: synthesizeSamplesFromSegments(result.segments || []);
|
||||
const samples = normalizeSamples(rawSamples);
|
||||
|
||||
const feedSeconds = segments
|
||||
.filter((segment) => segment.motionClass === "feed")
|
||||
.reduce((total, segment) => total + segment.durationSeconds, 0);
|
||||
const rapidSeconds = segments
|
||||
.filter((segment) => segment.motionClass === "rapid")
|
||||
.reduce((total, segment) => total + segment.durationSeconds, 0);
|
||||
|
||||
return {
|
||||
apiName: "web-rtcp-5axis-linuxcnc-tp-program-timing",
|
||||
semanticBoundary: SEMANTIC_BOUNDARY,
|
||||
sourceBasis: "LinuxCNC interpreter canonical motion events queued through LinuxCNC src/emc/tp WASM",
|
||||
plannerRuntimeReady: result.ok === true
|
||||
&& result.addFailures === 0
|
||||
&& result.tpDone === 1
|
||||
&& segments.length === motion.length,
|
||||
sampleCount: Number(result.sampleCount) || samples.length,
|
||||
samples,
|
||||
totalSeconds: elapsedSeconds,
|
||||
totalMinutes: elapsedSeconds / 60,
|
||||
feedSeconds,
|
||||
rapidSeconds,
|
||||
motionCount: motion.length,
|
||||
emittedMotionCount: result.emittedMotionCount || 0,
|
||||
acceptedMotionCount: result.acceptedMotionCount || 0,
|
||||
totalCycles: result.totalCycles || 0,
|
||||
cycleTime: result.cycleTime || 0.001,
|
||||
addFailures: result.addFailures || 0,
|
||||
tpDone: result.tpDone === 1,
|
||||
finalQueueDepth: result.finalQueueDepth || 0,
|
||||
segments,
|
||||
raw: result,
|
||||
};
|
||||
}
|
||||
|
||||
function normalizeSamples(samples) {
|
||||
return samples.map((sample, index) => ({
|
||||
sampleIndex: Number(sample.sampleIndex ?? index),
|
||||
cycle: Number(sample.cycle) || 0,
|
||||
timeSeconds: Number(sample.timeSeconds) || 0,
|
||||
motionIndex: Number(sample.motionIndex) || 0,
|
||||
line: Number.isFinite(Number(sample.line)) ? Number(sample.line) : null,
|
||||
type: sample.type || "-",
|
||||
axes: {
|
||||
x: numberOrZero(sample.x),
|
||||
y: numberOrZero(sample.y),
|
||||
z: numberOrZero(sample.z),
|
||||
a: numberOrZero(sample.a),
|
||||
b: numberOrZero(sample.b),
|
||||
c: numberOrZero(sample.c),
|
||||
u: numberOrZero(sample.u),
|
||||
v: numberOrZero(sample.v),
|
||||
w: numberOrZero(sample.w),
|
||||
},
|
||||
currentVelocity: numberOrZero(sample.currentVelocity),
|
||||
currentVelocityMmPerMin: numberOrZero(sample.currentVelocity) * 60,
|
||||
requestedVelocity: numberOrZero(sample.requestedVelocity),
|
||||
requestedVelocityMmPerMin: numberOrZero(sample.requestedVelocity) * 60,
|
||||
distanceToGo: numberOrZero(sample.distanceToGo),
|
||||
dtg: {
|
||||
x: numberOrZero(sample.dtgX),
|
||||
y: numberOrZero(sample.dtgY),
|
||||
z: numberOrZero(sample.dtgZ),
|
||||
},
|
||||
queueDepth: Number(sample.queueDepth) || 0,
|
||||
activeDepth: Number(sample.activeDepth) || 0,
|
||||
}));
|
||||
}
|
||||
|
||||
function synthesizeSamplesFromSegments(segments) {
|
||||
return segments.map((segment, index) => ({
|
||||
sampleIndex: index,
|
||||
cycle: Number(segment.cycles) || 0,
|
||||
timeSeconds: Number(segment.elapsedSeconds) || 0,
|
||||
motionIndex: Number(segment.index) || index,
|
||||
line: segment.line,
|
||||
type: segment.type,
|
||||
x: segment.x,
|
||||
y: segment.y,
|
||||
z: segment.z,
|
||||
a: segment.a,
|
||||
b: segment.b,
|
||||
c: segment.c,
|
||||
u: segment.u,
|
||||
v: segment.v,
|
||||
w: segment.w,
|
||||
currentVelocity: segment.velocity,
|
||||
requestedVelocity: segment.velocity,
|
||||
distanceToGo: segment.distanceToGo ?? 0,
|
||||
dtgX: segment.dtgX ?? 0,
|
||||
dtgY: segment.dtgY ?? 0,
|
||||
dtgZ: segment.dtgZ ?? 0,
|
||||
queueDepth: segment.queueDepth,
|
||||
activeDepth: segment.activeDepth,
|
||||
}));
|
||||
}
|
||||
|
||||
function axesFromRuntimeRecord(record, fallback = {}) {
|
||||
return {
|
||||
x: numberOrFallback(record?.x, fallback.x, 0),
|
||||
y: numberOrFallback(record?.y, fallback.y, 0),
|
||||
z: numberOrFallback(record?.z, fallback.z, 0),
|
||||
a: numberOrFallback(record?.a, fallback.a, 0),
|
||||
b: numberOrFallback(record?.b, fallback.b, 0),
|
||||
c: numberOrFallback(record?.c, fallback.c, 0),
|
||||
u: numberOrFallback(record?.u, fallback.u, 0),
|
||||
v: numberOrFallback(record?.v, fallback.v, 0),
|
||||
w: numberOrFallback(record?.w, fallback.w, 0),
|
||||
};
|
||||
}
|
||||
|
||||
function numberOrFallback(...values) {
|
||||
for (const value of values) {
|
||||
const number = Number(value);
|
||||
if (Number.isFinite(number)) return number;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
function numberOrZero(value) {
|
||||
const number = Number(value);
|
||||
return Number.isFinite(number) ? number : 0;
|
||||
}
|
||||
Reference in New Issue
Block a user