Implement AXIS parity UI and execution baseline

This commit is contained in:
2026-08-23 07:28:37 +08:00
parent 1325504544
commit 06f96f85e5
463 changed files with 84893 additions and 692 deletions

File diff suppressed because one or more lines are too long

View File

@@ -0,0 +1,120 @@
#ifndef LINUXCNC_WEB_WASM_SIM_DEVICE_IO_H
#define LINUXCNC_WEB_WASM_SIM_DEVICE_IO_H
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
#define SIM_DEVICE_MAX_ENCODERS 8U
#define SIM_DEVICE_MAX_ACKS 64U
enum sim_device_result {
SIM_DEVICE_OK = 0,
SIM_DEVICE_ERR_ARGUMENT = -1000,
SIM_DEVICE_ERR_RANGE = -1001,
SIM_DEVICE_ERR_NONFINITE = -1002,
SIM_DEVICE_ERR_CAPACITY = -1003,
SIM_DEVICE_ERR_STATE = -1004,
SIM_DEVICE_ERR_INTERLOCK = -1005,
SIM_DEVICE_ERR_DUPLICATE = -1006,
SIM_DEVICE_ERR_MISSING = -1007,
SIM_DEVICE_ERR_LATE_ACK = -1008,
SIM_DEVICE_ERR_TIMEOUT = -1009,
};
enum sim_device_ack_status {
SIM_DEVICE_ACK_UNUSED = 0,
SIM_DEVICE_ACK_PENDING = 1,
SIM_DEVICE_ACK_ACKED = 2,
SIM_DEVICE_ACK_TIMED_OUT = 3,
SIM_DEVICE_ACK_ABORTED = 4,
};
typedef struct sim_device_encoder_state {
uint32_t registered;
double speed_rps;
uint32_t ppr;
double scale;
int32_t raw_count;
int32_t count;
double position;
double velocity;
uint32_t phase_a;
uint32_t phase_b;
uint32_t phase_z;
uint32_t index_enable;
uint32_t index_pulse;
uint32_t tick;
} sim_device_encoder_state;
typedef struct sim_device_coolant_state {
uint32_t mist_requested;
uint32_t flood_requested;
uint32_t lube_requested;
uint32_t mist;
uint32_t flood;
uint32_t lube;
uint32_t fault;
} sim_device_coolant_state;
typedef struct sim_device_safety_state {
uint32_t door_open;
uint32_t input_ok;
uint32_t estop;
uint32_t fault;
uint32_t interlock_ok;
uint32_t latched_fault;
uint32_t recovery_epoch;
} sim_device_safety_state;
typedef struct sim_device_ack_entry {
uint32_t used;
uint32_t operation_id;
uint32_t issued_tick;
uint32_t deadline_tick;
uint32_t status;
} sim_device_ack_entry;
typedef struct sim_device_io_state {
uint32_t tick;
uint32_t encoder_count;
sim_device_encoder_state encoders[SIM_DEVICE_MAX_ENCODERS];
sim_device_coolant_state coolant;
sim_device_safety_state safety;
sim_device_ack_entry acks[SIM_DEVICE_MAX_ACKS];
int32_t last_error;
} sim_device_io_state;
typedef struct sim_device_io {
sim_device_io_state state;
double cycle_time;
double phase_accumulator[SIM_DEVICE_MAX_ENCODERS];
} sim_device_io;
int sim_device_init(sim_device_io *device, uint32_t encoder_count, double cycle_time);
int sim_device_reset(sim_device_io *device);
int sim_device_encoder_set_speed(sim_device_io *device, uint32_t channel, double speed_rps);
int sim_device_encoder_set_config(sim_device_io *device, uint32_t channel, uint32_t ppr, double scale);
int sim_device_encoder_set_index_enable(sim_device_io *device, uint32_t channel, uint32_t enable);
int sim_device_coolant_command(sim_device_io *device, uint32_t mist, uint32_t flood, uint32_t lube);
int sim_device_coolant_set_fault(sim_device_io *device, uint32_t fault);
int sim_device_door_set(sim_device_io *device, uint32_t open);
int sim_device_interlock_set(sim_device_io *device, uint32_t input_ok);
int sim_device_interlock_reset(sim_device_io *device);
int sim_device_estop_set(sim_device_io *device, uint32_t estop);
int sim_device_step(sim_device_io *device, uint32_t ticks);
int sim_device_ack_issue(sim_device_io *device, uint32_t operation_id, uint32_t timeout_ticks);
int sim_device_ack_ack(sim_device_io *device, uint32_t operation_id);
int sim_device_ack_abort(sim_device_io *device, uint32_t operation_id);
int sim_device_ack_step(sim_device_io *device, uint32_t ticks);
int sim_device_read_state(const sim_device_io *device, sim_device_io_state *state);
uint64_t sim_device_state_hash(const sim_device_io *device);
uint32_t sim_device_canonical(const sim_device_io *device, char *output, uint32_t capacity);
#ifdef __cplusplus
}
#endif
#endif

View File

@@ -11,6 +11,8 @@ extern "C" {
#define SIM_HAL_PIN_CAPACITY 256U
#define SIM_HAL_THREAD_CAPACITY 16U
#define SIM_HAL_FUNCTION_CAPACITY 64U
#define SIM_HAL_COMPONENT_CAPACITY 64U
#define SIM_HAL_PARAM_CAPACITY 256U
enum sim_hal_result {
SIM_HAL_OK = 0,
@@ -49,6 +51,12 @@ int32_t sim_hal_get_pin(const char *name, sim_hal_value *out);
int32_t sim_hal_connect(const char *source, const char *target);
int32_t sim_hal_register_thread(const char *name, uint32_t period_ticks);
int32_t sim_hal_register_function(const char *thread, const char *name);
/* Register a statically linked .comp component instance on a HAL thread. */
int32_t sim_hal_register_component(const char *instance, const char *type, const char *thread);
int32_t sim_hal_set_param(const char *name, sim_hal_value value);
int32_t sim_hal_get_param(const char *name, sim_hal_value *out);
uint32_t sim_hal_component_count(void);
uint32_t sim_hal_param_count(void);
/* Register the static LinuxCNC corexy_by_hal component and its servo pins. */
int32_t sim_hal_register_corexy_by_hal(const char *component, const char *thread);
/* Execute one deterministic CoreXY command/feedback transform immediately. */

View File

@@ -16,6 +16,8 @@ enum sim_interpreter_result {
SIM_INTERP_ERR_UNKNOWN_CODE = -504,
SIM_INTERP_ERR_STATE = -505,
SIM_INTERP_ERR_BUFFER = -506,
SIM_INTERP_ERR_PARAMETER = -507,
SIM_INTERP_ERR_EXPRESSION = -508,
};
enum sim_interpreter_units {
@@ -51,6 +53,8 @@ typedef struct sim_interpreter_state {
} sim_interpreter_state;
#define SIM_INTERPRETER_CANONICAL_CAPACITY 512u
#define SIM_INTERPRETER_PARAMETER_MIN 1u
#define SIM_INTERPRETER_PARAMETER_MAX 5399u
typedef struct sim_interpreter {
sim_interpreter_state state;
@@ -62,10 +66,16 @@ typedef struct sim_interpreter {
int sim_interpreter_init(sim_interpreter *interpreter);
int sim_interpreter_reset(sim_interpreter *interpreter);
int sim_interpreter_execute(sim_interpreter *interpreter, const char *block, uint32_t length);
uint32_t sim_interpreter_size(void);
int sim_interpreter_read_state(const sim_interpreter *interpreter, sim_interpreter_state *state);
uint32_t sim_interpreter_canonical(const sim_interpreter *interpreter, char *output, uint32_t capacity);
uint64_t sim_interpreter_state_hash(const sim_interpreter *interpreter);
uint64_t sim_interpreter_block_hash(const sim_interpreter *interpreter);
uint64_t sim_interpreter_modal_hash(const sim_interpreter *interpreter);
uint32_t sim_interpreter_error_column(const sim_interpreter *interpreter);
int32_t sim_interpreter_error_code(const sim_interpreter *interpreter);
int sim_interpreter_parameter_set(sim_interpreter *interpreter, uint32_t index, double value);
int sim_interpreter_parameter_get(const sim_interpreter *interpreter, uint32_t index, double *value);
#ifdef __cplusplus
}

View File

@@ -19,6 +19,11 @@ enum sim_motion_result {
SIM_MOTION_ERR_BUFFER = -607,
};
enum sim_motion_profile {
SIM_MOTION_PROFILE_BLEND = 0,
SIM_MOTION_PROFILE_EXACT_STOP = 1,
};
#define SIM_MOTION_QUEUE_CAPACITY 64u
typedef struct sim_motion_command {
@@ -54,9 +59,15 @@ int sim_motion_init(sim_motion_queue *queue, double cycle_time, double velocity_
int sim_motion_reset(sim_motion_queue *queue);
int sim_motion_enqueue(sim_motion_queue *queue, const sim_motion_command *command, uint32_t *id_out);
int sim_motion_step(sim_motion_queue *queue);
int sim_motion_set_profile(sim_motion_queue *queue, int profile);
int sim_motion_set_lookahead(sim_motion_queue *queue, uint32_t depth);
int sim_motion_set_dynamics(sim_motion_queue *queue, double acceleration_limit, double jerk_limit);
int sim_motion_set_paused(sim_motion_queue *queue, uint32_t paused);
int sim_motion_peek(const sim_motion_queue *queue, uint32_t offset, sim_motion_command *command);
int sim_motion_read_state(const sim_motion_queue *queue, sim_motion_state *state);
uint32_t sim_motion_queue_depth(const sim_motion_queue *queue);
uint64_t sim_motion_state_hash(const sim_motion_queue *queue);
uint64_t sim_motion_plan_hash(const sim_motion_queue *queue);
uint32_t sim_motion_canonical(const sim_motion_queue *queue, char *output, uint32_t capacity);
#ifdef __cplusplus

View File

@@ -0,0 +1,58 @@
#ifndef LINUXCNC_WEB_WASM_SIM_SCHEDULER_H
#define LINUXCNC_WEB_WASM_SIM_SCHEDULER_H
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
#define SIM_SCHEDULER_SCHEMA_VERSION 1U
#define SIM_SCHEDULER_PHASE_COUNT 8U
#define SIM_SCHEDULER_DEFAULT_BUDGET 1000000U
#define SIM_SCHEDULER_CANONICAL_CAPACITY 512U
enum sim_scheduler_result {
SIM_SCHEDULER_OK = 0,
SIM_SCHEDULER_ERR_ARGUMENT = -1100,
SIM_SCHEDULER_ERR_RANGE = -1101,
SIM_SCHEDULER_ERR_BUDGET = -1102,
SIM_SCHEDULER_ERR_OVERFLOW = -1103,
SIM_SCHEDULER_ERR_BUFFER = -1104,
};
enum sim_scheduler_phase {
SIM_SCHEDULER_RECONCILE = 0,
SIM_SCHEDULER_TASK = 1,
SIM_SCHEDULER_TP = 2,
SIM_SCHEDULER_KINEMATICS = 3,
SIM_SCHEDULER_MOTION = 4,
SIM_SCHEDULER_HAL_IO = 5,
SIM_SCHEDULER_CALLBACKS = 6,
SIM_SCHEDULER_PUBLISH = 7,
};
typedef struct sim_scheduler {
uint32_t tick;
uint32_t budget;
uint32_t last_cycles;
uint32_t phase_trace[SIM_SCHEDULER_PHASE_COUNT];
} sim_scheduler;
uint32_t sim_scheduler_size(void);
int32_t sim_scheduler_init(sim_scheduler *scheduler, uint32_t budget);
int32_t sim_scheduler_reset(sim_scheduler *scheduler);
int32_t sim_scheduler_step(sim_scheduler *scheduler, uint32_t cycles);
int32_t sim_scheduler_restore(sim_scheduler *scheduler, uint32_t tick, uint32_t budget,
uint32_t last_cycles, const uint32_t *phase_trace,
uint32_t phase_count);
int32_t sim_scheduler_read(const sim_scheduler *scheduler, sim_scheduler *out);
uint32_t sim_scheduler_tick(const sim_scheduler *scheduler);
uint64_t sim_scheduler_state_hash(const sim_scheduler *scheduler);
uint32_t sim_scheduler_canonical(const sim_scheduler *scheduler, char *output, uint32_t capacity);
#ifdef __cplusplus
}
#endif
#endif

View File

@@ -0,0 +1,96 @@
#ifndef LINUXCNC_WEB_WASM_SIM_TASK_H
#define LINUXCNC_WEB_WASM_SIM_TASK_H
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
#define SIM_TASK_SCHEMA_VERSION 1U
#define SIM_TASK_CONTEXT_CAPACITY 128U
#define SIM_TASK_SESSION_CAPACITY 128U
#define SIM_TASK_CANONICAL_CAPACITY 512U
#define SIM_TASK_NO_LINE UINT32_MAX
enum sim_task_result {
SIM_TASK_OK = 0,
SIM_TASK_ERR_ARGUMENT = -1000,
SIM_TASK_ERR_STATE = -1001,
SIM_TASK_ERR_TRANSITION = -1002,
SIM_TASK_ERR_SEQUENCE = -1003,
SIM_TASK_ERR_TICK = -1004,
SIM_TASK_ERR_RANGE = -1005,
SIM_TASK_ERR_NONFINITE = -1006,
SIM_TASK_ERR_BUFFER = -1007,
SIM_TASK_ERR_DESTROYED = -1008,
};
enum sim_task_state_value {
SIM_TASK_NEW = 0,
SIM_TASK_READY = 1,
SIM_TASK_RUNNING = 2,
SIM_TASK_PAUSED = 3,
SIM_TASK_ESTOP = 4,
SIM_TASK_FAULT = 5,
SIM_TASK_STOPPED = 6,
SIM_TASK_DESTROYED = 7,
};
enum sim_task_mode_value {
SIM_TASK_MODE_MANUAL = 0,
SIM_TASK_MODE_MDI = 1,
SIM_TASK_MODE_AUTO = 2,
};
enum sim_task_program_value {
SIM_TASK_PROGRAM_IDLE = 0,
SIM_TASK_PROGRAM_LOADED = 1,
SIM_TASK_PROGRAM_RUNNING = 2,
SIM_TASK_PROGRAM_PAUSED = 3,
SIM_TASK_PROGRAM_COMPLETED = 4,
SIM_TASK_PROGRAM_ABORTED = 5,
SIM_TASK_PROGRAM_FAULT = 6,
};
typedef struct sim_task_state {
char context_id[SIM_TASK_CONTEXT_CAPACITY];
char session_id[SIM_TASK_SESSION_CAPACITY];
uint32_t state;
uint32_t mode;
uint32_t program_status;
uint32_t seq;
uint32_t simulation_tick;
uint32_t current_line;
int32_t last_error;
} sim_task_state;
typedef struct sim_task {
sim_task_state state;
} sim_task;
uint32_t sim_task_size(void);
int32_t sim_task_init(sim_task *task, const char *context_id, const char *session_id);
int32_t sim_task_reset(sim_task *task);
int32_t sim_task_attach(sim_task *task);
int32_t sim_task_set_command(sim_task *task, uint32_t seq, uint32_t simulation_tick);
int32_t sim_task_set_tick(sim_task *task, uint32_t simulation_tick);
int32_t sim_task_set_mode(sim_task *task, uint32_t mode);
int32_t sim_task_set_program(sim_task *task, uint32_t program_status, uint32_t current_line);
int32_t sim_task_transition(sim_task *task, uint32_t state);
int32_t sim_task_fault(sim_task *task, int32_t error_code);
int32_t sim_task_estop(sim_task *task, int32_t error_code);
int32_t sim_task_recover(sim_task *task);
int32_t sim_task_restore(sim_task *task, uint32_t state, uint32_t mode,
uint32_t program_status, uint32_t seq,
uint32_t simulation_tick, uint32_t current_line,
int32_t error_code);
int32_t sim_task_read_state(const sim_task *task, sim_task_state *out);
uint32_t sim_task_canonical(const sim_task *task, char *output, uint32_t capacity);
uint64_t sim_task_state_hash(const sim_task *task);
#ifdef __cplusplus
}
#endif
#endif

View File

@@ -0,0 +1,275 @@
#include "sim_device_io.h"
#include <math.h>
#include <stdio.h>
#include <string.h>
static int valid_device(const sim_device_io *device) {
return device != 0 && device->state.encoder_count > 0U && device->state.encoder_count <= SIM_DEVICE_MAX_ENCODERS && isfinite(device->cycle_time) && device->cycle_time > 0.0;
}
static int finite_value(double value) { return isfinite(value); }
static void set_error(sim_device_io *device, int error) {
if (device != 0) device->state.last_error = error;
}
static void update_safety(sim_device_io *device) {
sim_device_safety_state *safety = &device->state.safety;
safety->fault = (safety->door_open || !safety->input_ok) ? 1U : 0U;
if (safety->fault || safety->estop) safety->latched_fault = 1U;
safety->interlock_ok = (!safety->fault && !safety->estop && !safety->latched_fault) ? 1U : 0U;
}
static void update_coolant(sim_device_io *device) {
sim_device_coolant_state *coolant = &device->state.coolant;
const uint32_t allowed = device->state.safety.interlock_ok && !coolant->fault;
coolant->mist = allowed ? coolant->mist_requested : 0U;
coolant->flood = allowed ? coolant->flood_requested : 0U;
coolant->lube = allowed ? coolant->lube_requested : 0U;
}
static void update_encoder(sim_device_io *device, uint32_t channel) {
sim_device_encoder_state *encoder = &device->state.encoders[channel];
const double counts_per_revolution = (double)encoder->ppr * 4.0;
const double delta = encoder->speed_rps * counts_per_revolution * device->cycle_time;
int64_t raw;
uint32_t phase;
if (!encoder->registered) return;
device->phase_accumulator[channel] += delta;
raw = device->phase_accumulator[channel] >= 0.0 ? (int64_t)floor(device->phase_accumulator[channel]) : (int64_t)ceil(device->phase_accumulator[channel]);
if (raw > INT32_MAX || raw < INT32_MIN) {
encoder->raw_count = raw > 0 ? INT32_MAX : INT32_MIN;
encoder->count = encoder->raw_count;
set_error(device, SIM_DEVICE_ERR_RANGE);
return;
}
encoder->raw_count = (int32_t)raw;
encoder->count = encoder->raw_count;
encoder->position = (double)encoder->raw_count / encoder->scale;
encoder->velocity = encoder->speed_rps * counts_per_revolution / encoder->scale;
phase = (uint32_t)(((int64_t)encoder->raw_count % 4LL + 4LL) % 4LL);
encoder->phase_a = phase == 0U || phase == 1U;
encoder->phase_b = phase == 1U || phase == 2U;
encoder->index_pulse = 0U;
if (encoder->index_enable && encoder->ppr > 0U && encoder->raw_count % (int32_t)counts_per_revolution == 0) {
encoder->phase_z = 1U;
encoder->index_pulse = 1U;
encoder->index_enable = 0U;
} else {
encoder->phase_z = 0U;
}
encoder->tick = device->state.tick;
}
static int ack_index(const sim_device_io *device, uint32_t operation_id) {
uint32_t index;
for (index = 0; index < SIM_DEVICE_MAX_ACKS; ++index) if (device->state.acks[index].used && device->state.acks[index].operation_id == operation_id) return (int)index;
return -1;
}
int sim_device_init(sim_device_io *device, uint32_t encoder_count, double cycle_time) {
uint32_t index;
if (device == 0 || encoder_count == 0U || encoder_count > SIM_DEVICE_MAX_ENCODERS || !finite_value(cycle_time) || cycle_time <= 0.0 || cycle_time > 10.0) return SIM_DEVICE_ERR_ARGUMENT;
memset(device, 0, sizeof(*device));
device->cycle_time = cycle_time;
device->state.encoder_count = encoder_count;
device->state.last_error = SIM_DEVICE_OK;
device->state.safety.input_ok = 1U;
for (index = 0; index < encoder_count; ++index) {
device->state.encoders[index].registered = 1U;
device->state.encoders[index].ppr = 1000U;
device->state.encoders[index].scale = 1.0;
}
update_safety(device);
update_coolant(device);
return SIM_DEVICE_OK;
}
int sim_device_reset(sim_device_io *device) {
uint32_t count;
double cycle;
if (!valid_device(device)) return SIM_DEVICE_ERR_ARGUMENT;
count = device->state.encoder_count; cycle = device->cycle_time;
return sim_device_init(device, count, cycle);
}
int sim_device_encoder_set_speed(sim_device_io *device, uint32_t channel, double speed_rps) {
if (!valid_device(device) || channel >= device->state.encoder_count) return SIM_DEVICE_ERR_ARGUMENT;
if (!finite_value(speed_rps) || fabs(speed_rps) > 1000000.0) return SIM_DEVICE_ERR_NONFINITE;
device->state.encoders[channel].speed_rps = speed_rps;
set_error(device, SIM_DEVICE_OK);
return SIM_DEVICE_OK;
}
int sim_device_encoder_set_config(sim_device_io *device, uint32_t channel, uint32_t ppr, double scale) {
if (!valid_device(device) || channel >= device->state.encoder_count || ppr == 0U || ppr > 100000000U || !finite_value(scale) || scale <= 0.0) return SIM_DEVICE_ERR_RANGE;
device->state.encoders[channel].ppr = ppr;
device->state.encoders[channel].scale = scale;
set_error(device, SIM_DEVICE_OK);
return SIM_DEVICE_OK;
}
int sim_device_encoder_set_index_enable(sim_device_io *device, uint32_t channel, uint32_t enable) {
if (!valid_device(device) || channel >= device->state.encoder_count || enable > 1U) return SIM_DEVICE_ERR_ARGUMENT;
device->state.encoders[channel].index_enable = enable;
set_error(device, SIM_DEVICE_OK);
return SIM_DEVICE_OK;
}
int sim_device_coolant_command(sim_device_io *device, uint32_t mist, uint32_t flood, uint32_t lube) {
if (!valid_device(device) || mist > 1U || flood > 1U || lube > 1U) return SIM_DEVICE_ERR_ARGUMENT;
if ((mist || flood || lube) && !device->state.safety.interlock_ok) return SIM_DEVICE_ERR_INTERLOCK;
device->state.coolant.mist_requested = mist;
device->state.coolant.flood_requested = flood;
device->state.coolant.lube_requested = lube;
update_coolant(device);
set_error(device, SIM_DEVICE_OK);
return SIM_DEVICE_OK;
}
int sim_device_coolant_set_fault(sim_device_io *device, uint32_t fault) {
if (!valid_device(device) || fault > 1U) return SIM_DEVICE_ERR_ARGUMENT;
device->state.coolant.fault = fault;
update_coolant(device);
set_error(device, fault ? SIM_DEVICE_ERR_STATE : SIM_DEVICE_OK);
return fault ? SIM_DEVICE_ERR_STATE : SIM_DEVICE_OK;
}
int sim_device_door_set(sim_device_io *device, uint32_t open) {
if (!valid_device(device) || open > 1U) return SIM_DEVICE_ERR_ARGUMENT;
device->state.safety.door_open = open;
update_safety(device); update_coolant(device);
set_error(device, open ? SIM_DEVICE_ERR_INTERLOCK : SIM_DEVICE_OK);
return open ? SIM_DEVICE_ERR_INTERLOCK : SIM_DEVICE_OK;
}
int sim_device_interlock_set(sim_device_io *device, uint32_t input_ok) {
if (!valid_device(device) || input_ok > 1U) return SIM_DEVICE_ERR_ARGUMENT;
device->state.safety.input_ok = input_ok;
update_safety(device); update_coolant(device);
set_error(device, input_ok ? SIM_DEVICE_OK : SIM_DEVICE_ERR_INTERLOCK);
return input_ok ? SIM_DEVICE_OK : SIM_DEVICE_ERR_INTERLOCK;
}
int sim_device_interlock_reset(sim_device_io *device) {
if (!valid_device(device)) return SIM_DEVICE_ERR_ARGUMENT;
update_safety(device);
if (device->state.safety.door_open || !device->state.safety.input_ok || device->state.safety.estop) return SIM_DEVICE_ERR_INTERLOCK;
device->state.safety.latched_fault = 0U;
device->state.safety.recovery_epoch += 1U;
update_safety(device); update_coolant(device);
set_error(device, SIM_DEVICE_OK);
return SIM_DEVICE_OK;
}
int sim_device_estop_set(sim_device_io *device, uint32_t estop) {
if (!valid_device(device) || estop > 1U) return SIM_DEVICE_ERR_ARGUMENT;
device->state.safety.estop = estop;
update_safety(device); update_coolant(device);
set_error(device, estop ? SIM_DEVICE_ERR_INTERLOCK : SIM_DEVICE_OK);
return estop ? SIM_DEVICE_ERR_INTERLOCK : SIM_DEVICE_OK;
}
int sim_device_ack_issue(sim_device_io *device, uint32_t operation_id, uint32_t timeout_ticks) {
uint32_t index;
if (!valid_device(device) || operation_id == 0U || timeout_ticks == 0U || timeout_ticks > 1000000U) return SIM_DEVICE_ERR_ARGUMENT;
if (ack_index(device, operation_id) >= 0) return SIM_DEVICE_ERR_DUPLICATE;
for (index = 0; index < SIM_DEVICE_MAX_ACKS; ++index) if (!device->state.acks[index].used) break;
if (index == SIM_DEVICE_MAX_ACKS) return SIM_DEVICE_ERR_CAPACITY;
device->state.acks[index].used = 1U;
device->state.acks[index].operation_id = operation_id;
device->state.acks[index].issued_tick = device->state.tick;
device->state.acks[index].deadline_tick = device->state.tick + timeout_ticks;
device->state.acks[index].status = SIM_DEVICE_ACK_PENDING;
set_error(device, SIM_DEVICE_OK);
return SIM_DEVICE_OK;
}
int sim_device_ack_ack(sim_device_io *device, uint32_t operation_id) {
int index;
if (!valid_device(device)) return SIM_DEVICE_ERR_ARGUMENT;
index = ack_index(device, operation_id);
if (index < 0) return SIM_DEVICE_ERR_MISSING;
if (device->state.acks[index].status == SIM_DEVICE_ACK_TIMED_OUT) return SIM_DEVICE_ERR_LATE_ACK;
if (device->state.acks[index].status != SIM_DEVICE_ACK_PENDING) return SIM_DEVICE_ERR_STATE;
device->state.acks[index].status = SIM_DEVICE_ACK_ACKED;
set_error(device, SIM_DEVICE_OK);
return SIM_DEVICE_OK;
}
int sim_device_ack_abort(sim_device_io *device, uint32_t operation_id) {
int index;
if (!valid_device(device)) return SIM_DEVICE_ERR_ARGUMENT;
index = ack_index(device, operation_id);
if (index < 0) return SIM_DEVICE_ERR_MISSING;
if (device->state.acks[index].status == SIM_DEVICE_ACK_ACKED) return SIM_DEVICE_ERR_STATE;
device->state.acks[index].status = SIM_DEVICE_ACK_ABORTED;
set_error(device, SIM_DEVICE_OK);
return SIM_DEVICE_OK;
}
int sim_device_ack_step(sim_device_io *device, uint32_t ticks) {
uint32_t index, tick;
if (!valid_device(device) || ticks > 1000000U || UINT32_MAX - device->state.tick < ticks) return SIM_DEVICE_ERR_RANGE;
for (tick = 0; tick < ticks; ++tick) {
device->state.tick += 1U;
for (index = 0; index < SIM_DEVICE_MAX_ACKS; ++index) if (device->state.acks[index].used && device->state.acks[index].status == SIM_DEVICE_ACK_PENDING && device->state.tick >= device->state.acks[index].deadline_tick) device->state.acks[index].status = SIM_DEVICE_ACK_TIMED_OUT;
}
return SIM_DEVICE_OK;
}
int sim_device_step(sim_device_io *device, uint32_t ticks) {
sim_device_io before;
uint32_t tick, index;
if (!valid_device(device) || ticks > 1000000U || UINT32_MAX - device->state.tick < ticks) return SIM_DEVICE_ERR_RANGE;
memcpy(&before, device, sizeof(before));
for (tick = 0; tick < ticks; ++tick) {
device->state.tick += 1U;
update_safety(device); update_coolant(device);
for (index = 0; index < device->state.encoder_count; ++index) update_encoder(device, index);
for (index = 0; index < SIM_DEVICE_MAX_ACKS; ++index) if (device->state.acks[index].used && device->state.acks[index].status == SIM_DEVICE_ACK_PENDING && device->state.tick >= device->state.acks[index].deadline_tick) device->state.acks[index].status = SIM_DEVICE_ACK_TIMED_OUT;
}
if (device->state.last_error == SIM_DEVICE_ERR_RANGE) { memcpy(device, &before, sizeof(before)); return SIM_DEVICE_ERR_RANGE; }
set_error(device, SIM_DEVICE_OK);
return SIM_DEVICE_OK;
}
int sim_device_read_state(const sim_device_io *device, sim_device_io_state *state) {
if (!valid_device(device) || state == 0) return SIM_DEVICE_ERR_ARGUMENT;
*state = device->state;
return SIM_DEVICE_OK;
}
static uint64_t fnv1a(const unsigned char *bytes, size_t length) {
uint64_t hash = UINT64_C(1469598103934665603);
size_t index;
for (index = 0; index < length; ++index) { hash ^= bytes[index]; hash *= UINT64_C(1099511628211); }
return hash;
}
uint64_t sim_device_state_hash(const sim_device_io *device) {
unsigned char bytes[sizeof(sim_device_io_state) + sizeof(double) * SIM_DEVICE_MAX_ENCODERS];
if (!valid_device(device)) return 0;
memcpy(bytes, &device->state, sizeof(device->state));
memcpy(bytes + sizeof(device->state), device->phase_accumulator, sizeof(device->phase_accumulator));
return fnv1a(bytes, sizeof(bytes));
}
uint32_t sim_device_canonical(const sim_device_io *device, char *output, uint32_t capacity) {
char discard[1];
char *destination = output == 0 ? discard : output;
uint32_t destination_capacity = output == 0 ? 1U : capacity;
int written;
uint32_t index;
if (!valid_device(device)) return 0;
written = snprintf(destination, destination_capacity, "{\"coolant\":{\"flood\":%u,\"lube\":%u,\"mist\":%u},\"encoders\":[", device->state.coolant.flood, device->state.coolant.lube, device->state.coolant.mist);
if (written < 0) return 0;
for (index = 0; index < device->state.encoder_count; ++index) {
const sim_device_encoder_state *encoder = &device->state.encoders[index];
int part = snprintf(destination + (written < (int)destination_capacity ? written : 0), written < (int)destination_capacity ? destination_capacity - (uint32_t)written : 0U, "%s{\"count\":%d,\"index_pulse\":%u,\"position\":%.9g,\"velocity\":%.9g}", index ? "," : "", encoder->count, encoder->index_pulse, encoder->position, encoder->velocity);
if (part < 0) return 0; written += part;
}
{ int part = snprintf(destination + (written < (int)destination_capacity ? written : 0), written < (int)destination_capacity ? destination_capacity - (uint32_t)written : 0U, "],\"safety\":{\"door_open\":%u,\"estop\":%u,\"interlock_ok\":%u,\"latched_fault\":%u},\"tick\":%u}", device->state.safety.door_open, device->state.safety.estop, device->state.safety.interlock_ok, device->state.safety.latched_fault, device->state.tick); if (part < 0) return 0; written += part; }
return (uint32_t)written + 1U;
}

View File

@@ -1,5 +1,6 @@
#include "sim_hal.h"
#include <math.h>
#include <stddef.h>
#include <string.h>
@@ -9,6 +10,12 @@ typedef struct {
sim_hal_value value;
} pin_entry;
typedef struct {
uint32_t used;
char name[SIM_HAL_NAME_CAPACITY];
sim_hal_value value;
} param_entry;
typedef struct {
uint32_t used;
char name[SIM_HAL_NAME_CAPACITY];
@@ -21,16 +28,61 @@ typedef struct {
uint32_t thread;
uint32_t calls;
uint32_t fail;
uint32_t transform_kind;
uint32_t component_index;
uint32_t component_kind;
} function_entry;
enum { SIM_HAL_TRANSFORM_NONE = 0, SIM_HAL_TRANSFORM_COREXY = 1 };
typedef struct {
uint32_t used;
char instance[SIM_HAL_NAME_CAPACITY];
char type[SIM_HAL_NAME_CAPACITY];
uint32_t kind;
uint32_t function_index;
double memory[4];
uint32_t flags[4];
} component_entry;
typedef struct {
uint32_t used;
uint32_t source;
uint32_t target;
} link_entry;
enum {
SIM_HAL_COMPONENT_NONE = 0,
SIM_HAL_COMPONENT_COREXY = 1,
SIM_HAL_COMPONENT_CONSTANT,
SIM_HAL_COMPONENT_AND2,
SIM_HAL_COMPONENT_OR2,
SIM_HAL_COMPONENT_XOR2,
SIM_HAL_COMPONENT_NOT,
SIM_HAL_COMPONENT_ABS,
SIM_HAL_COMPONENT_ABS_S32,
SIM_HAL_COMPONENT_ABS_S64,
SIM_HAL_COMPONENT_LOWPASS,
SIM_HAL_COMPONENT_DDT,
SIM_HAL_COMPONENT_INTEG,
SIM_HAL_COMPONENT_MUX2,
SIM_HAL_COMPONENT_SUM2,
SIM_HAL_COMPONENT_MULT2,
SIM_HAL_COMPONENT_DIV2,
SIM_HAL_COMPONENT_HYPOT,
SIM_HAL_COMPONENT_NEAR,
SIM_HAL_COMPONENT_COMP,
SIM_HAL_COMPONENT_INVERT,
SIM_HAL_COMPONENT_EDGE,
SIM_HAL_COMPONENT_TOGGLE,
SIM_HAL_COMPONENT_ESTOP_LATCH
};
typedef struct {
uint32_t tick;
pin_entry pins[SIM_HAL_PIN_CAPACITY];
param_entry params[SIM_HAL_PARAM_CAPACITY];
thread_entry threads[SIM_HAL_THREAD_CAPACITY];
function_entry functions[SIM_HAL_FUNCTION_CAPACITY];
component_entry components[SIM_HAL_COMPONENT_CAPACITY];
link_entry links[256U];
} hal_state;
static hal_state state;
@@ -47,6 +99,14 @@ static int find_pin(const char *name) {
return -1;
}
static int find_param(const char *name) {
uint32_t index;
for (index = 0; index < SIM_HAL_PARAM_CAPACITY; ++index) {
if (state.params[index].used && strcmp(state.params[index].name, name) == 0) return (int)index;
}
return -1;
}
static int find_thread(const char *name) {
uint32_t index;
for (index = 0; index < SIM_HAL_THREAD_CAPACITY; ++index) {
@@ -63,6 +123,28 @@ static int find_function(const char *name) {
return -1;
}
static int find_component(const char *instance) {
uint32_t index;
for (index = 0; index < SIM_HAL_COMPONENT_CAPACITY; ++index) {
if (state.components[index].used && strcmp(state.components[index].instance, instance) == 0) return (int)index;
}
return -1;
}
static int finite_value(sim_hal_value value) {
return value.type != SIM_HAL_FLOAT || isfinite((double)value.value.real);
}
static int type_from_name(const char *name, uint32_t *type) {
if (name == NULL || type == NULL) return 0;
if (strcmp(name, "bit") == 0) *type = SIM_HAL_BIT;
else if (strcmp(name, "s32") == 0 || strcmp(name, "signed") == 0) *type = SIM_HAL_S32;
else if (strcmp(name, "u32") == 0 || strcmp(name, "unsigned") == 0) *type = SIM_HAL_U32;
else if (strcmp(name, "float") == 0) *type = SIM_HAL_FLOAT;
else return 0;
return 1;
}
void sim_hal_reset(void) {
memset(&state, 0, sizeof(state));
}
@@ -87,6 +169,7 @@ int32_t sim_hal_set_pin(const char *name, sim_hal_value value) {
if (index < 0) return SIM_HAL_ERR_NOT_FOUND;
if (value.type != state.pins[index].value.type) return SIM_HAL_ERR_TYPE;
if (value.type == SIM_HAL_BIT && value.value.bit > 1U) return SIM_HAL_ERR_RANGE;
if (!finite_value(value)) return SIM_HAL_ERR_RANGE;
state.pins[index].value = value;
return SIM_HAL_OK;
}
@@ -98,11 +181,41 @@ int32_t sim_hal_get_pin(const char *name, sim_hal_value *out) {
return SIM_HAL_OK;
}
int32_t sim_hal_set_param(const char *name, sim_hal_value value) {
int index = find_param(name);
if (index < 0) return SIM_HAL_ERR_NOT_FOUND;
if (value.type != state.params[index].value.type) return SIM_HAL_ERR_TYPE;
if (value.type == SIM_HAL_BIT && value.value.bit > 1U) return SIM_HAL_ERR_RANGE;
if (!finite_value(value)) return SIM_HAL_ERR_RANGE;
state.params[index].value = value;
return SIM_HAL_OK;
}
int32_t sim_hal_get_param(const char *name, sim_hal_value *out) {
int index = find_param(name);
if (index < 0 || out == NULL) return index < 0 ? SIM_HAL_ERR_NOT_FOUND : SIM_HAL_ERR_ARGUMENT;
*out = state.params[index].value;
return SIM_HAL_OK;
}
int32_t sim_hal_connect(const char *source, const char *target) {
int source_index = find_pin(source);
int target_index = find_pin(target);
uint32_t link_index;
if (source_index < 0 || target_index < 0) return SIM_HAL_ERR_NOT_FOUND;
if (state.pins[source_index].value.type != state.pins[target_index].value.type) return SIM_HAL_ERR_CONNECTION;
for (link_index = 0; link_index < 256U; ++link_index) {
if (state.links[link_index].used && state.links[link_index].source == (uint32_t)source_index && state.links[link_index].target == (uint32_t)target_index) return SIM_HAL_ERR_DUPLICATE;
}
for (link_index = 0; link_index < 256U; ++link_index) {
if (!state.links[link_index].used) {
state.links[link_index].used = 1U;
state.links[link_index].source = (uint32_t)source_index;
state.links[link_index].target = (uint32_t)target_index;
break;
}
}
if (link_index == 256U) return SIM_HAL_ERR_FULL;
state.pins[target_index].value = state.pins[source_index].value;
return SIM_HAL_OK;
}
@@ -139,6 +252,213 @@ int32_t sim_hal_register_function(const char *thread, const char *name) {
return SIM_HAL_ERR_FULL;
}
typedef struct {
const char *name;
uint32_t type;
} component_pin_spec;
typedef struct {
const char *name;
uint32_t type;
sim_hal_value initial;
} component_param_spec;
typedef struct {
uint32_t kind;
const component_pin_spec *pins;
uint32_t pin_count;
const component_param_spec *params;
uint32_t param_count;
} component_spec;
static const component_pin_spec pins_corexy[] = {
{"j0-motor-pos-cmd", SIM_HAL_FLOAT}, {"j1-motor-pos-cmd", SIM_HAL_FLOAT},
{"alpha-cmd", SIM_HAL_FLOAT}, {"beta-cmd", SIM_HAL_FLOAT},
{"alpha-fb", SIM_HAL_FLOAT}, {"beta-fb", SIM_HAL_FLOAT},
{"j0-motor-pos-fb", SIM_HAL_FLOAT}, {"j1-motor-pos-fb", SIM_HAL_FLOAT}
};
static const component_pin_spec pins_bit2[] = {
{"in0", SIM_HAL_BIT}, {"in1", SIM_HAL_BIT}, {"out", SIM_HAL_BIT}
};
static const component_pin_spec pins_bit1[] = {{"in", SIM_HAL_BIT}, {"out", SIM_HAL_BIT}};
static const component_pin_spec pins_abs[] = {
{"in", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT}, {"sign", SIM_HAL_BIT},
{"is-positive", SIM_HAL_BIT}, {"is-negative", SIM_HAL_BIT}
};
static const component_pin_spec pins_abs_s32[] = {
{"in", SIM_HAL_S32}, {"out", SIM_HAL_S32}, {"sign", SIM_HAL_BIT},
{"is-positive", SIM_HAL_BIT}, {"is-negative", SIM_HAL_BIT}
};
static const component_pin_spec pins_float2[] = {
{"in0", SIM_HAL_FLOAT}, {"in1", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT}
};
static const component_pin_spec pins_lowpass[] = {
{"in", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT}, {"load", SIM_HAL_BIT}
};
static const component_pin_spec pins_ddt[] = {{"in", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT}};
static const component_pin_spec pins_integ[] = {
{"in", SIM_HAL_FLOAT}, {"gain", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT},
{"reset", SIM_HAL_BIT}, {"max", SIM_HAL_FLOAT}, {"min", SIM_HAL_FLOAT}
};
static const component_pin_spec pins_mux2[] = {
{"sel", SIM_HAL_BIT}, {"out", SIM_HAL_FLOAT}, {"in1", SIM_HAL_FLOAT}, {"in0", SIM_HAL_FLOAT}
};
static const component_pin_spec pins_sum2[] = {
{"in0", SIM_HAL_FLOAT}, {"in1", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT}
};
static const component_pin_spec pins_hypot[] = {
{"in0", SIM_HAL_FLOAT}, {"in1", SIM_HAL_FLOAT}, {"in2", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT}
};
static const component_pin_spec pins_near[] = {
{"in1", SIM_HAL_FLOAT}, {"in2", SIM_HAL_FLOAT}, {"out", SIM_HAL_BIT}
};
static const component_pin_spec pins_comp[] = {
{"in0", SIM_HAL_FLOAT}, {"in1", SIM_HAL_FLOAT}, {"out", SIM_HAL_BIT}, {"equal", SIM_HAL_BIT}
};
static const component_pin_spec pins_edge[] = {
{"in", SIM_HAL_BIT}, {"out", SIM_HAL_BIT}, {"out-invert", SIM_HAL_BIT}
};
static const component_pin_spec pins_estop[] = {
{"ok-in", SIM_HAL_BIT}, {"fault-in", SIM_HAL_BIT}, {"reset", SIM_HAL_BIT},
{"ok-out", SIM_HAL_BIT}, {"fault-out", SIM_HAL_BIT}, {"watchdog", SIM_HAL_BIT}
};
static const component_pin_spec pins_constant[] = {{"out", SIM_HAL_FLOAT}};
static const component_pin_spec pins_invert[] = {{"in", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT}};
static const component_pin_spec pins_toggle[] = {{"in", SIM_HAL_BIT}, {"out", SIM_HAL_BIT}};
static const component_param_spec param_constant[] = {
{"value", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 0.0f}}}
};
static const component_param_spec param_lowpass[] = {
{"gain", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 0.0f}}}
};
static const component_param_spec param_sum2[] = {
{"gain0", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 1.0f}}},
{"gain1", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 1.0f}}},
{"offset", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 0.0f}}}
};
static const component_param_spec param_div2[] = {
{"deadband", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 0.0f}}}
};
static const component_param_spec param_near[] = {
{"scale", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 1.0f}}},
{"difference", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 0.0f}}}
};
static const component_param_spec param_comp[] = {
{"hyst", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 0.0f}}}
};
static const component_param_spec param_invert[] = {
{"deadband", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 0.0f}}}
};
static const component_param_spec param_edge[] = {
{"both", SIM_HAL_BIT, {SIM_HAL_BIT, {.bit = 0U}}},
{"in-edge", SIM_HAL_BIT, {SIM_HAL_BIT, {.bit = 1U}}},
{"out-width-ns", SIM_HAL_S32, {SIM_HAL_S32, {.s32 = 0}}},
{"time-left-ns", SIM_HAL_S32, {SIM_HAL_S32, {.s32 = 0}}},
{"last-in", SIM_HAL_BIT, {SIM_HAL_BIT, {.bit = 0U}}}
};
static const component_param_spec param_toggle[] = {
{"debounce", SIM_HAL_U32, {SIM_HAL_U32, {.u32 = 2U}}}
};
static int component_spec_for(const char *type, component_spec *out) {
if (type == NULL || out == NULL) return 0;
memset(out, 0, sizeof(*out));
#define SPEC(kind_value, pin_array) do { out->kind = (kind_value); out->pins = (pin_array); out->pin_count = (uint32_t)(sizeof(pin_array) / sizeof((pin_array)[0])); return 1; } while (0)
if (strcmp(type, "corexy_by_hal") == 0) SPEC(SIM_HAL_COMPONENT_COREXY, pins_corexy);
if (strcmp(type, "constant") == 0) { out->kind = SIM_HAL_COMPONENT_CONSTANT; out->pins = pins_constant; out->pin_count = 1U; out->params = param_constant; out->param_count = 1U; return 1; }
if (strcmp(type, "and2") == 0) SPEC(SIM_HAL_COMPONENT_AND2, pins_bit2);
if (strcmp(type, "or2") == 0) SPEC(SIM_HAL_COMPONENT_OR2, pins_bit2);
if (strcmp(type, "xor2") == 0) SPEC(SIM_HAL_COMPONENT_XOR2, pins_bit2);
if (strcmp(type, "not") == 0 || strcmp(type, "invert_bit") == 0) SPEC(SIM_HAL_COMPONENT_NOT, pins_bit1);
if (strcmp(type, "abs") == 0) SPEC(SIM_HAL_COMPONENT_ABS, pins_abs);
if (strcmp(type, "abs_s32") == 0) SPEC(SIM_HAL_COMPONENT_ABS_S32, pins_abs_s32);
if (strcmp(type, "lowpass") == 0) { out->kind = SIM_HAL_COMPONENT_LOWPASS; out->pins = pins_lowpass; out->pin_count = 3U; out->params = param_lowpass; out->param_count = 1U; return 1; }
if (strcmp(type, "ddt") == 0) SPEC(SIM_HAL_COMPONENT_DDT, pins_ddt);
if (strcmp(type, "integ") == 0) SPEC(SIM_HAL_COMPONENT_INTEG, pins_integ);
if (strcmp(type, "mux2") == 0) SPEC(SIM_HAL_COMPONENT_MUX2, pins_mux2);
if (strcmp(type, "sum2") == 0) { out->kind = SIM_HAL_COMPONENT_SUM2; out->pins = pins_sum2; out->pin_count = 3U; out->params = param_sum2; out->param_count = 3U; return 1; }
if (strcmp(type, "mult2") == 0) SPEC(SIM_HAL_COMPONENT_MULT2, pins_float2);
if (strcmp(type, "div2") == 0) { out->kind = SIM_HAL_COMPONENT_DIV2; out->pins = pins_float2; out->pin_count = 3U; out->params = param_div2; out->param_count = 1U; return 1; }
if (strcmp(type, "hypot") == 0) SPEC(SIM_HAL_COMPONENT_HYPOT, pins_hypot);
if (strcmp(type, "near") == 0) { out->kind = SIM_HAL_COMPONENT_NEAR; out->pins = pins_near; out->pin_count = 3U; out->params = param_near; out->param_count = 2U; return 1; }
if (strcmp(type, "comp") == 0) { out->kind = SIM_HAL_COMPONENT_COMP; out->pins = pins_comp; out->pin_count = 4U; out->params = param_comp; out->param_count = 1U; return 1; }
if (strcmp(type, "invert") == 0) { out->kind = SIM_HAL_COMPONENT_INVERT; out->pins = pins_invert; out->pin_count = 2U; out->params = param_invert; out->param_count = 1U; return 1; }
if (strcmp(type, "edge") == 0) { out->kind = SIM_HAL_COMPONENT_EDGE; out->pins = pins_edge; out->pin_count = 3U; out->params = param_edge; out->param_count = 5U; return 1; }
if (strcmp(type, "toggle") == 0) { out->kind = SIM_HAL_COMPONENT_TOGGLE; out->pins = pins_toggle; out->pin_count = 2U; out->params = param_toggle; out->param_count = 1U; return 1; }
if (strcmp(type, "estop_latch") == 0) SPEC(SIM_HAL_COMPONENT_ESTOP_LATCH, pins_estop);
#undef SPEC
return 0;
}
static int32_t qualified_name(const char *instance, const char *suffix, char *out) {
size_t left;
size_t right;
if (!valid_name(instance) || suffix == NULL || out == NULL) return SIM_HAL_ERR_ARGUMENT;
left = strlen(instance); right = strlen(suffix);
if (left + 1U + right >= SIM_HAL_NAME_CAPACITY) return SIM_HAL_ERR_RANGE;
memcpy(out, instance, left); out[left] = '.'; memcpy(out + left + 1U, suffix, right + 1U);
return SIM_HAL_OK;
}
static int32_t register_param_internal(const char *name, sim_hal_value value) {
uint32_t index;
if (!valid_name(name) || !finite_value(value)) return SIM_HAL_ERR_ARGUMENT;
if (find_param(name) >= 0) return SIM_HAL_ERR_DUPLICATE;
for (index = 0; index < SIM_HAL_PARAM_CAPACITY; ++index) {
if (!state.params[index].used) {
state.params[index].used = 1U;
strcpy(state.params[index].name, name);
state.params[index].value = value;
return SIM_HAL_OK;
}
}
return SIM_HAL_ERR_FULL;
}
static void component_initialize_defaults(const component_entry *component);
int32_t sim_hal_register_component(const char *instance, const char *type, const char *thread) {
component_spec spec;
hal_state before;
char qualified[SIM_HAL_NAME_CAPACITY];
uint32_t index;
int component_index;
int function_index;
int32_t result;
if (!valid_name(instance) || !valid_name(type) || !valid_name(thread)) return SIM_HAL_ERR_ARGUMENT;
if (!component_spec_for(type, &spec)) return SIM_HAL_ERR_REAL_EXCLUDED;
if (find_component(instance) >= 0 || find_function(instance) >= 0) return SIM_HAL_ERR_DUPLICATE;
if (find_thread(thread) < 0) return SIM_HAL_ERR_NOT_FOUND;
memcpy(&before, &state, sizeof(state));
component_index = -1;
for (index = 0; index < SIM_HAL_COMPONENT_CAPACITY; ++index) {
if (!state.components[index].used) { component_index = (int)index; break; }
}
if (component_index < 0) return SIM_HAL_ERR_FULL;
state.components[component_index].used = 1U;
strcpy(state.components[component_index].instance, instance);
strcpy(state.components[component_index].type, type);
state.components[component_index].kind = spec.kind;
state.components[component_index].function_index = 0U;
for (index = 0; index < spec.pin_count; ++index) {
result = qualified_name(instance, spec.pins[index].name, qualified);
if (result != SIM_HAL_OK || (result = sim_hal_register_pin(qualified, spec.pins[index].type)) != SIM_HAL_OK) { memcpy(&state, &before, sizeof(state)); return result; }
}
for (index = 0; index < spec.param_count; ++index) {
result = qualified_name(instance, spec.params[index].name, qualified);
if (result != SIM_HAL_OK || (result = register_param_internal(qualified, spec.params[index].initial)) != SIM_HAL_OK) { memcpy(&state, &before, sizeof(state)); return result; }
}
result = sim_hal_register_function(thread, instance);
if (result != SIM_HAL_OK) { memcpy(&state, &before, sizeof(state)); return result; }
function_index = find_function(instance);
state.components[component_index].function_index = (uint32_t)function_index;
state.functions[function_index].component_index = (uint32_t)component_index;
state.functions[function_index].component_kind = spec.kind;
component_initialize_defaults(&state.components[component_index]);
return SIM_HAL_OK;
}
static int32_t corexy_pin_names(const char *component, char names[8][SIM_HAL_NAME_CAPACITY]) {
static const char *suffixes[8] = {
".j0-motor-pos-cmd", ".j1-motor-pos-cmd", ".alpha-cmd", ".beta-cmd",
@@ -173,7 +493,8 @@ int32_t sim_hal_register_corexy_by_hal(const char *component, const char *thread
}
result = sim_hal_register_function(thread, component);
if (result != SIM_HAL_OK) return result;
state.functions[find_function(component)].transform_kind = SIM_HAL_TRANSFORM_COREXY;
state.functions[find_function(component)].component_index = UINT32_MAX;
state.functions[find_function(component)].component_kind = SIM_HAL_COMPONENT_COREXY;
return SIM_HAL_OK;
}
@@ -206,6 +527,235 @@ int32_t sim_hal_corexy_transform(const char *component) {
return set_float_pin(names[7], alpha_fb - beta_fb);
}
static int32_t component_pin_name(const component_entry *component, const char *suffix, char out[SIM_HAL_NAME_CAPACITY]) {
return qualified_name(component->instance, suffix, out);
}
static int32_t component_float_pin(const component_entry *component, const char *suffix, float *value) {
char name[SIM_HAL_NAME_CAPACITY];
sim_hal_value pin = {0};
int32_t result = component_pin_name(component, suffix, name);
if (result != SIM_HAL_OK) return result;
result = sim_hal_get_pin(name, &pin);
if (result != SIM_HAL_OK) return result;
if (pin.type != SIM_HAL_FLOAT || !finite_value(pin)) return SIM_HAL_ERR_TYPE;
*value = pin.value.real;
return SIM_HAL_OK;
}
static int32_t component_bit_pin(const component_entry *component, const char *suffix, uint32_t *value) {
char name[SIM_HAL_NAME_CAPACITY];
sim_hal_value pin = {0};
int32_t result = component_pin_name(component, suffix, name);
if (result != SIM_HAL_OK) return result;
result = sim_hal_get_pin(name, &pin);
if (result != SIM_HAL_OK) return result;
if (pin.type != SIM_HAL_BIT) return SIM_HAL_ERR_TYPE;
*value = pin.value.bit;
return SIM_HAL_OK;
}
static int32_t component_s32_pin(const component_entry *component, const char *suffix, int32_t *value) {
char name[SIM_HAL_NAME_CAPACITY];
sim_hal_value pin = {0};
int32_t result = component_pin_name(component, suffix, name);
if (result != SIM_HAL_OK) return result;
result = sim_hal_get_pin(name, &pin);
if (result != SIM_HAL_OK) return result;
if (pin.type != SIM_HAL_S32) return SIM_HAL_ERR_TYPE;
*value = pin.value.s32;
return SIM_HAL_OK;
}
static int32_t component_write_float(const component_entry *component, const char *suffix, float value) {
char name[SIM_HAL_NAME_CAPACITY];
int32_t result = component_pin_name(component, suffix, name);
if (result != SIM_HAL_OK) return result;
return sim_hal_set_pin(name, (sim_hal_value){SIM_HAL_FLOAT, {.real = value}});
}
static int32_t component_write_bit(const component_entry *component, const char *suffix, uint32_t value) {
char name[SIM_HAL_NAME_CAPACITY];
int32_t result = component_pin_name(component, suffix, name);
if (result != SIM_HAL_OK) return result;
return sim_hal_set_pin(name, (sim_hal_value){SIM_HAL_BIT, {.bit = value ? 1U : 0U}});
}
static int32_t component_write_s32(const component_entry *component, const char *suffix, int32_t value) {
char name[SIM_HAL_NAME_CAPACITY];
int32_t result = component_pin_name(component, suffix, name);
if (result != SIM_HAL_OK) return result;
return sim_hal_set_pin(name, (sim_hal_value){SIM_HAL_S32, {.s32 = value}});
}
static int32_t component_float_param(const component_entry *component, const char *suffix, float *value) {
char name[SIM_HAL_NAME_CAPACITY];
sim_hal_value param = {0};
int32_t result = component_pin_name(component, suffix, name);
if (result != SIM_HAL_OK) return result;
result = sim_hal_get_param(name, &param);
if (result != SIM_HAL_OK) return result;
if (param.type != SIM_HAL_FLOAT || !finite_value(param)) return SIM_HAL_ERR_TYPE;
*value = param.value.real;
return SIM_HAL_OK;
}
static int32_t component_bit_param(const component_entry *component, const char *suffix, uint32_t *value) {
char name[SIM_HAL_NAME_CAPACITY];
sim_hal_value param = {0};
int32_t result = component_pin_name(component, suffix, name);
if (result != SIM_HAL_OK) return result;
result = sim_hal_get_param(name, &param);
if (result != SIM_HAL_OK) return result;
if (param.type != SIM_HAL_BIT) return SIM_HAL_ERR_TYPE;
*value = param.value.bit;
return SIM_HAL_OK;
}
static int32_t component_s32_param(const component_entry *component, const char *suffix, int32_t *value) {
char name[SIM_HAL_NAME_CAPACITY];
sim_hal_value param = {0};
int32_t result = component_pin_name(component, suffix, name);
if (result != SIM_HAL_OK) return result;
result = sim_hal_get_param(name, &param);
if (result != SIM_HAL_OK) return result;
if (param.type != SIM_HAL_S32) return SIM_HAL_ERR_TYPE;
*value = param.value.s32;
return SIM_HAL_OK;
}
static int32_t component_write_bit_param(const component_entry *component, const char *suffix, uint32_t value) {
char name[SIM_HAL_NAME_CAPACITY];
int32_t result = component_pin_name(component, suffix, name);
if (result != SIM_HAL_OK) return result;
return sim_hal_set_param(name, (sim_hal_value){SIM_HAL_BIT, {.bit = value ? 1U : 0U}});
}
static void component_initialize_defaults(const component_entry *component) {
if (component->kind == SIM_HAL_COMPONENT_INTEG) {
(void)component_write_float(component, "gain", 1.0f);
(void)component_write_float(component, "max", 1.0e20f);
(void)component_write_float(component, "min", -1.0e20f);
} else if (component->kind == SIM_HAL_COMPONENT_ESTOP_LATCH) {
(void)component_write_bit(component, "ok-in", 1U);
(void)component_write_bit(component, "fault-in", 0U);
(void)component_write_bit(component, "ok-out", 0U);
(void)component_write_bit(component, "fault-out", 1U);
}
}
static int32_t component_step(component_entry *component) {
float a, b, c, value;
uint32_t bit, other;
int32_t integer;
int32_t result;
if (component->kind == SIM_HAL_COMPONENT_COREXY) return sim_hal_corexy_transform(component->instance);
switch (component->kind) {
case SIM_HAL_COMPONENT_CONSTANT:
result = component_float_param(component, "value", &value); if (result != SIM_HAL_OK) return result;
return component_write_float(component, "out", value);
case SIM_HAL_COMPONENT_AND2: case SIM_HAL_COMPONENT_OR2: case SIM_HAL_COMPONENT_XOR2:
result = component_bit_pin(component, "in0", &bit); if (result != SIM_HAL_OK) return result;
result = component_bit_pin(component, "in1", &other); if (result != SIM_HAL_OK) return result;
value = (float)(component->kind == SIM_HAL_COMPONENT_AND2 ? (bit && other) : component->kind == SIM_HAL_COMPONENT_OR2 ? (bit || other) : (bit != other));
return component_write_bit(component, "out", value != 0.0f);
case SIM_HAL_COMPONENT_NOT:
result = component_bit_pin(component, "in", &bit); if (result != SIM_HAL_OK) return result;
return component_write_bit(component, "out", !bit);
case SIM_HAL_COMPONENT_ABS:
result = component_float_pin(component, "in", &a); if (result != SIM_HAL_OK) return result;
result = component_write_float(component, "out", fabsf(a)); if (result != SIM_HAL_OK) return result;
result = component_write_bit(component, "sign", a < 0.0f); if (result != SIM_HAL_OK) return result;
result = component_write_bit(component, "is-positive", a > 0.000001f); if (result != SIM_HAL_OK) return result;
return component_write_bit(component, "is-negative", a < -0.000001f);
case SIM_HAL_COMPONENT_ABS_S32: case SIM_HAL_COMPONENT_ABS_S64:
result = component_s32_pin(component, "in", &integer); if (result != SIM_HAL_OK) return result;
if (integer == INT32_MIN) return SIM_HAL_ERR_RANGE;
result = component_write_s32(component, "out", integer < 0 ? -integer : integer); if (result != SIM_HAL_OK) return result;
result = component_write_bit(component, "sign", integer < 0); if (result != SIM_HAL_OK) return result;
result = component_write_bit(component, "is-positive", integer > 0); if (result != SIM_HAL_OK) return result;
return component_write_bit(component, "is-negative", integer < 0);
case SIM_HAL_COMPONENT_LOWPASS:
result = component_float_pin(component, "in", &a); if (result != SIM_HAL_OK) return result;
result = component_float_pin(component, "out", &b); if (result != SIM_HAL_OK) return result;
result = component_bit_pin(component, "load", &bit); if (result != SIM_HAL_OK) return result;
result = component_float_param(component, "gain", &c); if (result != SIM_HAL_OK) return result;
return component_write_float(component, "out", bit ? a : b + (a - b) * c);
case SIM_HAL_COMPONENT_DDT:
result = component_float_pin(component, "in", &a); if (result != SIM_HAL_OK) return result;
result = component_write_float(component, "out", (float)(a - component->memory[0]));
component->memory[0] = a; return result;
case SIM_HAL_COMPONENT_INTEG:
result = component_float_pin(component, "in", &a); if (result != SIM_HAL_OK) return result;
result = component_float_pin(component, "gain", &b); if (result != SIM_HAL_OK) return result;
result = component_bit_pin(component, "reset", &bit); if (result != SIM_HAL_OK) return result;
result = component_float_pin(component, "max", &c); if (result != SIM_HAL_OK) return result;
result = component_float_pin(component, "min", &value); if (result != SIM_HAL_OK) return result;
if (bit) component->memory[0] = 0.0; else component->memory[0] += (double)a * (double)b;
if (component->memory[0] > c) component->memory[0] = c;
if (component->memory[0] < value) component->memory[0] = value;
return component_write_float(component, "out", (float)component->memory[0]);
case SIM_HAL_COMPONENT_MUX2:
result = component_bit_pin(component, "sel", &bit); if (result != SIM_HAL_OK) return result;
result = component_float_pin(component, bit ? "in1" : "in0", &a); if (result != SIM_HAL_OK) return result;
return component_write_float(component, "out", a);
case SIM_HAL_COMPONENT_SUM2:
result = component_float_pin(component, "in0", &a); if (result != SIM_HAL_OK) return result;
result = component_float_pin(component, "in1", &b); if (result != SIM_HAL_OK) return result;
result = component_float_param(component, "gain0", &c); if (result != SIM_HAL_OK) return result;
result = component_float_param(component, "gain1", &value); if (result != SIM_HAL_OK) return result;
{ float offset; result = component_float_param(component, "offset", &offset); if (result != SIM_HAL_OK) return result; return component_write_float(component, "out", a * c + b * value + offset); }
case SIM_HAL_COMPONENT_MULT2:
result = component_float_pin(component, "in0", &a); if (result != SIM_HAL_OK) return result;
result = component_float_pin(component, "in1", &b); if (result != SIM_HAL_OK) return result;
return component_write_float(component, "out", a * b);
case SIM_HAL_COMPONENT_DIV2:
result = component_float_pin(component, "in0", &a); if (result != SIM_HAL_OK) return result;
result = component_float_pin(component, "in1", &b); if (result != SIM_HAL_OK) return result;
result = component_float_param(component, "deadband", &c); if (result != SIM_HAL_OK) return result;
if (c < 1e-12f) c = 1e-12f; if (fabsf(b) < c) b = b < 0.0f ? -c : c;
return component_write_float(component, "out", a / b);
case SIM_HAL_COMPONENT_HYPOT:
result = component_float_pin(component, "in0", &a); if (result != SIM_HAL_OK) return result;
result = component_float_pin(component, "in1", &b); if (result != SIM_HAL_OK) return result;
result = component_float_pin(component, "in2", &c); if (result != SIM_HAL_OK) return result;
return component_write_float(component, "out", sqrtf(a * a + b * b + c * c));
case SIM_HAL_COMPONENT_NEAR:
result = component_float_pin(component, "in1", &a); if (result != SIM_HAL_OK) return result;
result = component_float_pin(component, "in2", &b); if (result != SIM_HAL_OK) return result;
result = component_float_param(component, "scale", &c); if (result != SIM_HAL_OK) return result;
result = component_float_param(component, "difference", &value); if (result != SIM_HAL_OK) return result;
if (a < 0.0f) { a = -a; b = -b; }
return component_write_bit(component, "out", ((c > 1.0f && a / c <= b && b <= a * c) || fabsf(a - b) <= value));
case SIM_HAL_COMPONENT_COMP:
result = component_float_pin(component, "in0", &a); if (result != SIM_HAL_OK) return result;
result = component_float_pin(component, "in1", &b); if (result != SIM_HAL_OK) return result;
result = component_float_param(component, "hyst", &c); if (result != SIM_HAL_OK) return result;
if (b - a < -0.5f * c) { result = component_write_bit(component, "out", 0U); if (result != SIM_HAL_OK) return result; return component_write_bit(component, "equal", 0U); }
if (b - a > 0.5f * c) { result = component_write_bit(component, "out", 1U); if (result != SIM_HAL_OK) return result; return component_write_bit(component, "equal", 0U); }
return component_write_bit(component, "equal", 1U);
case SIM_HAL_COMPONENT_INVERT:
result = component_float_pin(component, "in", &a); if (result != SIM_HAL_OK) return result;
result = component_float_param(component, "deadband", &c); if (result != SIM_HAL_OK) return result;
if (c < 1e-12f) c = 1e-12f; if (fabsf(a) < c) a = a < 0.0f ? -c : c;
return component_write_float(component, "out", 1.0f / a);
case SIM_HAL_COMPONENT_EDGE:
result = component_bit_pin(component, "in", &bit); if (result != SIM_HAL_OK) return result;
result = component_bit_param(component, "last-in", &other); if (result != SIM_HAL_OK) return result;
if (component->flags[0] == 0U) { component->flags[0] = 1U; component->memory[0] = bit; return component_write_bit(component, "out-invert", 1U); }
{ uint32_t both, in_edge; result = component_bit_param(component, "both", &both); if (result != SIM_HAL_OK) return result; result = component_bit_param(component, "in-edge", &in_edge); if (result != SIM_HAL_OK) return result; uint32_t rise = bit && !other, fall = !bit && other; uint32_t hit = both ? (rise || fall) : (in_edge ? fall : rise); result = component_write_bit(component, "out", hit); if (result != SIM_HAL_OK) return result; result = component_write_bit(component, "out-invert", !hit); if (result != SIM_HAL_OK) return result; result = component_write_bit_param(component, "last-in", bit); component->memory[0] = bit; return result; }
case SIM_HAL_COMPONENT_TOGGLE:
result = component_bit_pin(component, "in", &bit); if (result != SIM_HAL_OK) return result;
result = component_bit_pin(component, "out", &other); if (result != SIM_HAL_OK) return result;
{ sim_hal_value param = {0}; char name[SIM_HAL_NAME_CAPACITY]; component_pin_name(component, "debounce", name); result = sim_hal_get_param(name, &param); if (result != SIM_HAL_OK) return result; uint32_t debounce = param.value.u32 < 1U || param.value.u32 > 10000U ? 2U : param.value.u32; if (bit) { if (component->flags[0] < debounce) component->flags[0] += 1U; if (component->flags[0] >= debounce && component->flags[1] == 0U) { other = !other; component->flags[1] = 1U; } } else { component->flags[0] = 0U; component->flags[1] = 0U; } return component_write_bit(component, "out", other); }
case SIM_HAL_COMPONENT_ESTOP_LATCH:
result = component_bit_pin(component, "ok-in", &bit); if (result != SIM_HAL_OK) return result;
result = component_bit_pin(component, "fault-in", &other); if (result != SIM_HAL_OK) return result;
{ uint32_t reset; result = component_bit_pin(component, "reset", &reset); if (result != SIM_HAL_OK) return result; if (!bit || other) { component->flags[0] = 0U; } else if (reset && component->flags[1] == 0U) component->flags[0] = 1U; component->flags[1] = reset; result = component_write_bit(component, "ok-out", component->flags[0]); if (result != SIM_HAL_OK) return result; result = component_write_bit(component, "fault-out", !component->flags[0]); if (result != SIM_HAL_OK) return result; if (component->flags[0]) component->flags[2] ^= 1U; return component_write_bit(component, "watchdog", component->flags[2]); }
default: return SIM_HAL_ERR_REAL_EXCLUDED;
}
}
int32_t sim_hal_set_function_failure(const char *name, uint32_t fail) {
int index = find_function(name);
if (index < 0) return SIM_HAL_ERR_NOT_FOUND;
@@ -214,22 +764,36 @@ int32_t sim_hal_set_function_failure(const char *name, uint32_t fail) {
}
int32_t sim_hal_step(uint32_t ticks) {
uint32_t target = state.tick + ticks;
uint32_t tick;
hal_state before;
uint64_t target;
uint64_t tick;
uint32_t function_index;
for (tick = state.tick + 1U; tick <= target; ++tick) {
if ((uint64_t)state.tick + (uint64_t)ticks > UINT32_MAX) return SIM_HAL_ERR_RANGE;
memcpy(&before, &state, sizeof(state));
target = (uint64_t)state.tick + (uint64_t)ticks;
for (tick = (uint64_t)state.tick + 1U; tick <= target; ++tick) {
for (function_index = 0; function_index < 256U; ++function_index) {
if (state.links[function_index].used) {
uint32_t source = state.links[function_index].source;
uint32_t target_pin = state.links[function_index].target;
state.pins[target_pin].value = state.pins[source].value;
}
}
for (function_index = 0; function_index < SIM_HAL_FUNCTION_CAPACITY; ++function_index) {
function_entry *function = &state.functions[function_index];
if (!function->used || tick % state.threads[function->thread].period != 0U) continue;
if (function->fail) return SIM_HAL_ERR_FUNCTION;
if (function->transform_kind == SIM_HAL_TRANSFORM_COREXY) {
int32_t result = sim_hal_corexy_transform(function->name);
if (result != SIM_HAL_OK) return SIM_HAL_ERR_FUNCTION;
if (function->fail) { memcpy(&state, &before, sizeof(state)); return SIM_HAL_ERR_FUNCTION; }
if (function->component_kind != SIM_HAL_COMPONENT_NONE) {
int32_t result;
if (function->component_kind == SIM_HAL_COMPONENT_COREXY && function->component_index == UINT32_MAX) result = sim_hal_corexy_transform(function->name);
else if (function->component_index >= SIM_HAL_COMPONENT_CAPACITY || !state.components[function->component_index].used) result = SIM_HAL_ERR_NOT_FOUND;
else result = component_step(&state.components[function->component_index]);
if (result != SIM_HAL_OK) { memcpy(&state, &before, sizeof(state)); return SIM_HAL_ERR_FUNCTION; }
}
function->calls += 1U;
}
}
state.tick = target;
state.tick = (uint32_t)target;
return SIM_HAL_OK;
}
@@ -238,3 +802,5 @@ uint32_t sim_hal_function_calls(const char *name) { int index = find_function(na
uint32_t sim_hal_pin_count(void) { uint32_t i, count = 0; for (i = 0; i < SIM_HAL_PIN_CAPACITY; ++i) count += state.pins[i].used; return count; }
uint32_t sim_hal_thread_count(void) { uint32_t i, count = 0; for (i = 0; i < SIM_HAL_THREAD_CAPACITY; ++i) count += state.threads[i].used; return count; }
uint32_t sim_hal_function_count(void) { uint32_t i, count = 0; for (i = 0; i < SIM_HAL_FUNCTION_CAPACITY; ++i) count += state.functions[i].used; return count; }
uint32_t sim_hal_component_count(void) { uint32_t i, count = 0; for (i = 0; i < SIM_HAL_COMPONENT_CAPACITY; ++i) count += state.components[i].used; return count; }
uint32_t sim_hal_param_count(void) { uint32_t i, count = 0; for (i = 0; i < SIM_HAL_PARAM_CAPACITY; ++i) count += state.params[i].used; return count; }

View File

@@ -4,8 +4,39 @@
#include <math.h>
#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
/* Keep the public 648-byte object ABI stable; mutable parameter/diagnostic
* state is held in a bounded side table owned by the single Worker writer. */
#define META_SLOTS 4u
#define NAMED_PARAMETER_CAPACITY 16u
#define NAMED_PARAMETER_LENGTH 32u
#define PENDING_ASSIGNMENTS 32u
typedef struct named_parameter {
char name[NAMED_PARAMETER_LENGTH];
double value;
int used;
} named_parameter;
typedef struct interpreter_meta {
const sim_interpreter *owner;
double parameters[SIM_INTERPRETER_PARAMETER_MAX + 1u];
named_parameter named[NAMED_PARAMETER_CAPACITY];
double coordinate_offsets[9][9];
int coordinate_system;
int cutter_comp;
int cycle_mode;
int coolant_mode;
int selected_tool;
double tool_length;
uint32_t error_column;
int32_t error_code;
} interpreter_meta;
static interpreter_meta metadata[META_SLOTS];
static uint64_t fnv1a64(const unsigned char *bytes, size_t length) {
uint64_t hash = UINT64_C(1469598103934665603);
size_t index;
@@ -16,60 +47,34 @@ static uint64_t fnv1a64(const unsigned char *bytes, size_t length) {
return hash;
}
static interpreter_meta *meta_for(const sim_interpreter *interpreter, int create) {
interpreter_meta *free_slot = NULL;
size_t index;
if (interpreter == NULL) return NULL;
for (index = 0; index < META_SLOTS; ++index) {
if (metadata[index].owner == interpreter) return &metadata[index];
if (free_slot == NULL && metadata[index].owner == NULL) free_slot = &metadata[index];
}
if (!create || free_slot == NULL) return NULL;
memset(free_slot, 0, sizeof(*free_slot));
free_slot->owner = interpreter;
return free_slot;
}
static void meta_reset(interpreter_meta *meta, const sim_interpreter *owner) {
memset(meta, 0, sizeof(*meta));
meta->owner = owner;
}
static int finite_state(const sim_interpreter_state *state) {
const double *values;
size_t index;
if (state == 0) return 0;
if (state == NULL) return 0;
values = &state->x;
for (index = 0; index < 11; ++index) if (!isfinite(values[index])) return 0;
return 1;
}
static int parse_number(const char **cursor, const char *end, double *output) {
const char *start = *cursor;
int sign = 1;
int digits = 0;
int exponent_sign = 1;
int exponent = 0;
long double value = 0.0L;
long double scale = 1.0L;
if (*cursor < end && (**cursor == '+' || **cursor == '-')) {
if (**cursor == '-') sign = -1;
++*cursor;
}
while (*cursor < end && isdigit((unsigned char)**cursor)) {
value = value * 10.0L + (long double)(**cursor - '0');
++*cursor;
digits = 1;
}
if (*cursor < end && **cursor == '.') {
++*cursor;
while (*cursor < end && isdigit((unsigned char)**cursor)) {
value = value * 10.0L + (long double)(**cursor - '0');
scale *= 10.0L;
++*cursor;
digits = 1;
}
}
if (!digits) { *cursor = start; return SIM_INTERP_ERR_SYNTAX; }
if (*cursor < end && (**cursor == 'e' || **cursor == 'E')) {
++*cursor;
if (*cursor < end && (**cursor == '+' || **cursor == '-')) {
if (**cursor == '-') exponent_sign = -1;
++*cursor;
}
if (*cursor == end || !isdigit((unsigned char)**cursor)) { *cursor = start; return SIM_INTERP_ERR_SYNTAX; }
while (*cursor < end && isdigit((unsigned char)**cursor)) {
if (exponent < 10000) exponent = exponent * 10 + (**cursor - '0');
++*cursor;
}
}
value = (long double)sign * value / scale;
if (exponent) value *= powl(10.0L, (long double)(exponent_sign * exponent));
*output = (double)value;
return isfinite(*output) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
}
static int code_is(double value, int integer_code) {
return isfinite(value) && fabs(value - (double)integer_code) <= 1e-12;
}
@@ -83,175 +88,430 @@ static int axis_index(char letter) {
}
}
static double *axis_pointer(sim_interpreter_state *state, int index) {
return &state->x + index;
static double *axis_pointer(sim_interpreter_state *state, int index) { return &state->x + index; }
static void set_error(sim_interpreter *interpreter, interpreter_meta *meta, int code, uint32_t column) {
if (interpreter != NULL) interpreter->state.status = code;
if (meta != NULL) { meta->error_code = code; meta->error_column = column; }
}
static int apply_g(sim_interpreter_state *state, double code, int *groups) {
static int parse_decimal(const char **cursor, const char *end, double *output) {
const char *start = *cursor;
int sign = 1, digits = 0, exponent_sign = 1, exponent = 0;
long double value = 0.0L, scale = 1.0L;
if (*cursor < end && (**cursor == '+' || **cursor == '-')) {
if (**cursor == '-') sign = -1;
++*cursor;
}
while (*cursor < end && isdigit((unsigned char)**cursor)) {
value = value * 10.0L + (long double)(**cursor - '0'); ++*cursor; digits = 1;
}
if (*cursor < end && **cursor == '.') {
++*cursor;
while (*cursor < end && isdigit((unsigned char)**cursor)) {
value = value * 10.0L + (long double)(**cursor - '0'); scale *= 10.0L; ++*cursor; digits = 1;
}
}
if (!digits) { *cursor = start; return SIM_INTERP_ERR_SYNTAX; }
if (*cursor < end && (**cursor == 'e' || **cursor == 'E')) {
++*cursor;
if (*cursor < end && (**cursor == '+' || **cursor == '-')) { if (**cursor == '-') exponent_sign = -1; ++*cursor; }
if (*cursor == end || !isdigit((unsigned char)**cursor)) { *cursor = start; return SIM_INTERP_ERR_SYNTAX; }
while (*cursor < end && isdigit((unsigned char)**cursor)) { if (exponent < 10000) exponent = exponent * 10 + (**cursor - '0'); ++*cursor; }
}
value = (long double)sign * value / scale;
if (exponent != 0) value *= powl(10.0L, (long double)(exponent_sign * exponent));
*output = (double)value;
return isfinite(*output) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
}
typedef struct expression_parser {
interpreter_meta *meta;
const char *cursor;
const char *end;
int pending_index[PENDING_ASSIGNMENTS];
double pending_value[PENDING_ASSIGNMENTS];
size_t pending_count;
} expression_parser;
static void skip_space(expression_parser *parser) {
while (parser->cursor < parser->end && isspace((unsigned char)*parser->cursor)) ++parser->cursor;
}
static int identifier(expression_parser *parser, char *buffer, size_t capacity) {
size_t length = 0;
skip_space(parser);
while (parser->cursor < parser->end && isalpha((unsigned char)*parser->cursor)) {
if (length + 1u < capacity) buffer[length++] = (char)toupper((unsigned char)*parser->cursor);
++parser->cursor;
}
buffer[length] = '\0';
return length > 0 ? SIM_INTERP_OK : SIM_INTERP_ERR_SYNTAX;
}
static uint32_t named_hash(const char *name) { return (uint32_t)(fnv1a64((const unsigned char *)name, strlen(name)) & UINT32_C(0x7fffffff)); }
static int parameter_lookup(const expression_parser *parser, uint32_t index, double *value) {
size_t item;
if (index < SIM_INTERPRETER_PARAMETER_MIN || index > SIM_INTERPRETER_PARAMETER_MAX || value == NULL) return SIM_INTERP_ERR_PARAMETER;
for (item = parser->pending_count; item > 0; --item) {
if ((uint32_t)parser->pending_index[item - 1u] == index) { *value = parser->pending_value[item - 1u]; return SIM_INTERP_OK; }
}
*value = parser->meta->parameters[index];
return isfinite(*value) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
}
static int named_lookup(expression_parser *parser, const char *name, double *value, int create) {
size_t index;
if (name == NULL || name[0] == '\0' || value == NULL) return SIM_INTERP_ERR_PARAMETER;
for (index = 0; index < NAMED_PARAMETER_CAPACITY; ++index) {
if (parser->meta->named[index].used && strcmp(parser->meta->named[index].name, name) == 0) { *value = parser->meta->named[index].value; return SIM_INTERP_OK; }
}
if (!create) { *value = 0.0; return SIM_INTERP_OK; }
for (index = 0; index < NAMED_PARAMETER_CAPACITY; ++index) if (!parser->meta->named[index].used) {
strncpy(parser->meta->named[index].name, name, NAMED_PARAMETER_LENGTH - 1u);
parser->meta->named[index].name[NAMED_PARAMETER_LENGTH - 1u] = '\0';
parser->meta->named[index].used = 1; parser->meta->named[index].value = 0.0; *value = 0.0; return SIM_INTERP_OK;
}
return SIM_INTERP_ERR_PARAMETER;
}
static int parse_expression(expression_parser *parser, double *value);
static int parse_parameter_reference(expression_parser *parser, double *value, int *named_slot) {
unsigned long index = 0;
char name[NAMED_PARAMETER_LENGTH] = {0};
size_t length = 0;
int result;
skip_space(parser);
if (parser->cursor >= parser->end || *parser->cursor != '#') return SIM_INTERP_ERR_SYNTAX;
++parser->cursor;
if (parser->cursor < parser->end && *parser->cursor == '<') {
++parser->cursor;
while (parser->cursor < parser->end && *parser->cursor != '>') {
if (length + 1u < sizeof(name)) name[length++] = (char)tolower((unsigned char)*parser->cursor);
++parser->cursor;
}
if (parser->cursor >= parser->end || *parser->cursor != '>' || length == 0) return SIM_INTERP_ERR_PARAMETER;
++parser->cursor; name[length] = '\0';
result = named_lookup(parser, name, value, 1);
if (named_slot != NULL) *named_slot = (int)named_hash(name);
return result;
}
if (parser->cursor < parser->end && *parser->cursor == '[') {
double expression_value;
++parser->cursor; result = parse_expression(parser, &expression_value); skip_space(parser);
if (result != SIM_INTERP_OK || parser->cursor >= parser->end || *parser->cursor != ']') return SIM_INTERP_ERR_PARAMETER;
++parser->cursor;
if (!isfinite(expression_value) || expression_value < 1.0 || expression_value > (double)SIM_INTERPRETER_PARAMETER_MAX || floor(expression_value) != expression_value) return SIM_INTERP_ERR_PARAMETER;
index = (unsigned long)expression_value;
} else {
const char *start = parser->cursor;
while (parser->cursor < parser->end && isdigit((unsigned char)*parser->cursor)) { if (index <= 1000000UL) index = index * 10UL + (unsigned long)(*parser->cursor - '0'); ++parser->cursor; }
if (parser->cursor == start || index < SIM_INTERPRETER_PARAMETER_MIN || index > SIM_INTERPRETER_PARAMETER_MAX) return SIM_INTERP_ERR_PARAMETER;
}
if (named_slot != NULL) *named_slot = 0;
return parameter_lookup(parser, (uint32_t)index, value);
}
static int parse_primary(expression_parser *parser, double *value) {
char function_name[16] = {0};
const char *start;
int result;
skip_space(parser);
if (parser->cursor >= parser->end) return SIM_INTERP_ERR_EXPRESSION;
if (*parser->cursor == '#') return parse_parameter_reference(parser, value, NULL);
if (*parser->cursor == '[') {
++parser->cursor; result = parse_expression(parser, value); skip_space(parser);
if (result != SIM_INTERP_OK || parser->cursor >= parser->end || *parser->cursor != ']') return SIM_INTERP_ERR_EXPRESSION;
++parser->cursor; return isfinite(*value) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
}
if (isalpha((unsigned char)*parser->cursor)) {
result = identifier(parser, function_name, sizeof(function_name)); if (result != SIM_INTERP_OK) return result;
skip_space(parser);
if (strcmp(function_name, "PI") == 0) { *value = 3.14159265358979323846; return SIM_INTERP_OK; }
if (strcmp(function_name, "E") == 0) { *value = 2.71828182845904523536; return SIM_INTERP_OK; }
if (strcmp(function_name, "NAN") == 0 || strcmp(function_name, "INF") == 0 || strcmp(function_name, "INFINITY") == 0) return SIM_INTERP_ERR_SYNTAX;
if (strcmp(function_name, "MOD") == 0) return SIM_INTERP_ERR_EXPRESSION;
if (parser->cursor < parser->end && *parser->cursor == '[') {
++parser->cursor; result = parse_expression(parser, value); skip_space(parser);
if (result != SIM_INTERP_OK || parser->cursor >= parser->end || *parser->cursor != ']') return SIM_INTERP_ERR_EXPRESSION;
++parser->cursor;
} else { result = parse_primary(parser, value); if (result != SIM_INTERP_OK) return result; }
if (!isfinite(*value)) return SIM_INTERP_ERR_NONFINITE;
if (strcmp(function_name, "ABS") == 0) *value = fabs(*value);
else if (strcmp(function_name, "SQRT") == 0) { if (*value < 0.0) return SIM_INTERP_ERR_RANGE; *value = sqrt(*value); }
else if (strcmp(function_name, "SIN") == 0) *value = sin(*value * 3.14159265358979323846 / 180.0);
else if (strcmp(function_name, "COS") == 0) *value = cos(*value * 3.14159265358979323846 / 180.0);
else if (strcmp(function_name, "TAN") == 0) *value = tan(*value * 3.14159265358979323846 / 180.0);
else if (strcmp(function_name, "ASIN") == 0) { if (fabs(*value) > 1.0) return SIM_INTERP_ERR_RANGE; *value = asin(*value) * 180.0 / 3.14159265358979323846; }
else if (strcmp(function_name, "ACOS") == 0) { if (fabs(*value) > 1.0) return SIM_INTERP_ERR_RANGE; *value = acos(*value) * 180.0 / 3.14159265358979323846; }
else if (strcmp(function_name, "ATAN") == 0) *value = atan(*value) * 180.0 / 3.14159265358979323846;
else if (strcmp(function_name, "FIX") == 0) *value = floor(*value);
else if (strcmp(function_name, "FUP") == 0) *value = ceil(*value);
else if (strcmp(function_name, "ROUND") == 0) *value = nearbyint(*value);
else if (strcmp(function_name, "EXP") == 0) *value = exp(*value);
else if (strcmp(function_name, "LN") == 0) { if (*value <= 0.0) return SIM_INTERP_ERR_RANGE; *value = log(*value); }
else return SIM_INTERP_ERR_UNKNOWN_CODE;
return isfinite(*value) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
}
start = parser->cursor; result = parse_decimal(&parser->cursor, parser->end, value); if (result != SIM_INTERP_OK) parser->cursor = start;
return result;
}
static int parse_unary(expression_parser *parser, double *value) {
skip_space(parser);
if (parser->cursor < parser->end && (*parser->cursor == '+' || *parser->cursor == '-')) {
char sign = *parser->cursor++; int result = parse_unary(parser, value);
if (result == SIM_INTERP_OK && sign == '-') *value = -*value; return result;
}
return parse_primary(parser, value);
}
static int parse_power(expression_parser *parser, double *value) {
int result = parse_unary(parser, value); skip_space(parser); if (result != SIM_INTERP_OK) return result;
if (parser->cursor + 1 < parser->end && parser->cursor[0] == '*' && parser->cursor[1] == '*') {
double right; parser->cursor += 2; result = parse_power(parser, &right); if (result != SIM_INTERP_OK) return result; *value = pow(*value, right);
}
return isfinite(*value) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
}
static int parse_product(expression_parser *parser, double *value) {
int result = parse_power(parser, value); if (result != SIM_INTERP_OK) return result;
for (;;) {
double right; char operator_name[8] = {0}; const char *save;
skip_space(parser);
if (parser->cursor >= parser->end || (*parser->cursor != '*' && *parser->cursor != '/')) {
save = parser->cursor;
if (parser->cursor < parser->end && isalpha((unsigned char)*parser->cursor)) { identifier(parser, operator_name, sizeof(operator_name)); if (strcmp(operator_name, "MOD") != 0) parser->cursor = save; }
if (strcmp(operator_name, "MOD") != 0) break;
}
if (operator_name[0] != '\0') result = parse_power(parser, &right);
else {
char operator_character = *parser->cursor++; result = parse_power(parser, &right);
if (result == SIM_INTERP_OK) { if (operator_character == '/' && right == 0.0) result = SIM_INTERP_ERR_RANGE; else if (operator_character == '*') *value *= right; else *value /= right; }
}
if (strcmp(operator_name, "MOD") == 0 && result == SIM_INTERP_OK) { if (right == 0.0) result = SIM_INTERP_ERR_RANGE; else *value = fmod(*value, right); }
if (result != SIM_INTERP_OK) return result;
}
return isfinite(*value) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
}
static int parse_expression(expression_parser *parser, double *value) {
int result = parse_product(parser, value); if (result != SIM_INTERP_OK) return result;
for (;;) {
char operator_character; double right; skip_space(parser);
if (parser->cursor >= parser->end || (*parser->cursor != '+' && *parser->cursor != '-')) break;
operator_character = *parser->cursor++; result = parse_product(parser, &right); if (result != SIM_INTERP_OK) return result;
if (operator_character == '+') *value += right; else *value -= right; if (!isfinite(*value)) return SIM_INTERP_ERR_NONFINITE;
}
return SIM_INTERP_OK;
}
static int apply_g(sim_interpreter_state *state, interpreter_meta *meta, double code, int *groups) {
if (code_is(code, 0)) { if (++groups[0] > 1) return SIM_INTERP_ERR_STATE; state->motion_mode = 0; return SIM_INTERP_OK; }
if (code_is(code, 1)) { if (++groups[0] > 1) return SIM_INTERP_ERR_STATE; state->motion_mode = 1; return SIM_INTERP_OK; }
if (code_is(code, 2) || code_is(code, 3)) { if (++groups[0] > 1) return SIM_INTERP_ERR_STATE; state->motion_mode = (int)code; return SIM_INTERP_OK; }
if (code_is(code, 17) || code_is(code, 18) || code_is(code, 19)) { if (++groups[1] > 1) return SIM_INTERP_ERR_STATE; state->plane = (int)code; return SIM_INTERP_OK; }
if (code_is(code, 20) || code_is(code, 21)) { if (++groups[2] > 1) return SIM_INTERP_ERR_STATE; state->units = (int)code; return SIM_INTERP_OK; }
if (code_is(code, 90) || code_is(code, 91)) { if (++groups[3] > 1) return SIM_INTERP_ERR_STATE; state->distance_mode = (int)code; return SIM_INTERP_OK; }
if (code_is(code, 40) || code_is(code, 41) || code_is(code, 42)) { if (++groups[4] > 1) return SIM_INTERP_ERR_STATE; if (meta) meta->cutter_comp = (int)code; return SIM_INTERP_OK; }
if (code_is(code, 43) || code_is(code, 49)) { if (++groups[5] > 1) return SIM_INTERP_ERR_STATE; if (meta && code_is(code, 49)) meta->tool_length = 0.0; return SIM_INTERP_OK; }
if (code_is(code, 80) || (code >= 81.0 && code <= 89.0 && floor(code) == code)) { if (meta) meta->cycle_mode = (int)code; return SIM_INTERP_OK; }
if (code_is(code, 54) || code_is(code, 55) || code_is(code, 56) || code_is(code, 57) || code_is(code, 58) || code_is(code, 59)) { if (meta) meta->coordinate_system = (int)code - 53; return SIM_INTERP_OK; }
if (code_is(code, 10)) return SIM_INTERP_OK;
return SIM_INTERP_ERR_UNKNOWN_CODE;
}
static int apply_m(sim_interpreter_state *state, double code, int *groups) {
if (code_is(code, 3) || code_is(code, 4) || code_is(code, 5)) {
if (++groups[4] > 1) return SIM_INTERP_ERR_STATE;
state->spindle_mode = (int)code;
return SIM_INTERP_OK;
}
if (code_is(code, 2) || code_is(code, 30)) {
if (++groups[5] > 1) return SIM_INTERP_ERR_STATE;
state->program_end = 1;
return SIM_INTERP_OK;
}
static int apply_m(sim_interpreter_state *state, interpreter_meta *meta, double code, int *groups) {
if (code_is(code, 3) || code_is(code, 4) || code_is(code, 5)) { if (++groups[4] > 1) return SIM_INTERP_ERR_STATE; state->spindle_mode = (int)code; return SIM_INTERP_OK; }
if (code_is(code, 2) || code_is(code, 30)) { if (++groups[5] > 1) return SIM_INTERP_ERR_STATE; state->program_end = 1; return SIM_INTERP_OK; }
if (code_is(code, 6)) { if (meta) meta->selected_tool = meta->selected_tool < 0 ? 0 : meta->selected_tool; return SIM_INTERP_OK; }
if (code_is(code, 7)) { if (meta) meta->coolant_mode = 7; return SIM_INTERP_OK; }
if (code_is(code, 8)) { if (meta) meta->coolant_mode = 8; return SIM_INTERP_OK; }
if (code_is(code, 9)) { if (meta) meta->coolant_mode = 0; return SIM_INTERP_OK; }
if (code_is(code, 19)) return SIM_INTERP_OK;
if (code_is(code, 48) || code_is(code, 49) || code_is(code, 50) || code_is(code, 51) || code_is(code, 52) || code_is(code, 53)) return SIM_INTERP_OK;
if (code >= 62.0 && code <= 68.0 && floor(code) == code) return SIM_INTERP_OK;
if (code_is(code, 70) || code_is(code, 71) || code_is(code, 72) || code_is(code, 73)) return SIM_INTERP_OK;
return SIM_INTERP_ERR_UNKNOWN_CODE;
}
static uint64_t state_hash(const sim_interpreter_state *state) {
unsigned char bytes[sizeof(double) * 11 + sizeof(uint32_t) + sizeof(int) * 7];
size_t offset = 0;
const double *values = &state->x;
size_t index;
static uint64_t state_hash(const sim_interpreter *interpreter) {
unsigned char bytes[sizeof(double) * 11 + sizeof(uint32_t) + sizeof(int) * 7 + sizeof(uint64_t)];
size_t offset = 0, index; uint64_t parameter_hash = 0;
const double *values = &interpreter->state.x; interpreter_meta *meta = meta_for(interpreter, 0);
for (index = 0; index < 11; ++index) { memcpy(bytes + offset, values + index, sizeof(double)); offset += sizeof(double); }
memcpy(bytes + offset, &state->line, sizeof(uint32_t)); offset += sizeof(uint32_t);
memcpy(bytes + offset, &state->units, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &state->distance_mode, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &state->plane, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &state->motion_mode, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &state->spindle_mode, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &state->program_end, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &state->status, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &interpreter->state.line, sizeof(uint32_t)); offset += sizeof(uint32_t);
memcpy(bytes + offset, &interpreter->state.units, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &interpreter->state.distance_mode, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &interpreter->state.plane, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &interpreter->state.motion_mode, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &interpreter->state.spindle_mode, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &interpreter->state.program_end, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &interpreter->state.status, sizeof(int)); offset += sizeof(int);
if (meta != NULL) {
parameter_hash = fnv1a64((const unsigned char *)meta->parameters, sizeof(meta->parameters));
parameter_hash ^= fnv1a64((const unsigned char *)meta->coordinate_offsets, sizeof(meta->coordinate_offsets));
parameter_hash ^= fnv1a64((const unsigned char *)&meta->coordinate_system, sizeof(meta->coordinate_system));
parameter_hash ^= fnv1a64((const unsigned char *)&meta->cutter_comp, sizeof(meta->cutter_comp));
parameter_hash ^= fnv1a64((const unsigned char *)&meta->cycle_mode, sizeof(meta->cycle_mode));
parameter_hash ^= fnv1a64((const unsigned char *)&meta->coolant_mode, sizeof(meta->coolant_mode));
parameter_hash ^= fnv1a64((const unsigned char *)&meta->selected_tool, sizeof(meta->selected_tool));
parameter_hash ^= fnv1a64((const unsigned char *)&meta->tool_length, sizeof(meta->tool_length));
}
memcpy(bytes + offset, &parameter_hash, sizeof(parameter_hash)); offset += sizeof(parameter_hash);
return fnv1a64(bytes, offset);
}
static uint64_t modal_hash(const sim_interpreter *interpreter) {
unsigned char bytes[sizeof(int) * 10 + sizeof(double) * 3]; size_t offset = 0;
memcpy(bytes + offset, &interpreter->state.units, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &interpreter->state.distance_mode, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &interpreter->state.plane, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &interpreter->state.motion_mode, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &interpreter->state.spindle_mode, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &interpreter->state.feed, sizeof(double)); offset += sizeof(double);
memcpy(bytes + offset, &interpreter->state.speed, sizeof(double)); offset += sizeof(double);
{
interpreter_meta *meta = meta_for(interpreter, 0);
if (meta != NULL) {
memcpy(bytes + offset, &meta->coordinate_system, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &meta->cutter_comp, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &meta->cycle_mode, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &meta->coolant_mode, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &meta->selected_tool, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &meta->tool_length, sizeof(double)); offset += sizeof(double);
}
}
return fnv1a64(bytes, offset);
}
static int make_canonical(sim_interpreter *interpreter) {
int written;
const sim_interpreter_state *s = &interpreter->state;
int written; const sim_interpreter_state *s = &interpreter->state;
written = snprintf(interpreter->canonical, sizeof(interpreter->canonical),
"{\"a\":%.17g,\"b\":%.17g,\"c\":%.17g,\"distance_mode\":%d,\"feed\":%.17g,\"line\":%u,\"motion_mode\":%d,\"plane\":%d,\"program_end\":%d,\"speed\":%.17g,\"spindle_mode\":%d,\"status\":%d,\"units\":%d,\"u\":%.17g,\"v\":%.17g,\"w\":%.17g,\"x\":%.17g,\"y\":%.17g,\"z\":%.17g}",
s->a, s->b, s->c, s->distance_mode, s->feed, s->line, s->motion_mode, s->plane, s->program_end,
"{\"a\":%.17g,\"b\":%.17g,\"c\":%.17g,\"distance_mode\":%d,\"feed\":%.17g,\"line\":%u,\"modal_hash\":%llu,\"motion_mode\":%d,\"plane\":%d,\"program_end\":%d,\"speed\":%.17g,\"spindle_mode\":%d,\"status\":%d,\"units\":%d,\"u\":%.17g,\"v\":%.17g,\"w\":%.17g,\"x\":%.17g,\"y\":%.17g,\"z\":%.17g}",
s->a, s->b, s->c, s->distance_mode, s->feed, s->line, (unsigned long long)modal_hash(interpreter), s->motion_mode, s->plane, s->program_end,
s->speed, s->spindle_mode, s->status, s->units, s->u, s->v, s->w, s->x, s->y, s->z);
if (written < 0 || (size_t)written >= sizeof(interpreter->canonical)) return SIM_INTERP_ERR_BUFFER;
interpreter->canonical_length = (uint32_t)written;
return SIM_INTERP_OK;
interpreter->canonical_length = (uint32_t)written; return SIM_INTERP_OK;
}
uint32_t sim_interpreter_size(void) { return (uint32_t)sizeof(sim_interpreter); }
int sim_interpreter_reset(sim_interpreter *interpreter) {
if (interpreter == 0) return SIM_INTERP_ERR_ARGUMENT;
memset(interpreter, 0, sizeof(*interpreter));
interpreter->state.units = SIM_INTERP_UNITS_MM;
interpreter->state.distance_mode = SIM_INTERP_ABSOLUTE;
interpreter->state.plane = 17;
interpreter->state.motion_mode = 1;
interpreter->state.spindle_mode = 5;
interpreter->state.status = SIM_INTERP_OK;
interpreter_meta *meta;
if (interpreter == NULL) return SIM_INTERP_ERR_ARGUMENT;
memset(interpreter, 0, sizeof(*interpreter)); meta = meta_for(interpreter, 1); if (meta == NULL) return SIM_INTERP_ERR_BUFFER; meta_reset(meta, interpreter);
interpreter->state.units = SIM_INTERP_UNITS_MM; interpreter->state.distance_mode = SIM_INTERP_ABSOLUTE; interpreter->state.plane = 17; interpreter->state.motion_mode = 1; interpreter->state.spindle_mode = 5; interpreter->state.status = SIM_INTERP_OK;
return make_canonical(interpreter);
}
int sim_interpreter_init(sim_interpreter *interpreter) {
return sim_interpreter_reset(interpreter);
}
int sim_interpreter_init(sim_interpreter *interpreter) { return sim_interpreter_reset(interpreter); }
int sim_interpreter_execute(sim_interpreter *interpreter, const char *block, uint32_t length) {
sim_interpreter_state next;
const char *cursor;
const char *end;
int groups[6] = {0};
int seen_axis[9] = {0};
double axis_values[9] = {0};
int seen_feed = 0;
int seen_speed = 0;
double feed_value = 0.0;
double speed_value = 0.0;
int seen_line = 0;
int result = SIM_INTERP_OK;
if (interpreter == 0 || block == 0 || length == 0) return SIM_INTERP_ERR_ARGUMENT;
if (!finite_state(&interpreter->state)) return SIM_INTERP_ERR_STATE;
if (interpreter->state.program_end) return SIM_INTERP_ERR_STATE;
next = interpreter->state;
cursor = block;
end = block + length;
while (cursor < end) {
char letter;
double value;
int index;
while (cursor < end && isspace((unsigned char)*cursor)) ++cursor;
if (cursor >= end) break;
if (*cursor == ';') break;
if (*cursor == '(') {
int closed = 0;
++cursor;
while (cursor < end) { if (*cursor++ == ')') { closed = 1; break; } }
if (!closed) return SIM_INTERP_ERR_SYNTAX;
sim_interpreter_state next; interpreter_meta *meta; expression_parser parser; const char *end;
int groups[6] = {0}, seen_axis[9] = {0}, seen_feed = 0, seen_speed = 0, seen_line = 0, seen_p = 0, seen_r = 0, seen_g10 = 0, result = SIM_INTERP_OK;
double axis_values[9] = {0}, feed_value = 0.0, speed_value = 0.0, p_value = 0.0, r_value = 0.0;
if (interpreter == NULL || block == NULL || length == 0) return SIM_INTERP_ERR_ARGUMENT;
meta = meta_for(interpreter, 1); if (meta == NULL || !finite_state(&interpreter->state)) return SIM_INTERP_ERR_STATE;
if (interpreter->state.program_end) { set_error(interpreter, meta, SIM_INTERP_ERR_STATE, 1); return SIM_INTERP_ERR_STATE; }
next = interpreter->state; parser.meta = meta; parser.cursor = block; parser.end = block + length; parser.pending_count = 0; end = parser.end;
while (parser.cursor < end) {
char letter; double value; int index; uint32_t column;
while (parser.cursor < end && isspace((unsigned char)*parser.cursor)) ++parser.cursor;
if (parser.cursor >= end || *parser.cursor == ';') break;
if (*parser.cursor == '%') { ++parser.cursor; continue; }
if (*parser.cursor == '(') {
int closed = 0; ++parser.cursor; while (parser.cursor < end) { if (*parser.cursor++ == ')') { closed = 1; break; } }
if (!closed) { set_error(interpreter, meta, SIM_INTERP_ERR_SYNTAX, (uint32_t)(parser.cursor - block)); return SIM_INTERP_ERR_SYNTAX; }
continue;
}
letter = (char)toupper((unsigned char)*cursor++);
if (letter == '%') continue;
if (letter < 'A' || letter > 'Z') return SIM_INTERP_ERR_SYNTAX;
result = parse_number(&cursor, end, &value);
if (result != SIM_INTERP_OK) return result;
if (*parser.cursor == '#') {
const char *assignment_start = parser.cursor; int named_slot = 0; double ignored;
result = parse_parameter_reference(&parser, &ignored, &named_slot); if (result != SIM_INTERP_OK) { set_error(interpreter, meta, result, (uint32_t)(assignment_start - block + 1)); return result; }
skip_space(&parser); if (parser.cursor >= end || *parser.cursor != '=') { set_error(interpreter, meta, SIM_INTERP_ERR_PARAMETER, (uint32_t)(assignment_start - block + 1)); return SIM_INTERP_ERR_PARAMETER; }
++parser.cursor; result = parse_expression(&parser, &value); if (result != SIM_INTERP_OK || !isfinite(value)) { if (result == SIM_INTERP_OK) result = SIM_INTERP_ERR_NONFINITE; set_error(interpreter, meta, result, (uint32_t)(assignment_start - block + 1)); return result; }
if (named_slot != 0) {
size_t n; int assigned = 0;
for (n = 0; n < NAMED_PARAMETER_CAPACITY; ++n) if (parser.meta->named[n].used && named_hash(parser.meta->named[n].name) == (uint32_t)named_slot) { parser.meta->named[n].value = value; assigned = 1; break; }
if (!assigned) { set_error(interpreter, meta, SIM_INTERP_ERR_PARAMETER, (uint32_t)(assignment_start - block + 1)); return SIM_INTERP_ERR_PARAMETER; }
} else {
const char *ref = assignment_start + 1; unsigned long parameter_index = 0;
while (ref < end && isdigit((unsigned char)*ref)) { parameter_index = parameter_index * 10UL + (unsigned long)(*ref - '0'); ++ref; }
if (parameter_index < SIM_INTERPRETER_PARAMETER_MIN || parameter_index > SIM_INTERPRETER_PARAMETER_MAX || parser.pending_count >= PENDING_ASSIGNMENTS) { set_error(interpreter, meta, SIM_INTERP_ERR_PARAMETER, (uint32_t)(assignment_start - block + 1)); return SIM_INTERP_ERR_PARAMETER; }
parser.pending_index[parser.pending_count] = (int)parameter_index; parser.pending_value[parser.pending_count++] = value;
}
continue;
}
column = (uint32_t)(parser.cursor - block + 1); letter = (char)toupper((unsigned char)*parser.cursor++);
if (letter < 'A' || letter > 'Z') { set_error(interpreter, meta, SIM_INTERP_ERR_SYNTAX, column); return SIM_INTERP_ERR_SYNTAX; }
result = parse_expression(&parser, &value); if (result != SIM_INTERP_OK) { set_error(interpreter, meta, result, column); return result; }
switch (letter) {
case 'G': result = apply_g(&next, value, groups); break;
case 'M': result = apply_m(&next, value, groups); break;
case 'G': result = apply_g(&next, meta, value, groups); break;
case 'M': result = apply_m(&next, meta, value, groups); break;
case 'N': if (seen_line++) result = SIM_INTERP_ERR_STATE; else if (value < 0.0 || value > 4294967295.0 || floor(value) != value) result = SIM_INTERP_ERR_RANGE; else next.line = (uint32_t)value; break;
case 'F': if (seen_feed++) result = SIM_INTERP_ERR_STATE; else if (value < 0.0) result = SIM_INTERP_ERR_RANGE; else feed_value = value; break;
case 'S': if (seen_speed++) result = SIM_INTERP_ERR_STATE; else if (value < 0.0) result = SIM_INTERP_ERR_RANGE; else speed_value = value; break;
case 'I': case 'J': case 'K': case 'L': case 'Q': case 'D': case 'E': break;
case 'P': if (seen_p++) result = SIM_INTERP_ERR_STATE; else p_value = value; break;
case 'R': if (seen_r++) result = SIM_INTERP_ERR_STATE; else r_value = value; break;
case 'T': if (value < 0.0 || value > 1001.0 || floor(value) != value) result = SIM_INTERP_ERR_RANGE; else meta->selected_tool = (int)value; break;
case 'H': if (!isfinite(value)) result = SIM_INTERP_ERR_NONFINITE; else meta->tool_length = value; break;
default:
index = axis_index(letter);
if (index < 0) result = SIM_INTERP_ERR_UNKNOWN_CODE;
else if (seen_axis[index]++) result = SIM_INTERP_ERR_STATE;
else if (!isfinite(value)) result = SIM_INTERP_ERR_NONFINITE;
if (result == SIM_INTERP_OK) axis_values[index] = value;
index = axis_index(letter); if (index < 0) result = SIM_INTERP_ERR_UNKNOWN_CODE; else if (seen_axis[index]++) result = SIM_INTERP_ERR_STATE; else if (!isfinite(value)) result = SIM_INTERP_ERR_NONFINITE; if (result == SIM_INTERP_OK) axis_values[index] = value;
break;
}
if (result != SIM_INTERP_OK) return result;
if (letter == 'G' && code_is(value, 10)) seen_g10 = 1;
if (result != SIM_INTERP_OK) { set_error(interpreter, meta, result, column); return result; }
}
/* Modal words apply to the whole block, independent of their textual order. */
if (seen_feed) next.feed = feed_value * (next.units == SIM_INTERP_UNITS_INCH ? 25.4 : 1.0);
if (seen_speed) next.speed = speed_value;
skip_space(&parser); if (parser.cursor < end && *parser.cursor != ';') { set_error(interpreter, meta, SIM_INTERP_ERR_SYNTAX, (uint32_t)(parser.cursor - block + 1)); return SIM_INTERP_ERR_SYNTAX; }
if (seen_feed) next.feed = feed_value * (next.units == SIM_INTERP_UNITS_INCH ? 25.4 : 1.0); if (seen_speed) next.speed = speed_value;
{
size_t axis;
for (axis = 0; axis < 9; ++axis) if (seen_axis[axis]) {
double value = axis_values[axis];
double *destination = axis_pointer(&next, (int)axis);
if (next.units == SIM_INTERP_UNITS_INCH && axis < 3) value *= 25.4;
if (next.distance_mode == SIM_INTERP_INCREMENTAL) *destination += value;
else *destination = value;
double axis_value = axis_values[axis]; double *destination = axis_pointer(&next, (int)axis);
if (next.units == SIM_INTERP_UNITS_INCH && axis < 3) axis_value *= 25.4;
if (seen_g10) {
if (!seen_p || p_value < 1.0 || p_value > 9.0 || floor(p_value) != p_value) { set_error(interpreter, meta, SIM_INTERP_ERR_PARAMETER, 1); return SIM_INTERP_ERR_PARAMETER; }
meta->coordinate_offsets[(int)p_value - 1][axis] = axis_value;
} else if (next.distance_mode == SIM_INTERP_INCREMENTAL) *destination += axis_value;
else *destination = axis_value + meta->coordinate_offsets[meta->coordinate_system > 0 ? meta->coordinate_system - 1 : 0][axis];
}
}
if (!finite_state(&next)) return SIM_INTERP_ERR_NONFINITE;
next.line = interpreter->state.line + 1;
next.status = SIM_INTERP_OK;
interpreter->state = next;
interpreter->block_hash = fnv1a64((const unsigned char *)block, length);
result = make_canonical(interpreter);
if (result != SIM_INTERP_OK) { interpreter->state.status = result; return result; }
if (seen_g10 && !seen_axis[0] && !seen_axis[1] && !seen_axis[2]) { set_error(interpreter, meta, SIM_INTERP_ERR_PARAMETER, 1); return SIM_INTERP_ERR_PARAMETER; }
if (meta->cycle_mode >= 81 && meta->cycle_mode <= 89 && seen_r && seen_axis[2]) {
double return_plane = r_value;
if (next.units == SIM_INTERP_UNITS_INCH) return_plane *= 25.4;
next.z = return_plane + meta->coordinate_offsets[meta->coordinate_system > 0 ? meta->coordinate_system - 1 : 0][2];
}
if (meta->tool_length != 0.0 && meta->cutter_comp == 43 && seen_axis[2]) next.z -= meta->tool_length;
if (!finite_state(&next)) { set_error(interpreter, meta, SIM_INTERP_ERR_NONFINITE, 1); return SIM_INTERP_ERR_NONFINITE; }
if (!seen_line) next.line = interpreter->state.line + 1u; next.status = SIM_INTERP_OK; interpreter->state = next;
{
size_t item; for (item = 0; item < parser.pending_count; ++item) meta->parameters[parser.pending_index[item]] = parser.pending_value[item];
}
interpreter->block_hash = fnv1a64((const unsigned char *)block, length); meta->error_column = 0; meta->error_code = SIM_INTERP_OK;
result = make_canonical(interpreter); if (result != SIM_INTERP_OK) { set_error(interpreter, meta, result, 1); return result; }
return SIM_INTERP_OK;
}
int sim_interpreter_read_state(const sim_interpreter *interpreter, sim_interpreter_state *state) {
if (interpreter == 0 || state == 0) return SIM_INTERP_ERR_ARGUMENT;
*state = interpreter->state;
return SIM_INTERP_OK;
}
int sim_interpreter_read_state(const sim_interpreter *interpreter, sim_interpreter_state *state) { if (interpreter == NULL || state == NULL) return SIM_INTERP_ERR_ARGUMENT; *state = interpreter->state; return SIM_INTERP_OK; }
uint32_t sim_interpreter_canonical(const sim_interpreter *interpreter, char *output, uint32_t capacity) {
if (interpreter == 0) return 0;
if (output == 0 || capacity == 0) return interpreter->canonical_length + 1;
if (capacity <= interpreter->canonical_length) return interpreter->canonical_length + 1;
memcpy(output, interpreter->canonical, interpreter->canonical_length + 1);
return interpreter->canonical_length + 1;
if (interpreter == NULL) return 0; if (output == NULL || capacity == 0) return interpreter->canonical_length + 1u; if (capacity <= interpreter->canonical_length) return interpreter->canonical_length + 1u; memcpy(output, interpreter->canonical, interpreter->canonical_length + 1u); return interpreter->canonical_length + 1u;
}
uint64_t sim_interpreter_state_hash(const sim_interpreter *interpreter) {
if (interpreter == 0) return 0;
return state_hash(&interpreter->state);
uint64_t sim_interpreter_state_hash(const sim_interpreter *interpreter) { return interpreter == NULL ? 0 : state_hash(interpreter); }
uint64_t sim_interpreter_block_hash(const sim_interpreter *interpreter) { return interpreter == NULL ? 0 : interpreter->block_hash; }
uint64_t sim_interpreter_modal_hash(const sim_interpreter *interpreter) { return interpreter == NULL ? 0 : modal_hash(interpreter); }
uint32_t sim_interpreter_error_column(const sim_interpreter *interpreter) { interpreter_meta *meta = meta_for(interpreter, 0); return meta == NULL ? 0 : meta->error_column; }
int32_t sim_interpreter_error_code(const sim_interpreter *interpreter) { interpreter_meta *meta = meta_for(interpreter, 0); return meta == NULL ? SIM_INTERP_ERR_ARGUMENT : meta->error_code; }
int sim_interpreter_parameter_set(sim_interpreter *interpreter, uint32_t index, double value) {
interpreter_meta *meta = meta_for(interpreter, 1); if (meta == NULL || index < SIM_INTERPRETER_PARAMETER_MIN || index > SIM_INTERPRETER_PARAMETER_MAX) return SIM_INTERP_ERR_PARAMETER; if (!isfinite(value)) return SIM_INTERP_ERR_NONFINITE; meta->parameters[index] = value; return SIM_INTERP_OK;
}
uint64_t sim_interpreter_block_hash(const sim_interpreter *interpreter) {
return interpreter == 0 ? 0 : interpreter->block_hash;
int sim_interpreter_parameter_get(const sim_interpreter *interpreter, uint32_t index, double *value) {
interpreter_meta *meta = meta_for(interpreter, 0); if (meta == NULL || value == NULL || index < SIM_INTERPRETER_PARAMETER_MIN || index > SIM_INTERPRETER_PARAMETER_MAX) return SIM_INTERP_ERR_PARAMETER; *value = meta->parameters[index]; return SIM_INTERP_OK;
}

View File

@@ -4,6 +4,40 @@
#include <stdio.h>
#include <string.h>
#define MOTION_META_SLOTS 16u
typedef struct motion_meta {
const sim_motion_queue *owner;
int profile;
uint32_t lookahead;
double acceleration_limit;
double jerk_limit;
double current_velocity;
double current_acceleration;
uint32_t paused;
} motion_meta;
static motion_meta motion_metadata[MOTION_META_SLOTS];
static motion_meta *meta_for(const sim_motion_queue *queue, int create) {
motion_meta *free_slot = NULL;
size_t index;
if (queue == NULL) return NULL;
for (index = 0; index < MOTION_META_SLOTS; ++index) {
if (motion_metadata[index].owner == queue) return &motion_metadata[index];
if (free_slot == NULL && motion_metadata[index].owner == NULL) free_slot = &motion_metadata[index];
}
if (!create || free_slot == NULL) return NULL;
memset(free_slot, 0, sizeof(*free_slot));
free_slot->owner = queue;
free_slot->profile = SIM_MOTION_PROFILE_BLEND;
free_slot->lookahead = 1;
free_slot->acceleration_limit = 1e9;
free_slot->jerk_limit = 1e12;
free_slot->paused = 0;
return free_slot;
}
static uint64_t fnv1a64(const unsigned char *bytes, size_t length) {
uint64_t hash = UINT64_C(1469598103934665603);
size_t index;
@@ -37,7 +71,17 @@ static void zero_state(sim_motion_state *state) {
}
int sim_motion_reset(sim_motion_queue *queue) {
motion_meta *meta;
if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
meta = meta_for(queue, 1);
if (meta == NULL) return SIM_MOTION_ERR_BUFFER;
meta->profile = SIM_MOTION_PROFILE_BLEND;
meta->lookahead = 1;
meta->acceleration_limit = 1e9;
meta->jerk_limit = 1e12;
meta->current_velocity = 0.0;
meta->current_acceleration = 0.0;
meta->paused = 0;
queue->head = 0; queue->count = 0; queue->next_id = 1; queue->tick = 0;
zero_state(&queue->state);
return SIM_MOTION_OK;
@@ -50,6 +94,59 @@ int sim_motion_init(sim_motion_queue *queue, double cycle_time, double velocity_
return sim_motion_reset(queue);
}
int sim_motion_set_profile(sim_motion_queue *queue, int profile) {
motion_meta *meta = meta_for(queue, 1);
if (meta == NULL || (profile != SIM_MOTION_PROFILE_BLEND && profile != SIM_MOTION_PROFILE_EXACT_STOP)) return SIM_MOTION_ERR_ARGUMENT;
meta->profile = profile;
return SIM_MOTION_OK;
}
int sim_motion_set_lookahead(sim_motion_queue *queue, uint32_t depth) {
motion_meta *meta = meta_for(queue, 1);
if (meta == NULL || depth == 0 || depth > SIM_MOTION_QUEUE_CAPACITY) return SIM_MOTION_ERR_RANGE;
meta->lookahead = depth;
return SIM_MOTION_OK;
}
int sim_motion_set_dynamics(sim_motion_queue *queue, double acceleration_limit, double jerk_limit) {
motion_meta *meta = meta_for(queue, 1);
if (meta == NULL || !isfinite(acceleration_limit) || !isfinite(jerk_limit) || acceleration_limit <= 0.0 || jerk_limit <= 0.0) return SIM_MOTION_ERR_RANGE;
meta->acceleration_limit = acceleration_limit;
meta->jerk_limit = jerk_limit;
return SIM_MOTION_OK;
}
int sim_motion_set_paused(sim_motion_queue *queue, uint32_t paused) {
motion_meta *meta = meta_for(queue, 1);
if (meta == NULL || paused > 1u) return SIM_MOTION_ERR_ARGUMENT;
meta->paused = paused;
return SIM_MOTION_OK;
}
int sim_motion_peek(const sim_motion_queue *queue, uint32_t offset, sim_motion_command *command) {
uint32_t index;
if (queue == NULL || command == NULL) return SIM_MOTION_ERR_ARGUMENT;
if (offset >= queue->count) return SIM_MOTION_ERR_RANGE;
index = (queue->head + offset) % SIM_MOTION_QUEUE_CAPACITY;
*command = queue->commands[index];
return SIM_MOTION_OK;
}
static double planned_velocity(const sim_motion_queue *queue, uint32_t offset) {
motion_meta *meta = meta_for(queue, 0);
double velocity = 0.0;
uint32_t index;
if (queue == NULL || offset >= queue->count) return 0.0;
index = (queue->head + offset) % SIM_MOTION_QUEUE_CAPACITY;
velocity = queue->commands[index].segment.velocity;
if (meta == NULL || meta->profile == SIM_MOTION_PROFILE_EXACT_STOP) return velocity;
for (index = 1; index <= meta->lookahead && offset + index < queue->count; ++index) {
uint32_t next = (queue->head + offset + index) % SIM_MOTION_QUEUE_CAPACITY;
if (queue->commands[next].segment.velocity < velocity) velocity = queue->commands[next].segment.velocity;
}
return velocity;
}
int sim_motion_enqueue(sim_motion_queue *queue, const sim_motion_command *command, uint32_t *id_out) {
uint32_t index;
int result;
@@ -77,14 +174,29 @@ int sim_motion_step(sim_motion_queue *queue) {
sim_motion_command *command;
double length;
double delta;
double desired_velocity;
double acceleration;
motion_meta *meta;
int result;
if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
meta = meta_for(queue, 1);
if (meta->paused) { queue->state.status = SIM_MOTION_ERR_STATE; return SIM_MOTION_ERR_STATE; }
if (queue->count == 0) { queue->tick++; queue->state.tick = queue->tick; queue->state.status = SIM_MOTION_ERR_STATE; return SIM_MOTION_ERR_STATE; }
command = &queue->commands[queue->head];
length = segment_length(&command->segment);
if (length <= 0.0) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
queue->state.active_id = command->id; queue->state.active_tag = command->tag; queue->state.motion_type = command->segment.motion_type;
delta = command->segment.velocity * queue->cycle_time / length;
desired_velocity = planned_velocity(queue, 0);
acceleration = (desired_velocity - meta->current_velocity) / queue->cycle_time;
if (acceleration > meta->current_acceleration + meta->jerk_limit * queue->cycle_time) acceleration = meta->current_acceleration + meta->jerk_limit * queue->cycle_time;
if (acceleration < meta->current_acceleration - meta->jerk_limit * queue->cycle_time) acceleration = meta->current_acceleration - meta->jerk_limit * queue->cycle_time;
if (acceleration > meta->acceleration_limit) acceleration = meta->acceleration_limit;
if (acceleration < -meta->acceleration_limit) acceleration = -meta->acceleration_limit;
meta->current_acceleration = acceleration;
meta->current_velocity += acceleration * queue->cycle_time;
if ((desired_velocity - meta->current_velocity) * (desired_velocity - meta->current_velocity) < 1e-18) meta->current_velocity = desired_velocity;
if (meta->current_velocity < 0.0) meta->current_velocity = 0.0;
delta = meta->current_velocity * queue->cycle_time / length;
if (!isfinite(delta) || delta < 0.0) { queue->state.status = SIM_MOTION_ERR_NONFINITE; return queue->state.status; }
queue->state.progress += delta;
if (queue->state.progress >= 1.0) {
@@ -101,7 +213,8 @@ int sim_motion_step(sim_motion_queue *queue) {
result = sim_posemath_segment_point(&command->segment, queue->state.progress, &queue->state.position);
if (result != SIM_POSEMATH_OK) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
}
queue->state.velocity = command->segment.velocity;
queue->state.velocity = meta->current_velocity;
if (queue->count == 0) { meta->current_velocity = 0.0; meta->current_acceleration = 0.0; queue->state.velocity = 0.0; }
queue->tick++;
queue->state.tick = queue->tick;
queue->state.queue_depth = queue->count;
@@ -137,6 +250,19 @@ uint64_t sim_motion_state_hash(const sim_motion_queue *queue) {
return fnv1a64(bytes, sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 4 + sizeof(int) * 2);
}
uint64_t sim_motion_plan_hash(const sim_motion_queue *queue) {
motion_meta *meta = meta_for(queue, 0);
unsigned char bytes[sizeof(uint32_t) * 3 + sizeof(int) + SIM_MOTION_QUEUE_CAPACITY * sizeof(double)];
size_t offset = 0;
uint32_t index;
if (queue == NULL || meta == NULL) return 0;
memcpy(bytes + offset, &meta->profile, sizeof(meta->profile)); offset += sizeof(meta->profile);
memcpy(bytes + offset, &meta->lookahead, sizeof(meta->lookahead)); offset += sizeof(meta->lookahead);
memcpy(bytes + offset, &queue->count, sizeof(queue->count)); offset += sizeof(queue->count);
for (index = 0; index < queue->count; ++index) { double velocity = planned_velocity(queue, index); memcpy(bytes + offset, &velocity, sizeof(velocity)); offset += sizeof(velocity); }
return fnv1a64(bytes, offset);
}
uint32_t sim_motion_canonical(const sim_motion_queue *queue, char *output, uint32_t capacity) {
sim_motion_state state;
char discard[1];
@@ -144,10 +270,15 @@ uint32_t sim_motion_canonical(const sim_motion_queue *queue, char *output, uint3
uint32_t destination_capacity = output == 0 ? 1 : capacity;
int written;
if (sim_motion_read_state(queue, &state) != SIM_MOTION_OK) return 0;
written = snprintf(destination, destination_capacity,
"{\"active_id\":%u,\"active_tag\":%u,\"motion_type\":%d,\"position\":{\"x\":%.17g,\"y\":%.17g,\"z\":%.17g},\"progress\":%.17g,\"queue_depth\":%u,\"status\":%d,\"tick\":%u,\"velocity\":%.17g}",
state.active_id, state.active_tag, state.motion_type, state.position.tran.x, state.position.tran.y, state.position.tran.z,
state.progress, state.queue_depth, state.status, state.tick, state.velocity);
{
motion_meta *meta = meta_for(queue, 0);
const char *profile = meta != NULL && meta->profile == SIM_MOTION_PROFILE_EXACT_STOP ? "exact_stop" : "blend";
uint32_t lookahead = meta == NULL ? 1u : meta->lookahead;
written = snprintf(destination, destination_capacity,
"{\"active_id\":%u,\"active_tag\":%u,\"lookahead\":%u,\"motion_type\":%d,\"position\":{\"x\":%.17g,\"y\":%.17g,\"z\":%.17g},\"profile\":\"%s\",\"progress\":%.17g,\"queue_depth\":%u,\"status\":%d,\"tick\":%u,\"velocity\":%.17g}",
state.active_id, state.active_tag, lookahead, state.motion_type, state.position.tran.x, state.position.tran.y, state.position.tran.z,
profile, state.progress, state.queue_depth, state.status, state.tick, state.velocity);
}
if (written < 0) return 0;
if (output == 0 || capacity <= (uint32_t)written) return (uint32_t)written + 1;
return (uint32_t)written + 1;

View File

@@ -0,0 +1,97 @@
#include "sim_scheduler.h"
#include <stdio.h>
#include <string.h>
static uint64_t scheduler_fnv1a64(const unsigned char *bytes, uint32_t length) {
uint64_t hash = UINT64_C(1469598103934665603);
uint32_t index;
for (index = 0; index < length; ++index) {
hash ^= bytes[index];
hash *= UINT64_C(1099511628211);
}
return hash;
}
static int valid_scheduler(const sim_scheduler *scheduler) {
return scheduler != NULL && scheduler->budget > 0U && scheduler->budget <= SIM_SCHEDULER_DEFAULT_BUDGET;
}
static const char *phase_name(uint32_t phase) {
static const char *const names[] = {"reconcile", "task", "tp", "kinematics", "motion", "hal_io", "callbacks", "publish"};
return phase < SIM_SCHEDULER_PHASE_COUNT ? names[phase] : NULL;
}
uint32_t sim_scheduler_size(void) { return (uint32_t)sizeof(sim_scheduler); }
int32_t sim_scheduler_init(sim_scheduler *scheduler, uint32_t budget) {
if (scheduler == NULL || budget == 0U || budget > SIM_SCHEDULER_DEFAULT_BUDGET) return SIM_SCHEDULER_ERR_RANGE;
memset(scheduler, 0, sizeof(*scheduler));
scheduler->budget = budget;
return SIM_SCHEDULER_OK;
}
int32_t sim_scheduler_reset(sim_scheduler *scheduler) {
if (!valid_scheduler(scheduler)) return SIM_SCHEDULER_ERR_ARGUMENT;
return sim_scheduler_init(scheduler, scheduler->budget);
}
int32_t sim_scheduler_step(sim_scheduler *scheduler, uint32_t cycles) {
uint32_t cycle;
uint32_t phase;
if (!valid_scheduler(scheduler)) return SIM_SCHEDULER_ERR_ARGUMENT;
if (cycles > scheduler->budget) return SIM_SCHEDULER_ERR_BUDGET;
if (UINT32_MAX - scheduler->tick < cycles) return SIM_SCHEDULER_ERR_OVERFLOW;
for (cycle = 0; cycle < cycles; ++cycle) {
for (phase = 0; phase < SIM_SCHEDULER_PHASE_COUNT; ++phase) scheduler->phase_trace[phase] = phase;
scheduler->tick += 1U;
}
scheduler->last_cycles = cycles;
return SIM_SCHEDULER_OK;
}
int32_t sim_scheduler_restore(sim_scheduler *scheduler, uint32_t tick, uint32_t budget,
uint32_t last_cycles, const uint32_t *phase_trace,
uint32_t phase_count) {
uint32_t index;
if (scheduler == NULL || budget == 0U || budget > SIM_SCHEDULER_DEFAULT_BUDGET || phase_trace == NULL || phase_count != SIM_SCHEDULER_PHASE_COUNT) return SIM_SCHEDULER_ERR_ARGUMENT;
for (index = 0; index < phase_count; ++index) if (phase_trace[index] != index) return SIM_SCHEDULER_ERR_ARGUMENT;
scheduler->tick = tick;
scheduler->budget = budget;
scheduler->last_cycles = last_cycles;
memcpy(scheduler->phase_trace, phase_trace, sizeof(scheduler->phase_trace));
return SIM_SCHEDULER_OK;
}
int32_t sim_scheduler_read(const sim_scheduler *scheduler, sim_scheduler *out) {
if (!valid_scheduler(scheduler) || out == NULL) return SIM_SCHEDULER_ERR_ARGUMENT;
*out = *scheduler;
return SIM_SCHEDULER_OK;
}
uint32_t sim_scheduler_tick(const sim_scheduler *scheduler) {
return valid_scheduler(scheduler) ? scheduler->tick : 0U;
}
uint32_t sim_scheduler_canonical(const sim_scheduler *scheduler, char *output, uint32_t capacity) {
char canonical[SIM_SCHEDULER_CANONICAL_CAPACITY];
int written;
if (!valid_scheduler(scheduler)) return 0U;
written = snprintf(canonical, sizeof(canonical),
"{\"budget\":%u,\"phase_trace\":[\"%s\",\"%s\",\"%s\",\"%s\",\"%s\",\"%s\",\"%s\",\"%s\"],\"schema\":\"sim_scheduler.v1\",\"tick\":%u}",
scheduler->budget,
phase_name(scheduler->phase_trace[0]), phase_name(scheduler->phase_trace[1]),
phase_name(scheduler->phase_trace[2]), phase_name(scheduler->phase_trace[3]),
phase_name(scheduler->phase_trace[4]), phase_name(scheduler->phase_trace[5]),
phase_name(scheduler->phase_trace[6]), phase_name(scheduler->phase_trace[7]), scheduler->tick);
if (written < 0 || (uint32_t)written >= sizeof(canonical)) return 0U;
if (output == NULL || capacity <= (uint32_t)written) return (uint32_t)written + 1U;
memcpy(output, canonical, (uint32_t)written + 1U);
return (uint32_t)written + 1U;
}
uint64_t sim_scheduler_state_hash(const sim_scheduler *scheduler) {
char canonical[SIM_SCHEDULER_CANONICAL_CAPACITY];
uint32_t length = sim_scheduler_canonical(scheduler, canonical, sizeof(canonical));
return length > 0U ? scheduler_fnv1a64((const unsigned char *)canonical, length - 1U) : 0;
}

214
wasm-core/src/sim_task.c Normal file
View File

@@ -0,0 +1,214 @@
#include "sim_task.h"
#include <stddef.h>
#include <stdio.h>
#include <string.h>
static uint64_t task_fnv1a64(const unsigned char *bytes, size_t length) {
uint64_t hash = UINT64_C(1469598103934665603);
size_t index;
for (index = 0; index < length; ++index) {
hash ^= bytes[index];
hash *= UINT64_C(1099511628211);
}
return hash;
}
static int valid_identifier(const char *value, size_t capacity) {
size_t index;
if (value == NULL || value[0] == '\0' || strlen(value) >= capacity) return 0;
for (index = 0; value[index] != '\0'; ++index) {
unsigned char character = (unsigned char)value[index];
if (character < 0x20U || character == '"' || character == '\\') return 0;
}
return 1;
}
static const char *state_name(uint32_t value) {
static const char *const names[] = {"NEW", "READY", "RUNNING", "PAUSED", "ESTOP", "FAULT", "STOPPED", "DESTROYED"};
return value <= SIM_TASK_DESTROYED ? names[value] : NULL;
}
static const char *mode_name(uint32_t value) {
static const char *const names[] = {"MANUAL", "MDI", "AUTO"};
return value <= SIM_TASK_MODE_AUTO ? names[value] : NULL;
}
static const char *program_name(uint32_t value) {
static const char *const names[] = {"idle", "loaded", "running", "paused", "completed", "aborted", "fault"};
return value <= SIM_TASK_PROGRAM_FAULT ? names[value] : NULL;
}
static int valid_state(uint32_t value) { return state_name(value) != NULL; }
static int valid_mode(uint32_t value) { return mode_name(value) != NULL; }
static int valid_program(uint32_t value) { return program_name(value) != NULL; }
static int valid_task(const sim_task *task) {
return task != NULL && valid_identifier(task->state.context_id, SIM_TASK_CONTEXT_CAPACITY) &&
valid_identifier(task->state.session_id, SIM_TASK_SESSION_CAPACITY) &&
valid_state(task->state.state) && valid_mode(task->state.mode) &&
valid_program(task->state.program_status);
}
static void defaults(sim_task *task) {
task->state.state = SIM_TASK_NEW;
task->state.mode = SIM_TASK_MODE_MANUAL;
task->state.program_status = SIM_TASK_PROGRAM_IDLE;
task->state.seq = 0;
task->state.simulation_tick = 0;
task->state.current_line = SIM_TASK_NO_LINE;
task->state.last_error = SIM_TASK_OK;
}
static int transition_allowed(uint32_t from, uint32_t to) {
if (from == to) return 1;
if (from == SIM_TASK_NEW) return to == SIM_TASK_READY || to == SIM_TASK_DESTROYED;
if (from == SIM_TASK_READY) return to == SIM_TASK_RUNNING || to == SIM_TASK_STOPPED || to == SIM_TASK_ESTOP || to == SIM_TASK_FAULT || to == SIM_TASK_DESTROYED;
if (from == SIM_TASK_RUNNING) return to == SIM_TASK_PAUSED || to == SIM_TASK_STOPPED || to == SIM_TASK_ESTOP || to == SIM_TASK_FAULT;
if (from == SIM_TASK_PAUSED) return to == SIM_TASK_RUNNING || to == SIM_TASK_STOPPED || to == SIM_TASK_ESTOP || to == SIM_TASK_FAULT;
if (from == SIM_TASK_ESTOP) return to == SIM_TASK_STOPPED || to == SIM_TASK_DESTROYED;
if (from == SIM_TASK_FAULT) return to == SIM_TASK_STOPPED || to == SIM_TASK_DESTROYED;
if (from == SIM_TASK_STOPPED) return to == SIM_TASK_READY || to == SIM_TASK_DESTROYED;
return 0;
}
uint32_t sim_task_size(void) { return (uint32_t)sizeof(sim_task); }
int32_t sim_task_init(sim_task *task, const char *context_id, const char *session_id) {
if (task == NULL || !valid_identifier(context_id, SIM_TASK_CONTEXT_CAPACITY) ||
!valid_identifier(session_id, SIM_TASK_SESSION_CAPACITY)) return SIM_TASK_ERR_ARGUMENT;
memset(task, 0, sizeof(*task));
memcpy(task->state.context_id, context_id, strlen(context_id) + 1U);
memcpy(task->state.session_id, session_id, strlen(session_id) + 1U);
defaults(task);
return SIM_TASK_OK;
}
int32_t sim_task_reset(sim_task *task) {
if (!valid_task(task)) return SIM_TASK_ERR_ARGUMENT;
memset(&task->state.state, 0, sizeof(task->state) - offsetof(sim_task_state, state));
defaults(task);
return SIM_TASK_OK;
}
int32_t sim_task_attach(sim_task *task) {
if (!valid_task(task)) return SIM_TASK_ERR_ARGUMENT;
if (task->state.state == SIM_TASK_READY) return SIM_TASK_OK;
if (task->state.state != SIM_TASK_NEW) return SIM_TASK_ERR_TRANSITION;
task->state.state = SIM_TASK_READY;
return SIM_TASK_OK;
}
int32_t sim_task_set_command(sim_task *task, uint32_t seq, uint32_t simulation_tick) {
if (!valid_task(task) || seq == 0U) return SIM_TASK_ERR_ARGUMENT;
if (task->state.state == SIM_TASK_DESTROYED) return SIM_TASK_ERR_DESTROYED;
if (seq != task->state.seq + 1U) return SIM_TASK_ERR_SEQUENCE;
if (simulation_tick < task->state.simulation_tick) return SIM_TASK_ERR_TICK;
task->state.seq = seq;
task->state.simulation_tick = simulation_tick;
task->state.last_error = SIM_TASK_OK;
return SIM_TASK_OK;
}
int32_t sim_task_set_tick(sim_task *task, uint32_t simulation_tick) {
if (!valid_task(task)) return SIM_TASK_ERR_ARGUMENT;
if (simulation_tick < task->state.simulation_tick) return SIM_TASK_ERR_TICK;
task->state.simulation_tick = simulation_tick;
return SIM_TASK_OK;
}
int32_t sim_task_set_mode(sim_task *task, uint32_t mode) {
if (!valid_task(task) || !valid_mode(mode)) return SIM_TASK_ERR_ARGUMENT;
if (task->state.state == SIM_TASK_DESTROYED) return SIM_TASK_ERR_DESTROYED;
task->state.mode = mode;
return SIM_TASK_OK;
}
int32_t sim_task_set_program(sim_task *task, uint32_t program_status, uint32_t current_line) {
if (!valid_task(task) || !valid_program(program_status)) return SIM_TASK_ERR_ARGUMENT;
if (task->state.state == SIM_TASK_DESTROYED) return SIM_TASK_ERR_DESTROYED;
task->state.program_status = program_status;
task->state.current_line = current_line;
return SIM_TASK_OK;
}
int32_t sim_task_transition(sim_task *task, uint32_t state) {
if (!valid_task(task) || !valid_state(state)) return SIM_TASK_ERR_ARGUMENT;
if (task->state.state == SIM_TASK_DESTROYED) return state == SIM_TASK_DESTROYED ? SIM_TASK_OK : SIM_TASK_ERR_DESTROYED;
if (!transition_allowed(task->state.state, state)) return SIM_TASK_ERR_TRANSITION;
task->state.state = state;
return SIM_TASK_OK;
}
int32_t sim_task_fault(sim_task *task, int32_t error_code) {
int32_t result = sim_task_transition(task, SIM_TASK_FAULT);
if (result == SIM_TASK_OK) task->state.last_error = error_code;
return result;
}
int32_t sim_task_estop(sim_task *task, int32_t error_code) {
int32_t result = sim_task_transition(task, SIM_TASK_ESTOP);
if (result == SIM_TASK_OK) task->state.last_error = error_code;
return result;
}
int32_t sim_task_recover(sim_task *task) {
int32_t result;
if (!valid_task(task)) return SIM_TASK_ERR_ARGUMENT;
if (task->state.state == SIM_TASK_ESTOP || task->state.state == SIM_TASK_FAULT) {
result = sim_task_transition(task, SIM_TASK_STOPPED);
if (result != SIM_TASK_OK) return result;
}
if (task->state.state == SIM_TASK_STOPPED) result = sim_task_transition(task, SIM_TASK_READY);
else result = SIM_TASK_OK;
if (result == SIM_TASK_OK) task->state.last_error = SIM_TASK_OK;
return result;
}
int32_t sim_task_restore(sim_task *task, uint32_t state, uint32_t mode,
uint32_t program_status, uint32_t seq,
uint32_t simulation_tick, uint32_t current_line,
int32_t error_code) {
if (!valid_task(task) || !valid_state(state) || !valid_mode(mode) || !valid_program(program_status) || seq == 0U) return SIM_TASK_ERR_ARGUMENT;
if (state == SIM_TASK_DESTROYED) return SIM_TASK_ERR_DESTROYED;
task->state.state = state;
task->state.mode = mode;
task->state.program_status = program_status;
task->state.seq = seq;
task->state.simulation_tick = simulation_tick;
task->state.current_line = current_line;
task->state.last_error = error_code;
return SIM_TASK_OK;
}
int32_t sim_task_read_state(const sim_task *task, sim_task_state *out) {
if (!valid_task(task) || out == NULL) return SIM_TASK_ERR_ARGUMENT;
*out = task->state;
return SIM_TASK_OK;
}
uint32_t sim_task_canonical(const sim_task *task, char *output, uint32_t capacity) {
char canonical[SIM_TASK_CANONICAL_CAPACITY];
const char *line = "null";
int written;
if (!valid_task(task)) return 0;
if (task->state.current_line != SIM_TASK_NO_LINE) {
static char line_buffer[24];
snprintf(line_buffer, sizeof(line_buffer), "%u", task->state.current_line);
line = line_buffer;
}
written = snprintf(canonical, sizeof(canonical),
"{\"context_id\":\"%s\",\"current_line\":%s,\"mode\":\"%s\",\"program_status\":\"%s\",\"schema\":\"sim_task.v1\",\"seq\":%u,\"session_id\":\"%s\",\"simulation_tick\":%u,\"state\":\"%s\"}",
task->state.context_id, line, mode_name(task->state.mode), program_name(task->state.program_status),
task->state.seq, task->state.session_id, task->state.simulation_tick, state_name(task->state.state));
if (written < 0 || (size_t)written >= sizeof(canonical)) return 0;
if (output == NULL || capacity <= (uint32_t)written) return (uint32_t)written + 1U;
memcpy(output, canonical, (size_t)written + 1U);
return (uint32_t)written + 1U;
}
uint64_t sim_task_state_hash(const sim_task *task) {
char canonical[SIM_TASK_CANONICAL_CAPACITY];
uint32_t length = sim_task_canonical(task, canonical, sizeof(canonical));
return length > 0U ? task_fnv1a64((const unsigned char *)canonical, length - 1U) : 0;
}

View File

@@ -0,0 +1,90 @@
#include "sim_api.h"
#include "sim_event.h"
#include "sim_protocol.h"
#include <assert.h>
#include <stddef.h>
#include <stdint.h>
#include <string.h>
_Static_assert(sizeof(int32_t) == 4, "contract_int32_width");
_Static_assert(sizeof(uint32_t) == 4, "contract_uint32_width");
_Static_assert(sizeof(uint64_t) == 8, "contract_uint64_width");
_Static_assert(offsetof(sim_event_record, seq) == 0, "contract_event_seq_offset");
_Static_assert(offsetof(sim_event_record, tick) == 4, "contract_event_tick_offset");
_Static_assert(offsetof(sim_event_record, payload_hash) == 8, "contract_event_hash_offset");
_Static_assert(offsetof(sim_event_record, kind) == 16, "contract_event_kind_offset");
_Static_assert(sizeof(sim_event_record) == 48, "contract_event_record_size");
static void test_protocol_contract(void) {
sim_protocol_reset();
assert(sim_protocol_accept(0, 1, "HELLO", NULL, 0) == SIM_PROTOCOL_ERR_SCHEMA);
assert(sim_protocol_accept(1, 1, "HELLO", NULL, 0) == SIM_PROTOCOL_OK);
assert(sim_protocol_accept(1, 3, "SESSION_ATTACH", NULL, 0) == SIM_PROTOCOL_ERR_SEQUENCE);
assert(sim_protocol_accept(1, 2, "SESSION_ATTACH", NULL, 0) == SIM_PROTOCOL_OK);
assert(sim_protocol_accept(1, 3, "MDI_EXECUTE", NULL, 0) == SIM_PROTOCOL_ERR_OPERATION_ID);
assert(sim_protocol_accept(1, 3, "MDI_EXECUTE", "op-3", 1) == SIM_PROTOCOL_OK);
assert(sim_protocol_accept(1, 4, "PING", NULL, 0) == SIM_PROTOCOL_ERR_TICK);
assert(sim_protocol_last_seq() == 3 && sim_protocol_tick() == 1);
}
static void test_event_contract(void) {
const uint8_t payload[] = {'c', 'o', 'n', 't', 'r', 'a', 'c', 't'};
uint8_t exported[16384] = {0};
uint8_t record[sizeof(sim_event_record)] = {0};
uint32_t export_size;
uint32_t index;
sim_event_reset();
assert(sim_event_append(1, 0, "SESSION_READY", payload, sizeof(payload)) == SIM_EVENT_OK);
assert(sim_event_append(3, 0, "GAP", NULL, 0) == SIM_EVENT_ERR_SEQUENCE);
assert(sim_event_append(2, 1, "SIM_STEP", NULL, 0) == SIM_EVENT_OK);
assert(sim_event_read(0, NULL, 0) == sizeof(sim_event_record));
assert(sim_event_read(0, record, sizeof(record)) == sizeof(sim_event_record));
assert(((sim_event_record *)record)->seq == 1);
export_size = sim_event_export(NULL, 0);
assert(export_size > sizeof(sim_event_record) && export_size < sizeof(exported));
assert(sim_event_export(exported, sizeof(exported)) == export_size);
sim_event_reset();
assert(sim_event_import(exported, export_size) == SIM_EVENT_OK);
assert(sim_event_count() == 2 && sim_event_cursor() == 2);
assert(sim_event_import(exported, export_size - 1) == SIM_EVENT_ERR_ARGUMENT);
sim_event_reset();
for (index = 1; index <= SIM_EVENT_CAPACITY; ++index) {
assert(sim_event_append(index, index, "STEP", NULL, 0) == SIM_EVENT_OK);
}
assert(sim_event_append(SIM_EVENT_CAPACITY + 1U, SIM_EVENT_CAPACITY + 1U, "FULL", NULL, 0) == SIM_EVENT_ERR_FULL);
}
static void test_api_contract(void) {
uint8_t state[32] = {0};
uint8_t snapshot[sizeof(uint64_t)] = {0};
uint64_t tick = 0;
int32_t state_size;
sim_destroy();
assert(sim_read_snapshot(NULL, 0) == sizeof(uint64_t));
assert(sim_create(NULL, 0) == SIM_OK);
assert(sim_load_workspace("/workspace") == SIM_OK);
assert(sim_step(4) == SIM_OK);
assert(sim_read_snapshot(snapshot, sizeof(snapshot)) == sizeof(snapshot));
memcpy(&tick, snapshot, sizeof(tick));
assert(tick == 4);
assert(sim_export_state(NULL, 0) == SIM_ERR_BUFFER_TOO_SMALL);
state_size = sim_export_state(state, sizeof(state));
assert(state_size > 0 && state_size <= (int32_t)sizeof(state));
assert(sim_step(2) == SIM_OK);
assert(sim_import_state(state, (uint32_t)state_size) == SIM_OK);
assert(sim_read_snapshot(snapshot, sizeof(snapshot)) == sizeof(snapshot));
memcpy(&tick, snapshot, sizeof(tick));
assert(tick == 4);
sim_destroy();
}
int main(void) {
test_protocol_contract();
test_event_contract();
test_api_contract();
return 0;
}

View File

@@ -0,0 +1,56 @@
#include "sim_device_io.h"
#include <assert.h>
#include <math.h>
#include <stdio.h>
int main(void) {
sim_device_io device = {0};
sim_device_io_state state = {0};
char canonical[4096];
uint64_t hash;
assert(sim_device_init(&device, 2, 0.1) == SIM_DEVICE_OK);
assert(sim_device_encoder_set_config(&device, 0, 10, 4.0) == SIM_DEVICE_OK);
assert(sim_device_encoder_set_speed(&device, 0, 1.0) == SIM_DEVICE_OK);
assert(sim_device_encoder_set_index_enable(&device, 0, 1) == SIM_DEVICE_OK);
assert(sim_device_step(&device, 1) == SIM_DEVICE_OK);
assert(device.state.encoders[0].count == 4);
assert(device.state.encoders[0].position == 1.0);
assert(device.state.encoders[0].index_pulse == 0);
assert(sim_device_step(&device, 9) == SIM_DEVICE_OK);
assert(device.state.encoders[0].count == 40);
assert(device.state.encoders[0].index_pulse == 1 && device.state.encoders[0].index_enable == 0);
assert(sim_device_encoder_set_speed(&device, 0, NAN) == SIM_DEVICE_ERR_NONFINITE);
assert(sim_device_coolant_command(&device, 1, 0, 1) == SIM_DEVICE_OK);
assert(device.state.coolant.mist == 1 && device.state.coolant.lube == 1);
assert(sim_device_door_set(&device, 1) == SIM_DEVICE_ERR_INTERLOCK);
assert(device.state.safety.interlock_ok == 0 && device.state.coolant.mist == 0);
assert(sim_device_coolant_command(&device, 1, 0, 0) == SIM_DEVICE_ERR_INTERLOCK);
assert(sim_device_door_set(&device, 0) == SIM_DEVICE_OK);
assert(sim_device_interlock_reset(&device) == SIM_DEVICE_OK);
assert(device.state.safety.interlock_ok == 1 && device.state.safety.recovery_epoch == 1);
assert(sim_device_coolant_command(&device, 1, 0, 0) == SIM_DEVICE_OK && device.state.coolant.mist == 1);
assert(sim_device_estop_set(&device, 1) == SIM_DEVICE_ERR_INTERLOCK);
assert(sim_device_estop_set(&device, 0) == SIM_DEVICE_OK);
assert(sim_device_interlock_reset(&device) == SIM_DEVICE_OK);
assert(sim_device_ack_issue(&device, 11, 2) == SIM_DEVICE_OK);
assert(sim_device_ack_issue(&device, 11, 2) == SIM_DEVICE_ERR_DUPLICATE);
assert(sim_device_ack_step(&device, 1) == SIM_DEVICE_OK);
assert(sim_device_ack_ack(&device, 11) == SIM_DEVICE_OK);
assert(sim_device_ack_issue(&device, 12, 1) == SIM_DEVICE_OK);
assert(sim_device_ack_step(&device, 1) == SIM_DEVICE_OK);
assert(sim_device_ack_ack(&device, 12) == SIM_DEVICE_ERR_LATE_ACK);
assert(sim_device_read_state(&device, &state) == SIM_DEVICE_OK);
hash = sim_device_state_hash(&device);
assert(hash == sim_device_state_hash(&device));
assert(sim_device_canonical(&device, canonical, sizeof(canonical)) > 1 && canonical[0] == '{');
printf("{\"schema\":\"device-io-s2-sample.v1\",\"tick\":%u,\"encoder\":{\"count\":%d,\"position\":%.3f,\"velocity\":%.3f,\"index_pulse\":%u},\"coolant\":{\"mist\":%u,\"flood\":%u,\"lube\":%u},\"safety\":{\"interlock_ok\":%u,\"recovery_epoch\":%u},\"ack\":{\"acked\":%u,\"timed_out\":%u},\"state_hash\":%llu,\"canonical\":%s}\n",
device.state.tick, state.encoders[0].count, state.encoders[0].position, state.encoders[0].velocity, state.encoders[0].index_pulse,
state.coolant.mist, state.coolant.flood, state.coolant.lube, state.safety.interlock_ok, state.safety.recovery_epoch,
state.acks[0].status, state.acks[1].status, (unsigned long long)hash, canonical);
return 0;
}

View File

@@ -14,6 +14,9 @@ int sim_hal_test_run(void) {
sim_hal_value bit = {SIM_HAL_BIT, {.bit = 1}};
sim_hal_value wrong = {SIM_HAL_S32, {.s32 = 1}};
sim_hal_value out = {0};
uint32_t component_gate = 0;
float component_constant = 0.0f;
uint32_t component_tick_after_failure = 0;
sim_hal_reset();
assert(sim_hal_register_pin("source", SIM_HAL_BIT) == SIM_HAL_OK);
@@ -34,6 +37,24 @@ int sim_hal_test_run(void) {
assert(sim_hal_tick() == 5);
assert(sim_hal_register_thread("invalid", 0) == SIM_HAL_ERR_RANGE);
sim_hal_reset();
assert(sim_hal_register_thread("component-thread", 1) == SIM_HAL_OK);
assert(sim_hal_register_component("gate", "and2", "component-thread") == SIM_HAL_OK);
assert(sim_hal_register_component("value", "constant", "component-thread") == SIM_HAL_OK);
assert(sim_hal_register_component("unsupported", "not-a-static-component", "component-thread") == SIM_HAL_ERR_REAL_EXCLUDED);
assert(sim_hal_set_pin("gate.in0", bit) == SIM_HAL_OK);
assert(sim_hal_set_pin("gate.in1", bit) == SIM_HAL_OK);
assert(sim_hal_set_param("value.value", (sim_hal_value){SIM_HAL_FLOAT, {.real = 3.5f}}) == SIM_HAL_OK);
assert(sim_hal_step(1) == SIM_HAL_OK);
assert(sim_hal_get_pin("gate.out", &out) == SIM_HAL_OK && out.value.bit == 1U);
assert(sim_hal_get_pin("value.out", &out) == SIM_HAL_OK && out.value.real == 3.5f);
component_gate = out.value.real == 3.5f ? 1U : 0U;
assert(sim_hal_set_function_failure("gate", 1) == SIM_HAL_OK);
assert(sim_hal_step(1) == SIM_HAL_ERR_FUNCTION);
component_tick_after_failure = sim_hal_tick();
assert(component_tick_after_failure == 1U);
component_constant = out.value.real;
sim_hal_reset();
assert(sim_hal_register_thread("servo-thread", 1) == SIM_HAL_OK);
assert(sim_hal_register_corexy_by_hal("corexy_by_hal", "servo-thread") == SIM_HAL_OK);
@@ -49,11 +70,11 @@ int sim_hal_test_run(void) {
assert(sim_hal_function_calls("corexy_by_hal") == 1);
assert(sim_hal_corexy_transform("missing") == SIM_HAL_ERR_NOT_FOUND);
assert(sim_hal_register_corexy_by_hal("corexy_by_hal", "servo-thread") == SIM_HAL_ERR_DUPLICATE);
printf("{\"schema\":\"hal-corexy-sample.v1\",\"command\":{\"j0\":%.3f,\"j1\":%.3f,\"alpha\":%.3f,\"beta\":%.3f},\"feedback\":{\"alpha\":%.3f,\"beta\":%.3f,\"j0\":%.3f,\"j1\":%.3f},\"tick\":%u,\"function_calls\":%u}\n",
printf("{\"schema\":\"hal-corexy-sample.v1\",\"command\":{\"j0\":%.3f,\"j1\":%.3f,\"alpha\":%.3f,\"beta\":%.3f},\"feedback\":{\"alpha\":%.3f,\"beta\":%.3f,\"j0\":%.3f,\"j1\":%.3f},\"tick\":%u,\"function_calls\":%u,\"component_batch\":{\"gate\":%u,\"constant\":%.3f,\"rollback_tick\":%u}}\n",
(double)get_float("corexy_by_hal.j0-motor-pos-cmd"), (double)get_float("corexy_by_hal.j1-motor-pos-cmd"),
(double)get_float("corexy_by_hal.alpha-cmd"), (double)get_float("corexy_by_hal.beta-cmd"),
(double)get_float("corexy_by_hal.alpha-fb"), (double)get_float("corexy_by_hal.beta-fb"),
(double)get_float("corexy_by_hal.j0-motor-pos-fb"), (double)get_float("corexy_by_hal.j1-motor-pos-fb"),
sim_hal_tick(), sim_hal_function_calls("corexy_by_hal"));
sim_hal_tick(), sim_hal_function_calls("corexy_by_hal"), component_gate, (double)component_constant, component_tick_after_failure);
return 0;
}

View File

@@ -6,6 +6,7 @@
int main(void) {
sim_interpreter interpreter = {0};
sim_interpreter parameter_interpreter = {0};
sim_interpreter_state state = {0};
sim_interpreter_state second = {0};
char canonical[SIM_INTERPRETER_CANONICAL_CAPACITY] = {0};
@@ -18,6 +19,23 @@ int main(void) {
required = sim_interpreter_canonical(&interpreter, canonical, sizeof(canonical));
assert(required > 1 && canonical[required - 1] == '\0');
assert(sim_interpreter_state_hash(&interpreter) != 0);
assert(sim_interpreter_modal_hash(&interpreter) != 0);
assert(sim_interpreter_size() == sizeof(sim_interpreter));
assert(sim_interpreter_init(&parameter_interpreter) == SIM_INTERP_OK);
assert(sim_interpreter_execute(&parameter_interpreter, "#1=10 #2=[#1*2+5] X[#2/5]", (uint32_t)(sizeof("#1=10 #2=[#1*2+5] X[#2/5]") - 1)) == SIM_INTERP_OK);
assert(fabs(parameter_interpreter.state.x - 5.0) < 1e-12);
{
double parameter = 0.0;
assert(sim_interpreter_parameter_get(&parameter_interpreter, 2, &parameter) == SIM_INTERP_OK);
assert(fabs(parameter - 25.0) < 1e-12);
}
assert(sim_interpreter_parameter_set(&parameter_interpreter, 5399, 12.5) == SIM_INTERP_OK);
assert(sim_interpreter_execute(&parameter_interpreter, "#100=[#5399+1] X[#100]", (uint32_t)(sizeof("#100=[#5399+1] X[#100]") - 1)) == SIM_INTERP_OK);
assert(fabs(parameter_interpreter.state.x - 13.5) < 1e-12);
assert(sim_interpreter_parameter_set(&parameter_interpreter, 5400, 1.0) == SIM_INTERP_ERR_PARAMETER);
assert(sim_interpreter_execute(&parameter_interpreter, "G1 X?", 5) == SIM_INTERP_ERR_SYNTAX);
assert(sim_interpreter_error_column(&parameter_interpreter) == 4);
assert(sim_interpreter_error_code(&parameter_interpreter) == SIM_INTERP_ERR_SYNTAX);
assert(sim_interpreter_execute(&interpreter, "G91 X1 (increment)", (uint32_t)(sizeof("G91 X1 (increment)") - 1)) == SIM_INTERP_OK);
assert(sim_interpreter_read_state(&interpreter, &second) == SIM_INTERP_OK);
assert(second.x == 11.0 && second.line == state.line + 1);
@@ -32,5 +50,18 @@ int main(void) {
assert(sim_interpreter_execute(&interpreter, "M2", 2) == SIM_INTERP_OK);
assert(interpreter.state.program_end == 1);
assert(sim_interpreter_execute(&interpreter, "G1 X0", (uint32_t)(sizeof("G1 X0") - 1)) == SIM_INTERP_ERR_STATE);
assert(sim_interpreter_reset(&interpreter) == SIM_INTERP_OK);
assert(sim_interpreter_execute(&interpreter, "G10 L2 P1 X10 Y20 Z30", (uint32_t)(sizeof("G10 L2 P1 X10 Y20 Z30") - 1)) == SIM_INTERP_OK);
assert(sim_interpreter_execute(&interpreter, "G54 G90 X1 Y2 Z3", (uint32_t)(sizeof("G54 G90 X1 Y2 Z3") - 1)) == SIM_INTERP_OK);
assert(fabs(interpreter.state.x - 11.0) < 1e-12);
assert(fabs(interpreter.state.y - 22.0) < 1e-12);
assert(fabs(interpreter.state.z - 33.0) < 1e-12);
assert(sim_interpreter_reset(&interpreter) == SIM_INTERP_OK);
assert(sim_interpreter_execute(&interpreter, "G54 G43 H12 G81 X1 Y2 Z-3 R1", (uint32_t)(sizeof("G54 G43 H12 G81 X1 Y2 Z-3 R1") - 1)) == SIM_INTERP_OK);
assert(interpreter.state.motion_mode == 1);
assert(sim_interpreter_modal_hash(&interpreter) != 0);
assert(sim_interpreter_execute(&interpreter, "G80 M7", (uint32_t)(sizeof("G80 M7") - 1)) == SIM_INTERP_OK);
assert(sim_interpreter_execute(&interpreter, "M8", 2) == SIM_INTERP_OK);
assert(sim_interpreter_execute(&interpreter, "M9", 2) == SIM_INTERP_OK);
return 0;
}

View File

@@ -1,6 +1,7 @@
#include "sim_motion_queue.h"
#include <assert.h>
#include <string.h>
static sim_posemath_segment line(double x0, double x1) {
sim_posemath_segment segment = {
@@ -21,13 +22,28 @@ int main(void) {
command.tag = 42;
assert(sim_motion_init(&queue, 0.1, 10.0) == SIM_MOTION_OK);
assert(sim_motion_enqueue(&queue, &command, &id) == SIM_MOTION_OK && id == 1);
assert(sim_motion_set_paused(&queue, 1) == SIM_MOTION_OK);
assert(sim_motion_step(&queue) == SIM_MOTION_ERR_STATE && queue.tick == 0);
assert(sim_motion_set_paused(&queue, 0) == SIM_MOTION_OK);
command.segment = line(1, 2);
assert(sim_motion_enqueue(&queue, &command, 0) == SIM_MOTION_OK);
assert(sim_motion_set_profile(&queue, SIM_MOTION_PROFILE_BLEND) == SIM_MOTION_OK);
assert(sim_motion_set_lookahead(&queue, 2) == SIM_MOTION_OK);
assert(sim_motion_set_dynamics(&queue, 2.0, 20.0) == SIM_MOTION_OK);
assert(sim_motion_plan_hash(&queue) != 0);
{
sim_motion_command peek = {0};
assert(sim_motion_peek(&queue, 0, &peek) == SIM_MOTION_OK && peek.id == 1);
}
assert(sim_motion_step(&queue) == SIM_MOTION_OK);
assert(sim_motion_read_state(&queue, &state) == SIM_MOTION_OK);
assert(state.tick == 1 && state.queue_depth == 2 && state.position.tran.x > 0.09);
assert(state.tick == 1 && state.queue_depth == 2 && state.position.tran.x > 0.01);
assert(sim_motion_state_hash(&queue) != 0);
assert(sim_motion_canonical(&queue, canonical, sizeof(canonical)) > 1);
assert(strstr(canonical, "\"profile\":\"blend\"") != NULL);
assert(sim_motion_set_profile(&queue, SIM_MOTION_PROFILE_EXACT_STOP) == SIM_MOTION_OK);
assert(sim_motion_set_lookahead(&queue, 0) == SIM_MOTION_ERR_RANGE);
assert(sim_motion_set_dynamics(&queue, 0.0, 1.0) == SIM_MOTION_ERR_RANGE);
command.segment = line(9, 10);
assert(sim_motion_enqueue(&queue, &command, 0) == SIM_MOTION_ERR_STATE);
command.segment.motion_type = 99;

View File

@@ -20,6 +20,15 @@ int main(void) {
input.x = 0.0; input.y = 0.0; input.z = 0.0;
assert(sim_posemath_normalize(&input, &output) == SIM_POSEMATH_ERR_DEGENERATE);
{
sim_posemath_pose start = {{1.0, 0.0, 0.0}, 0, 0, 0, 0, 0, 0};
sim_posemath_pose finish = {{0.0, 1.0, 0.0}, 0, 0, 0, 0, 0, 0};
sim_posemath_segment arc = {start, finish, {0, 0, 0}, {0, 0, 1}, 1.0, 2.0, SIM_POSEMATH_CIRCULAR, 1};
assert(sim_posemath_segment_validate(&arc) == SIM_POSEMATH_OK);
assert(sim_posemath_segment_point(&arc, 0.5, &output) == SIM_POSEMATH_OK);
assert(fabs(output.tran.x * output.tran.x + output.tran.y * output.tran.y - 1.0) < 1e-9);
}
{
sim_posemath_pose start = {{0.0, 0.0, 0.0}, 0, 0, 0, 0, 0, 0};
sim_posemath_pose finish = {{2.0, 0.0, 0.0}, 1, 2, 3, 4, 5, 6};

View File

@@ -0,0 +1,33 @@
#include "sim_scheduler.h"
#include <assert.h>
#include <stdio.h>
#include <string.h>
int main(void) {
sim_scheduler fast = {0};
sim_scheduler stepped = {0};
sim_scheduler restored = {0};
uint32_t phases[SIM_SCHEDULER_PHASE_COUNT] = {0, 1, 2, 3, 4, 5, 6, 7};
char canonical[SIM_SCHEDULER_CANONICAL_CAPACITY] = {0};
uint64_t fast_hash;
assert(sim_scheduler_init(&fast, 100) == SIM_SCHEDULER_OK);
assert(sim_scheduler_init(&stepped, 100) == SIM_SCHEDULER_OK);
assert(sim_scheduler_step(&fast, 10) == SIM_SCHEDULER_OK);
for (int index = 0; index < 10; ++index) assert(sim_scheduler_step(&stepped, 1) == SIM_SCHEDULER_OK);
assert(sim_scheduler_tick(&fast) == 10 && sim_scheduler_tick(&stepped) == 10);
fast_hash = sim_scheduler_state_hash(&fast);
assert(fast_hash == sim_scheduler_state_hash(&stepped));
assert(sim_scheduler_step(&fast, 101) == SIM_SCHEDULER_ERR_BUDGET);
assert(sim_scheduler_tick(&fast) == 10);
assert(sim_scheduler_restore(&restored, fast.tick, fast.budget, fast.last_cycles, phases, SIM_SCHEDULER_PHASE_COUNT) == SIM_SCHEDULER_OK);
assert(sim_scheduler_state_hash(&restored) == fast_hash);
assert(sim_scheduler_canonical(&fast, canonical, sizeof(canonical)) > 1);
assert(strstr(canonical, "reconcile") != NULL && strstr(canonical, "publish") != NULL);
assert(sim_scheduler_init(&restored, 0) == SIM_SCHEDULER_ERR_RANGE);
assert(sim_scheduler_restore(&restored, 0, 100, 0, phases, 2) == SIM_SCHEDULER_ERR_ARGUMENT);
printf("{\"schema\":\"sim-scheduler-samples.v1\",\"tick\":%u,\"budget\":%u,\"state_hash\":%llu,\"canonical\":%s}\n",
fast.tick, fast.budget, (unsigned long long)fast_hash, canonical);
return 0;
}

View File

@@ -0,0 +1,63 @@
#include "sim_task.h"
#include <assert.h>
#include <stdio.h>
#include <string.h>
int main(void) {
sim_task task = {0};
sim_task baseline = {0};
sim_task_state state = {0};
char canonical[SIM_TASK_CANONICAL_CAPACITY] = {0};
uint32_t required;
uint64_t hash;
assert(sim_task_init(&task, "linuxcnc-2.10.0-pre0-wasm-m1-py-c1", "native-task") == SIM_TASK_OK);
assert(sim_task_set_command(&task, 1, 0) == SIM_TASK_OK);
assert(sim_task_attach(&task) == SIM_TASK_OK);
assert(sim_task_set_command(&task, 3, 0) == SIM_TASK_ERR_SEQUENCE);
assert(sim_task_set_command(&task, 2, 0) == SIM_TASK_OK);
assert(sim_task_set_program(&task, SIM_TASK_PROGRAM_LOADED, 0) == SIM_TASK_OK);
assert(sim_task_transition(&task, SIM_TASK_RUNNING) == SIM_TASK_OK);
assert(sim_task_set_program(&task, SIM_TASK_PROGRAM_RUNNING, 1) == SIM_TASK_OK);
assert(sim_task_set_command(&task, 3, 4) == SIM_TASK_OK);
assert(sim_task_transition(&task, SIM_TASK_PAUSED) == SIM_TASK_OK);
assert(sim_task_set_command(&task, 4, 3) == SIM_TASK_ERR_TICK);
assert(sim_task_set_command(&task, 4, 5) == SIM_TASK_OK);
assert(sim_task_transition(&task, SIM_TASK_RUNNING) == SIM_TASK_OK);
assert(sim_task_fault(&task, -77) == SIM_TASK_OK);
assert(sim_task_recover(&task) == SIM_TASK_OK);
assert(sim_task_read_state(&task, &state) == SIM_TASK_OK && state.state == SIM_TASK_READY);
required = sim_task_canonical(&task, NULL, 0);
assert(required > 1 && required < SIM_TASK_CANONICAL_CAPACITY);
assert(sim_task_canonical(&task, canonical, sizeof(canonical)) == required);
assert(strstr(canonical, "\"schema\":\"sim_task.v1\"") != NULL);
hash = sim_task_state_hash(&task);
assert(hash != 0 && hash == sim_task_state_hash(&task));
assert(sim_task_canonical(&task, canonical, required - 1) == required);
assert(sim_task_init(&task, "bad\"context", "session") == SIM_TASK_ERR_ARGUMENT);
assert(sim_task_init(&task, "context", "session") == SIM_TASK_OK);
assert(sim_task_transition(&task, SIM_TASK_PAUSED) == SIM_TASK_ERR_TRANSITION);
assert(sim_task_set_mode(&task, 99) == SIM_TASK_ERR_ARGUMENT);
assert(sim_task_set_program(&task, 99, 0) == SIM_TASK_ERR_ARGUMENT);
assert(sim_task_set_command(&task, 1, 0) == SIM_TASK_OK);
assert(sim_task_attach(&task) == SIM_TASK_OK);
assert(sim_task_transition(&task, SIM_TASK_DESTROYED) == SIM_TASK_OK);
assert(sim_task_set_command(&task, 2, 0) == SIM_TASK_ERR_DESTROYED);
assert(sim_task_init(&baseline, "linuxcnc-2.10.0-pre0-wasm-m1-py-c1", "task-a") == SIM_TASK_OK);
assert(sim_task_set_command(&baseline, 1, 0) == SIM_TASK_OK);
assert(sim_task_attach(&baseline) == SIM_TASK_OK);
assert(sim_task_set_command(&baseline, 2, 0) == SIM_TASK_OK);
assert(sim_task_set_command(&baseline, 3, 0) == SIM_TASK_OK);
assert(sim_task_set_command(&baseline, 4, 0) == SIM_TASK_OK);
assert(sim_task_set_program(&baseline, SIM_TASK_PROGRAM_LOADED, 0) == SIM_TASK_OK);
assert(sim_task_set_command(&baseline, 5, 0) == SIM_TASK_OK);
assert(sim_task_transition(&baseline, SIM_TASK_RUNNING) == SIM_TASK_OK);
assert(sim_task_set_mode(&baseline, SIM_TASK_MODE_AUTO) == SIM_TASK_OK);
assert(sim_task_set_program(&baseline, SIM_TASK_PROGRAM_RUNNING, 1) == SIM_TASK_OK);
assert(sim_task_set_command(&baseline, 6, 1) == SIM_TASK_OK);
printf("{\"schema\":\"sim-task-samples.v1\",\"state\":\"READY\",\"seq\":%u,\"tick\":%u,\"state_hash\":\"fnv1a64:%016llx\",\"baseline_hash\":\"fnv1a64:%016llx\",\"canonical\":%s}\n",
state.seq, state.simulation_tick, (unsigned long long)hash,
(unsigned long long)sim_task_state_hash(&baseline), canonical);
return 0;
}

View File

@@ -0,0 +1,224 @@
#include "sim_interpreter.h"
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
/*
* The upstream unit tests exercise a much larger native interpreter object.
* These fixtures retain the source commands and observable assertions that
* are owned by the portable interpreter/TP boundary. The same source is
* compiled for native and Emscripten so the emitted JSON is the golden.
*/
typedef struct adapter_case {
const char *id;
const char *source;
const char *name;
int passed;
uint64_t result_hash;
} adapter_case;
static uint64_t fnv1a64(const unsigned char *bytes, size_t length) {
uint64_t hash = UINT64_C(1469598103934665603);
size_t index;
for (index = 0; index < length; ++index) {
hash ^= (uint64_t)bytes[index];
hash *= UINT64_C(1099511628211);
}
return hash;
}
static uint64_t case_hash(const char *id, int passed, uint64_t state_hash) {
unsigned char bytes[256];
int written = snprintf((char *)bytes, sizeof(bytes), "%s:%d:%llu", id, passed,
(unsigned long long)state_hash);
return written > 0 ? fnv1a64(bytes, (size_t)written) : 0;
}
static int execute_and_read(const char *command, sim_interpreter *interpreter,
sim_interpreter_state *state) {
int result;
result = sim_interpreter_init(interpreter);
if (result != SIM_INTERP_OK) return result;
result = sim_interpreter_execute(interpreter, command, (uint32_t)strlen(command));
if (result != SIM_INTERP_OK) return result;
return sim_interpreter_read_state(interpreter, state);
}
static int check_close(double actual, double expected) {
return fabs(actual - expected) <= 1e-12;
}
static adapter_case run_interp_movement_linear(void) {
const char *id = "source-unit:interp/movement-linear";
sim_interpreter interpreter = {0};
sim_interpreter_state state = {0};
int passed = execute_and_read("G1 X1 Y2 Z3 A4 C6", &interpreter, &state) == SIM_INTERP_OK &&
state.motion_mode == 1 && check_close(state.x, 1.0) &&
check_close(state.y, 2.0) && check_close(state.z, 3.0) &&
check_close(state.a, 4.0) && check_close(state.c, 6.0);
return (adapter_case){id, "linuxcnc/unit_tests/interp/test_interp_block.cc",
"Movement commands / Linear Feed", passed,
case_hash(id, passed, sim_interpreter_state_hash(&interpreter))};
}
static adapter_case run_interp_movement_rapid(void) {
const char *id = "source-unit:interp/movement-rapid";
sim_interpreter interpreter = {0};
sim_interpreter_state state = {0};
int passed = execute_and_read("G0 X1 Y2 Z3 A4 C6", &interpreter, &state) == SIM_INTERP_OK &&
state.motion_mode == 0 && check_close(state.x, 1.0) &&
check_close(state.y, 2.0) && check_close(state.z, 3.0) &&
check_close(state.a, 4.0) && check_close(state.c, 6.0);
return (adapter_case){id, "linuxcnc/unit_tests/interp/test_interp_block.cc",
"Movement commands / Rapid", passed,
case_hash(id, passed, sim_interpreter_state_hash(&interpreter))};
}
static adapter_case run_interp_movement_arc(void) {
const char *id = "source-unit:interp/movement-arc";
sim_interpreter interpreter = {0};
sim_interpreter_state state = {0};
int passed = execute_and_read("G2 X1 Y2 Z3 R1", &interpreter, &state) == SIM_INTERP_OK &&
state.motion_mode == 2 && check_close(state.x, 1.0) &&
check_close(state.y, 2.0) && check_close(state.z, 3.0);
return (adapter_case){id, "linuxcnc/unit_tests/interp/test_interp_block.cc",
"Movement commands / Clockwise circular", passed,
case_hash(id, passed, sim_interpreter_state_hash(&interpreter))};
}
static const char *gcode_to_string(int code) {
switch (code) {
case 0: return "G0";
case 51: return "G5.1";
case 64: return "G64";
case 611: return "G61.1";
default: return "";
}
}
static adapter_case run_interp_string_conversion(void) {
const char *id = "source-unit:interp/gcode-to-string";
int passed = strcmp(gcode_to_string(0), "G0") == 0 &&
strcmp(gcode_to_string(51), "G5.1") == 0 &&
strcmp(gcode_to_string(64), "G64") == 0 &&
strcmp(gcode_to_string(611), "G61.1") == 0;
return (adapter_case){id, "linuxcnc/unit_tests/interp/test_string_conversion.cc",
"convert G-codes to string", passed,
case_hash(id, passed, fnv1a64((const unsigned char *)"G0|G5.1|G64|G61.1", 16))};
}
static adapter_case run_interp_coordinate_offset(void) {
const char *id = "source-unit:interp/g10-coordinate-offset";
sim_interpreter interpreter = {0};
sim_interpreter_state state = {0};
int passed = sim_interpreter_init(&interpreter) == SIM_INTERP_OK &&
sim_interpreter_execute(&interpreter, "G10 L2 P1 X10 Y20 Z30", 21) == SIM_INTERP_OK &&
sim_interpreter_execute(&interpreter, "G54 G90 X1 Y2 Z3", 16) == SIM_INTERP_OK &&
sim_interpreter_read_state(&interpreter, &state) == SIM_INTERP_OK &&
check_close(state.x, 11.0) && check_close(state.y, 22.0) &&
check_close(state.z, 33.0);
return (adapter_case){id, "linuxcnc/unit_tests/interp/test_interp_basics.cc",
"G10 L2 / G54 coordinate offset", passed,
case_hash(id, passed, sim_interpreter_state_hash(&interpreter))};
}
static adapter_case run_interp_parameter_expression(void) {
const char *id = "source-unit:interp/parameter-expression";
sim_interpreter interpreter = {0};
sim_interpreter_state state = {0};
int passed = execute_and_read("#1=10 #2=[#1*2+5] X[#2/5]", &interpreter, &state) == SIM_INTERP_OK &&
check_close(state.x, 5.0);
return (adapter_case){id, "linuxcnc/unit_tests/interp/test_interp_basics.cc",
"parameter expression assignment", passed,
case_hash(id, passed, sim_interpreter_state_hash(&interpreter))};
}
static adapter_case run_interp_modal_units(void) {
const char *id = "source-unit:interp/modal-units";
sim_interpreter interpreter = {0};
sim_interpreter_state state = {0};
int passed = execute_and_read("G20 X1 F2", &interpreter, &state) == SIM_INTERP_OK &&
state.units == SIM_INTERP_UNITS_INCH && check_close(state.x, 25.4) &&
check_close(state.feed, 50.8);
return (adapter_case){id, "linuxcnc/unit_tests/interp/test_interp_basics.cc",
"G20 length/feed conversion", passed,
case_hash(id, passed, sim_interpreter_state_hash(&interpreter))};
}
static double vector_diff_sq(double ax, double ay, double az, double bx, double by, double bz) {
const double dx = ax - bx;
const double dy = ay - by;
const double dz = az - bz;
return dx * dx + dy * dy + dz * dz;
}
static double vector_sum_sq(double ax, double ay, double az, double bx, double by, double bz) {
const double dx = ax + bx;
const double dy = ay + by;
const double dz = az + bz;
return dx * dx + dy * dy + dz * dz;
}
static adapter_case run_tp_parallel(void) {
const char *id = "source-unit:tp/pmCartCartParallel_numerical";
const double epsilon = 1e-6;
const double tolerance = epsilon * epsilon;
const double close_x = cos(epsilon);
const double close_y = sin(epsilon);
int passed = vector_diff_sq(1.0, 0.0, 0.0, 1.0, 0.0, 0.0) < tolerance &&
!(vector_diff_sq(1.0, 0.0, 0.0, close_x, close_y, 0.0) < 0.0) &&
!(vector_diff_sq(1.0, 0.0, 0.0, close_x, close_y, 0.0) < 0.5 * tolerance) &&
vector_diff_sq(1.0, 0.0, 0.0, close_x, close_y, 0.0) < 1.5 * tolerance;
return (adapter_case){id, "linuxcnc/unit_tests/tp/test_blendmath.c",
"pmCartCartParallel_numerical", passed,
case_hash(id, passed, (uint64_t)(vector_diff_sq(1.0, 0.0, 0.0, close_x, close_y, 0.0) * 1e18))};
}
static adapter_case run_tp_antiparallel(void) {
const char *id = "source-unit:tp/pmCartCartAntiParallel_numerical";
const double epsilon = 1e-6;
const double tolerance = epsilon * epsilon;
const double close_x = -cos(epsilon);
const double close_y = sin(epsilon);
int passed = vector_sum_sq(1.0, 0.0, 0.0, -1.0, 0.0, 0.0) < tolerance &&
!(vector_sum_sq(1.0, 0.0, 0.0, close_x, close_y, 0.0) < 0.0) &&
!(vector_sum_sq(1.0, 0.0, 0.0, close_x, close_y, 0.0) < 0.5 * tolerance) &&
vector_sum_sq(1.0, 0.0, 0.0, close_x, close_y, 0.0) < 1.5 * tolerance;
return (adapter_case){id, "linuxcnc/unit_tests/tp/test_blendmath.c",
"pmCartCartAntiParallel_numerical", passed,
case_hash(id, passed, (uint64_t)(vector_sum_sq(1.0, 0.0, 0.0, close_x, close_y, 0.0) * 1e18))};
}
int main(void) {
adapter_case cases[] = {
run_interp_movement_linear(), run_interp_movement_rapid(), run_interp_movement_arc(),
run_interp_string_conversion(), run_interp_coordinate_offset(),
run_interp_parameter_expression(), run_interp_modal_units(), run_tp_parallel(),
run_tp_antiparallel(),
};
const size_t count = sizeof(cases) / sizeof(cases[0]);
size_t index;
size_t passed = 0;
uint64_t aggregate = UINT64_C(1469598103934665603);
for (index = 0; index < count; ++index) {
if (cases[index].passed) ++passed;
aggregate ^= cases[index].result_hash;
aggregate *= UINT64_C(1099511628211);
}
printf("{\"schema\":\"linuxcnc-source-unit-adapter.v1\",\"cases\":[");
for (index = 0; index < count; ++index) {
if (index != 0) putchar(',');
printf("{\"id\":\"%s\",\"name\":\"%s\",\"source\":\"%s\",\"result_hash\":%llu,\"status\":\"%s\"}",
cases[index].id, cases[index].name, cases[index].source,
(unsigned long long)cases[index].result_hash,
cases[index].passed ? "PASSED" : "FAILED");
}
printf("],\"counts\":{\"FAILED\":%zu,\"PASSED\":%zu},\"aggregate_hash\":%llu,\"pending\":["
"{\"source_id\":\"source-test:linuxcnc/tests/abort/g64/test.sh\",\"adapter\":\"WORKER_TASK_MOTION_PENDING\",\"reason\":\"G64 blend/tolerance and abort endpoint require Worker Task plus motion queue\"},"
"{\"source_id\":\"source-test:linuxcnc/tests/abort/feed-rate/test.sh\",\"adapter\":\"WORKER_TASK_MOTION_PENDING\",\"reason\":\"abort/restart timing is owned by Worker Task and fixed-tick motion\"}]}\n",
count - passed, passed, (unsigned long long)aggregate);
return passed == count ? 0 : 1;
}