wangdequan/linuxcncWebWASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
469881794cfe7392af76c662558cdf0ef01ea1ae
linuxcncWebWASM/wasm-core/src/sim_linuxcnc_tp.c

519 lines
30 KiB
C
Raw Blame History

#include "sim_linuxcnc_tp.h"
#include "motion.h"
#include "tp.h"
#include "emcpose.h"
#include "motion_types.h"
#include <math.h>
#include <stddef.h>
#include <stdio.h>
#include <string.h>
extern int tcGetPosReal(TC_STRUCT const *tc, int of_point, EmcPose *pos);
struct sim_linuxcnc_tp {
TP_STRUCT tp;
emcmot_status_t status;
emcmot_config_t config;
double axis_velocity[9];
double axis_acceleration[9];
double axis_min[9];
double axis_max[9];
double velocity_scale;
double jog_scale;
double scale_minimum;
uint32_t tick;
uint32_t last_active_id;
int just_completed;
int initialized;
int step_id;
};
static sim_linuxcnc_tp *active_planner;
/* Same-build checkpoint: TP/TC contain the integration and blend state which
cannot be reconstructed by re-enqueuing the remaining geometric path.
The queue pointer is removed on export and rebound on restore. All other
members of these source structs are values, not process addresses. */
typedef struct {
uint32_t magic, planner_size, tc_size, capacity;
sim_linuxcnc_tp planner;
TC_STRUCT queue[TP_DEFAULT_QUEUE_SIZE];
} tp_checkpoint;
uint32_t sim_linuxcnc_tp_checkpoint_size(void) { return (uint32_t)sizeof(tp_checkpoint); }
int32_t sim_linuxcnc_tp_checkpoint_export(const sim_linuxcnc_tp *planner, uint8_t *output, uint32_t capacity) {
if (planner == NULL || !planner->initialized || output == NULL || capacity != sizeof(tp_checkpoint) || planner->tp.queue.size != TP_DEFAULT_QUEUE_SIZE) return -1228;
tp_checkpoint *saved = (tp_checkpoint *)output;
memset(saved, 0, sizeof(*saved));
saved->magic = UINT32_C(0x54504331);
saved->planner_size = sizeof(sim_linuxcnc_tp); saved->tc_size = sizeof(TC_STRUCT); saved->capacity = TP_DEFAULT_QUEUE_SIZE;
saved->planner = *planner;
saved->planner.tp.queue.queue = NULL;
memcpy(saved->queue, planner->tp.queue.queue, sizeof(saved->queue));
return 0;
}
int32_t sim_linuxcnc_tp_checkpoint_restore(sim_linuxcnc_tp *planner, const uint8_t *input, uint32_t length) {
if (planner == NULL || !planner->initialized || input == NULL || length != sizeof(tp_checkpoint)) return -1228;
const tp_checkpoint *saved = (const tp_checkpoint *)input;
const TC_QUEUE_STRUCT *queue = &saved->planner.tp.queue;
if (saved->magic != UINT32_C(0x54504331) || saved->planner_size != sizeof(sim_linuxcnc_tp) || saved->tc_size != sizeof(TC_STRUCT) || saved->capacity != TP_DEFAULT_QUEUE_SIZE || saved->planner.initialized != 1 || queue->queue != NULL || queue->size != TP_DEFAULT_QUEUE_SIZE || queue->start < 0 || queue->start >= queue->size || queue->end < 0 || queue->end >= queue->size || queue->_len < 0 || queue->_rlen < 0 || queue->_len > queue->size || queue->_rlen > queue->size - queue->_len) return -1228;
if (!isfinite(saved->planner.tp.cycleTime) || saved->planner.tp.cycleTime != planner->tp.cycleTime || memcmp(&saved->planner.config, &planner->config, sizeof(planner->config)) != 0 || memcmp(saved->planner.axis_min, planner->axis_min, sizeof(planner->axis_min)) != 0 || memcmp(saved->planner.axis_max, planner->axis_max, sizeof(planner->axis_max)) != 0 || memcmp(saved->planner.axis_velocity, planner->axis_velocity, sizeof(planner->axis_velocity)) != 0 || memcmp(saved->planner.axis_acceleration, planner->axis_acceleration, sizeof(planner->axis_acceleration)) != 0) return -1229;
TC_STRUCT *storage = planner->tp.queue.queue;
*planner = saved->planner;
planner->tp.queue.queue = storage;
memcpy(storage, saved->queue, sizeof(saved->queue));
active_planner = planner;
tpMotData(&planner->status, &planner->config);
return 0;
}
static void noop_dio(int index, char value) { (void)index; (void)value; }
static void noop_aio(int index, double value) { (void)index; (void)value; }
static void noop_rotary_unlock(int index, int value) { (void)index; (void)value; }
static int noop_rotary_locked(int index) { (void)index; return 0; }
static double axis_velocity(int axis) { return active_planner && axis >= 0 && axis < 9 ? active_planner->axis_velocity[axis] : 0.0; }
static double axis_acceleration(int axis) { return active_planner && axis >= 0 && axis < 9 ? active_planner->axis_acceleration[axis] : 0.0; }
static double pose_delta(const EmcPose *start, const EmcPose *end, int axis) {
const double values_start[9] = {start->tran.x, start->tran.y, start->tran.z, start->a, start->b, start->c, start->u, start->v, start->w};
const double values_end[9] = {end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c, end->u, end->v, end->w};
return values_end[axis] - values_start[axis];
}
static void xyzac_line_limits(const sim_linuxcnc_tp *planner, const EmcPose *start, const EmcPose *end, double *velocity, double *acceleration) {
double linear_length = 0.0, angular_length = 0.0, max_velocity_time = 0.0, max_acceleration_time = 0.0;
int has_linear = 0, has_angular = 0;
for (int axis = 0; axis < 9; ++axis) {
const double delta = fabs(pose_delta(start, end, axis));
const double velocity_limit = planner->axis_velocity[axis];
const double acceleration_limit = planner->axis_acceleration[axis];
if (delta <= 1e-12) continue;
if (axis < 3 || axis >= 6) { has_linear = 1; linear_length += delta * delta; }
else { has_angular = 1; angular_length += delta * delta; }
if (velocity_limit > 0.0) max_velocity_time = fmax(max_velocity_time, delta / velocity_limit);
if (acceleration_limit > 0.0) max_acceleration_time = fmax(max_acceleration_time, delta / acceleration_limit);
}
const double distance = has_linear ? sqrt(linear_length) : has_angular ? sqrt(angular_length) : 0.0;
if (!(distance > 0.0)) return;
if (max_velocity_time > 0.0) *velocity = fmin(*velocity, distance / max_velocity_time);
if (max_acceleration_time > 0.0) *acceleration = fmin(*acceleration, distance / max_acceleration_time);
}
static void xyzac_circle_limits(const sim_linuxcnc_tp *planner, const EmcPose *start, const EmcPose *end, const PmCartesian *center, const PmCartesian *normal, int turn, double *velocity, double *initial_velocity, double *acceleration) {
const double sx = start->tran.x - center->x, sy = start->tran.y - center->y, sz = start->tran.z - center->z;
const double ex = end->tran.x - center->x, ey = end->tran.y - center->y, ez = end->tran.z - center->z;
// ARC_FEED measures radius in the active plane. For the XYZAC case the
// normal is +Z, so the axial helix delta must not become a radial spiral
// term (the previous 3-D radius caused a small but accumulating TP error).
const double normal_abs_x = fabs(normal->x), normal_abs_y = fabs(normal->y), normal_abs_z = fabs(normal->z);
const double start_radius = normal_abs_z >= fmax(normal_abs_x, normal_abs_y) ? hypot(sx, sy) : normal_abs_y >= normal_abs_x ? hypot(sx, sz) : hypot(sy, sz);
const double end_radius = normal_abs_z >= fmax(normal_abs_x, normal_abs_y) ? hypot(ex, ey) : normal_abs_y >= normal_abs_x ? hypot(ex, ez) : hypot(ey, ez);
const double dot = sx * ex + sy * ey + sz * ez;
const double cross_x = sy * ez - sz * ey, cross_y = sz * ex - sx * ez, cross_z = sx * ey - sy * ex;
double angle = fabs(atan2(normal->x * cross_x + normal->y * cross_y + normal->z * cross_z, dot));
if (angle <= 1e-12) angle = 2.0 * M_PI;
angle += 2.0 * M_PI * (fabs((double)turn) - 1.0);
const double spiral = end_radius - start_radius;
const double circular_length = fmin(start_radius, end_radius) * fabs(angle);
const double spiral_length = hypot(circular_length, spiral);
const double effective_radius = sqrt((spiral / fmax(fabs(angle), 1e-12)) * (spiral / fmax(fabs(angle), 1e-12)) + fmin(start_radius, end_radius) * fmin(start_radius, end_radius));
const double planar_velocity = fmin(sqrt(planner->axis_acceleration[0] * sqrt(3.0) * 0.5 * effective_radius), fmin(planner->axis_velocity[0], fmin(planner->axis_velocity[1], planner->axis_velocity[2])));
double total_xyz_length = hypot(spiral_length, normal->x * (end->tran.x - start->tran.x) + normal->y * (end->tran.y - start->tran.y) + normal->z * (end->tran.z - start->tran.z));
double time_motion_velocity = 0.0, time_motion_acceleration = 0.0;
for (int axis = 0; axis < 9; ++axis) {
const double delta = fabs(pose_delta(start, end, axis));
if (planner->axis_velocity[axis] > 0.0) time_motion_velocity = fmax(time_motion_velocity, delta / planner->axis_velocity[axis]);
if (planner->axis_acceleration[axis] > 0.0) time_motion_acceleration = fmax(time_motion_acceleration, delta / planner->axis_acceleration[axis]);
}
const double time_spiral_velocity = planar_velocity > 0.0 ? spiral_length / planar_velocity : INFINITY;
const double time_spiral_acceleration = planner->axis_acceleration[0] > 0.0 ? spiral_length / planner->axis_acceleration[0] : INFINITY;
const double max_velocity = total_xyz_length / fmax(time_motion_velocity, time_spiral_velocity);
const double max_acceleration = total_xyz_length / fmax(time_motion_acceleration, time_spiral_acceleration);
*velocity = fmin(*velocity, max_velocity);
*initial_velocity = max_velocity;
*acceleration = fmin(*acceleration, max_acceleration);
}
static EmcPose to_emc_pose(const sim_posemath_pose *value) {
EmcPose result = {0};
if (value != NULL) {
result.tran.x = value->tran.x; result.tran.y = value->tran.y; result.tran.z = value->tran.z;
result.a = value->a; result.b = value->b; result.c = value->c;
result.u = value->u; result.v = value->v; result.w = value->w;
}
return result;
}
static PmCartesian to_pm_cart(const sim_posemath_cartesian *value) {
PmCartesian result = {0, 0, 0};
if (value != NULL) { result.x = value->x; result.y = value->y; result.z = value->z; }
return result;
}
static sim_posemath_pose from_emc_pose(const EmcPose *value) {
sim_posemath_pose result = {{0, 0, 0}, 0, 0, 0, 0, 0, 0};
if (value != NULL) {
result.tran.x = value->tran.x; result.tran.y = value->tran.y; result.tran.z = value->tran.z;
result.a = value->a; result.b = value->b; result.c = value->c;
result.u = value->u; result.v = value->v; result.w = value->w;
}
return result;
}
static uint64_t hash_bytes(const unsigned char *bytes, size_t length) {
uint64_t hash = UINT64_C(1469598103934665603);
for (size_t index = 0; index < length; ++index) { hash ^= bytes[index]; hash *= UINT64_C(1099511628211); }
return hash;
}
uint32_t sim_linuxcnc_tp_size(void) { return (uint32_t)sizeof(sim_linuxcnc_tp); }
uint32_t sim_linuxcnc_tp_queue_capacity(const sim_linuxcnc_tp *planner) { return planner == NULL || !planner->initialized ? 0u : (uint32_t)TP_DEFAULT_QUEUE_SIZE; }
int32_t sim_linuxcnc_tp_init(sim_linuxcnc_tp *planner, double cycle_time, double velocity_limit, double acceleration_limit) {
if (planner == NULL || !isfinite(cycle_time) || cycle_time <= 0.0 || cycle_time > 60.0 || !isfinite(velocity_limit) || velocity_limit <= 0.0 || !isfinite(acceleration_limit) || acceleration_limit <= 0.0) return -1200;
memset(planner, 0, sizeof(*planner));
planner->config.numJoints = 5; planner->config.numExtraJoints = 0; planner->config.numSpindles = 1;
planner->config.numDIO = 0; planner->config.numAIO = 0; planner->config.numMiscError = 0;
planner->config.trajCycleTime = cycle_time; planner->config.servoCycleTime = cycle_time;
planner->config.limitVel = velocity_limit; planner->config.maxFeedScale = 2.0;
planner->config.arcBlendEnable = 1; planner->config.arcBlendFallbackEnable = 0;
planner->config.arcBlendOptDepth = 50; planner->config.arcBlendGapCycles = 4;
planner->config.arcBlendRampFreq = 100.0; planner->config.arcBlendTangentKinkRatio = 0.1;
// The caller configures each axis from INI before motion is queued.
// Standalone planners use their supplied trajectory limits.
for (int axis = 0; axis < 9; ++axis) {
planner->axis_velocity[axis] = velocity_limit;
planner->axis_acceleration[axis] = acceleration_limit;
planner->axis_min[axis] = -INFINITY;
planner->axis_max[axis] = INFINITY;
}
planner->velocity_scale = 1.0;
planner->jog_scale = 1.0;
planner->scale_minimum = 0.0;
planner->status.feed_scale = 1.0; planner->status.rapid_scale = 1.0; planner->status.net_feed_scale = 1.0;
planner->status.enables_new = FS_ENABLED | FH_ENABLED; planner->status.enables_queued = planner->status.enables_new;
planner->status.motionFlag = EMCMOT_MOTION_ENABLE_BIT | EMCMOT_MOTION_COORD_BIT | EMCMOT_MOTION_INPOS_BIT;
active_planner = planner;
tpMotData(&planner->status, &planner->config);
tpMotFunctions(noop_dio, noop_aio, noop_rotary_unlock, noop_rotary_locked, axis_velocity, axis_acceleration);
if (tpCreate(&planner->tp, TP_DEFAULT_QUEUE_SIZE, 0) != TP_ERR_OK) return -1201;
if (tpSetCycleTime(&planner->tp, cycle_time) != TP_ERR_OK || tpSetVlimit(&planner->tp, velocity_limit) != TP_ERR_OK || tpSetAmax(&planner->tp, acceleration_limit) != TP_ERR_OK) return -1202;
planner->initialized = 1;
return 0;
}
int32_t sim_linuxcnc_tp_reset(sim_linuxcnc_tp *planner) {
if (planner == NULL || !planner->initialized) return -1203;
active_planner = planner;
planner->last_active_id = 0;
planner->just_completed = 0;
return tpClear(&planner->tp) == TP_ERR_OK ? 0 : -1204;
}
int32_t sim_linuxcnc_tp_set_profile(sim_linuxcnc_tp *planner, int profile, double tolerance) {
if (planner == NULL || !planner->initialized || profile < 0 || profile > 2 || !isfinite(tolerance) || tolerance < 0.0) return -1205;
active_planner = planner;
return tpSetTermCond(&planner->tp, profile, tolerance) == TP_ERR_OK ? 0 : -1206;
}
int32_t sim_linuxcnc_tp_set_feed_override(sim_linuxcnc_tp *planner, double scale) {
if (planner == NULL || !planner->initialized || !isfinite(scale) || scale < 0.0 || scale > planner->config.maxFeedScale) return -1207;
planner->status.net_feed_scale = scale; planner->status.feed_scale = scale; return 0;
}
int32_t sim_linuxcnc_tp_set_velocity_scale(sim_linuxcnc_tp *planner, double scale) {
if (planner == NULL || !planner->initialized || !isfinite(scale) || scale <= 0.0 || scale < planner->scale_minimum || scale > 1.0) return -1211;
active_planner = planner;
if (tpSetVlimit(&planner->tp, planner->config.limitVel * scale) != TP_ERR_OK) return -1212;
planner->velocity_scale = scale;
return 0;
}
int32_t sim_linuxcnc_tp_set_jog_scale(sim_linuxcnc_tp *planner, double scale) {
if (planner == NULL || !planner->initialized || !isfinite(scale) || scale <= 0.0 || scale < planner->scale_minimum || scale > 1.0) return -1213;
planner->jog_scale = scale;
return 0;
}
int32_t sim_linuxcnc_tp_set_axis_limit(sim_linuxcnc_tp *planner, int axis, double minimum, double maximum) {
if (planner == NULL || !planner->initialized || axis < 0 || axis >= 9 || !isfinite(minimum) || !isfinite(maximum) || minimum >= maximum) return -1214;
planner->axis_min[axis] = minimum;
planner->axis_max[axis] = maximum;
return 0;
}
int32_t sim_linuxcnc_tp_set_axis_dynamics(sim_linuxcnc_tp *planner, int axis, double velocity, double acceleration) {
if (planner == NULL || !planner->initialized || axis < 0 || axis >= 9 || !isfinite(velocity) || velocity <= 0.0 || !isfinite(acceleration) || acceleration <= 0.0) return -1214;
planner->axis_velocity[axis] = velocity;
planner->axis_acceleration[axis] = acceleration;
return 0;
}
int32_t sim_linuxcnc_tp_set_feed_limit(sim_linuxcnc_tp *planner, double maximum) {
if (planner == NULL || !planner->initialized || !isfinite(maximum) || maximum < 1.0) return -1207;
planner->config.maxFeedScale = maximum;
return 0;
}
int32_t sim_linuxcnc_tp_set_scale_minimum(sim_linuxcnc_tp *planner, double minimum) {
if (planner == NULL || !planner->initialized || !isfinite(minimum) || minimum <= 0.0 || minimum > 1.0) return -1211;
planner->scale_minimum = minimum;
return 0;
}
int32_t sim_linuxcnc_tp_set_blend(sim_linuxcnc_tp *planner, int enable, int fallback, int depth, int gap, double ramp, double kink) {
if (planner == NULL || !planner->initialized || (enable != 0 && enable != 1) || (fallback != 0 && fallback != 1) || depth < 1 || gap < 0 || !isfinite(ramp) || ramp <= 0.0 || !isfinite(kink) || kink <= 0.0) return -1205;
planner->config.arcBlendEnable = enable;
planner->config.arcBlendFallbackEnable = fallback;
planner->config.arcBlendOptDepth = depth;
planner->config.arcBlendGapCycles = gap;
planner->config.arcBlendRampFreq = ramp;
planner->config.arcBlendTangentKinkRatio = kink;
return 0;
}
int32_t sim_linuxcnc_tp_jog(sim_linuxcnc_tp *planner, int axis, int direction, double increment, uint32_t tag) {
EmcPose end;
double *value;
double velocity;
struct state_tag_t state_tag = {0};
if (planner == NULL || !planner->initialized || axis < 0 || axis >= 9 || (direction != -1 && direction != 1) || !isfinite(increment) || increment < 0.0) return -1215;
active_planner = planner;
end = planner->tp.goalPos;
value = axis == 0 ? &end.tran.x : axis == 1 ? &end.tran.y : axis == 2 ? &end.tran.z : axis == 3 ? &end.a : axis == 4 ? &end.b : axis == 5 ? &end.c : axis == 6 ? &end.u : axis == 7 ? &end.v : &end.w;
// command.c::EMCMOT_JOG_CONT targets the directional jog limit until abort.
// Zero is the continuous-mode encoding; positive increments retain their ABI.
if (increment == 0.0) {
double target = direction > 0 ? planner->axis_max[axis] : planner->axis_min[axis];
if ((target - *value) * direction <= 0.0) return -1217;
*value = target;
} else {
*value += (double)direction * increment;
}
if (!isfinite(*value) || *value < planner->axis_min[axis] || *value > planner->axis_max[axis]) return -1217;
velocity = planner->axis_velocity[axis] * planner->jog_scale;
state_tag.fields_float[GM_FIELD_FLOAT_LINE_NUMBER] = (float)tag;
if (tpSetId(&planner->tp, (int)tag) != TP_ERR_OK) return -1218;
return tpAddLine(&planner->tp, end, TC_LINEAR, velocity, velocity, planner->axis_acceleration[axis], planner->status.enables_new, 0, -1, state_tag) == TP_ERR_OK ? 0 : -1219;
}
double sim_linuxcnc_tp_velocity_scale(const sim_linuxcnc_tp *planner) {
return planner == NULL || !planner->initialized ? 0.0 : planner->velocity_scale;
}
double sim_linuxcnc_tp_velocity_limit(const sim_linuxcnc_tp *planner) {
return planner == NULL || !planner->initialized ? 0.0 : planner->tp.vLimit;
}
double sim_linuxcnc_tp_velocity_mm_min(const sim_linuxcnc_tp *planner) {
return planner == NULL || !planner->initialized ? 0.0 : planner->status.current_vel * 60.0;
}
double sim_linuxcnc_tp_jog_scale(const sim_linuxcnc_tp *planner) {
return planner == NULL || !planner->initialized ? 0.0 : planner->jog_scale;
}
double sim_linuxcnc_tp_jog_velocity(const sim_linuxcnc_tp *planner, int axis) {
return planner == NULL || !planner->initialized || axis < 0 || axis >= 9 ? 0.0 : planner->axis_velocity[axis] * planner->jog_scale;
}
double sim_linuxcnc_tp_feed_override(const sim_linuxcnc_tp *planner) {
return planner == NULL || !planner->initialized ? 0.0 : planner->status.net_feed_scale;
}
int32_t sim_linuxcnc_tp_set_dynamics(sim_linuxcnc_tp *planner, double acceleration_limit) {
if (planner == NULL || !planner->initialized || !isfinite(acceleration_limit) || acceleration_limit <= 0.0) return -1217;
active_planner = planner;
for (int axis = 0; axis < 9; ++axis) planner->axis_acceleration[axis] = acceleration_limit;
return tpSetAmax(&planner->tp, acceleration_limit) == TP_ERR_OK ? 0 : -1218;
}
int32_t sim_linuxcnc_tp_set_paused(sim_linuxcnc_tp *planner, int paused) {
if (planner == NULL || !planner->initialized || (paused != 0 && paused != 1)) return -1219;
active_planner = planner;
planner->status.stepping = 0;
if (paused) return tpPause(&planner->tp) == TP_ERR_OK ? 0 : -1220;
return tpResume(&planner->tp) == TP_ERR_OK ? 0 : -1221;
}
int32_t sim_linuxcnc_tp_step_line(sim_linuxcnc_tp *planner) {
if (planner == NULL || !planner->initialized) return -1219;
TC_STRUCT *active = tcqItem(&planner->tp.queue, 0);
if (active == NULL) return -1219;
active_planner = planner;
planner->step_id = active->id;
planner->status.stepping = 1;
return tpResume(&planner->tp) == TP_ERR_OK ? 0 : -1221;
}
int32_t sim_linuxcnc_tp_is_stepping(const sim_linuxcnc_tp *planner) {
return planner != NULL && planner->initialized && planner->status.stepping;
}
double sim_linuxcnc_tp_axis_limit(const sim_linuxcnc_tp *planner, int axis, int maximum) {
if (planner == NULL || !planner->initialized || axis < 0 || axis > 8) return NAN;
return maximum ? planner->axis_max[axis] : planner->axis_min[axis];
}
int32_t sim_linuxcnc_tp_abort(sim_linuxcnc_tp *planner) {
if (planner == NULL || !planner->initialized) return -1222;
active_planner = planner;
planner->last_active_id = 0;
planner->just_completed = 0;
planner->status.stepping = 0;
if (tpAbort(&planner->tp) != TP_ERR_OK) return -1223;
// A browser PROGRAM_ABORT has no background emcmot servo thread after
// the command response. Complete the source TP abort transition here so
// the authoritative snapshot cannot retain an active queue indefinitely.
return tpClear(&planner->tp) == TP_ERR_OK ? 0 : -1223;
}
int32_t sim_linuxcnc_tp_set_position(sim_linuxcnc_tp *planner, const sim_posemath_pose *position) {
if (planner == NULL || !planner->initialized || position == NULL) return -1208;
active_planner = planner;
EmcPose pose = to_emc_pose(position);
return tpSetPos(&planner->tp, &pose) == TP_ERR_OK ? 0 : -1209;
}
int32_t sim_linuxcnc_tp_set_goal_position(sim_linuxcnc_tp *planner, const sim_posemath_pose *position) {
if (planner == NULL || !planner->initialized || position == NULL) return -1228;
planner->tp.goalPos = to_emc_pose(position);
return 0;
}
int32_t sim_linuxcnc_tp_add_line(sim_linuxcnc_tp *planner, const sim_posemath_pose *end, double velocity, double acceleration, uint32_t tag, int canon_motion_type) {
if (planner == NULL || !planner->initialized || end == NULL || !isfinite(velocity) || velocity < 0.0 || !isfinite(acceleration) || acceleration < 0.0) return -1210;
active_planner = planner;
struct state_tag_t state_tag = {0}; state_tag.fields_float[GM_FIELD_FLOAT_LINE_NUMBER] = (float)tag;
EmcPose pose = to_emc_pose(end), start = planner->tp.goalPos;
xyzac_line_limits(planner, &start, &pose, &velocity, &acceleration);
if (tpSetId(&planner->tp, (int)tag) != TP_ERR_OK) return -1211;
return tpAddLine(&planner->tp, pose, canon_motion_type, velocity, velocity, acceleration, planner->status.enables_new, 0, -1, state_tag) == TP_ERR_OK ? 0 : -1211;
}
int32_t sim_linuxcnc_tp_add_circle(sim_linuxcnc_tp *planner, const sim_posemath_pose *end, const sim_posemath_cartesian *center, const sim_posemath_cartesian *normal, int turn, double velocity, double acceleration, uint32_t tag, int canon_motion_type) {
if (planner == NULL || !planner->initialized || end == NULL || center == NULL || normal == NULL || !isfinite(velocity) || velocity < 0.0 || !isfinite(acceleration) || acceleration < 0.0) return -1212;
active_planner = planner;
struct state_tag_t state_tag = {0}; state_tag.fields_float[GM_FIELD_FLOAT_LINE_NUMBER] = (float)tag;
EmcPose pose = to_emc_pose(end), start = planner->tp.goalPos; PmCartesian pm_center = to_pm_cart(center); PmCartesian pm_normal = to_pm_cart(normal);
double initial_velocity = velocity;
xyzac_circle_limits(planner, &start, &pose, &pm_center, &pm_normal, turn, &velocity, &initial_velocity, &acceleration);
if (tpSetId(&planner->tp, (int)tag) != TP_ERR_OK) return -1212;
// emccanon computes ini_maxvel independently of the programmed feed for
// ARC_FEED. For this XYZAC case the planar XYZ limit (20 mm/s) is the
// governing bound; passing the feed here loses the native corner ramp.
return tpAddCircle(&planner->tp, pose, pm_center, pm_normal, turn, canon_motion_type, velocity, initial_velocity, acceleration, planner->status.enables_new, 0, state_tag) == TP_ERR_OK ? 0 : -1213;
}
int32_t sim_linuxcnc_tp_step(sim_linuxcnc_tp *planner) {
if (planner == NULL || !planner->initialized) return -1214;
active_planner = planner;
planner->just_completed = 0;
const int prior_id = tpGetExecId(&planner->tp);
const int prior_depth = tcqLen(&planner->tp.queue);
const int was_stepping = planner->status.stepping;
const int result = tpRunCycle(&planner->tp, (long)llround(planner->tp.cycleTime * 1e9));
// motion/control.c stops a step when the execution ID changes. Compare
// the next queued ID too, before the following TC can start moving.
TC_STRUCT *next = tcqItem(&planner->tp.queue, 0);
if (planner->status.stepping && (next == NULL || next->id != planner->step_id)) {
tpPause(&planner->tp);
planner->status.stepping = 0;
}
planner->tick += 1u;
if (!was_stepping && prior_id > 0 && prior_depth > 0 && tcqLen(&planner->tp.queue) == 0) {
planner->last_active_id = (uint32_t)prior_id;
planner->just_completed = 1;
}
return result >= TP_ERR_OK && result <= TP_ERR_WAITING ? 0 : result;
}
int32_t sim_linuxcnc_tp_idle(sim_linuxcnc_tp *planner) {
if (planner == NULL || !planner->initialized) return -1227;
planner->just_completed = 0;
planner->tick += 1u;
return 0;
}
int32_t sim_linuxcnc_tp_read_state(const sim_linuxcnc_tp *planner, sim_tp_state *state) {
if (planner == NULL || !planner->initialized || state == NULL) return -1215;
EmcPose position = {0};
if (tpGetPos(&planner->tp, &position) != TP_ERR_OK) return -1216;
memset(state, 0, sizeof(*state)); state->position = from_emc_pose(&position); state->velocity = planner->status.current_vel;
TC_STRUCT *active = tcqItem(&planner->tp.queue, 0);
if (active != NULL && active->target > 0.0) state->progress = active->progress / active->target;
// tpQueueDepth() exposes TP's legacy cached depth, which is only updated
// when appending a TC. Native emcmot reports the live tcq length after a
// segment pop, so use the upstream queue directly for oracle parity.
state->tick = planner->tick;
const uint32_t live_depth = (uint32_t)tcqLen(&planner->tp.queue);
state->queue_depth = live_depth > 0u ? live_depth : planner->just_completed ? 1u : 0u;
{
const int exec_id = tpGetExecId((TP_STRUCT *)&planner->tp);
state->active_id = state->queue_depth > 0 && exec_id > 0 ? (uint32_t)exec_id : planner->just_completed ? planner->last_active_id : 0u;
}
state->motion_type = tpGetMotionType((TP_STRUCT *)&planner->tp);
state->status = tpIsDone((TP_STRUCT *)&planner->tp) ? 0 : 1; return 0;
}
uint32_t sim_linuxcnc_tp_queue_depth(const sim_linuxcnc_tp *planner) {
return planner == NULL || !planner->initialized ? 0u : (uint32_t)tcqLen(&planner->tp.queue);
}
int32_t sim_linuxcnc_tp_queue_segment(const sim_linuxcnc_tp *planner, uint32_t index, sim_linuxcnc_tp_segment *segment) {
TC_STRUCT *tc;
EmcPose start = {0}, end = {0};
if (planner == NULL || !planner->initialized || segment == NULL) return -1224;
tc = tcqItem(&planner->tp.queue, (int)index);
if (tc == NULL || tcGetPosReal(tc, TC_GET_STARTPOINT, &start) != TP_ERR_OK || tcGetPosReal(tc, TC_GET_ENDPOINT, &end) != TP_ERR_OK) return -1225;
memset(segment, 0, sizeof(*segment));
segment->segment.start = from_emc_pose(&start);
segment->segment.end = from_emc_pose(&end);
segment->segment.velocity = tc->reqvel;
segment->segment.acceleration = tc->maxaccel;
segment->segment.motion_type = tc->motion_type;
if (tc->motion_type == TC_CIRCULAR) {
const double turns = fabs(tc->coords.circle.xyz.angle) / (2.0 * M_PI);
segment->segment.turn = (tc->coords.circle.xyz.angle < 0.0 ? -1 : 1) * (turns >= 1.0 ? (int)llround(turns) : 1);
}
if (tc->motion_type == TC_CIRCULAR) {
segment->segment.center.x = tc->coords.circle.xyz.center.x;
segment->segment.center.y = tc->coords.circle.xyz.center.y;
segment->segment.center.z = tc->coords.circle.xyz.center.z;
segment->segment.normal.x = tc->coords.circle.xyz.normal.x;
segment->segment.normal.y = tc->coords.circle.xyz.normal.y;
segment->segment.normal.z = tc->coords.circle.xyz.normal.z;
} else {
segment->segment.normal.z = 1.0;
}
segment->id = tc->id;
segment->term_cond = tc->term_cond;
segment->progress = tc->progress;
segment->target = tc->target;
return 0;
}
int32_t sim_linuxcnc_tp_restore_progress(sim_linuxcnc_tp *planner, const sim_posemath_pose *position, double velocity, uint32_t tick) {
if (planner == NULL || !planner->initialized || position == NULL || !isfinite(velocity) || velocity < 0.0) return -1226;
active_planner = planner;
planner->tp.currentPos = to_emc_pose(position);
planner->status.current_vel = velocity;
planner->tick = tick;
return 0;
}
uint64_t sim_linuxcnc_tp_state_hash(const sim_linuxcnc_tp *planner) {
sim_tp_state state; if (sim_linuxcnc_tp_read_state(planner, &state) != 0) return 0; return hash_bytes((const unsigned char *)&state, sizeof(state));
}
uint32_t sim_linuxcnc_tp_canonical(const sim_linuxcnc_tp *planner, char *output, uint32_t capacity) {
sim_tp_state state; int written;
if (sim_linuxcnc_tp_read_state(planner, &state) != 0) return 0;
written = snprintf(output, capacity, "{\"active_id\":%u,\"motion_type\":%d,\"position\":{\"a\":%.17g,\"c\":%.17g,\"x\":%.17g,\"y\":%.17g,\"z\":%.17g},\"queue_depth\":%u,\"schema\":\"linuxcnc-source-tp.v1\",\"tick\":%u,\"velocity\":%.17g}", state.active_id, state.motion_type, state.position.a, state.position.c, state.position.tran.x, state.position.tran.y, state.position.tran.z, state.queue_depth, state.tick, state.velocity);
return written < 0 ? 0 : (uint32_t)written + 1u;
}
Reference in New Issue View Git Blame Copy Permalink