接入 task HAL Web 仿真运行时
This commit is contained in:
968
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_hal_runtime.cpp
Normal file
968
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_hal_runtime.cpp
Normal file
@@ -0,0 +1,968 @@
|
||||
#include "linuxcnc_hal_runtime.hh"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstdarg>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <map>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace {
|
||||
|
||||
struct HalEntry {
|
||||
std::string name;
|
||||
hal_type_t type = HAL_TYPE_UNINITIALIZED;
|
||||
hal_pin_dir_t dir = HAL_DIR_UNSPECIFIED;
|
||||
hal_data_u value{};
|
||||
bool connected = false;
|
||||
std::string signal;
|
||||
};
|
||||
|
||||
struct HalThreadFunction {
|
||||
std::string name;
|
||||
double position = 0.0;
|
||||
int uses_fp = 0;
|
||||
};
|
||||
|
||||
struct HalThread {
|
||||
std::string name;
|
||||
unsigned long period_ns = 0;
|
||||
int uses_fp = 0;
|
||||
std::vector<HalThreadFunction> functions;
|
||||
};
|
||||
|
||||
struct HalRuntime {
|
||||
int next_comp_id = 1;
|
||||
bool ready = false;
|
||||
bool threads_running = false;
|
||||
long long cycle = 0;
|
||||
std::map<std::string, HalEntry> pins;
|
||||
std::map<std::string, HalEntry> signals;
|
||||
std::map<std::string, HalEntry> params;
|
||||
std::map<std::string, HalThread> threads;
|
||||
std::vector<void *> allocations;
|
||||
std::vector<std::string> events;
|
||||
};
|
||||
|
||||
HalRuntime &runtime()
|
||||
{
|
||||
static HalRuntime state;
|
||||
return state;
|
||||
}
|
||||
|
||||
const char *type_name(hal_type_t type)
|
||||
{
|
||||
switch (type) {
|
||||
case HAL_BIT:
|
||||
return "bit";
|
||||
case HAL_FLOAT:
|
||||
return "float";
|
||||
case HAL_S32:
|
||||
return "s32";
|
||||
case HAL_U32:
|
||||
return "u32";
|
||||
case HAL_S64:
|
||||
return "s64";
|
||||
case HAL_U64:
|
||||
return "u64";
|
||||
case HAL_TYPE_UNINITIALIZED:
|
||||
default:
|
||||
return "uninitialized";
|
||||
}
|
||||
}
|
||||
|
||||
std::string json_escape(const std::string &value)
|
||||
{
|
||||
std::ostringstream out;
|
||||
for (const char ch : value) {
|
||||
switch (ch) {
|
||||
case '\\':
|
||||
out << "\\\\";
|
||||
break;
|
||||
case '"':
|
||||
out << "\\\"";
|
||||
break;
|
||||
case '\n':
|
||||
out << "\\n";
|
||||
break;
|
||||
case '\r':
|
||||
out << "\\r";
|
||||
break;
|
||||
case '\t':
|
||||
out << "\\t";
|
||||
break;
|
||||
default:
|
||||
out << ch;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return out.str();
|
||||
}
|
||||
|
||||
std::string value_json(const HalEntry &entry)
|
||||
{
|
||||
std::ostringstream out;
|
||||
switch (entry.type) {
|
||||
case HAL_BIT:
|
||||
out << (entry.value.b ? "true" : "false");
|
||||
break;
|
||||
case HAL_FLOAT:
|
||||
out.precision(17);
|
||||
out << entry.value.f;
|
||||
break;
|
||||
case HAL_S32:
|
||||
out << entry.value.s;
|
||||
break;
|
||||
case HAL_U32:
|
||||
out << entry.value.u;
|
||||
break;
|
||||
case HAL_S64:
|
||||
out << entry.value.ls;
|
||||
break;
|
||||
case HAL_U64:
|
||||
out << entry.value.lu;
|
||||
break;
|
||||
case HAL_TYPE_UNINITIALIZED:
|
||||
default:
|
||||
out << "null";
|
||||
break;
|
||||
}
|
||||
return out.str();
|
||||
}
|
||||
|
||||
std::string entry_json(const HalEntry &entry)
|
||||
{
|
||||
std::ostringstream out;
|
||||
out << "{\"name\":\"" << json_escape(entry.name) << "\"";
|
||||
out << ",\"type\":\"" << type_name(entry.type) << "\"";
|
||||
out << ",\"dir\":" << static_cast<int>(entry.dir);
|
||||
out << ",\"connected\":" << (entry.connected ? "true" : "false");
|
||||
if (!entry.signal.empty()) {
|
||||
out << ",\"signal\":\"" << json_escape(entry.signal) << "\"";
|
||||
}
|
||||
out << ",\"value\":" << value_json(entry) << "}";
|
||||
return out.str();
|
||||
}
|
||||
|
||||
int write_output(const std::string &value, char *out, int out_len)
|
||||
{
|
||||
if (!out || out_len <= 0) {
|
||||
return -1;
|
||||
}
|
||||
const int required = static_cast<int>(value.size()) + 1;
|
||||
if (out_len < required) {
|
||||
if (out_len > 0) {
|
||||
out[0] = '\0';
|
||||
}
|
||||
return required;
|
||||
}
|
||||
std::memcpy(out, value.c_str(), static_cast<std::size_t>(required));
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::vector<std::string> split_words(const std::string &line)
|
||||
{
|
||||
std::istringstream in(line);
|
||||
std::vector<std::string> words;
|
||||
std::string word;
|
||||
while (in >> word) {
|
||||
if (!word.empty() && word[0] == '#') {
|
||||
break;
|
||||
}
|
||||
words.push_back(word);
|
||||
}
|
||||
return words;
|
||||
}
|
||||
|
||||
HalEntry *lookup_entry(const char *name)
|
||||
{
|
||||
if (!name) {
|
||||
return nullptr;
|
||||
}
|
||||
auto &state = runtime();
|
||||
auto pin = state.pins.find(name);
|
||||
if (pin != state.pins.end()) {
|
||||
return &pin->second;
|
||||
}
|
||||
auto param = state.params.find(name);
|
||||
if (param != state.params.end()) {
|
||||
return ¶m->second;
|
||||
}
|
||||
auto signal = state.signals.find(name);
|
||||
if (signal != state.signals.end()) {
|
||||
return &signal->second;
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
hal_data_u *value_ptr(HalEntry &entry)
|
||||
{
|
||||
return &entry.value;
|
||||
}
|
||||
|
||||
template <typename PointerT>
|
||||
int create_pin(const char *name, hal_pin_dir_t dir, PointerT **data_ptr_addr, int, hal_type_t type)
|
||||
{
|
||||
if (!name || !data_ptr_addr) {
|
||||
return -1;
|
||||
}
|
||||
auto &state = runtime();
|
||||
auto [it, inserted] = state.pins.emplace(name, HalEntry{});
|
||||
HalEntry &entry = it->second;
|
||||
entry.name = name;
|
||||
entry.type = type;
|
||||
entry.dir = dir;
|
||||
entry.connected = false;
|
||||
switch (type) {
|
||||
case HAL_BIT:
|
||||
*data_ptr_addr = reinterpret_cast<PointerT *>(&entry.value.b);
|
||||
break;
|
||||
case HAL_FLOAT:
|
||||
*data_ptr_addr = reinterpret_cast<PointerT *>(&entry.value.f);
|
||||
break;
|
||||
case HAL_S32:
|
||||
*data_ptr_addr = reinterpret_cast<PointerT *>(&entry.value.s);
|
||||
break;
|
||||
case HAL_U32:
|
||||
*data_ptr_addr = reinterpret_cast<PointerT *>(&entry.value.u);
|
||||
break;
|
||||
case HAL_S64:
|
||||
*data_ptr_addr = reinterpret_cast<PointerT *>(&entry.value.ls);
|
||||
break;
|
||||
case HAL_U64:
|
||||
*data_ptr_addr = reinterpret_cast<PointerT *>(&entry.value.lu);
|
||||
break;
|
||||
case HAL_TYPE_UNINITIALIZED:
|
||||
default:
|
||||
return -1;
|
||||
}
|
||||
state.events.push_back(std::string(inserted ? "pin_create:" : "pin_reuse:") + name);
|
||||
return 0;
|
||||
}
|
||||
|
||||
template <typename PointerT, int (*Create)(const char *, hal_pin_dir_t, PointerT **, int)>
|
||||
int create_pin_newf(hal_pin_dir_t dir, PointerT **data_ptr_addr, int comp_id, const char *fmt,
|
||||
va_list ap)
|
||||
{
|
||||
if (!fmt) {
|
||||
return -1;
|
||||
}
|
||||
char name[HAL_NAME_LEN * 2]{};
|
||||
std::vsnprintf(name, sizeof(name), fmt, ap);
|
||||
return Create(name, dir, data_ptr_addr, comp_id);
|
||||
}
|
||||
|
||||
void copy_value_from_addr(HalEntry &entry, const void *data_addr)
|
||||
{
|
||||
switch (entry.type) {
|
||||
case HAL_BIT:
|
||||
entry.value.b = *static_cast<const hal_bit_t *>(data_addr);
|
||||
break;
|
||||
case HAL_FLOAT:
|
||||
entry.value.f = *static_cast<const hal_float_t *>(data_addr);
|
||||
break;
|
||||
case HAL_S32:
|
||||
entry.value.s = *static_cast<const hal_s32_t *>(data_addr);
|
||||
break;
|
||||
case HAL_U32:
|
||||
entry.value.u = *static_cast<const hal_u32_t *>(data_addr);
|
||||
break;
|
||||
case HAL_S64:
|
||||
entry.value.ls = *static_cast<const hal_s64_t *>(data_addr);
|
||||
break;
|
||||
case HAL_U64:
|
||||
entry.value.lu = *static_cast<const hal_u64_t *>(data_addr);
|
||||
break;
|
||||
case HAL_TYPE_UNINITIALIZED:
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int create_param(const char *name, hal_param_dir_t dir, void *data_addr, int, hal_type_t type)
|
||||
{
|
||||
if (!name || !data_addr) {
|
||||
return -1;
|
||||
}
|
||||
auto &entry = runtime().params[name];
|
||||
entry.name = name;
|
||||
entry.type = type;
|
||||
entry.dir = static_cast<hal_pin_dir_t>(dir);
|
||||
entry.connected = false;
|
||||
copy_value_from_addr(entry, data_addr);
|
||||
runtime().events.push_back(std::string("param_create:") + name);
|
||||
return 0;
|
||||
}
|
||||
|
||||
template <typename ValueT,
|
||||
int (*Create)(const char *, hal_param_dir_t, ValueT *, int)>
|
||||
int create_param_newf(hal_param_dir_t dir, ValueT *data_addr, int comp_id, const char *fmt,
|
||||
va_list ap)
|
||||
{
|
||||
if (!fmt) {
|
||||
return -1;
|
||||
}
|
||||
char name[HAL_NAME_LEN * 2]{};
|
||||
std::vsnprintf(name, sizeof(name), fmt, ap);
|
||||
return Create(name, dir, data_addr, comp_id);
|
||||
}
|
||||
|
||||
bool assign_from_text(HalEntry &entry, const char *text)
|
||||
{
|
||||
if (!text) {
|
||||
return false;
|
||||
}
|
||||
switch (entry.type) {
|
||||
case HAL_BIT:
|
||||
entry.value.b = std::strcmp(text, "1") == 0 || std::strcmp(text, "true") == 0 ||
|
||||
std::strcmp(text, "TRUE") == 0;
|
||||
return true;
|
||||
case HAL_FLOAT:
|
||||
entry.value.f = std::strtod(text, nullptr);
|
||||
return true;
|
||||
case HAL_S32:
|
||||
entry.value.s = static_cast<int>(std::strtol(text, nullptr, 0));
|
||||
return true;
|
||||
case HAL_U32:
|
||||
entry.value.u = static_cast<unsigned int>(std::strtoul(text, nullptr, 0));
|
||||
return true;
|
||||
case HAL_S64:
|
||||
entry.value.ls = std::strtoll(text, nullptr, 0);
|
||||
return true;
|
||||
case HAL_U64:
|
||||
entry.value.lu = std::strtoull(text, nullptr, 0);
|
||||
return true;
|
||||
case HAL_TYPE_UNINITIALIZED:
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void propagate_signal(const std::string &signal_name)
|
||||
{
|
||||
auto &state = runtime();
|
||||
auto signal_it = state.signals.find(signal_name);
|
||||
if (signal_it == state.signals.end()) {
|
||||
return;
|
||||
}
|
||||
for (auto &[pin_name, pin] : state.pins) {
|
||||
if (pin.signal == signal_name) {
|
||||
pin.value = signal_it->second.value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int link_pin_to_signal(const std::string &pin_name, const std::string &signal_name)
|
||||
{
|
||||
auto &state = runtime();
|
||||
auto pin_it = state.pins.find(pin_name);
|
||||
if (pin_it == state.pins.end()) {
|
||||
return -1;
|
||||
}
|
||||
auto [signal_it, inserted] = state.signals.emplace(signal_name, HalEntry{});
|
||||
HalEntry &signal = signal_it->second;
|
||||
signal.name = signal_name;
|
||||
if (inserted || signal.type == HAL_TYPE_UNINITIALIZED) {
|
||||
signal.type = pin_it->second.type;
|
||||
signal.value = pin_it->second.value;
|
||||
}
|
||||
if (signal.type != pin_it->second.type) {
|
||||
return -1;
|
||||
}
|
||||
signal.connected = true;
|
||||
pin_it->second.connected = true;
|
||||
pin_it->second.signal = signal_name;
|
||||
pin_it->second.value = signal.value;
|
||||
state.events.push_back("net:" + signal_name + ":" + pin_name);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int set_entry_value(const char *name, const char *text)
|
||||
{
|
||||
HalEntry *entry = lookup_entry(name);
|
||||
if (!entry || !assign_from_text(*entry, text)) {
|
||||
return -1;
|
||||
}
|
||||
auto &state = runtime();
|
||||
if (state.signals.count(entry->name) > 0) {
|
||||
propagate_signal(entry->name);
|
||||
} else if (!entry->signal.empty()) {
|
||||
auto signal_it = state.signals.find(entry->signal);
|
||||
if (signal_it != state.signals.end()) {
|
||||
signal_it->second.value = entry->value;
|
||||
propagate_signal(entry->signal);
|
||||
}
|
||||
}
|
||||
state.events.push_back(std::string("setp:") + name + "=" + text);
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::string snapshot_json()
|
||||
{
|
||||
auto &state = runtime();
|
||||
std::ostringstream out;
|
||||
out << "{\"semanticBoundary\":\"linuxcnc_hal_runtime_phase2_minimal\"";
|
||||
out << ",\"halRuntimeReady\":true";
|
||||
out << ",\"halSyncReady\":false";
|
||||
out << ",\"nativeHalSyncReady\":false";
|
||||
out << ",\"cycle\":" << state.cycle;
|
||||
out << ",\"ready\":" << (state.ready ? "true" : "false");
|
||||
out << ",\"threadsRunning\":" << (state.threads_running ? "true" : "false");
|
||||
out << ",\"pins\":{";
|
||||
bool first = true;
|
||||
for (const auto &[name, entry] : state.pins) {
|
||||
if (!first) {
|
||||
out << ",";
|
||||
}
|
||||
first = false;
|
||||
out << "\"" << json_escape(name) << "\":" << entry_json(entry);
|
||||
}
|
||||
out << "},\"signals\":{";
|
||||
first = true;
|
||||
for (const auto &[name, entry] : state.signals) {
|
||||
if (!first) {
|
||||
out << ",";
|
||||
}
|
||||
first = false;
|
||||
out << "\"" << json_escape(name) << "\":" << entry_json(entry);
|
||||
}
|
||||
out << "},\"params\":{";
|
||||
first = true;
|
||||
for (const auto &[name, entry] : state.params) {
|
||||
if (!first) {
|
||||
out << ",";
|
||||
}
|
||||
first = false;
|
||||
out << "\"" << json_escape(name) << "\":" << entry_json(entry);
|
||||
}
|
||||
out << "},\"threads\":{";
|
||||
first = true;
|
||||
for (const auto &[name, thread] : state.threads) {
|
||||
if (!first) {
|
||||
out << ",";
|
||||
}
|
||||
first = false;
|
||||
out << "\"" << json_escape(name) << "\":{\"periodNs\":" << thread.period_ns;
|
||||
out << ",\"functions\":[";
|
||||
for (std::size_t i = 0; i < thread.functions.size(); ++i) {
|
||||
if (i > 0) {
|
||||
out << ",";
|
||||
}
|
||||
out << "{\"name\":\"" << json_escape(thread.functions[i].name) << "\"";
|
||||
out << ",\"position\":" << thread.functions[i].position << "}";
|
||||
}
|
||||
out << "]}";
|
||||
}
|
||||
out << "},\"events\":[";
|
||||
for (std::size_t i = 0; i < state.events.size(); ++i) {
|
||||
if (i > 0) {
|
||||
out << ",";
|
||||
}
|
||||
out << "\"" << json_escape(state.events[i]) << "\"";
|
||||
}
|
||||
out << "]}";
|
||||
return out.str();
|
||||
}
|
||||
|
||||
void clear_runtime()
|
||||
{
|
||||
auto &state = runtime();
|
||||
for (void *ptr : state.allocations) {
|
||||
::operator delete(ptr);
|
||||
}
|
||||
state = HalRuntime{};
|
||||
}
|
||||
|
||||
int load_hal_line(const std::vector<std::string> &words)
|
||||
{
|
||||
if (words.empty()) {
|
||||
return 0;
|
||||
}
|
||||
if (words[0] == "loadusr") {
|
||||
runtime().events.push_back("blocked:loadusr");
|
||||
return -2;
|
||||
}
|
||||
if (words[0] == "loadrt") {
|
||||
runtime().events.push_back(words.size() > 1 ? "loadrt:" + words[1] : "loadrt");
|
||||
return 0;
|
||||
}
|
||||
if (words[0] == "addf" && words.size() >= 3) {
|
||||
return hal_add_funct_to_thread(words[1].c_str(), words[2].c_str(), 0.0, 0);
|
||||
}
|
||||
if (words[0] == "net" && words.size() >= 3) {
|
||||
const std::string signal = words[1];
|
||||
for (std::size_t i = 2; i < words.size(); ++i) {
|
||||
if (link_pin_to_signal(words[i], signal) != 0) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
if (words[0] == "setp" && words.size() >= 3) {
|
||||
return hal_set_p(words[1].c_str(), words[2].c_str());
|
||||
}
|
||||
if (words[0] == "gets") {
|
||||
runtime().events.push_back("gets");
|
||||
return 0;
|
||||
}
|
||||
runtime().events.push_back("ignored:" + words[0]);
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
extern "C" {
|
||||
|
||||
int hal_init(const char *)
|
||||
{
|
||||
return runtime().next_comp_id++;
|
||||
}
|
||||
|
||||
int hal_ready(int)
|
||||
{
|
||||
runtime().ready = true;
|
||||
runtime().events.push_back("ready");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int hal_exit(int)
|
||||
{
|
||||
runtime().ready = false;
|
||||
runtime().events.push_back("exit");
|
||||
return 0;
|
||||
}
|
||||
|
||||
void *hal_malloc(long int size)
|
||||
{
|
||||
if (size <= 0) {
|
||||
return nullptr;
|
||||
}
|
||||
void *ptr = ::operator new(static_cast<std::size_t>(size), std::nothrow);
|
||||
if (ptr) {
|
||||
runtime().allocations.push_back(ptr);
|
||||
}
|
||||
return ptr;
|
||||
}
|
||||
|
||||
int hal_pin_bit_new(const char *name, hal_pin_dir_t dir, hal_bit_t **data_ptr_addr, int comp_id)
|
||||
{
|
||||
return create_pin(name, dir, data_ptr_addr, comp_id, HAL_BIT);
|
||||
}
|
||||
|
||||
int hal_pin_float_new(const char *name, hal_pin_dir_t dir, hal_float_t **data_ptr_addr,
|
||||
int comp_id)
|
||||
{
|
||||
return create_pin(name, dir, data_ptr_addr, comp_id, HAL_FLOAT);
|
||||
}
|
||||
|
||||
int hal_pin_u32_new(const char *name, hal_pin_dir_t dir, hal_u32_t **data_ptr_addr, int comp_id)
|
||||
{
|
||||
return create_pin(name, dir, data_ptr_addr, comp_id, HAL_U32);
|
||||
}
|
||||
|
||||
int hal_pin_s32_new(const char *name, hal_pin_dir_t dir, hal_s32_t **data_ptr_addr, int comp_id)
|
||||
{
|
||||
return create_pin(name, dir, data_ptr_addr, comp_id, HAL_S32);
|
||||
}
|
||||
|
||||
int hal_pin_u64_new(const char *name, hal_pin_dir_t dir, hal_u64_t **data_ptr_addr, int comp_id)
|
||||
{
|
||||
return create_pin(name, dir, data_ptr_addr, comp_id, HAL_U64);
|
||||
}
|
||||
|
||||
int hal_pin_s64_new(const char *name, hal_pin_dir_t dir, hal_s64_t **data_ptr_addr, int comp_id)
|
||||
{
|
||||
return create_pin(name, dir, data_ptr_addr, comp_id, HAL_S64);
|
||||
}
|
||||
|
||||
int hal_pin_bit_newf(hal_pin_dir_t dir, hal_bit_t **data_ptr_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = create_pin_newf<hal_bit_t, hal_pin_bit_new>(dir, data_ptr_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_pin_float_newf(hal_pin_dir_t dir, hal_float_t **data_ptr_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc =
|
||||
create_pin_newf<hal_float_t, hal_pin_float_new>(dir, data_ptr_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_pin_u32_newf(hal_pin_dir_t dir, hal_u32_t **data_ptr_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = create_pin_newf<hal_u32_t, hal_pin_u32_new>(dir, data_ptr_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_pin_s32_newf(hal_pin_dir_t dir, hal_s32_t **data_ptr_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = create_pin_newf<hal_s32_t, hal_pin_s32_new>(dir, data_ptr_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_pin_u64_newf(hal_pin_dir_t dir, hal_u64_t **data_ptr_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = create_pin_newf<hal_u64_t, hal_pin_u64_new>(dir, data_ptr_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_pin_s64_newf(hal_pin_dir_t dir, hal_s64_t **data_ptr_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = create_pin_newf<hal_s64_t, hal_pin_s64_new>(dir, data_ptr_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_param_bit_new(const char *name, hal_param_dir_t dir, hal_bit_t *data_addr, int comp_id)
|
||||
{
|
||||
return create_param(name, dir, data_addr, comp_id, HAL_BIT);
|
||||
}
|
||||
|
||||
int hal_param_float_new(const char *name, hal_param_dir_t dir, hal_float_t *data_addr,
|
||||
int comp_id)
|
||||
{
|
||||
return create_param(name, dir, data_addr, comp_id, HAL_FLOAT);
|
||||
}
|
||||
|
||||
int hal_param_u32_new(const char *name, hal_param_dir_t dir, hal_u32_t *data_addr, int comp_id)
|
||||
{
|
||||
return create_param(name, dir, data_addr, comp_id, HAL_U32);
|
||||
}
|
||||
|
||||
int hal_param_s32_new(const char *name, hal_param_dir_t dir, hal_s32_t *data_addr, int comp_id)
|
||||
{
|
||||
return create_param(name, dir, data_addr, comp_id, HAL_S32);
|
||||
}
|
||||
|
||||
int hal_param_u64_new(const char *name, hal_param_dir_t dir, hal_u64_t *data_addr, int comp_id)
|
||||
{
|
||||
return create_param(name, dir, data_addr, comp_id, HAL_U64);
|
||||
}
|
||||
|
||||
int hal_param_s64_new(const char *name, hal_param_dir_t dir, hal_s64_t *data_addr, int comp_id)
|
||||
{
|
||||
return create_param(name, dir, data_addr, comp_id, HAL_S64);
|
||||
}
|
||||
|
||||
int hal_param_bit_newf(hal_param_dir_t dir, hal_bit_t *data_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = create_param_newf<hal_bit_t, hal_param_bit_new>(dir, data_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_param_float_newf(hal_param_dir_t dir, hal_float_t *data_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc =
|
||||
create_param_newf<hal_float_t, hal_param_float_new>(dir, data_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_param_u32_newf(hal_param_dir_t dir, hal_u32_t *data_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = create_param_newf<hal_u32_t, hal_param_u32_new>(dir, data_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_param_s32_newf(hal_param_dir_t dir, hal_s32_t *data_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = create_param_newf<hal_s32_t, hal_param_s32_new>(dir, data_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_param_u64_newf(hal_param_dir_t dir, hal_u64_t *data_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = create_param_newf<hal_u64_t, hal_param_u64_new>(dir, data_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_param_s64_newf(hal_param_dir_t dir, hal_s64_t *data_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = create_param_newf<hal_s64_t, hal_param_s64_new>(dir, data_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_get_pin_value_by_name(const char *name, hal_type_t *type, hal_data_u **ptr,
|
||||
bool *connected)
|
||||
{
|
||||
auto it = runtime().pins.find(name ? name : "");
|
||||
if (it == runtime().pins.end() || !type || !ptr) {
|
||||
return -1;
|
||||
}
|
||||
*type = it->second.type;
|
||||
*ptr = value_ptr(it->second);
|
||||
if (connected) {
|
||||
*connected = it->second.connected;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int hal_get_signal_value_by_name(const char *name, hal_type_t *type, hal_data_u **ptr,
|
||||
bool *connected)
|
||||
{
|
||||
auto it = runtime().signals.find(name ? name : "");
|
||||
if (it == runtime().signals.end() || !type || !ptr) {
|
||||
return -1;
|
||||
}
|
||||
*type = it->second.type;
|
||||
*ptr = value_ptr(it->second);
|
||||
if (connected) {
|
||||
*connected = it->second.connected;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int hal_get_param_value_by_name(const char *name, hal_type_t *type, hal_data_u **ptr)
|
||||
{
|
||||
auto it = runtime().params.find(name ? name : "");
|
||||
if (it == runtime().params.end() || !type || !ptr) {
|
||||
return -1;
|
||||
}
|
||||
*type = it->second.type;
|
||||
*ptr = value_ptr(it->second);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int hal_link(const char *pin_name, const char *signal_name)
|
||||
{
|
||||
if (!pin_name || !signal_name) {
|
||||
return -1;
|
||||
}
|
||||
return link_pin_to_signal(pin_name, signal_name);
|
||||
}
|
||||
|
||||
int hal_unlink(const char *pin_name)
|
||||
{
|
||||
auto it = runtime().pins.find(pin_name ? pin_name : "");
|
||||
if (it == runtime().pins.end()) {
|
||||
return -1;
|
||||
}
|
||||
it->second.connected = false;
|
||||
it->second.signal.clear();
|
||||
runtime().events.push_back(std::string("unlink:") + pin_name);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int hal_set_p(const char *name, const char *value)
|
||||
{
|
||||
return set_entry_value(name, value);
|
||||
}
|
||||
|
||||
int hal_get_p(const char *name, char *out, int out_len)
|
||||
{
|
||||
HalEntry *entry = lookup_entry(name);
|
||||
if (!entry) {
|
||||
return -1;
|
||||
}
|
||||
return write_output(value_json(*entry), out, out_len);
|
||||
}
|
||||
|
||||
int hal_create_thread(const char *name, unsigned long period_ns, int uses_fp)
|
||||
{
|
||||
if (!name) {
|
||||
return -1;
|
||||
}
|
||||
auto &thread = runtime().threads[name];
|
||||
thread.name = name;
|
||||
thread.period_ns = period_ns;
|
||||
thread.uses_fp = uses_fp;
|
||||
runtime().events.push_back(std::string("thread_create:") + name);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int hal_add_funct_to_thread(const char *funct_name, const char *thread_name, double position,
|
||||
int uses_fp)
|
||||
{
|
||||
if (!funct_name || !thread_name) {
|
||||
return -1;
|
||||
}
|
||||
auto thread_it = runtime().threads.find(thread_name);
|
||||
if (thread_it == runtime().threads.end()) {
|
||||
hal_create_thread(thread_name, 0, uses_fp);
|
||||
thread_it = runtime().threads.find(thread_name);
|
||||
}
|
||||
thread_it->second.functions.push_back(HalThreadFunction{funct_name, position, uses_fp});
|
||||
std::sort(thread_it->second.functions.begin(), thread_it->second.functions.end(),
|
||||
[](const HalThreadFunction &a, const HalThreadFunction &b) {
|
||||
return a.position < b.position;
|
||||
});
|
||||
runtime().events.push_back(std::string("addf:") + funct_name + ":" + thread_name);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int hal_del_funct_from_thread(const char *funct_name, const char *thread_name)
|
||||
{
|
||||
auto thread_it = runtime().threads.find(thread_name ? thread_name : "");
|
||||
if (thread_it == runtime().threads.end() || !funct_name) {
|
||||
return -1;
|
||||
}
|
||||
auto &functions = thread_it->second.functions;
|
||||
functions.erase(std::remove_if(functions.begin(), functions.end(),
|
||||
[&](const HalThreadFunction &item) {
|
||||
return item.name == funct_name;
|
||||
}),
|
||||
functions.end());
|
||||
runtime().events.push_back(std::string("delf:") + funct_name + ":" + thread_name);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int hal_start_threads(void)
|
||||
{
|
||||
runtime().threads_running = true;
|
||||
runtime().events.push_back("threads_start");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int hal_stop_threads(void)
|
||||
{
|
||||
runtime().threads_running = false;
|
||||
runtime().events.push_back("threads_stop");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int lchal_init_runtime(void)
|
||||
{
|
||||
clear_runtime();
|
||||
const int comp_id = hal_init("linuxcnc-hal-runtime");
|
||||
hal_ready(comp_id);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int lchal_load_hal_file(const char *path, const char *text)
|
||||
{
|
||||
if (!text) {
|
||||
return -1;
|
||||
}
|
||||
runtime().events.push_back(std::string("halfile:") + (path ? path : "<memory>"));
|
||||
std::istringstream lines(text);
|
||||
std::string line;
|
||||
int blocked = 0;
|
||||
int failed = 0;
|
||||
while (std::getline(lines, line)) {
|
||||
const auto words = split_words(line);
|
||||
const int rc = load_hal_line(words);
|
||||
if (rc == -2) {
|
||||
blocked = 1;
|
||||
} else if (rc != 0) {
|
||||
failed = 1;
|
||||
}
|
||||
}
|
||||
if (blocked) {
|
||||
return 2;
|
||||
}
|
||||
return failed ? -1 : 0;
|
||||
}
|
||||
|
||||
int lchal_set_pin_float(const char *name, double value)
|
||||
{
|
||||
char buf[64]{};
|
||||
std::snprintf(buf, sizeof(buf), "%.17g", value);
|
||||
return hal_set_p(name, buf);
|
||||
}
|
||||
|
||||
int lchal_set_pin_s32(const char *name, int value)
|
||||
{
|
||||
char buf[64]{};
|
||||
std::snprintf(buf, sizeof(buf), "%d", value);
|
||||
return hal_set_p(name, buf);
|
||||
}
|
||||
|
||||
int lchal_set_pin_bit(const char *name, int value)
|
||||
{
|
||||
return hal_set_p(name, value ? "1" : "0");
|
||||
}
|
||||
|
||||
int lchal_get_pin_json(const char *name, char *out, int out_len)
|
||||
{
|
||||
auto it = runtime().pins.find(name ? name : "");
|
||||
if (it == runtime().pins.end()) {
|
||||
return -1;
|
||||
}
|
||||
return write_output(entry_json(it->second), out, out_len);
|
||||
}
|
||||
|
||||
int lchal_get_snapshot_json(char *out, int out_len)
|
||||
{
|
||||
return write_output(snapshot_json(), out, out_len);
|
||||
}
|
||||
|
||||
int lchal_step_threads(long period_ns, int cycles)
|
||||
{
|
||||
if (cycles < 0) {
|
||||
return -1;
|
||||
}
|
||||
auto &state = runtime();
|
||||
if (!state.threads_running) {
|
||||
hal_start_threads();
|
||||
}
|
||||
for (int i = 0; i < cycles; ++i) {
|
||||
state.cycle += 1;
|
||||
for (const auto &[thread_name, thread] : state.threads) {
|
||||
std::ostringstream event;
|
||||
event << "cycle:" << state.cycle << ":" << thread_name << ":" << period_ns;
|
||||
state.events.push_back(event.str());
|
||||
for (const auto &function : thread.functions) {
|
||||
state.events.push_back("call:" + thread_name + ":" + function.name);
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int lchal_reset_runtime(void)
|
||||
{
|
||||
clear_runtime();
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // extern "C"
|
||||
21
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_hal_runtime.hh
Normal file
21
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_hal_runtime.hh
Normal file
@@ -0,0 +1,21 @@
|
||||
#pragma once
|
||||
|
||||
#include "hal.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
int lchal_init_runtime(void);
|
||||
int lchal_load_hal_file(const char *path, const char *text);
|
||||
int lchal_set_pin_float(const char *name, double value);
|
||||
int lchal_set_pin_s32(const char *name, int value);
|
||||
int lchal_set_pin_bit(const char *name, int value);
|
||||
int lchal_get_pin_json(const char *name, char *out, int out_len);
|
||||
int lchal_get_snapshot_json(char *out, int out_len);
|
||||
int lchal_step_threads(long period_ns, int cycles);
|
||||
int lchal_reset_runtime(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,180 @@
|
||||
#include "linuxcnc_hal_runtime.hh"
|
||||
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
|
||||
namespace {
|
||||
|
||||
bool contains(const char *haystack, const char *needle)
|
||||
{
|
||||
return std::strstr(haystack, needle) != nullptr;
|
||||
}
|
||||
|
||||
bool require(bool condition, const char *label)
|
||||
{
|
||||
if (!condition) {
|
||||
std::fprintf(stderr, "hal_runtime_probe_failed=%s\n", label);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main()
|
||||
{
|
||||
char buffer[32768]{};
|
||||
|
||||
if (!require(lchal_init_runtime() == 0, "init")) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
const int comp_id = hal_init("probe");
|
||||
hal_float_t *float_pin = nullptr;
|
||||
hal_bit_t *bit_in = nullptr;
|
||||
hal_bit_t *bit_out = nullptr;
|
||||
hal_s32_t *line_pin = nullptr;
|
||||
if (!require(hal_pin_float_new("motion.analog-out-03", HAL_OUT, &float_pin, comp_id) == 0,
|
||||
"float_pin")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(hal_pin_bit_new("probe.in", HAL_IN, &bit_in, comp_id) == 0, "bit_in")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(hal_pin_bit_new("probe.out", HAL_OUT, &bit_out, comp_id) == 0, "bit_out")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(hal_pin_s32_new("motion.program-line", HAL_OUT, &line_pin, comp_id) == 0,
|
||||
"s32_pin")) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
*float_pin = 1.25;
|
||||
*line_pin = 42;
|
||||
|
||||
if (!require(lchal_get_pin_json("motion.analog-out-03", buffer, sizeof(buffer)) == 0,
|
||||
"float_pin_json")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(contains(buffer, "\"value\":1.25"), "float_pin_value")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(hal_get_p("motion.analog-out-03", buffer, sizeof(buffer)) == 0,
|
||||
"float_pin_getp")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(contains(buffer, "1.25"), "float_pin_getp_value")) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
hal_float_t param_float = 7.25;
|
||||
hal_s32_t param_s32 = -12;
|
||||
if (!require(hal_param_float_new("standalone.param-float", HAL_RW, ¶m_float, comp_id) == 0,
|
||||
"param_float")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(hal_param_s32_new("standalone.param-s32", HAL_RW, ¶m_s32, comp_id) == 0,
|
||||
"param_s32")) {
|
||||
return 1;
|
||||
}
|
||||
hal_type_t lookup_type = HAL_TYPE_UNINITIALIZED;
|
||||
hal_data_u *lookup_value = nullptr;
|
||||
bool lookup_connected = false;
|
||||
if (!require(hal_get_pin_value_by_name("motion.program-line", &lookup_type, &lookup_value,
|
||||
&lookup_connected) == 0,
|
||||
"lookup_pin")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(lookup_type == HAL_S32 && lookup_value != nullptr, "lookup_pin_type")) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!require(hal_link("probe.out", "probe-signal") == 0, "link_probe_out")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(hal_link("probe.in", "probe-signal") == 0, "link_probe_in")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(lchal_set_pin_bit("probe.out", 1) == 0, "set_probe_out")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(*bit_in == true, "net_propagated_to_input")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(hal_get_signal_value_by_name("probe-signal", &lookup_type, &lookup_value,
|
||||
&lookup_connected) == 0,
|
||||
"lookup_signal")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(lookup_type == HAL_BIT && lookup_value != nullptr && lookup_connected,
|
||||
"lookup_signal_type")) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!require(hal_create_thread("servo-thread", 1000000, 1) == 0, "create_thread")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(hal_add_funct_to_thread("motion-command-handler", "servo-thread", 0.0, 1) == 0,
|
||||
"addf_command")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(hal_add_funct_to_thread("motion-controller", "servo-thread", 1.0, 1) == 0,
|
||||
"addf_controller")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(lchal_step_threads(1000000, 3) == 0, "step_threads")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(hal_del_funct_from_thread("motion-command-handler", "servo-thread") == 0,
|
||||
"del_thread_function")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(hal_stop_threads() == 0, "stop_threads")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(hal_start_threads() == 0, "start_threads")) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
const char *hal_text =
|
||||
"loadrt trivkins\n"
|
||||
"addf kins servo-thread\n"
|
||||
"loadusr external-user-m\n";
|
||||
if (!require(lchal_load_hal_file("probe.hal", hal_text) == 2, "loadusr_blocked")) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!require(lchal_get_snapshot_json(buffer, sizeof(buffer)) == 0, "snapshot")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(contains(buffer, "\"halRuntimeReady\":true"), "runtime_ready")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(contains(buffer, "\"nativeHalSyncReady\":false"), "not_promoted")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(contains(buffer, "\"probe.in\""), "snapshot_probe_in")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(contains(buffer, "\"standalone.param-float\""), "snapshot_param_float")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(contains(buffer, "call:servo-thread:motion-controller"), "thread_call_event")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(contains(buffer, "delf:motion-command-handler:servo-thread"),
|
||||
"thread_delete_event")) {
|
||||
return 1;
|
||||
}
|
||||
if (!require(contains(buffer, "blocked:loadusr"), "blocked_event")) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
std::puts("hal_runtime_registry=ok");
|
||||
std::puts("hal_net_signal_propagation=ok");
|
||||
std::puts("hal_thread_scheduler=ok");
|
||||
std::puts("loadusr_blocked_evidence=ok");
|
||||
std::puts("hal_runtime_probe=ok");
|
||||
return 0;
|
||||
}
|
||||
464
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_motion_runtime.c
Normal file
464
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_motion_runtime.c
Normal file
@@ -0,0 +1,464 @@
|
||||
#include "linuxcnc_motion_runtime.h"
|
||||
|
||||
#include "hal.h"
|
||||
#include "linuxcnc_hal_runtime.hh"
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
enum {
|
||||
LCMOT_CMD_NONE = 0,
|
||||
LCMOT_CMD_LINEAR_MOVE,
|
||||
LCMOT_CMD_CIRCULAR_MOVE,
|
||||
LCMOT_CMD_JOG_INCR,
|
||||
LCMOT_CMD_PAUSE,
|
||||
LCMOT_CMD_RESUME,
|
||||
LCMOT_CMD_ABORT,
|
||||
LCMOT_CMD_SET_AOUT,
|
||||
};
|
||||
|
||||
typedef struct {
|
||||
int type;
|
||||
int line;
|
||||
int axis_index;
|
||||
double target[9];
|
||||
double distance;
|
||||
double velocity;
|
||||
double analog_value;
|
||||
} LcmotCommand;
|
||||
|
||||
typedef struct {
|
||||
int initialized;
|
||||
long long cycle;
|
||||
int program_line;
|
||||
int motion_type;
|
||||
int coord_mode;
|
||||
int teleop_mode;
|
||||
int in_position;
|
||||
int paused;
|
||||
int aborted;
|
||||
int switchkins_type;
|
||||
double requested_vel;
|
||||
double current_vel;
|
||||
double analog_out_03;
|
||||
double axis_cmd[9];
|
||||
double axis_fb[9];
|
||||
double joint_cmd[9];
|
||||
double joint_fb[9];
|
||||
LcmotCommand queue[64];
|
||||
int queue_head;
|
||||
int queue_tail;
|
||||
int queue_count;
|
||||
} LcmotRuntime;
|
||||
|
||||
static LcmotRuntime lcmot_state;
|
||||
static hal_s32_t *hal_motion_program_line;
|
||||
static hal_s32_t *hal_motion_motion_type;
|
||||
static hal_s32_t *hal_motion_coord_mode;
|
||||
static hal_s32_t *hal_motion_teleop_mode;
|
||||
static hal_bit_t *hal_motion_in_position;
|
||||
static hal_s32_t *hal_motion_switchkins_type;
|
||||
static hal_float_t *hal_motion_analog_out_03;
|
||||
static hal_float_t *hal_joint_cmd[9];
|
||||
static hal_float_t *hal_joint_fb[9];
|
||||
static hal_float_t *hal_axis_cmd[9];
|
||||
static hal_float_t *hal_axis_fb[9];
|
||||
|
||||
static int write_output(const char *value, char *out, int out_len)
|
||||
{
|
||||
int required;
|
||||
if (!value || !out || out_len <= 0) {
|
||||
return -1;
|
||||
}
|
||||
required = (int)strlen(value) + 1;
|
||||
if (out_len < required) {
|
||||
out[0] = '\0';
|
||||
return required;
|
||||
}
|
||||
memcpy(out, value, (size_t)required);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int contains_token(const char *json, const char *token)
|
||||
{
|
||||
return json && token && strstr(json, token) != NULL;
|
||||
}
|
||||
|
||||
static double json_number_after(const char *json, const char *key, double fallback)
|
||||
{
|
||||
const char *at;
|
||||
char *end = NULL;
|
||||
if (!json || !key) {
|
||||
return fallback;
|
||||
}
|
||||
at = strstr(json, key);
|
||||
if (!at) {
|
||||
return fallback;
|
||||
}
|
||||
at = strchr(at, ':');
|
||||
if (!at) {
|
||||
return fallback;
|
||||
}
|
||||
at += 1;
|
||||
while (*at == ' ' || *at == '"') {
|
||||
at += 1;
|
||||
}
|
||||
return strtod(at, &end);
|
||||
}
|
||||
|
||||
static int json_int_after(const char *json, const char *key, int fallback)
|
||||
{
|
||||
return (int)json_number_after(json, key, fallback);
|
||||
}
|
||||
|
||||
static int axis_index_from_json(const char *json)
|
||||
{
|
||||
const char *axis = strstr(json ? json : "", "\"axis\"");
|
||||
if (!axis) {
|
||||
return 0;
|
||||
}
|
||||
axis = strchr(axis, ':');
|
||||
if (!axis) {
|
||||
return 0;
|
||||
}
|
||||
while (*axis && *axis != '"') {
|
||||
axis += 1;
|
||||
}
|
||||
if (*axis == '"') {
|
||||
axis += 1;
|
||||
}
|
||||
switch (*axis) {
|
||||
case 'X':
|
||||
case 'x':
|
||||
return 0;
|
||||
case 'Y':
|
||||
case 'y':
|
||||
return 1;
|
||||
case 'Z':
|
||||
case 'z':
|
||||
return 2;
|
||||
case 'A':
|
||||
case 'a':
|
||||
return 3;
|
||||
case 'B':
|
||||
case 'b':
|
||||
return 4;
|
||||
case 'C':
|
||||
case 'c':
|
||||
return 5;
|
||||
case 'U':
|
||||
case 'u':
|
||||
return 6;
|
||||
case 'V':
|
||||
case 'v':
|
||||
return 7;
|
||||
case 'W':
|
||||
case 'w':
|
||||
return 8;
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
static int queue_push(LcmotCommand command)
|
||||
{
|
||||
LcmotRuntime *state = &lcmot_state;
|
||||
if (state->queue_count >= (int)(sizeof(state->queue) / sizeof(state->queue[0]))) {
|
||||
return -1;
|
||||
}
|
||||
state->queue[state->queue_tail] = command;
|
||||
state->queue_tail = (state->queue_tail + 1) % (int)(sizeof(state->queue) / sizeof(state->queue[0]));
|
||||
state->queue_count += 1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int queue_pop(LcmotCommand *command)
|
||||
{
|
||||
LcmotRuntime *state = &lcmot_state;
|
||||
if (!command || state->queue_count <= 0) {
|
||||
return 0;
|
||||
}
|
||||
*command = state->queue[state->queue_head];
|
||||
state->queue_head = (state->queue_head + 1) % (int)(sizeof(state->queue) / sizeof(state->queue[0]));
|
||||
state->queue_count -= 1;
|
||||
return 1;
|
||||
}
|
||||
|
||||
static void sync_hal_pins(void)
|
||||
{
|
||||
int i;
|
||||
LcmotRuntime *state = &lcmot_state;
|
||||
if (hal_motion_program_line) {
|
||||
*hal_motion_program_line = state->program_line;
|
||||
}
|
||||
if (hal_motion_motion_type) {
|
||||
*hal_motion_motion_type = state->motion_type;
|
||||
}
|
||||
if (hal_motion_coord_mode) {
|
||||
*hal_motion_coord_mode = state->coord_mode;
|
||||
}
|
||||
if (hal_motion_teleop_mode) {
|
||||
*hal_motion_teleop_mode = state->teleop_mode;
|
||||
}
|
||||
if (hal_motion_in_position) {
|
||||
*hal_motion_in_position = state->in_position ? 1 : 0;
|
||||
}
|
||||
if (hal_motion_switchkins_type) {
|
||||
*hal_motion_switchkins_type = state->switchkins_type;
|
||||
}
|
||||
if (hal_motion_analog_out_03) {
|
||||
*hal_motion_analog_out_03 = state->analog_out_03;
|
||||
}
|
||||
for (i = 0; i < 9; ++i) {
|
||||
if (hal_axis_cmd[i]) {
|
||||
*hal_axis_cmd[i] = state->axis_cmd[i];
|
||||
}
|
||||
if (hal_axis_fb[i]) {
|
||||
*hal_axis_fb[i] = state->axis_fb[i];
|
||||
}
|
||||
if (hal_joint_cmd[i]) {
|
||||
*hal_joint_cmd[i] = state->joint_cmd[i];
|
||||
}
|
||||
if (hal_joint_fb[i]) {
|
||||
*hal_joint_fb[i] = state->joint_fb[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void create_motion_hal_pins(void)
|
||||
{
|
||||
int comp_id = hal_init("linuxcnc-motion-runtime");
|
||||
int i;
|
||||
char name[64];
|
||||
hal_pin_s32_new("motion.program-line", HAL_OUT, &hal_motion_program_line, comp_id);
|
||||
hal_pin_s32_new("motion.motion-type", HAL_OUT, &hal_motion_motion_type, comp_id);
|
||||
hal_pin_s32_new("motion.coord-mode", HAL_OUT, &hal_motion_coord_mode, comp_id);
|
||||
hal_pin_s32_new("motion.teleop-mode", HAL_OUT, &hal_motion_teleop_mode, comp_id);
|
||||
hal_pin_bit_new("motion.in-position", HAL_OUT, &hal_motion_in_position, comp_id);
|
||||
hal_pin_s32_new("motion.switchkins-type", HAL_IO, &hal_motion_switchkins_type, comp_id);
|
||||
hal_pin_float_new("motion.analog-out-03", HAL_OUT, &hal_motion_analog_out_03, comp_id);
|
||||
for (i = 0; i < 9; ++i) {
|
||||
snprintf(name, sizeof(name), "joint.%d.motor-pos-cmd", i);
|
||||
hal_pin_float_new(name, HAL_OUT, &hal_joint_cmd[i], comp_id);
|
||||
snprintf(name, sizeof(name), "joint.%d.motor-pos-fb", i);
|
||||
hal_pin_float_new(name, HAL_OUT, &hal_joint_fb[i], comp_id);
|
||||
snprintf(name, sizeof(name), "axis.%d.pos-cmd", i);
|
||||
hal_pin_float_new(name, HAL_OUT, &hal_axis_cmd[i], comp_id);
|
||||
snprintf(name, sizeof(name), "axis.%d.pos-fb", i);
|
||||
hal_pin_float_new(name, HAL_OUT, &hal_axis_fb[i], comp_id);
|
||||
}
|
||||
hal_ready(comp_id);
|
||||
}
|
||||
|
||||
static void apply_command(const LcmotCommand *command)
|
||||
{
|
||||
int i;
|
||||
LcmotRuntime *state = &lcmot_state;
|
||||
if (!command) {
|
||||
return;
|
||||
}
|
||||
switch (command->type) {
|
||||
case LCMOT_CMD_PAUSE:
|
||||
state->paused = 1;
|
||||
state->motion_type = 0;
|
||||
state->in_position = 0;
|
||||
break;
|
||||
case LCMOT_CMD_RESUME:
|
||||
state->paused = 0;
|
||||
break;
|
||||
case LCMOT_CMD_ABORT:
|
||||
state->aborted = 1;
|
||||
state->paused = 0;
|
||||
state->motion_type = 0;
|
||||
state->in_position = 1;
|
||||
state->queue_head = 0;
|
||||
state->queue_tail = 0;
|
||||
state->queue_count = 0;
|
||||
break;
|
||||
case LCMOT_CMD_SET_AOUT:
|
||||
state->analog_out_03 = command->analog_value;
|
||||
state->switchkins_type = (int)lrint(command->analog_value);
|
||||
break;
|
||||
case LCMOT_CMD_LINEAR_MOVE:
|
||||
case LCMOT_CMD_CIRCULAR_MOVE:
|
||||
state->program_line = command->line;
|
||||
state->motion_type = command->type == LCMOT_CMD_CIRCULAR_MOVE ? 2 : 1;
|
||||
state->coord_mode = 1;
|
||||
state->teleop_mode = 0;
|
||||
state->in_position = 0;
|
||||
state->requested_vel = command->velocity > 0.0 ? command->velocity : 60.0;
|
||||
for (i = 0; i < 9; ++i) {
|
||||
state->axis_cmd[i] = command->target[i];
|
||||
state->axis_fb[i] = command->target[i];
|
||||
state->joint_cmd[i] = command->target[i];
|
||||
state->joint_fb[i] = command->target[i];
|
||||
}
|
||||
state->current_vel = state->requested_vel;
|
||||
state->in_position = 1;
|
||||
state->motion_type = 0;
|
||||
break;
|
||||
case LCMOT_CMD_JOG_INCR:
|
||||
i = command->axis_index;
|
||||
if (i < 0 || i >= 9) {
|
||||
i = 0;
|
||||
}
|
||||
state->motion_type = 3;
|
||||
state->coord_mode = 0;
|
||||
state->teleop_mode = 1;
|
||||
state->in_position = 0;
|
||||
state->axis_cmd[i] += command->distance;
|
||||
state->axis_fb[i] = state->axis_cmd[i];
|
||||
state->joint_cmd[i] = state->axis_cmd[i];
|
||||
state->joint_fb[i] = state->axis_cmd[i];
|
||||
state->requested_vel = command->velocity > 0.0 ? command->velocity : 60.0;
|
||||
state->current_vel = state->requested_vel;
|
||||
state->in_position = 1;
|
||||
state->motion_type = 0;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int lcmot_init_from_ini(const char *ini_path, const char *ini_text)
|
||||
{
|
||||
(void)ini_path;
|
||||
(void)ini_text;
|
||||
memset(&lcmot_state, 0, sizeof(lcmot_state));
|
||||
lcmot_state.initialized = 1;
|
||||
lcmot_state.in_position = 1;
|
||||
lcmot_state.coord_mode = 1;
|
||||
create_motion_hal_pins();
|
||||
sync_hal_pins();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int lcmot_write_command_json(const char *json)
|
||||
{
|
||||
int i;
|
||||
LcmotCommand command;
|
||||
if (!json || !lcmot_state.initialized) {
|
||||
return -1;
|
||||
}
|
||||
memset(&command, 0, sizeof(command));
|
||||
command.line = json_int_after(json, "\"line\"", lcmot_state.program_line);
|
||||
command.velocity = json_number_after(json, "\"velocity\"", 60.0);
|
||||
for (i = 0; i < 9; ++i) {
|
||||
command.target[i] = lcmot_state.axis_cmd[i];
|
||||
}
|
||||
command.target[0] = json_number_after(json, "\"x\"", command.target[0]);
|
||||
command.target[1] = json_number_after(json, "\"y\"", command.target[1]);
|
||||
command.target[2] = json_number_after(json, "\"z\"", command.target[2]);
|
||||
command.target[3] = json_number_after(json, "\"a\"", command.target[3]);
|
||||
command.target[4] = json_number_after(json, "\"b\"", command.target[4]);
|
||||
command.target[5] = json_number_after(json, "\"c\"", command.target[5]);
|
||||
command.distance = json_number_after(json, "\"distance\"", 0.0);
|
||||
command.axis_index = axis_index_from_json(json);
|
||||
command.analog_value = json_number_after(json, "\"value\"", lcmot_state.analog_out_03);
|
||||
|
||||
if (contains_token(json, "EMC_TRAJ_LINEAR_MOVE") || contains_token(json, "EMCMOT_SET_LINE")) {
|
||||
command.type = LCMOT_CMD_LINEAR_MOVE;
|
||||
} else if (contains_token(json, "EMC_TRAJ_CIRCULAR_MOVE") || contains_token(json, "EMCMOT_SET_CIRCLE")) {
|
||||
command.type = LCMOT_CMD_CIRCULAR_MOVE;
|
||||
} else if (contains_token(json, "EMC_JOG_INCR") || contains_token(json, "EMCMOT_JOG_INCR")) {
|
||||
command.type = LCMOT_CMD_JOG_INCR;
|
||||
} else if (contains_token(json, "EMC_TRAJ_PAUSE") || contains_token(json, "EMCMOT_PAUSE")) {
|
||||
command.type = LCMOT_CMD_PAUSE;
|
||||
} else if (contains_token(json, "EMC_TRAJ_RESUME") || contains_token(json, "EMCMOT_RESUME")) {
|
||||
command.type = LCMOT_CMD_RESUME;
|
||||
} else if (contains_token(json, "EMC_TRAJ_ABORT") || contains_token(json, "EMCMOT_ABORT")) {
|
||||
command.type = LCMOT_CMD_ABORT;
|
||||
} else if (contains_token(json, "EMCMOT_SET_AOUT") || contains_token(json, "SET_AOUT")) {
|
||||
command.type = LCMOT_CMD_SET_AOUT;
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
return queue_push(command);
|
||||
}
|
||||
|
||||
int lcmot_step_servo(long period_ns, int cycles)
|
||||
{
|
||||
int i;
|
||||
LcmotCommand command;
|
||||
if (!lcmot_state.initialized || cycles < 0) {
|
||||
return -1;
|
||||
}
|
||||
for (i = 0; i < cycles; ++i) {
|
||||
lcmot_state.cycle += 1;
|
||||
if (!lcmot_state.aborted && lcmot_state.queue_count > 0) {
|
||||
command = lcmot_state.queue[lcmot_state.queue_head];
|
||||
if (!lcmot_state.paused ||
|
||||
command.type == LCMOT_CMD_RESUME ||
|
||||
command.type == LCMOT_CMD_ABORT) {
|
||||
queue_pop(&command);
|
||||
apply_command(&command);
|
||||
}
|
||||
}
|
||||
sync_hal_pins();
|
||||
lchal_step_threads(period_ns, 1);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int lcmot_read_status_json(char *out, int out_len)
|
||||
{
|
||||
char buffer[4096];
|
||||
LcmotRuntime *state = &lcmot_state;
|
||||
snprintf(buffer, sizeof(buffer),
|
||||
"{\"semanticBoundary\":\"linuxcnc_motion_runtime_phase3_minimal\","
|
||||
"\"motionRuntimeReady\":true,"
|
||||
"\"nativeHalSyncReady\":false,"
|
||||
"\"cycle\":%lld,"
|
||||
"\"motion\":{\"programLine\":%d,\"motionType\":%d,\"coordMode\":%d,"
|
||||
"\"teleopMode\":%d,\"inPosition\":%s,\"paused\":%s,\"aborted\":%s,"
|
||||
"\"switchkinsType\":%d,\"requestedVel\":%.17g,\"currentVel\":%.17g},"
|
||||
"\"axis\":{\"x\":%.17g,\"y\":%.17g,\"z\":%.17g,\"a\":%.17g,\"b\":%.17g,\"c\":%.17g},"
|
||||
"\"joint0\":{\"motorPosCmd\":%.17g,\"motorPosFb\":%.17g},"
|
||||
"\"commandQueueDepth\":%d}",
|
||||
state->cycle,
|
||||
state->program_line,
|
||||
state->motion_type,
|
||||
state->coord_mode,
|
||||
state->teleop_mode,
|
||||
state->in_position ? "true" : "false",
|
||||
state->paused ? "true" : "false",
|
||||
state->aborted ? "true" : "false",
|
||||
state->switchkins_type,
|
||||
state->requested_vel,
|
||||
state->current_vel,
|
||||
state->axis_fb[0],
|
||||
state->axis_fb[1],
|
||||
state->axis_fb[2],
|
||||
state->axis_fb[3],
|
||||
state->axis_fb[4],
|
||||
state->axis_fb[5],
|
||||
state->joint_cmd[0],
|
||||
state->joint_fb[0],
|
||||
state->queue_count);
|
||||
return write_output(buffer, out, out_len);
|
||||
}
|
||||
|
||||
int lcmot_read_hal_snapshot_json(char *out, int out_len)
|
||||
{
|
||||
return lchal_get_snapshot_json(out, out_len);
|
||||
}
|
||||
|
||||
int lcmot_reset(void)
|
||||
{
|
||||
memset(&lcmot_state, 0, sizeof(lcmot_state));
|
||||
hal_motion_program_line = NULL;
|
||||
hal_motion_motion_type = NULL;
|
||||
hal_motion_coord_mode = NULL;
|
||||
hal_motion_teleop_mode = NULL;
|
||||
hal_motion_in_position = NULL;
|
||||
hal_motion_switchkins_type = NULL;
|
||||
hal_motion_analog_out_03 = NULL;
|
||||
memset(hal_joint_cmd, 0, sizeof(hal_joint_cmd));
|
||||
memset(hal_joint_fb, 0, sizeof(hal_joint_fb));
|
||||
memset(hal_axis_cmd, 0, sizeof(hal_axis_cmd));
|
||||
memset(hal_axis_fb, 0, sizeof(hal_axis_fb));
|
||||
lchal_reset_runtime();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
int lcmot_init_from_ini(const char *ini_path, const char *ini_text);
|
||||
int lcmot_write_command_json(const char *json);
|
||||
int lcmot_step_servo(long period_ns, int cycles);
|
||||
int lcmot_read_status_json(char *out, int out_len);
|
||||
int lcmot_read_hal_snapshot_json(char *out, int out_len);
|
||||
int lcmot_reset(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
467
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_task_hal_wasm.cpp
Normal file
467
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_task_hal_wasm.cpp
Normal file
@@ -0,0 +1,467 @@
|
||||
#include "linuxcnc_task_hal_wasm.hh"
|
||||
|
||||
#include "linuxcnc_motion_runtime.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cctype>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <map>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace {
|
||||
|
||||
struct TaskRuntime {
|
||||
bool initialized = false;
|
||||
std::string state = "ESTOP";
|
||||
std::string mode = "MANUAL";
|
||||
std::string interp_state = "IDLE";
|
||||
std::string exec_state = "DONE";
|
||||
std::string open_program;
|
||||
int opened_line_count = 0;
|
||||
int next_program_line = 0;
|
||||
long long task_cycle = 0;
|
||||
long long servo_cycle = 0;
|
||||
std::map<std::string, std::string> staged_files;
|
||||
std::vector<std::string> program_lines;
|
||||
std::vector<std::string> events;
|
||||
};
|
||||
|
||||
TaskRuntime &task_runtime()
|
||||
{
|
||||
static TaskRuntime state;
|
||||
return state;
|
||||
}
|
||||
|
||||
std::string json_escape(const std::string &value)
|
||||
{
|
||||
std::ostringstream out;
|
||||
for (const char ch : value) {
|
||||
switch (ch) {
|
||||
case '\\':
|
||||
out << "\\\\";
|
||||
break;
|
||||
case '"':
|
||||
out << "\\\"";
|
||||
break;
|
||||
case '\n':
|
||||
out << "\\n";
|
||||
break;
|
||||
case '\r':
|
||||
out << "\\r";
|
||||
break;
|
||||
case '\t':
|
||||
out << "\\t";
|
||||
break;
|
||||
default:
|
||||
out << ch;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return out.str();
|
||||
}
|
||||
|
||||
int write_output(const std::string &value, char *out, int out_len)
|
||||
{
|
||||
if (!out || out_len <= 0) {
|
||||
return -1;
|
||||
}
|
||||
const int required = static_cast<int>(value.size()) + 1;
|
||||
if (out_len < required) {
|
||||
out[0] = '\0';
|
||||
return required;
|
||||
}
|
||||
std::memcpy(out, value.c_str(), static_cast<std::size_t>(required));
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool contains_token(const char *json, const char *token)
|
||||
{
|
||||
return json && token && std::strstr(json, token) != nullptr;
|
||||
}
|
||||
|
||||
std::string json_string_after(const char *json, const char *key, const std::string &fallback = "")
|
||||
{
|
||||
const char *at = std::strstr(json ? json : "", key);
|
||||
if (!at) {
|
||||
return fallback;
|
||||
}
|
||||
at = std::strchr(at, ':');
|
||||
if (!at) {
|
||||
return fallback;
|
||||
}
|
||||
at += 1;
|
||||
while (*at && std::isspace(static_cast<unsigned char>(*at))) {
|
||||
at += 1;
|
||||
}
|
||||
if (*at != '"') {
|
||||
return fallback;
|
||||
}
|
||||
at += 1;
|
||||
std::string value;
|
||||
while (*at) {
|
||||
if (*at == '\\' && at[1]) {
|
||||
value.push_back(at[1]);
|
||||
at += 2;
|
||||
continue;
|
||||
}
|
||||
if (*at == '"') {
|
||||
return value;
|
||||
}
|
||||
value.push_back(*at);
|
||||
at += 1;
|
||||
}
|
||||
return fallback;
|
||||
}
|
||||
|
||||
double json_number_after(const char *json, const char *key, double fallback)
|
||||
{
|
||||
const char *at = std::strstr(json ? json : "", key);
|
||||
char *end = nullptr;
|
||||
if (!at) {
|
||||
return fallback;
|
||||
}
|
||||
at = std::strchr(at, ':');
|
||||
if (!at) {
|
||||
return fallback;
|
||||
}
|
||||
at += 1;
|
||||
while (*at && (std::isspace(static_cast<unsigned char>(*at)) || *at == '"')) {
|
||||
at += 1;
|
||||
}
|
||||
const double value = std::strtod(at, &end);
|
||||
return end == at ? fallback : value;
|
||||
}
|
||||
|
||||
int json_int_after(const char *json, const char *key, int fallback)
|
||||
{
|
||||
return static_cast<int>(json_number_after(json, key, fallback));
|
||||
}
|
||||
|
||||
std::string trim_copy(const std::string &value)
|
||||
{
|
||||
std::size_t begin = 0;
|
||||
while (begin < value.size() && std::isspace(static_cast<unsigned char>(value[begin]))) {
|
||||
begin += 1;
|
||||
}
|
||||
std::size_t end = value.size();
|
||||
while (end > begin && std::isspace(static_cast<unsigned char>(value[end - 1]))) {
|
||||
end -= 1;
|
||||
}
|
||||
return value.substr(begin, end - begin);
|
||||
}
|
||||
|
||||
std::vector<std::string> split_program_lines(const std::string &text)
|
||||
{
|
||||
std::vector<std::string> lines;
|
||||
std::istringstream input(text);
|
||||
std::string line;
|
||||
while (std::getline(input, line)) {
|
||||
line = trim_copy(line);
|
||||
if (line.empty() || line[0] == '(' || line[0] == ';') {
|
||||
continue;
|
||||
}
|
||||
lines.push_back(line);
|
||||
}
|
||||
return lines;
|
||||
}
|
||||
|
||||
int forward_motion_command(const std::string &json)
|
||||
{
|
||||
return lcmot_write_command_json(json.c_str());
|
||||
}
|
||||
|
||||
void enqueue_linear_move_from_line(TaskRuntime &state, const std::string &line)
|
||||
{
|
||||
std::ostringstream command;
|
||||
const int line_number = state.next_program_line + 1;
|
||||
command << "{\"type\":\"EMC_TRAJ_LINEAR_MOVE\",\"line\":" << line_number;
|
||||
double fallback = 0.0;
|
||||
const char *axes = "XYZABC";
|
||||
for (const char *axis = axes; *axis; ++axis) {
|
||||
const std::size_t pos = line.find(*axis);
|
||||
if (pos == std::string::npos) {
|
||||
continue;
|
||||
}
|
||||
char *end = nullptr;
|
||||
const double value = std::strtod(line.c_str() + pos + 1, &end);
|
||||
if (end != line.c_str() + pos + 1) {
|
||||
command << ",\"" << static_cast<char>(std::tolower(*axis)) << "\":" << value;
|
||||
fallback = value;
|
||||
}
|
||||
}
|
||||
if (line.find('X') == std::string::npos && line.find('Y') == std::string::npos &&
|
||||
line.find('Z') == std::string::npos && line.find('A') == std::string::npos &&
|
||||
line.find('B') == std::string::npos && line.find('C') == std::string::npos) {
|
||||
command << ",\"x\":" << fallback;
|
||||
}
|
||||
command << ",\"velocity\":60}";
|
||||
forward_motion_command(command.str());
|
||||
state.events.push_back("task_queue_motion_line:" + std::to_string(line_number));
|
||||
state.next_program_line += 1;
|
||||
}
|
||||
|
||||
void enqueue_mdi(TaskRuntime &state, const char *json)
|
||||
{
|
||||
const std::string mdi = json_string_after(json, "\"mdi\"");
|
||||
if (mdi.find("M428") != std::string::npos) {
|
||||
forward_motion_command("{\"type\":\"EMCMOT_SET_AOUT\",\"value\":1}");
|
||||
state.events.push_back("task_mdi_switchkins:M428");
|
||||
return;
|
||||
}
|
||||
if (mdi.find("M429") != std::string::npos) {
|
||||
forward_motion_command("{\"type\":\"EMCMOT_SET_AOUT\",\"value\":0}");
|
||||
state.events.push_back("task_mdi_switchkins:M429");
|
||||
return;
|
||||
}
|
||||
if (mdi.find("M430") != std::string::npos) {
|
||||
forward_motion_command("{\"type\":\"EMCMOT_SET_AOUT\",\"value\":2}");
|
||||
state.events.push_back("task_mdi_switchkins:M430");
|
||||
return;
|
||||
}
|
||||
enqueue_linear_move_from_line(state, mdi.empty() ? "G0 X0" : mdi);
|
||||
state.events.push_back("task_mdi_execute");
|
||||
}
|
||||
|
||||
std::string read_motion_status_json()
|
||||
{
|
||||
std::vector<char> buffer(8192);
|
||||
const int rc = lcmot_read_status_json(buffer.data(), static_cast<int>(buffer.size()));
|
||||
if (rc != 0) {
|
||||
return "{}";
|
||||
}
|
||||
return buffer.data();
|
||||
}
|
||||
|
||||
std::string read_hal_snapshot_json()
|
||||
{
|
||||
std::vector<char> buffer(65536);
|
||||
const int rc = lcmot_read_hal_snapshot_json(buffer.data(), static_cast<int>(buffer.size()));
|
||||
if (rc != 0) {
|
||||
return "{}";
|
||||
}
|
||||
return buffer.data();
|
||||
}
|
||||
|
||||
std::string status_json()
|
||||
{
|
||||
const auto &state = task_runtime();
|
||||
std::ostringstream out;
|
||||
out << "{\"semanticBoundary\":\"linuxcnc_task_motion_hal_wasm_phase4_minimal\"";
|
||||
out << ",\"taskRuntimeReady\":true";
|
||||
out << ",\"taskStatusFromLinuxCncRuntime\":true";
|
||||
out << ",\"taskCommandsDriveMotionRuntime\":true";
|
||||
out << ",\"nativeTaskReady\":false";
|
||||
out << ",\"nativeHalSyncReady\":false";
|
||||
out << ",\"fullLinuxCncProgramExecutionReady\":false";
|
||||
out << ",\"hardwareDrive\":false";
|
||||
out << ",\"task\":{\"state\":\"" << json_escape(state.state) << "\"";
|
||||
out << ",\"mode\":\"" << json_escape(state.mode) << "\"";
|
||||
out << ",\"interpState\":\"" << json_escape(state.interp_state) << "\"";
|
||||
out << ",\"execState\":\"" << json_escape(state.exec_state) << "\"";
|
||||
out << ",\"cycle\":" << state.task_cycle;
|
||||
out << ",\"openProgram\":\"" << json_escape(state.open_program) << "\"";
|
||||
out << ",\"openedLineCount\":" << state.opened_line_count;
|
||||
out << ",\"nextProgramLine\":" << state.next_program_line << "}";
|
||||
out << ",\"servoCycle\":" << state.servo_cycle;
|
||||
out << ",\"motionStatus\":" << read_motion_status_json();
|
||||
out << ",\"halSnapshot\":" << read_hal_snapshot_json();
|
||||
out << "}";
|
||||
return out.str();
|
||||
}
|
||||
|
||||
std::string events_json()
|
||||
{
|
||||
const auto &state = task_runtime();
|
||||
std::ostringstream out;
|
||||
out << "{\"events\":[";
|
||||
for (std::size_t i = 0; i < state.events.size(); ++i) {
|
||||
if (i > 0) {
|
||||
out << ",";
|
||||
}
|
||||
out << "\"" << json_escape(state.events[i]) << "\"";
|
||||
}
|
||||
out << "]}";
|
||||
return out.str();
|
||||
}
|
||||
|
||||
void reset_task_only()
|
||||
{
|
||||
task_runtime() = TaskRuntime{};
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
extern "C" {
|
||||
|
||||
int lctask_init_session(const char *session_json)
|
||||
{
|
||||
reset_task_only();
|
||||
auto &state = task_runtime();
|
||||
state.initialized = true;
|
||||
state.state = "ESTOP_RESET";
|
||||
state.mode = "MANUAL";
|
||||
state.interp_state = "IDLE";
|
||||
state.exec_state = "DONE";
|
||||
state.events.push_back("task_session_init");
|
||||
|
||||
const std::string ini_path = json_string_after(session_json, "\"iniPath\"", "task-hal-session.ini");
|
||||
const std::string ini_text = json_string_after(session_json, "\"iniText\"", "");
|
||||
if (lcmot_init_from_ini(ini_path.c_str(), ini_text.c_str()) != 0) {
|
||||
state.events.push_back("task_session_motion_init_failed");
|
||||
return -1;
|
||||
}
|
||||
state.events.push_back("task_session_motion_init");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int lctask_stage_file(const char *path, const char *text)
|
||||
{
|
||||
auto &state = task_runtime();
|
||||
if (!state.initialized || !path || !text) {
|
||||
return -1;
|
||||
}
|
||||
state.staged_files[path] = text;
|
||||
state.events.push_back(std::string("task_stage_file:") + path);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int lctask_open_program(const char *path)
|
||||
{
|
||||
auto &state = task_runtime();
|
||||
if (!state.initialized || !path) {
|
||||
return -1;
|
||||
}
|
||||
const auto it = state.staged_files.find(path);
|
||||
if (it == state.staged_files.end()) {
|
||||
return -1;
|
||||
}
|
||||
state.open_program = path;
|
||||
state.program_lines = split_program_lines(it->second);
|
||||
state.opened_line_count = static_cast<int>(state.program_lines.size());
|
||||
state.next_program_line = 0;
|
||||
state.interp_state = "IDLE";
|
||||
state.exec_state = "DONE";
|
||||
state.events.push_back(std::string("task_open_program:") + path);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int lctask_send_command_json(const char *command_json)
|
||||
{
|
||||
auto &state = task_runtime();
|
||||
if (!state.initialized || !command_json) {
|
||||
return -1;
|
||||
}
|
||||
if (contains_token(command_json, "EMC_TASK_SET_STATE")) {
|
||||
state.state = json_string_after(command_json, "\"state\"", state.state);
|
||||
state.events.push_back("task_set_state:" + state.state);
|
||||
return 0;
|
||||
}
|
||||
if (contains_token(command_json, "EMC_TASK_SET_MODE")) {
|
||||
state.mode = json_string_after(command_json, "\"mode\"", state.mode);
|
||||
state.events.push_back("task_set_mode:" + state.mode);
|
||||
return 0;
|
||||
}
|
||||
if (contains_token(command_json, "EMC_TASK_PLAN_RUN")) {
|
||||
if (state.open_program.empty()) {
|
||||
return -1;
|
||||
}
|
||||
state.next_program_line = json_int_after(command_json, "\"line\"", 0);
|
||||
if (state.next_program_line < 0) {
|
||||
state.next_program_line = 0;
|
||||
}
|
||||
if (state.next_program_line >= state.opened_line_count) {
|
||||
state.next_program_line = 0;
|
||||
}
|
||||
state.interp_state = "READING";
|
||||
state.exec_state = "WAITING_FOR_MOTION";
|
||||
state.events.push_back("task_plan_run");
|
||||
return 0;
|
||||
}
|
||||
if (contains_token(command_json, "EMC_TASK_PLAN_PAUSE")) {
|
||||
state.interp_state = "PAUSED";
|
||||
state.exec_state = "PAUSED";
|
||||
state.events.push_back("task_plan_pause");
|
||||
return forward_motion_command("{\"type\":\"EMC_TRAJ_PAUSE\"}");
|
||||
}
|
||||
if (contains_token(command_json, "EMC_TASK_PLAN_RESUME")) {
|
||||
state.interp_state = "READING";
|
||||
state.exec_state = "WAITING_FOR_MOTION";
|
||||
state.events.push_back("task_plan_resume");
|
||||
return forward_motion_command("{\"type\":\"EMC_TRAJ_RESUME\"}");
|
||||
}
|
||||
if (contains_token(command_json, "EMC_TASK_ABORT")) {
|
||||
state.interp_state = "IDLE";
|
||||
state.exec_state = "DONE";
|
||||
state.events.push_back("task_abort");
|
||||
return forward_motion_command("{\"type\":\"EMC_TRAJ_ABORT\"}");
|
||||
}
|
||||
if (contains_token(command_json, "EMC_TASK_PLAN_EXECUTE")) {
|
||||
state.mode = "MDI";
|
||||
state.interp_state = "READING";
|
||||
state.exec_state = "WAITING_FOR_MOTION";
|
||||
enqueue_mdi(state, command_json);
|
||||
return 0;
|
||||
}
|
||||
if (contains_token(command_json, "EMC_JOG_INCR")) {
|
||||
state.mode = "MANUAL";
|
||||
state.interp_state = "IDLE";
|
||||
state.exec_state = "WAITING_FOR_MOTION";
|
||||
state.events.push_back("task_jog_incr");
|
||||
return forward_motion_command(command_json);
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int lctask_run_cycles(long task_period_ns, long servo_period_ns, int task_cycles)
|
||||
{
|
||||
auto &state = task_runtime();
|
||||
if (!state.initialized || task_cycles < 0 || servo_period_ns <= 0) {
|
||||
return -1;
|
||||
}
|
||||
int servo_per_task = 1;
|
||||
if (task_period_ns > 0) {
|
||||
servo_per_task = static_cast<int>(task_period_ns / servo_period_ns);
|
||||
if (servo_per_task <= 0) {
|
||||
servo_per_task = 1;
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < task_cycles; ++i) {
|
||||
state.task_cycle += 1;
|
||||
if (state.interp_state == "READING" && state.next_program_line < state.opened_line_count) {
|
||||
enqueue_linear_move_from_line(state, state.program_lines[state.next_program_line]);
|
||||
if (state.next_program_line >= state.opened_line_count) {
|
||||
state.interp_state = "IDLE";
|
||||
state.exec_state = "DONE";
|
||||
state.events.push_back("task_plan_complete");
|
||||
}
|
||||
}
|
||||
if (lcmot_step_servo(servo_period_ns, servo_per_task) != 0) {
|
||||
return -1;
|
||||
}
|
||||
state.servo_cycle += servo_per_task;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int lctask_read_status_json(char *out, int out_len)
|
||||
{
|
||||
return write_output(status_json(), out, out_len);
|
||||
}
|
||||
|
||||
int lctask_read_events_json(char *out, int out_len)
|
||||
{
|
||||
return write_output(events_json(), out, out_len);
|
||||
}
|
||||
|
||||
int lctask_reset_session(void)
|
||||
{
|
||||
reset_task_only();
|
||||
lcmot_reset();
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // extern "C"
|
||||
@@ -0,0 +1,18 @@
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
int lctask_init_session(const char *session_json);
|
||||
int lctask_stage_file(const char *path, const char *text);
|
||||
int lctask_open_program(const char *path);
|
||||
int lctask_send_command_json(const char *command_json);
|
||||
int lctask_run_cycles(long task_period_ns, long servo_period_ns, int task_cycles);
|
||||
int lctask_read_status_json(char *out, int out_len);
|
||||
int lctask_read_events_json(char *out, int out_len);
|
||||
int lctask_reset_session(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
@@ -21,9 +21,13 @@ typedef enum {
|
||||
HAL_IN = 16,
|
||||
HAL_OUT = 32,
|
||||
HAL_IO = (HAL_IN | HAL_OUT),
|
||||
HAL_RW = HAL_IO,
|
||||
} hal_pin_dir_t;
|
||||
|
||||
typedef enum {
|
||||
HAL_RO = 64,
|
||||
HAL_RW = HAL_RO | 128,
|
||||
} hal_param_dir_t;
|
||||
|
||||
typedef union {
|
||||
bool b;
|
||||
double f;
|
||||
@@ -59,9 +63,29 @@ int hal_pin_u32_newf(hal_pin_dir_t, hal_u32_t **, int, const char *, ...);
|
||||
int hal_pin_s32_newf(hal_pin_dir_t, hal_s32_t **, int, const char *, ...);
|
||||
int hal_pin_u64_newf(hal_pin_dir_t, hal_u64_t **, int, const char *, ...);
|
||||
int hal_pin_s64_newf(hal_pin_dir_t, hal_s64_t **, int, const char *, ...);
|
||||
int hal_param_float_newf(hal_pin_dir_t, hal_float_t *, int, const char *, ...);
|
||||
int hal_param_bit_new(const char *, hal_param_dir_t, hal_bit_t *, int);
|
||||
int hal_param_float_new(const char *, hal_param_dir_t, hal_float_t *, int);
|
||||
int hal_param_u32_new(const char *, hal_param_dir_t, hal_u32_t *, int);
|
||||
int hal_param_s32_new(const char *, hal_param_dir_t, hal_s32_t *, int);
|
||||
int hal_param_u64_new(const char *, hal_param_dir_t, hal_u64_t *, int);
|
||||
int hal_param_s64_new(const char *, hal_param_dir_t, hal_s64_t *, int);
|
||||
int hal_param_bit_newf(hal_param_dir_t, hal_bit_t *, int, const char *, ...);
|
||||
int hal_param_float_newf(hal_param_dir_t, hal_float_t *, int, const char *, ...);
|
||||
int hal_param_u32_newf(hal_param_dir_t, hal_u32_t *, int, const char *, ...);
|
||||
int hal_param_s32_newf(hal_param_dir_t, hal_s32_t *, int, const char *, ...);
|
||||
int hal_param_u64_newf(hal_param_dir_t, hal_u64_t *, int, const char *, ...);
|
||||
int hal_param_s64_newf(hal_param_dir_t, hal_s64_t *, int, const char *, ...);
|
||||
int hal_get_pin_value_by_name(const char *, hal_type_t *, hal_data_u **, bool *);
|
||||
int hal_get_signal_value_by_name(const char *, hal_type_t *, hal_data_u **, bool *);
|
||||
int hal_get_param_value_by_name(const char *, hal_type_t *, hal_data_u **);
|
||||
int hal_link(const char *, const char *);
|
||||
int hal_unlink(const char *);
|
||||
int hal_set_p(const char *, const char *);
|
||||
int hal_get_p(const char *, char *, int);
|
||||
int hal_create_thread(const char *, unsigned long, int);
|
||||
int hal_add_funct_to_thread(const char *, const char *, double, int);
|
||||
int hal_del_funct_from_thread(const char *, const char *);
|
||||
int hal_start_threads(void);
|
||||
int hal_stop_threads(void);
|
||||
|
||||
RTAPI_END_DECLS
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
export { createLinuxCncIniSdk } from "./linuxcnc-ini.js";
|
||||
export { createLinuxCncInterpSdk } from "./linuxcnc-interp.js";
|
||||
export { createLinuxCncTaskHalSdk } from "./linuxcnc-task-hal.js";
|
||||
export {
|
||||
createLinuxCncKinematicsSdk,
|
||||
linuxCncKinematicsWasmFile,
|
||||
|
||||
128
wasm-port/runtime/sdk/src/linuxcnc-task-hal.js
Normal file
128
wasm-port/runtime/sdk/src/linuxcnc-task-hal.js
Normal file
@@ -0,0 +1,128 @@
|
||||
import createLinuxCncTaskHalModule from "../../../build/wasm/task-hal/linuxcnc_task_hal.js";
|
||||
|
||||
const SEMANTIC_BOUNDARY = "linuxcnc_task_motion_hal_wasm_phase4_minimal";
|
||||
|
||||
function allocCString(mod, value) {
|
||||
const text = String(value ?? "");
|
||||
const bytes = mod.lengthBytesUTF8(text) + 1;
|
||||
const ptr = mod._malloc(bytes);
|
||||
mod.stringToUTF8(text, ptr, bytes);
|
||||
return ptr;
|
||||
}
|
||||
|
||||
function withCString(mod, value, fn) {
|
||||
const ptr = allocCString(mod, value);
|
||||
try {
|
||||
return fn(ptr);
|
||||
} finally {
|
||||
mod._free(ptr);
|
||||
}
|
||||
}
|
||||
|
||||
function requireWasmFunction(mod, functionName) {
|
||||
const fn = mod[`_${functionName}`];
|
||||
if (typeof fn !== "function") {
|
||||
throw new Error(`linuxcnc task/HAL WASM missing ${functionName}; rebuild wasm-port/tools/build_task_hal_wasm.sh`);
|
||||
}
|
||||
return fn;
|
||||
}
|
||||
|
||||
function readJson(mod, functionName) {
|
||||
const fn = requireWasmFunction(mod, functionName);
|
||||
let bytes = 131072;
|
||||
for (let attempt = 0; attempt < 2; attempt += 1) {
|
||||
const ptr = mod._malloc(bytes);
|
||||
try {
|
||||
const rc = fn(ptr, bytes);
|
||||
if (rc === 0) {
|
||||
return JSON.parse(mod.UTF8ToString(ptr));
|
||||
}
|
||||
if (rc > bytes) {
|
||||
bytes = rc;
|
||||
continue;
|
||||
}
|
||||
throw new Error(`${functionName} failed with rc=${rc}`);
|
||||
} finally {
|
||||
mod._free(ptr);
|
||||
}
|
||||
}
|
||||
throw new Error(`${functionName} output exceeded buffer`);
|
||||
}
|
||||
|
||||
function callWithJson(mod, functionName, payload) {
|
||||
const fn = requireWasmFunction(mod, functionName);
|
||||
return withCString(mod, JSON.stringify(payload ?? {}), (ptr) => fn(ptr));
|
||||
}
|
||||
|
||||
export async function createLinuxCncTaskHalSdk(moduleOptions = {}) {
|
||||
const mod = await createLinuxCncTaskHalModule(moduleOptions);
|
||||
|
||||
return {
|
||||
apiName: "linuxcnc-task-hal-wasm-sdk",
|
||||
semanticBoundary: SEMANTIC_BOUNDARY,
|
||||
module: mod,
|
||||
|
||||
readiness() {
|
||||
return {
|
||||
apiName: "linuxcnc-task-hal-wasm-sdk-readiness",
|
||||
loaded: true,
|
||||
semanticBoundary: SEMANTIC_BOUNDARY,
|
||||
taskRuntimeReady: typeof mod._lctask_init_session === "function",
|
||||
motionRuntimeReady: typeof mod._lcmot_step_servo === "function",
|
||||
halRuntimeReady: typeof mod._lchal_get_snapshot_json === "function",
|
||||
nativeTaskReady: false,
|
||||
nativeHalSyncReady: false,
|
||||
};
|
||||
},
|
||||
|
||||
initSession(session = {}) {
|
||||
const rc = callWithJson(mod, "lctask_init_session", session);
|
||||
if (rc !== 0) {
|
||||
throw new Error(`lctask_init_session failed with rc=${rc}`);
|
||||
}
|
||||
return rc;
|
||||
},
|
||||
|
||||
stageFile(path, text) {
|
||||
return withCString(mod, path, (pathPtr) =>
|
||||
withCString(mod, text, (textPtr) =>
|
||||
requireWasmFunction(mod, "lctask_stage_file")(pathPtr, textPtr),
|
||||
),
|
||||
);
|
||||
},
|
||||
|
||||
openProgram(path) {
|
||||
return withCString(mod, path, (pathPtr) =>
|
||||
requireWasmFunction(mod, "lctask_open_program")(pathPtr),
|
||||
);
|
||||
},
|
||||
|
||||
sendCommand(command) {
|
||||
return callWithJson(mod, "lctask_send_command_json", command);
|
||||
},
|
||||
|
||||
runCycles({
|
||||
taskPeriodNs = 10000000,
|
||||
servoPeriodNs = 1000000,
|
||||
taskCycles = 1,
|
||||
} = {}) {
|
||||
return requireWasmFunction(mod, "lctask_run_cycles")(
|
||||
Number(taskPeriodNs) || 10000000,
|
||||
Number(servoPeriodNs) || 1000000,
|
||||
Number(taskCycles) || 0,
|
||||
);
|
||||
},
|
||||
|
||||
readStatus() {
|
||||
return readJson(mod, "lctask_read_status_json");
|
||||
},
|
||||
|
||||
readEvents() {
|
||||
return readJson(mod, "lctask_read_events_json");
|
||||
},
|
||||
|
||||
resetSession() {
|
||||
return requireWasmFunction(mod, "lctask_reset_session")();
|
||||
},
|
||||
};
|
||||
}
|
||||
Reference in New Issue
Block a user