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

829 lines
42 KiB
C
Raw Blame History

#include "sim_hal.h"
#include <math.h>
#include <stddef.h>
#include <string.h>
typedef struct {
uint32_t used;
char name[SIM_HAL_NAME_CAPACITY];
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];
uint32_t period;
} thread_entry;
typedef struct {
uint32_t used;
char name[SIM_HAL_NAME_CAPACITY];
uint32_t thread;
uint32_t calls;
uint32_t fail;
uint32_t component_index;
uint32_t component_kind;
} function_entry;
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;
static int valid_name(const char *name) {
return name != NULL && name[0] != '\0' && strlen(name) < SIM_HAL_NAME_CAPACITY;
}
static int find_pin(const char *name) {
uint32_t index;
for (index = 0; index < SIM_HAL_PIN_CAPACITY; ++index) {
if (state.pins[index].used && strcmp(state.pins[index].name, name) == 0) return (int)index;
}
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) {
if (state.threads[index].used && strcmp(state.threads[index].name, name) == 0) return (int)index;
}
return -1;
}
static int find_function(const char *name) {
uint32_t index;
for (index = 0; index < SIM_HAL_FUNCTION_CAPACITY; ++index) {
if (state.functions[index].used && strcmp(state.functions[index].name, name) == 0) return (int)index;
}
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));
}
int32_t sim_hal_register_pin(const char *name, uint32_t type) {
uint32_t index;
if (!valid_name(name) || type < SIM_HAL_BIT || type > SIM_HAL_FLOAT) return SIM_HAL_ERR_ARGUMENT;
if (find_pin(name) >= 0) return SIM_HAL_ERR_DUPLICATE;
for (index = 0; index < SIM_HAL_PIN_CAPACITY; ++index) {
if (!state.pins[index].used) {
state.pins[index].used = 1;
strcpy(state.pins[index].name, name);
state.pins[index].value.type = type;
return SIM_HAL_OK;
}
}
return SIM_HAL_ERR_FULL;
}
int32_t sim_hal_set_pin(const char *name, sim_hal_value value) {
int index = find_pin(name);
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;
}
int32_t sim_hal_set_pin_scalar(const char *name, uint32_t type, double value) {
sim_hal_value scalar = {0};
scalar.type = type;
if (type == SIM_HAL_BIT) scalar.value.bit = value != 0.0;
else if (type == SIM_HAL_S32) scalar.value.s32 = (int32_t)value;
else if (type == SIM_HAL_U32) scalar.value.u32 = (uint32_t)value;
else if (type == SIM_HAL_FLOAT) scalar.value.real = (float)value;
else return SIM_HAL_ERR_TYPE;
return sim_hal_set_pin(name, scalar);
}
int32_t sim_hal_get_pin(const char *name, sim_hal_value *out) {
int index = find_pin(name);
if (index < 0 || out == NULL) return index < 0 ? SIM_HAL_ERR_NOT_FOUND : SIM_HAL_ERR_ARGUMENT;
*out = state.pins[index].value;
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_set_param_scalar(const char *name, uint32_t type, double value) {
sim_hal_value scalar = {0};
scalar.type = type;
if (type == SIM_HAL_BIT) scalar.value.bit = value != 0.0;
else if (type == SIM_HAL_S32) scalar.value.s32 = (int32_t)value;
else if (type == SIM_HAL_U32) scalar.value.u32 = (uint32_t)value;
else if (type == SIM_HAL_FLOAT) scalar.value.real = (float)value;
else return SIM_HAL_ERR_TYPE;
return sim_hal_set_param(name, scalar);
}
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;
}
int32_t sim_hal_register_thread(const char *name, uint32_t period_ticks) {
uint32_t index;
if (!valid_name(name) || period_ticks == 0U || period_ticks > 1000000U) return SIM_HAL_ERR_RANGE;
if (find_thread(name) >= 0) return SIM_HAL_ERR_DUPLICATE;
for (index = 0; index < SIM_HAL_THREAD_CAPACITY; ++index) {
if (!state.threads[index].used) {
state.threads[index].used = 1;
strcpy(state.threads[index].name, name);
state.threads[index].period = period_ticks;
return SIM_HAL_OK;
}
}
return SIM_HAL_ERR_FULL;
}
int32_t sim_hal_register_function(const char *thread, const char *name) {
uint32_t index;
int thread_index = find_thread(thread);
if (thread_index < 0) return SIM_HAL_ERR_NOT_FOUND;
if (!valid_name(name)) return SIM_HAL_ERR_ARGUMENT;
if (find_function(name) >= 0) return SIM_HAL_ERR_DUPLICATE;
for (index = 0; index < SIM_HAL_FUNCTION_CAPACITY; ++index) {
if (!state.functions[index].used) {
state.functions[index].used = 1;
strcpy(state.functions[index].name, name);
state.functions[index].thread = (uint32_t)thread_index;
return SIM_HAL_OK;
}
}
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",
".alpha-fb", ".beta-fb", ".j0-motor-pos-fb", ".j1-motor-pos-fb",
};
uint32_t index;
if (!valid_name(component)) return SIM_HAL_ERR_ARGUMENT;
for (index = 0; index < 8; ++index) {
size_t prefix = strlen(component);
size_t suffix = strlen(suffixes[index]);
if (prefix + suffix >= SIM_HAL_NAME_CAPACITY) return SIM_HAL_ERR_RANGE;
memcpy(names[index], component, prefix);
memcpy(names[index] + prefix, suffixes[index], suffix + 1);
}
return SIM_HAL_OK;
}
int32_t sim_hal_register_corexy_by_hal(const char *component, const char *thread) {
char names[8][SIM_HAL_NAME_CAPACITY];
uint32_t index;
int32_t result;
if (component == NULL || thread == NULL) return SIM_HAL_ERR_ARGUMENT;
result = corexy_pin_names(component, names);
if (result != SIM_HAL_OK) return result;
for (index = 0; index < 8; ++index) {
if (find_pin(names[index]) >= 0) return SIM_HAL_ERR_DUPLICATE;
}
if (find_function(component) >= 0) return SIM_HAL_ERR_DUPLICATE;
for (index = 0; index < 8; ++index) {
result = sim_hal_register_pin(names[index], SIM_HAL_FLOAT);
if (result != SIM_HAL_OK) return result;
}
result = sim_hal_register_function(thread, component);
if (result != SIM_HAL_OK) return result;
state.functions[find_function(component)].component_index = UINT32_MAX;
state.functions[find_function(component)].component_kind = SIM_HAL_COMPONENT_COREXY;
return SIM_HAL_OK;
}
static int32_t float_pin(const char *name, float *value) {
sim_hal_value pin = {0};
int32_t result = sim_hal_get_pin(name, &pin);
if (result != SIM_HAL_OK) return result;
if (pin.type != SIM_HAL_FLOAT) return SIM_HAL_ERR_TYPE;
*value = pin.value.real;
return SIM_HAL_OK;
}
static int32_t set_float_pin(const char *name, float value) {
sim_hal_value pin = {SIM_HAL_FLOAT, {.real = value}};
return sim_hal_set_pin(name, pin);
}
int32_t sim_hal_corexy_transform(const char *component) {
char names[8][SIM_HAL_NAME_CAPACITY];
float j0_cmd, j1_cmd, alpha_fb, beta_fb;
int32_t result = corexy_pin_names(component, names);
if (result != SIM_HAL_OK) return result;
result = float_pin(names[0], &j0_cmd); if (result != SIM_HAL_OK) return result;
result = float_pin(names[1], &j1_cmd); if (result != SIM_HAL_OK) return result;
result = float_pin(names[4], &alpha_fb); if (result != SIM_HAL_OK) return result;
result = float_pin(names[5], &beta_fb); if (result != SIM_HAL_OK) return result;
result = set_float_pin(names[2], 0.5f * (j0_cmd + j1_cmd)); if (result != SIM_HAL_OK) return result;
result = set_float_pin(names[3], 0.5f * (j0_cmd - j1_cmd)); if (result != SIM_HAL_OK) return result;
result = set_float_pin(names[6], alpha_fb + beta_fb); if (result != SIM_HAL_OK) return result;
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;
state.functions[index].fail = fail ? 1U : 0U;
return SIM_HAL_OK;
}
int32_t sim_hal_step(uint32_t ticks) {
hal_state before;
uint64_t target;
uint64_t tick;
uint32_t function_index;
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) { 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 = (uint32_t)target;
return SIM_HAL_OK;
}
uint32_t sim_hal_tick(void) { return state.tick; }
uint32_t sim_hal_function_calls(const char *name) { int index = find_function(name); return index < 0 ? 0U : state.functions[index].calls; }
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; }
Reference in New Issue View Git Blame Copy Permalink