按规划持续工作
结论：新增 vendored LinuxCNC trajectory planner WASM 构建和 Node smoke，覆盖线段、圆弧、队列规划探针，并纳入 host 聚合验证。
This commit is contained in:
442
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_tp_wasm.c
Normal file
442
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_tp_wasm.c
Normal file
@@ -0,0 +1,442 @@
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <emscripten/emscripten.h>
|
||||
|
||||
#include "emc/motion/motion.h"
|
||||
#include "emc/nml_intf/motion_types.h"
|
||||
#include "emc/tp/tp.h"
|
||||
|
||||
static emcmot_status_t status;
|
||||
static emcmot_config_t config;
|
||||
|
||||
static void dio_write(int index, char value)
|
||||
{
|
||||
(void)index;
|
||||
(void)value;
|
||||
}
|
||||
|
||||
static void aio_write(int index, double value)
|
||||
{
|
||||
(void)index;
|
||||
(void)value;
|
||||
}
|
||||
|
||||
static void set_rotary_unlock(int joint, int unlock)
|
||||
{
|
||||
(void)joint;
|
||||
(void)unlock;
|
||||
}
|
||||
|
||||
static int get_rotary_unlock(int joint)
|
||||
{
|
||||
(void)joint;
|
||||
return 1;
|
||||
}
|
||||
|
||||
static double axis_vel_limit(int axis)
|
||||
{
|
||||
return axis >= 0 && axis < EMCMOT_MAX_AXIS ? 10.0 : 0.0;
|
||||
}
|
||||
|
||||
static double axis_acc_limit(int axis)
|
||||
{
|
||||
return axis >= 0 && axis < EMCMOT_MAX_AXIS ? 20.0 : 0.0;
|
||||
}
|
||||
|
||||
static void init_motion_state(void)
|
||||
{
|
||||
memset(&status, 0, sizeof(status));
|
||||
memset(&config, 0, sizeof(config));
|
||||
|
||||
config.numJoints = 3;
|
||||
config.numSpindles = 1;
|
||||
config.numDIO = 4;
|
||||
config.numAIO = 4;
|
||||
config.arcBlendOptDepth = 0;
|
||||
config.arcBlendEnable = 0;
|
||||
config.arcBlendFallbackEnable = 0;
|
||||
config.arcBlendGapCycles = 4;
|
||||
config.arcBlendRampFreq = 20.0;
|
||||
config.arcBlendTangentKinkRatio = 0.1;
|
||||
config.maxFeedScale = 1.0;
|
||||
|
||||
status.net_feed_scale = 1.0;
|
||||
status.feed_scale = 1.0;
|
||||
status.rapid_scale = 1.0;
|
||||
status.enables_new = FS_ENABLED | SS_ENABLED;
|
||||
status.enables_queued = status.enables_new;
|
||||
status.spindle_status[0].at_speed = 1;
|
||||
status.spindle_status[0].direction = 1;
|
||||
status.spindleSync = 0;
|
||||
status.jerk = 0.0;
|
||||
status.planner_type = 0;
|
||||
|
||||
tpMotData(&status, &config);
|
||||
tpMotFunctions(
|
||||
dio_write,
|
||||
aio_write,
|
||||
set_rotary_unlock,
|
||||
get_rotary_unlock,
|
||||
axis_vel_limit,
|
||||
axis_acc_limit);
|
||||
}
|
||||
|
||||
static int pose_near(const EmcPose *actual, const EmcPose *expected)
|
||||
{
|
||||
return fabs(actual->tran.x - expected->tran.x) < 1e-9 &&
|
||||
fabs(actual->tran.y - expected->tran.y) < 1e-9 &&
|
||||
fabs(actual->tran.z - expected->tran.z) < 1e-9;
|
||||
}
|
||||
|
||||
static void append(char *out, size_t size, size_t *offset, const char *text)
|
||||
{
|
||||
if (*offset >= size) {
|
||||
return;
|
||||
}
|
||||
const int written = snprintf(out + *offset, size - *offset, "%s", text);
|
||||
if (written > 0) {
|
||||
*offset += (size_t)written;
|
||||
}
|
||||
}
|
||||
|
||||
static void append_linear_probe(char *out, size_t size, size_t *offset)
|
||||
{
|
||||
init_motion_state();
|
||||
|
||||
TP_STRUCT tp;
|
||||
EmcPose start;
|
||||
EmcPose end;
|
||||
struct state_tag_t tag;
|
||||
memset(&tp, 0, sizeof(tp));
|
||||
memset(&start, 0, sizeof(start));
|
||||
memset(&end, 0, sizeof(end));
|
||||
memset(&tag, 0, sizeof(tag));
|
||||
|
||||
end.tran.x = 1.0;
|
||||
|
||||
const int create_rc = tpCreate(&tp, TP_DEFAULT_QUEUE_SIZE, 1);
|
||||
const int set_cycle_rc = tpSetCycleTime(&tp, 0.001);
|
||||
const int set_pos_rc = tpSetPos(&tp, &start);
|
||||
const int set_vmax_rc = tpSetVmax(&tp, 1.0, 1.0);
|
||||
const int set_vlimit_rc = tpSetVlimit(&tp, 1.0);
|
||||
const int set_amax_rc = tpSetAmax(&tp, 10.0);
|
||||
const int set_term_rc = tpSetTermCond(&tp, TC_TERM_COND_STOP, 0.0);
|
||||
const int add_line_rc = tpAddLine(
|
||||
&tp,
|
||||
end,
|
||||
EMC_MOTION_TYPE_FEED,
|
||||
1.0,
|
||||
1.0,
|
||||
10.0,
|
||||
100.0,
|
||||
status.enables_new,
|
||||
0,
|
||||
-1,
|
||||
tag);
|
||||
|
||||
int cycle_rc = 0;
|
||||
int cycles = 0;
|
||||
for (; cycles < 10000 && !tpIsDone(&tp); ++cycles) {
|
||||
cycle_rc = tpRunCycle(&tp, 1000000);
|
||||
if (cycle_rc < 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
EmcPose final_pos;
|
||||
memset(&final_pos, 0, sizeof(final_pos));
|
||||
const int get_pos_rc = tpGetPos(&tp, &final_pos);
|
||||
|
||||
char line[1024];
|
||||
snprintf(line, sizeof(line),
|
||||
"tp_create=%d\n"
|
||||
"tp_set_cycle_time=%d\n"
|
||||
"tp_set_pos=%d\n"
|
||||
"tp_set_vmax=%d\n"
|
||||
"tp_set_vlimit=%d\n"
|
||||
"tp_set_amax=%d\n"
|
||||
"tp_set_term_cond=%d\n"
|
||||
"tp_add_line=%d\n"
|
||||
"tp_cycle_rc=%d\n"
|
||||
"tp_cycles=%d\n"
|
||||
"tp_get_pos=%d\n"
|
||||
"tp_done_after_line=%d\n"
|
||||
"tp_queue_depth=%d\n"
|
||||
"tp_final_pos=%.12g,%.12g,%.12g\n",
|
||||
create_rc,
|
||||
set_cycle_rc,
|
||||
set_pos_rc,
|
||||
set_vmax_rc,
|
||||
set_vlimit_rc,
|
||||
set_amax_rc,
|
||||
set_term_rc,
|
||||
add_line_rc,
|
||||
cycle_rc,
|
||||
cycles,
|
||||
get_pos_rc,
|
||||
tpIsDone(&tp),
|
||||
tpQueueDepth(&tp),
|
||||
final_pos.tran.x,
|
||||
final_pos.tran.y,
|
||||
final_pos.tran.z);
|
||||
append(out, size, offset, line);
|
||||
}
|
||||
|
||||
static void append_arc_probe(char *out, size_t size, size_t *offset)
|
||||
{
|
||||
init_motion_state();
|
||||
|
||||
TP_STRUCT tp;
|
||||
EmcPose start;
|
||||
EmcPose end;
|
||||
PmCartesian center;
|
||||
PmCartesian normal;
|
||||
struct state_tag_t tag;
|
||||
memset(&tp, 0, sizeof(tp));
|
||||
memset(&start, 0, sizeof(start));
|
||||
memset(&end, 0, sizeof(end));
|
||||
memset(¢er, 0, sizeof(center));
|
||||
memset(&normal, 0, sizeof(normal));
|
||||
memset(&tag, 0, sizeof(tag));
|
||||
|
||||
start.tran.x = 1.0;
|
||||
end.tran.y = 1.0;
|
||||
normal.z = 1.0;
|
||||
|
||||
const int create_rc = tpCreate(&tp, TP_DEFAULT_QUEUE_SIZE, 2);
|
||||
const int set_cycle_rc = tpSetCycleTime(&tp, 0.001);
|
||||
const int set_pos_rc = tpSetPos(&tp, &start);
|
||||
const int set_vmax_rc = tpSetVmax(&tp, 1.0, 1.0);
|
||||
const int set_vlimit_rc = tpSetVlimit(&tp, 1.0);
|
||||
const int set_amax_rc = tpSetAmax(&tp, 10.0);
|
||||
const int set_term_rc = tpSetTermCond(&tp, TC_TERM_COND_STOP, 0.0);
|
||||
const int add_circle_rc = tpAddCircle(
|
||||
&tp,
|
||||
end,
|
||||
center,
|
||||
normal,
|
||||
0,
|
||||
EMC_MOTION_TYPE_ARC,
|
||||
1.0,
|
||||
1.0,
|
||||
10.0,
|
||||
100.0,
|
||||
status.enables_new,
|
||||
0,
|
||||
tag);
|
||||
|
||||
int cycle_rc = 0;
|
||||
int cycles = 0;
|
||||
for (; cycles < 10000 && !tpIsDone(&tp); ++cycles) {
|
||||
cycle_rc = tpRunCycle(&tp, 1000000);
|
||||
if (cycle_rc < 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
EmcPose final_pos;
|
||||
memset(&final_pos, 0, sizeof(final_pos));
|
||||
const int get_pos_rc = tpGetPos(&tp, &final_pos);
|
||||
const int near_end = pose_near(&final_pos, &end);
|
||||
|
||||
char line[1024];
|
||||
snprintf(line, sizeof(line),
|
||||
"tp_arc_create=%d\n"
|
||||
"tp_arc_set_cycle_time=%d\n"
|
||||
"tp_arc_set_pos=%d\n"
|
||||
"tp_arc_set_vmax=%d\n"
|
||||
"tp_arc_set_vlimit=%d\n"
|
||||
"tp_arc_set_amax=%d\n"
|
||||
"tp_arc_set_term_cond=%d\n"
|
||||
"tp_add_circle=%d\n"
|
||||
"tp_arc_cycle_rc=%d\n"
|
||||
"tp_arc_cycles=%d\n"
|
||||
"tp_arc_get_pos=%d\n"
|
||||
"tp_done_after_arc=%d\n"
|
||||
"tp_arc_queue_depth=%d\n"
|
||||
"tp_arc_final_pos_near_end=%d\n"
|
||||
"tp_arc_final_pos=%.12g,%.12g,%.12g\n",
|
||||
create_rc,
|
||||
set_cycle_rc,
|
||||
set_pos_rc,
|
||||
set_vmax_rc,
|
||||
set_vlimit_rc,
|
||||
set_amax_rc,
|
||||
set_term_rc,
|
||||
add_circle_rc,
|
||||
cycle_rc,
|
||||
cycles,
|
||||
get_pos_rc,
|
||||
tpIsDone(&tp),
|
||||
tpQueueDepth(&tp),
|
||||
near_end,
|
||||
final_pos.tran.x,
|
||||
final_pos.tran.y,
|
||||
final_pos.tran.z);
|
||||
append(out, size, offset, line);
|
||||
}
|
||||
|
||||
static void append_queued_lines_probe(char *out, size_t size, size_t *offset)
|
||||
{
|
||||
init_motion_state();
|
||||
|
||||
TP_STRUCT tp;
|
||||
EmcPose start;
|
||||
EmcPose end1;
|
||||
EmcPose end2;
|
||||
struct state_tag_t tag;
|
||||
memset(&tp, 0, sizeof(tp));
|
||||
memset(&start, 0, sizeof(start));
|
||||
memset(&end1, 0, sizeof(end1));
|
||||
memset(&end2, 0, sizeof(end2));
|
||||
memset(&tag, 0, sizeof(tag));
|
||||
|
||||
end1.tran.x = 1.0;
|
||||
end2.tran.x = 1.0;
|
||||
end2.tran.y = 1.0;
|
||||
|
||||
const int create_rc = tpCreate(&tp, TP_DEFAULT_QUEUE_SIZE, 3);
|
||||
const int set_cycle_rc = tpSetCycleTime(&tp, 0.001);
|
||||
const int set_pos_rc = tpSetPos(&tp, &start);
|
||||
const int set_vmax_rc = tpSetVmax(&tp, 1.0, 1.0);
|
||||
const int set_vlimit_rc = tpSetVlimit(&tp, 1.0);
|
||||
const int set_amax_rc = tpSetAmax(&tp, 10.0);
|
||||
const int set_term_rc = tpSetTermCond(&tp, TC_TERM_COND_STOP, 0.0);
|
||||
const int add_line1_rc = tpAddLine(
|
||||
&tp,
|
||||
end1,
|
||||
EMC_MOTION_TYPE_FEED,
|
||||
1.0,
|
||||
1.0,
|
||||
10.0,
|
||||
100.0,
|
||||
status.enables_new,
|
||||
0,
|
||||
-1,
|
||||
tag);
|
||||
const int depth_after_line1 = tpQueueDepth(&tp);
|
||||
const int add_line2_rc = tpAddLine(
|
||||
&tp,
|
||||
end2,
|
||||
EMC_MOTION_TYPE_FEED,
|
||||
1.0,
|
||||
1.0,
|
||||
10.0,
|
||||
100.0,
|
||||
status.enables_new,
|
||||
0,
|
||||
-1,
|
||||
tag);
|
||||
const int depth_after_line2 = tpQueueDepth(&tp);
|
||||
|
||||
int cycle_rc = 0;
|
||||
int cycles = 0;
|
||||
for (; cycles < 20000 && !tpIsDone(&tp); ++cycles) {
|
||||
cycle_rc = tpRunCycle(&tp, 1000000);
|
||||
if (cycle_rc < 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
EmcPose final_pos;
|
||||
memset(&final_pos, 0, sizeof(final_pos));
|
||||
const int get_pos_rc = tpGetPos(&tp, &final_pos);
|
||||
const int near_end = pose_near(&final_pos, &end2);
|
||||
|
||||
char line[1024];
|
||||
snprintf(line, sizeof(line),
|
||||
"tp_queue_create=%d\n"
|
||||
"tp_queue_set_cycle_time=%d\n"
|
||||
"tp_queue_set_pos=%d\n"
|
||||
"tp_queue_set_vmax=%d\n"
|
||||
"tp_queue_set_vlimit=%d\n"
|
||||
"tp_queue_set_amax=%d\n"
|
||||
"tp_queue_set_term_cond=%d\n"
|
||||
"tp_queue_add_line1=%d\n"
|
||||
"tp_queue_depth_after_line1=%d\n"
|
||||
"tp_queue_add_line2=%d\n"
|
||||
"tp_queue_depth_after_line2=%d\n"
|
||||
"tp_queue_cycle_rc=%d\n"
|
||||
"tp_queue_cycles=%d\n"
|
||||
"tp_queue_get_pos=%d\n"
|
||||
"tp_done_after_queued_lines=%d\n"
|
||||
"tp_queue_final_depth=%d\n"
|
||||
"tp_queue_final_pos_near_end=%d\n"
|
||||
"tp_queue_final_pos=%.12g,%.12g,%.12g\n",
|
||||
create_rc,
|
||||
set_cycle_rc,
|
||||
set_pos_rc,
|
||||
set_vmax_rc,
|
||||
set_vlimit_rc,
|
||||
set_amax_rc,
|
||||
set_term_rc,
|
||||
add_line1_rc,
|
||||
depth_after_line1,
|
||||
add_line2_rc,
|
||||
depth_after_line2,
|
||||
cycle_rc,
|
||||
cycles,
|
||||
get_pos_rc,
|
||||
tpIsDone(&tp),
|
||||
tpQueueDepth(&tp),
|
||||
near_end,
|
||||
final_pos.tran.x,
|
||||
final_pos.tran.y,
|
||||
final_pos.tran.z);
|
||||
append(out, size, offset, line);
|
||||
}
|
||||
|
||||
EMSCRIPTEN_KEEPALIVE
|
||||
char *lctp_run_probe(void)
|
||||
{
|
||||
char *out = (char *)malloc(8192);
|
||||
if (!out) {
|
||||
return NULL;
|
||||
}
|
||||
out[0] = '\0';
|
||||
size_t offset = 0;
|
||||
|
||||
TP_STRUCT tp;
|
||||
TC_STRUCT tc;
|
||||
EmcPose pose;
|
||||
memset(&tp, 0, sizeof(tp));
|
||||
memset(&tc, 0, sizeof(tc));
|
||||
memset(&pose, 0, sizeof(pose));
|
||||
|
||||
pose.tran.x = 1.0;
|
||||
pose.tran.y = 2.0;
|
||||
pose.tran.z = 3.0;
|
||||
|
||||
char line[512];
|
||||
snprintf(line, sizeof(line),
|
||||
"sizeof_TP_STRUCT=%zu\n"
|
||||
"sizeof_TC_STRUCT=%zu\n"
|
||||
"tp_default_queue_size=%d\n"
|
||||
"tp_err_ok=%d\n"
|
||||
"tc_linear=%d\n"
|
||||
"tc_circular=%d\n"
|
||||
"pose_xyz=%.12g,%.12g,%.12g\n",
|
||||
sizeof(tp),
|
||||
sizeof(tc),
|
||||
TP_DEFAULT_QUEUE_SIZE,
|
||||
TP_ERR_OK,
|
||||
TC_LINEAR,
|
||||
TC_CIRCULAR,
|
||||
pose.tran.x,
|
||||
pose.tran.y,
|
||||
pose.tran.z);
|
||||
append(out, 8192, &offset, line);
|
||||
append_linear_probe(out, 8192, &offset);
|
||||
append_arc_probe(out, 8192, &offset);
|
||||
append_queued_lines_probe(out, 8192, &offset);
|
||||
return out;
|
||||
}
|
||||
|
||||
EMSCRIPTEN_KEEPALIVE
|
||||
void lctp_free_string(char *value)
|
||||
{
|
||||
free(value);
|
||||
}
|
||||
Reference in New Issue
Block a user