wangdequan/cnc_wams
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

接入 LinuxCNC TP 运行反馈

Browse Source
This commit is contained in:
wangdequan
2026-06-21 23:29:56 +08:00
parent 626bcfe8e3
commit 3771b9eafe
44 changed files with 7881 additions and 326 deletions
Download Patch File Download Diff File Expand all files Collapse all files

659
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_tp_wasm.c
View File

@@ -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(&center, 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();