meswork/wasm-simulator
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

满庭芳·源流归一

Browse Source 
结论：LinuxCNC rs274ngc 源码移植边界继续收拢。已将 rs274ngc 头文件纳入 manifest 跟踪，core 编译组仍只包含可编译源文件；启用 CNC_SIM_ENABLE_LINUXCNC_RS274_BACKEND 时 API 默认使用 LinuxCNC rs274 后端；M428/M429/M430 与 5axis/TRT/TDR/RTCP 相关路径继续保持来自 LinuxCNC 源码或 remap 配置的薄桥接。

验证：./test-linuxcnc-source-syntax.sh；./test-linuxcnc-api-native.sh；./test-native.sh；./test-linuxcnc-source-link.sh。
This commit is contained in:
cnc
2026-05-28 18:00:53 +08:00
parent 40a63c76f7
commit b40914747d
65 changed files with 5189 additions and 1117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

122
core/src/canon_event_sink.cpp
View File

@@ -2,10 +2,11 @@
#include <cmath>
#include "rtcp_kinematics.h"
namespace {
constexpr int kCssSpindleModeEvent = 960;
constexpr int kConstantRpmSpindleModeEvent = 970;
void rotate_xy(double *x, double *y, double degrees) {
const double radians = degrees * std::acos(-1.0) / 180.0;
const double c = std::cos(radians);
@@ -32,7 +33,9 @@ void CanonEventSink::reset() {
tool_length_offset_ = {};
xy_rotation_degrees_ = 0.0;
callback_status_ = 0;
kinematics_type_ = 1;
// LinuxCNC xyzbc-trt-kins with sparm=identityfirst starts at switchkins-type 0.
// Source: linuxcnc/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini.
kinematics_type_ = 0;
feed_override_ = false;
speed_override_ = false;
adaptive_feed_ = false;
@@ -67,6 +70,24 @@ void CanonEventSink::configure_rtcp(bool enabled, double tool_length) {
rtcp_tool_length_ = tool_length;
}
void CanonEventSink::configure_xyzbc_trt(const LinuxCncXyzbcTrtParameters &parameters) {
xyzbc_trt_parameters_ = parameters;
xyzbc_trt_parameters_configured_ = true;
use_xyzac_trt_ = false;
}
void CanonEventSink::configure_xyzac_trt(const LinuxCncXyzbcTrtParameters &parameters) {
xyzbc_trt_parameters_ = parameters;
xyzbc_trt_parameters_configured_ = true;
use_xyzac_trt_ = true;
}
void CanonEventSink::configure_fiveaxis(double pivot_length, int kinematics_type) {
fiveaxis_configured_ = true;
fiveaxis_pivot_length_ = pivot_length;
fiveaxis_kinematics_type_ = kinematics_type;
}
void CanonEventSink::set_tool_length(int h_code, double tool_length) {
if (h_code <= 0) {
default_rtcp_tool_length_ = tool_length;
@@ -119,15 +140,37 @@ void CanonEventSink::set_rtcp_state(bool enabled, int h_code, int line) {
emit(event);
}
void CanonEventSink::switch_kinematics(int kinematics_type, bool rtcp_enabled, int line) {
void CanonEventSink::switch_kinematics(int kinematics_type, int line) {
kinematics_type_ = kinematics_type;
rtcp_enabled_ = rtcp_enabled;
// Source: linuxcnc/configs/sim/axis/vismach/5axis/table-rotary-tilting/remap_subs/{428,429,430}remap.ngc.
// M68 E3 Q#<kinstype>, then M66 E0 L0 to force synchronization.
set_analog_output(3, static_cast<double>(kinematics_type_), false, line);
wait_input(0, 0, 0, 0.0, line);
CncSimEvent event = base_event(CNC_SIM_EVENT_KINEMATICS_SWITCH, line);
event.reserved = kinematics_type_;
event.feed = rtcp_enabled_ ? 1.0 : 0.0;
emit(event);
}
void CanonEventSink::switch_m_code_kinematics(int m_code, int default_kinematics_type, int line) {
if (fiveaxis_configured_) {
// Source: linuxcnc/configs/sim/axis/vismach/5axis/bridgemill/remap_subs/{428,429,430}remap.ngc
// M428 selects 5axiskins type 0, M429 selects identity type 1, M430 selects userk type 2.
if (m_code == 428) {
switch_kinematics(0, line);
return;
}
if (m_code == 429) {
switch_kinematics(1, line);
return;
}
if (m_code == 430) {
switch_kinematics(2, line);
return;
}
}
switch_kinematics(default_kinematics_type, line);
}
void CanonEventSink::set_g5x_offset(int index, const CncSimPose &offset, int line) {
CncSimEvent event = base_event(CNC_SIM_EVENT_SET_G5X_OFFSET, line);
event.tool = index;
@@ -198,7 +241,7 @@ void CanonEventSink::set_spindle_mode(int spindle, double mode, int line) {
CncSimEvent event = base_event(CNC_SIM_EVENT_COMMENT, line);
event.tool = spindle;
event.feed = mode;
event.reserved = mode == 0.0 ? 970 : 960;
event.reserved = mode == 0.0 ? kConstantRpmSpindleModeEvent : kCssSpindleModeEvent;
emit(event);
}
@@ -479,16 +522,67 @@ void CanonEventSink::emit(CncSimEvent event) {
}
void CanonEventSink::emit_rtcp_pivot(int line, const CncSimPose &start, const CncSimPose &end) {
if (!rtcp_enabled_ || rtcp_tool_length_ == 0.0 || callback_status_ != 0) {
if (!rtcp_enabled_ || callback_status_ != 0) {
return;
}
CncSimEvent event = base_event(CNC_SIM_EVENT_RTCP_PIVOT, line);
event.start = rtcp_pivot_from_tool_tip(start, rtcp_tool_length_);
event.end = rtcp_pivot_from_tool_tip(end, rtcp_tool_length_);
const RtcpVector tool = rtcp_tool_vector_from_pose(end, rtcp_tool_length_);
event.center.x = tool.x;
event.center.y = tool.y;
event.center.z = tool.z;
if (fiveaxis_configured_ && kinematics_type_ == fiveaxis_kinematics_type_) {
const LinuxCncFiveAxisJoints start_joints =
linuxcnc_5axis_inverse(start, fiveaxis_pivot_length_);
const LinuxCncFiveAxisJoints end_joints =
linuxcnc_5axis_inverse(end, fiveaxis_pivot_length_);
event.start.x = start_joints.x;
event.start.y = start_joints.y;
event.start.z = start_joints.z;
event.start.b = start_joints.b;
event.start.c = start_joints.c;
event.start.w = start_joints.w;
event.end.x = end_joints.x;
event.end.y = end_joints.y;
event.end.z = end_joints.z;
event.end.b = end_joints.b;
event.end.c = end_joints.c;
event.end.w = end_joints.w;
event.reserved = kinematics_type_;
event.dwell_seconds = fiveaxis_pivot_length_;
} else if (kinematics_type_ == 1 || kinematics_type_ == 2) {
LinuxCncFiveAxisJoints start_joints{};
LinuxCncFiveAxisJoints end_joints{};
if (kinematics_type_ == 1) {
LinuxCncXyzbcTrtParameters parameters = xyzbc_trt_parameters_configured_
? xyzbc_trt_parameters_
: linuxcnc_xyzbc_trt_default_parameters(rtcp_tool_length_);
parameters.tool_offset = rtcp_tool_length_;
if (use_xyzac_trt_) {
start_joints = linuxcnc_xyzac_trt_inverse(start, parameters);
end_joints = linuxcnc_xyzac_trt_inverse(end, parameters);
} else {
start_joints = linuxcnc_xyzbc_trt_inverse(start, parameters);
end_joints = linuxcnc_xyzbc_trt_inverse(end, parameters);
}
} else {
start_joints = linuxcnc_xyzbc_userk_inverse(start);
end_joints = linuxcnc_xyzbc_userk_inverse(end);
}
event.start.x = start_joints.x;
event.start.y = start_joints.y;
event.start.z = start_joints.z;
event.start.a = start_joints.a;
event.start.b = start_joints.b;
event.start.c = start_joints.c;
event.start.w = start_joints.w;
event.end.x = end_joints.x;
event.end.y = end_joints.y;
event.end.z = end_joints.z;
event.end.a = end_joints.a;
event.end.b = end_joints.b;
event.end.c = end_joints.c;
event.end.w = end_joints.w;
event.reserved = kinematics_type_;
event.dwell_seconds = rtcp_tool_length_;
} else {
return;
}
emit(event);
}

15
core/src/canon_event_sink.h
View File

@@ -1,6 +1,7 @@
#pragma once
#include "cnc_sim_api.h"
#include "rtcp_kinematics.h"
#include <unordered_map>
@@ -12,11 +13,15 @@ public:
bool callback_aborted() const;
void set_block_delete(bool enabled);
void configure_rtcp(bool enabled, double tool_length);
void configure_xyzbc_trt(const LinuxCncXyzbcTrtParameters &parameters);
void configure_xyzac_trt(const LinuxCncXyzbcTrtParameters &parameters);
void configure_fiveaxis(double pivot_length, int kinematics_type);
void set_tool_length(int h_code, double tool_length);
void set_tool_length_offset(const CncSimPose &offset);
void apply_tool_length_offset(const CncSimPose &offset);
void set_rtcp_state(bool enabled, int h_code, int line);
void switch_kinematics(int kinematics_type, bool rtcp_enabled, int line);
void switch_kinematics(int kinematics_type, int line);
void switch_m_code_kinematics(int m_code, int default_kinematics_type, int line);
void set_g5x_offset(int index, const CncSimPose &offset, int line);
void set_g92_offset(const CncSimPose &offset, int line);
void set_xy_rotation(double angle_degrees, int line);
@@ -97,9 +102,15 @@ private:
double default_rtcp_tool_length_ = 0.0;
double rtcp_tool_length_ = 0.0;
int rtcp_h_code_ = 0;
LinuxCncXyzbcTrtParameters xyzbc_trt_parameters_{};
bool xyzbc_trt_parameters_configured_ = false;
bool use_xyzac_trt_ = false;
bool fiveaxis_configured_ = false;
double fiveaxis_pivot_length_ = 250.0;
int fiveaxis_kinematics_type_ = 0;
CncSimPose tool_length_offset_{};
double xy_rotation_degrees_ = 0.0;
int kinematics_type_ = 1;
int kinematics_type_ = 0;
bool feed_override_ = false;
bool speed_override_ = false;
bool adaptive_feed_ = false;

86
core/src/cnc_sim_api.cpp
View File

@@ -9,7 +9,11 @@
struct CncSimHandle {
CncSimDialect dialect = CNC_SIM_DIALECT_LINUXCNC;
#ifdef CNC_SIM_ENABLE_LINUXCNC_RS274_BACKEND
GcodeBackendKind backend = GcodeBackendKind::LinuxCncRs274;
#else
GcodeBackendKind backend = GcodeBackendKind::Smoke;
#endif
std::string last_error;
CanonEventSink sink;
};
@@ -128,6 +132,86 @@ void apply_tool_length_table(CanonEventSink &sink, const std::string &compact) {
}
}
bool find_number_any(const std::string &compact, double *value, const char *first) {
return find_number_value(compact, first, value);
}
template <typename... Keys>
bool find_number_any(const std::string &compact, double *value, const char *first, Keys... rest) {
return find_number_value(compact, first, value) || find_number_any(compact, value, rest...);
}
bool contains_bool_any(const std::string &compact, bool *value, const char *first) {
return contains_bool_value(compact, first, value);
}
template <typename... Keys>
bool contains_bool_any(const std::string &compact, bool *value, const char *first, Keys... rest) {
return contains_bool_value(compact, first, value) || contains_bool_any(compact, value, rest...);
}
void apply_xyzbc_trt_config(CanonEventSink &sink, const std::string &compact) {
LinuxCncXyzbcTrtParameters parameters = linuxcnc_xyzbc_trt_default_parameters(0.0);
bool configured = false;
const bool xyzac = compact.find("\"trt\":\"xyzac\"") != std::string::npos ||
compact.find("\"kinematics\":\"xyzac-trt\"") != std::string::npos ||
compact.find("\"switchkins\":\"xyzac-trt\"") != std::string::npos;
const bool xyzbc = compact.find("\"trt\":\"xyzbc\"") != std::string::npos ||
compact.find("\"kinematics\":\"xyzbc-trt\"") != std::string::npos ||
compact.find("\"switchkins\":\"xyzbc-trt\"") != std::string::npos;
configured = find_number_any(compact, &parameters.x_rot_point, "xrotpoint", "x_rot_point", "x-rot-point") ||
configured;
configured = find_number_any(compact, &parameters.y_rot_point, "yrotpoint", "y_rot_point", "y-rot-point") ||
configured;
configured = find_number_any(compact, &parameters.z_rot_point, "zrotpoint", "z_rot_point", "z-rot-point") ||
configured;
configured = find_number_any(compact, &parameters.x_offset, "xoffset", "x_offset", "x-offset") ||
configured;
configured = find_number_any(compact, &parameters.y_offset, "yoffset", "y_offset", "y-offset") ||
configured;
configured = find_number_any(compact, &parameters.z_offset, "zoffset", "z_offset", "z-offset") ||
configured;
configured = contains_bool_any(compact,
&parameters.conventional_directions,
"conventionaldirections",
"conventional_directions",
"conventional-directions") ||
configured;
if (configured || xyzac || xyzbc) {
configured = true;
}
if (configured && xyzac) {
sink.configure_xyzac_trt(parameters);
} else if (configured) {
sink.configure_xyzbc_trt(parameters);
}
}
void apply_fiveaxis_config(CanonEventSink &sink, const std::string &compact) {
const bool fiveaxis = compact.find("\"kinematics\":\"5axiskins\"") != std::string::npos ||
compact.find("\"switchkins\":\"5axiskins\"") != std::string::npos ||
compact.find("\"kinematics\":\"5axis\"") != std::string::npos;
double pivot_length = 250.0;
const bool has_pivot_length = find_number_any(compact,
&pivot_length,
"pivotlength",
"pivot_length",
"pivot-length");
double kinematics_type = 0.0;
const bool has_kinematics_type = find_number_any(compact,
&kinematics_type,
"fiveaxiskinematicstype",
"fiveaxis_kinematics_type",
"fiveaxis-kinematics-type",
"kinematicstype",
"kinematics_type");
if (fiveaxis || has_pivot_length || has_kinematics_type) {
sink.configure_fiveaxis(pivot_length, static_cast<int>(kinematics_type));
}
}
int apply_config(CncSimHandle *handle, const std::string &json) {
if (json.empty()) {
return 0;
@@ -158,6 +242,8 @@ int apply_config(CncSimHandle *handle, const std::string &json) {
handle->sink.set_block_delete(block_delete);
}
apply_tool_length_table(handle->sink, compact);
apply_xyzbc_trt_config(handle->sink, compact);
apply_fiveaxis_config(handle->sink, compact);
return 0;
}

4
core/src/linuxcnc_canon_bridge.cpp
View File

@@ -15,6 +15,8 @@
namespace {
constexpr int kToolLengthOffsetEvent = 430;
CanonEventSink *active_sink = nullptr;
int active_line = 0;
bool flood_on = false;
@@ -515,7 +517,7 @@ void USE_TOOL_LENGTH_OFFSET(const EmcPose &offset) {
event.version = 1;
event.type = CNC_SIM_EVENT_COMMENT;
event.line = event_line();
event.reserved = 430;
event.reserved = kToolLengthOffsetEvent;
event.start = tool_offset;
active_sink->emit_raw(event);
}

2
core/src/linuxcnc_rs274_backend.cpp
View File

@@ -217,7 +217,7 @@ int parse_linuxcnc_rs274_backend(CanonEventSink &sink,
++line_number;
cnc_sim_linuxcnc_set_current_line(line_number);
std::vector<SimulatorGcodeControlAction> control_actions;
if (parse_simulator_gcode_control_line(line, &control_actions)) {
if (parse_simulator_gcode_control_line(line, sink.block_delete(), &control_actions)) {
bool simulator_only_line = true;
for (const auto &action : control_actions) {
emit_simulator_gcode_control_action(sink, action, line_number);

1356
core/src/rtcp_kinematics.cpp
View File

File diff suppressed because it is too large Load Diff

136
core/src/rtcp_kinematics.h
View File

@@ -8,10 +8,134 @@ struct RtcpVector {
double z;
};
// Rotation order is intrinsic tool orientation A then B then C, represented as
// Rz(C) * Ry(B) * Rx(A) applied to a local tool vector.
RtcpVector rtcp_rotate_abc_degrees(RtcpVector vector, double a_deg, double b_deg, double c_deg);
struct LinuxCncFiveAxisJoints {
double x;
double y;
double z;
double a;
double b;
double c;
double w;
};
RtcpVector rtcp_tool_vector_from_pose(const CncSimPose &pose, double tool_length);
CncSimPose rtcp_pivot_from_tool_tip(const CncSimPose &tool_tip, double tool_length);
CncSimPose rtcp_tool_tip_from_pivot(const CncSimPose &pivot, double tool_length);
struct LinuxCncAxisJoints {
double x;
double y;
double z;
double a;
double b;
double c;
double u;
double v;
double w;
};
struct LinuxCncXyzbcTrtParameters {
double x_rot_point;
double y_rot_point;
double z_rot_point;
double x_offset;
double y_offset;
double z_offset;
double tool_offset;
bool conventional_directions;
};
struct LinuxCncScaraParameters {
double d1;
double d2;
double d3;
double d4;
double d5;
double d6;
};
struct LinuxCncXyzabTdrParameters {
double tool_offset_z;
double x_offset;
double z_offset;
double x_rot_point;
double y_rot_point;
double z_rot_point;
};
struct LinuxCncPumaParameters {
double a2;
double a3;
double d3;
double d4;
double d6;
};
struct LinuxCncGenserParameters {
double a[6];
double alpha[6];
double d[6];
int unrotate[6];
};
struct LinuxCncScaraJoints {
double j0;
double j1;
double j2;
double j3;
double j4;
double j5;
};
LinuxCncAxisJoints linuxcnc_identity_default_joints();
CncSimPose linuxcnc_identity_forward(const LinuxCncAxisJoints &joints);
LinuxCncAxisJoints linuxcnc_identity_inverse(const CncSimPose &pose);
CncSimPose linuxcnc_rotatekins_forward(const LinuxCncAxisJoints &joints);
LinuxCncAxisJoints linuxcnc_rotatekins_inverse(const CncSimPose &pose);
CncSimPose linuxcnc_5axis_forward(const LinuxCncFiveAxisJoints &joints, double pivot_length);
LinuxCncFiveAxisJoints linuxcnc_5axis_inverse(const CncSimPose &pose, double pivot_length);
LinuxCncXyzbcTrtParameters linuxcnc_xyzbc_trt_default_parameters(double tool_offset);
CncSimPose linuxcnc_xyzbc_trt_forward(const LinuxCncFiveAxisJoints &joints,
const LinuxCncXyzbcTrtParameters &parameters);
LinuxCncFiveAxisJoints linuxcnc_xyzbc_trt_inverse(const CncSimPose &pose,
const LinuxCncXyzbcTrtParameters &parameters);
CncSimPose linuxcnc_xyzac_trt_forward(const LinuxCncFiveAxisJoints &joints,
const LinuxCncXyzbcTrtParameters &parameters);
LinuxCncFiveAxisJoints linuxcnc_xyzac_trt_inverse(const CncSimPose &pose,
const LinuxCncXyzbcTrtParameters &parameters);
CncSimPose linuxcnc_xyzbc_userk_forward(const LinuxCncFiveAxisJoints &joints);
LinuxCncFiveAxisJoints linuxcnc_xyzbc_userk_inverse(const CncSimPose &pose);
LinuxCncScaraParameters linuxcnc_scara_default_parameters();
CncSimPose linuxcnc_scara_forward(const LinuxCncScaraJoints &joints,
const LinuxCncScaraParameters &parameters,
int *iflags);
LinuxCncScaraJoints linuxcnc_scara_inverse(const CncSimPose &pose,
const LinuxCncScaraParameters &parameters,
int iflags);
LinuxCncXyzabTdrParameters linuxcnc_xyzab_tdr_default_parameters(double tool_offset_z);
CncSimPose linuxcnc_xyzab_tdr_identity_forward(const LinuxCncAxisJoints &joints);
LinuxCncAxisJoints linuxcnc_xyzab_tdr_identity_inverse(const CncSimPose &pose);
CncSimPose linuxcnc_xyzab_tdr_tcp_forward(const LinuxCncAxisJoints &joints,
const LinuxCncXyzabTdrParameters &parameters);
LinuxCncAxisJoints linuxcnc_xyzab_tdr_tcp_inverse(const CncSimPose &pose,
const LinuxCncXyzabTdrParameters &parameters);
LinuxCncPumaParameters linuxcnc_puma_default_parameters();
CncSimPose linuxcnc_puma_forward(const LinuxCncAxisJoints &joints,
const LinuxCncPumaParameters &parameters,
int *iflags);
LinuxCncAxisJoints linuxcnc_puma_inverse(const CncSimPose &pose,
const LinuxCncAxisJoints &current_joints,
const LinuxCncPumaParameters &parameters,
int iflags,
int *fflags);
LinuxCncGenserParameters linuxcnc_genser_puma560_parameters();
CncSimPose linuxcnc_genser_forward(const LinuxCncAxisJoints &joints,
const LinuxCncGenserParameters &parameters);
LinuxCncAxisJoints linuxcnc_genser_inverse(const CncSimPose &pose,
const LinuxCncAxisJoints &joint_estimate,
const LinuxCncGenserParameters &parameters,
int *iterations,
int max_iterations = 100);

211
core/src/simulator_gcode_controls.cpp
View File

@@ -6,31 +6,155 @@
namespace {
std::string strip_line_comment(const std::string &line) {
bool strip_line_comment(const std::string &line, bool block_delete, std::string *stripped) {
std::string out;
bool in_paren = false;
bool saw_code = false;
for (char raw : line) {
const char ch = static_cast<char>(std::toupper(static_cast<unsigned char>(raw)));
if (in_paren) {
if (ch == '(') {
return false;
}
if (ch == ')') {
in_paren = false;
}
continue;
}
if (ch == '(') {
saw_code = true;
in_paren = true;
continue;
}
if (ch == ';') {
break;
}
if (!saw_code && std::isspace(static_cast<unsigned char>(ch))) {
out.push_back(ch);
continue;
}
if (!saw_code && ch == '/') {
if (block_delete) {
return false;
}
saw_code = true;
continue;
}
saw_code = true;
out.push_back(ch);
}
return out;
if (in_paren) {
return false;
}
*stripped = out;
return true;
}
bool g_code_is(double actual, double expected) {
return std::fabs(actual - expected) < 0.0001;
bool linuxcnc_integer_word_value(double value, int *integer) {
*integer = static_cast<int>(std::floor(value));
const double fraction = value - *integer;
if (fraction > 0.9999) {
*integer = static_cast<int>(std::ceil(value));
return true;
}
return fraction <= 0.0001;
}
bool linuxcnc_g_word_value(double value, int *g_code) {
const double scaled = value * 10.0;
int parsed = static_cast<int>(std::floor(scaled));
const double fraction = scaled - parsed;
if (fraction > 0.999) {
parsed = static_cast<int>(std::ceil(scaled));
} else if (fraction > 0.001) {
return false;
}
if (parsed < 0 || parsed > 999) {
return false;
}
*g_code = parsed;
return true;
}
bool read_linuxcnc_real_number(const char *text, size_t start, double *value, size_t *end_index) {
const char first = text[start];
const char next = text[start + 1];
if (first == '[') {
double nested = 0.0;
size_t nested_end = start + 1;
if (!read_linuxcnc_real_number(text, start + 1, &nested, &nested_end) ||
text[nested_end] != ']') {
return false;
}
*value = nested;
*end_index = nested_end + 1;
return true;
}
if ((first == '+' || first == '-') &&
next != '\0' &&
!std::isdigit(static_cast<unsigned char>(next)) &&
next != '.') {
double nested = 0.0;
size_t nested_end = start + 1;
if (!read_linuxcnc_real_number(text, start + 1, &nested, &nested_end)) {
return false;
}
*value = first == '-' ? -nested : nested;
*end_index = nested_end;
return true;
}
size_t i = start;
while (text[i] == '+' || text[i] == '-') {
++i;
}
const size_t number_start = i;
while (std::isdigit(static_cast<unsigned char>(text[i])) || text[i] == '.') {
++i;
}
if (i == start || i == number_start) {
return false;
}
std::string token(text + start, i - start);
char *end = nullptr;
const double parsed = std::strtod(token.c_str(), &end);
if (end != token.c_str() + token.size()) {
return false;
}
*value = parsed;
*end_index = i;
return true;
}
bool skip_linuxcnc_leading_n_number(const std::string &stripped, std::string *without_n) {
size_t i = 0;
while (std::isspace(static_cast<unsigned char>(stripped[i]))) {
++i;
}
if (stripped[i] != 'N') {
*without_n = stripped;
return true;
}
++i;
const size_t integer_start = i;
while (std::isdigit(static_cast<unsigned char>(stripped[i]))) {
++i;
}
if (i == integer_start) {
return false;
}
if (stripped[i] == '.') {
++i;
const size_t fraction_start = i;
while (std::isdigit(static_cast<unsigned char>(stripped[i]))) {
++i;
}
if (i == fraction_start) {
return false;
}
}
*without_n = stripped.substr(i);
return true;
}
bool parse_m_control_line(const std::string &stripped,
@@ -46,9 +170,13 @@ bool parse_m_control_line(const std::string &stripped,
return false;
}
++i;
char *end = nullptr;
const long value = std::strtol(text + i, &end, 10);
if (end == text + i) {
double raw_value = 0.0;
size_t end = i;
if (!read_linuxcnc_real_number(text, i, &raw_value, &end)) {
return false;
}
int value = 0;
if (!linuxcnc_integer_word_value(raw_value, &value) || value < 0) {
return false;
}
@@ -56,19 +184,19 @@ bool parse_m_control_line(const std::string &stripped,
action.kind = SimulatorGcodeControlKind::KinematicsSwitch;
if (value == 428) {
action.value = 1;
action.rtcp_enabled = true;
action.m_code = 428;
} else if (value == 429) {
action.value = 0;
action.rtcp_enabled = false;
action.m_code = 429;
} else if (value == 430) {
action.value = 2;
action.rtcp_enabled = true;
action.m_code = 430;
} else {
return false;
}
actions->push_back(action);
saw_code = true;
i = static_cast<size_t>(end - text);
i = end;
}
return saw_code;
}
@@ -78,7 +206,9 @@ bool parse_rtcp_control_line(const std::string &stripped,
const char *text = stripped.c_str();
std::vector<SimulatorGcodeControlAction> parsed;
int h_code = 0;
bool saw_h_word = false;
bool saw_rtcp_code = false;
bool saw_rtcp_g_word = false;
for (size_t i = 0; text[i] != '\0';) {
if (std::isspace(static_cast<unsigned char>(text[i]))) {
@@ -90,26 +220,44 @@ bool parse_rtcp_control_line(const std::string &stripped,
return false;
}
++i;
char *end = nullptr;
const double value = std::strtod(text + i, &end);
if (end == text + i) {
double value = 0.0;
size_t end = i;
if (!read_linuxcnc_real_number(text, i, &value, &end)) {
return false;
}
if (letter == 'H') {
h_code = static_cast<int>(std::lround(value));
} else if (g_code_is(value, 43.4) || g_code_is(value, 43.5) || g_code_is(value, 49.0)) {
if (saw_h_word) {
return false;
}
if (!linuxcnc_integer_word_value(value, &h_code) || h_code < -1) {
return false;
}
saw_h_word = true;
} else {
int g_code = 0;
if (!linuxcnc_g_word_value(value, &g_code) ||
(g_code != 434 && g_code != 435 && g_code != 490)) {
return false;
}
if (saw_rtcp_g_word) {
return false;
}
SimulatorGcodeControlAction action{};
action.kind = SimulatorGcodeControlKind::RtcpState;
action.value = static_cast<int>(std::lround(value * 10.0));
action.rtcp_enabled = g_code_is(value, 43.4) || g_code_is(value, 43.5);
action.value = g_code;
action.rtcp_enabled = g_code == 434 || g_code == 435;
parsed.push_back(action);
saw_rtcp_code = true;
} else {
return false;
saw_rtcp_g_word = true;
}
i = static_cast<size_t>(end - text);
i = end;
}
for (const auto &action : parsed) {
if (!action.rtcp_enabled && saw_h_word) {
return false;
}
}
for (auto &action : parsed) {
action.h_code = action.rtcp_enabled ? h_code : 0;
actions->push_back(action);
@@ -121,13 +269,26 @@ bool parse_rtcp_control_line(const std::string &stripped,
bool parse_simulator_gcode_control_line(const std::string &line,
std::vector<SimulatorGcodeControlAction> *actions) {
return parse_simulator_gcode_control_line(line, false, actions);
}
bool parse_simulator_gcode_control_line(const std::string &line,
bool block_delete,
std::vector<SimulatorGcodeControlAction> *actions) {
actions->clear();
const std::string stripped = strip_line_comment(line);
if (parse_m_control_line(stripped, actions)) {
std::string stripped;
if (!strip_line_comment(line, block_delete, &stripped)) {
return false;
}
std::string control_line;
if (!skip_linuxcnc_leading_n_number(stripped, &control_line)) {
return false;
}
if (parse_m_control_line(control_line, actions)) {
return true;
}
actions->clear();
if (parse_rtcp_control_line(stripped, actions)) {
if (parse_rtcp_control_line(control_line, actions)) {
return true;
}
actions->clear();
@@ -138,7 +299,7 @@ void emit_simulator_gcode_control_action(CanonEventSink &sink,
const SimulatorGcodeControlAction &action,
int line) {
if (action.kind == SimulatorGcodeControlKind::KinematicsSwitch) {
sink.switch_kinematics(action.value, action.rtcp_enabled, line);
sink.switch_m_code_kinematics(action.m_code, action.value, line);
} else if (action.kind == SimulatorGcodeControlKind::RtcpState) {
sink.set_rtcp_state(action.rtcp_enabled, action.h_code, line);
}

4
core/src/simulator_gcode_controls.h
View File

@@ -13,12 +13,16 @@ enum class SimulatorGcodeControlKind {
struct SimulatorGcodeControlAction {
SimulatorGcodeControlKind kind;
int value;
int m_code;
bool rtcp_enabled;
int h_code;
};
bool parse_simulator_gcode_control_line(const std::string &line,
std::vector<SimulatorGcodeControlAction> *actions);
bool parse_simulator_gcode_control_line(const std::string &line,
bool block_delete,
std::vector<SimulatorGcodeControlAction> *actions);
void emit_simulator_gcode_control_action(CanonEventSink &sink,
const SimulatorGcodeControlAction &action,
int line);

317
core/src/smoke_gcode_parser.cpp
View File

@@ -149,11 +149,13 @@ constexpr double kRadiusToleranceMm = kRadiusToleranceInch * kMmPerInch;
constexpr double kCenterArcRadiusToleranceInch = 2.0 * 0.001 * M_SQRT2;
constexpr double kCenterArcRadiusToleranceMm = 2.0 * 0.01 * M_SQRT2;
constexpr double kSpiralRelativeTolerance = 0.001;
constexpr int kConstantRpmSpindleModeEvent = 970;
constexpr int kCutterCompOffEvent = 400;
constexpr int kCutterCompLeftEvent = 410;
constexpr int kCutterCompRightEvent = 420;
constexpr int kCutterCompLeftExplicitEvent = 411;
constexpr int kCutterCompRightExplicitEvent = 421;
constexpr int kToolLengthOffsetEvent = 430;
enum class LinuxCncReaderSymbol {
None,
@@ -178,6 +180,12 @@ size_t leading_special_word_start(const std::string &line);
bool is_numeric_m98_subprogram_header(const std::string &line);
OwordKind oword_kind(const std::string &line);
bool integer_word_value(const std::unordered_map<char, double> &words, char letter, int *value);
bool evaluate_expression(const std::string &text,
const std::vector<CallFrame> &call_stack,
const std::unordered_map<int, double> &parameters,
const std::unordered_map<std::string, double> &named_parameters,
double *value,
std::string *error);
template <size_t Size>
bool contains_int(const std::array<int, Size> &values, int value) {
@@ -685,6 +693,91 @@ LinuxCncIntegerReadResult read_linuxcnc_integer_value(const std::string &text,
return LinuxCncIntegerReadResult::Ok;
}
bool is_current_position_parameter_index(int index) {
return index >= 5420 && index <= 5428;
}
bool line_reads_current_position_parameter(const std::string &line,
const std::vector<CallFrame> &call_stack,
const std::unordered_map<int, double> &parameters,
const std::unordered_map<std::string, double> &named_parameters) {
for (size_t i = 0; i < line.size(); ++i) {
if (line[i] != '#') {
continue;
}
int index = 0;
size_t index_end = i + 1;
size_t parameter_start = i + 1;
while (parameter_start < line.size() &&
std::isspace(static_cast<unsigned char>(line[parameter_start]))) {
++parameter_start;
}
if (parameter_start < line.size() && line[parameter_start] == '[') {
const size_t close = find_matching_square_bracket(line, parameter_start);
if (close != std::string::npos) {
size_t bracket_index_end = parameter_start + 1;
const LinuxCncIntegerReadResult bracket_result =
read_linuxcnc_integer_value(line, parameter_start + 1, &index, &bracket_index_end);
size_t after_bracket_index = bracket_index_end;
while (after_bracket_index < close &&
std::isspace(static_cast<unsigned char>(line[after_bracket_index]))) {
++after_bracket_index;
}
bool has_current_position_index =
bracket_result == LinuxCncIntegerReadResult::Ok &&
after_bracket_index == close &&
is_current_position_parameter_index(index);
if (!has_current_position_index) {
double index_value = 0.0;
if (evaluate_expression(line.substr(parameter_start + 1,
close - parameter_start - 1),
call_stack,
parameters,
named_parameters,
&index_value,
nullptr)) {
int expression_index = 0;
has_current_position_index =
linuxcnc_integer_from_real(index_value, &expression_index) ==
LinuxCncIntegerReadResult::Ok &&
is_current_position_parameter_index(expression_index);
}
}
if (has_current_position_index) {
size_t after_close = close + 1;
while (after_close < line.size() &&
std::isspace(static_cast<unsigned char>(line[after_close]))) {
++after_close;
}
if (after_close < line.size() && line[after_close] == '=') {
continue;
}
return true;
}
}
continue;
}
const LinuxCncIntegerReadResult result =
read_linuxcnc_integer_value(line, i + 1, &index, &index_end);
if (result != LinuxCncIntegerReadResult::Ok ||
!is_current_position_parameter_index(index)) {
continue;
}
size_t after_index = index_end;
while (after_index < line.size() &&
std::isspace(static_cast<unsigned char>(line[after_index]))) {
++after_index;
}
if (after_index < line.size() && line[after_index] == '=') {
continue;
}
return true;
}
return false;
}
bool parse_linuxcnc_real_number(const char *text, double *value, size_t *consumed) {
size_t end = 0;
while (text[end] == '+' || text[end] == '-') {
@@ -1190,6 +1283,7 @@ bool parse_word_list_impl(const std::string &line, ParsedWordList *parsed, std::
parsed->theta_flag = false;
parsed->theta_value = 0.0;
std::array<bool, 26> seen_letters{};
std::array<double, 26> seen_letter_values{};
const char *text = line.c_str();
for (size_t i = 0; text[i] != '\0';) {
if (std::isspace(static_cast<unsigned char>(text[i]))) {
@@ -1338,13 +1432,27 @@ bool parse_word_list_impl(const std::string &line, ParsedWordList *parsed, std::
if (letter != 'G' && letter != 'M') {
const size_t letter_index = static_cast<size_t>(letter - 'A');
if (strict && seen_letters[letter_index]) {
if (error) {
*error = "Multiple " + std::string(1, static_cast<char>(std::tolower(static_cast<unsigned char>(letter)))) +
" words on one line";
if ((letter == 'P' || letter == 'Q') && seen_letter_values[letter_index] <= -1.0) {
parsed->words.erase(std::remove_if(parsed->words.begin(),
parsed->words.end(),
[letter](const Word &word) {
return word.letter == letter;
}),
parsed->words.end());
} else {
if (error) {
const char duplicate_letter =
(letter == 'U' || letter == 'V' || letter == 'W')
? letter
: static_cast<char>(std::tolower(static_cast<unsigned char>(letter)));
*error = "Multiple " + std::string(1, duplicate_letter) +
" words on one line";
}
return false;
}
return false;
}
seen_letters[letter_index] = true;
seen_letter_values[letter_index] = value;
}
parsed->words.push_back({letter, value});
i = next;
@@ -3309,7 +3417,7 @@ std::string expand_expressions_and_parameters(const std::string &line,
const size_t close = find_matching_square_bracket(line, function_start);
if (close == std::string::npos) {
if (error) {
*error = "unterminated bracket expression";
*error = "Unclosed expression";
}
return {};
}
@@ -3356,7 +3464,7 @@ std::string expand_expressions_and_parameters(const std::string &line,
const size_t close = find_matching_square_bracket(line, i);
if (close == std::string::npos) {
if (error) {
*error = "unterminated bracket expression";
*error = "Unclosed expression";
}
return {};
}
@@ -3391,7 +3499,7 @@ std::string expand_expressions_and_parameters(const std::string &line,
const size_t close = find_matching_square_bracket(line, end);
if (close == std::string::npos) {
if (error) {
*error = "unterminated indirect parameter expression";
*error = "Unclosed expression";
}
return {};
}
@@ -3400,7 +3508,7 @@ std::string expand_expressions_and_parameters(const std::string &line,
const size_t close = line.find('>', end + 1);
if (close == std::string::npos) {
if (error) {
*error = "unterminated indirect named parameter expression";
*error = "Named parameter not terminated";
}
return {};
}
@@ -3445,7 +3553,7 @@ std::string expand_expressions_and_parameters(const std::string &line,
const size_t close = find_matching_square_bracket(line, j);
if (close == std::string::npos) {
if (error) {
*error = "unterminated indexed parameter expression";
*error = "Unclosed expression";
}
return {};
}
@@ -3491,7 +3599,7 @@ std::string expand_expressions_and_parameters(const std::string &line,
const size_t close = line.find('>', j + 1);
if (close == std::string::npos) {
if (error) {
*error = "unterminated named parameter expression";
*error = "Named parameter not terminated";
}
return {};
}
@@ -3653,7 +3761,7 @@ bool parse_parameter_assignment(const std::string &line,
const size_t close = text.find('>', pos + 1);
if (close == std::string::npos) {
if (error) {
*error = "unterminated named parameter assignment";
*error = "Named parameter not terminated";
}
return true;
}
@@ -3669,7 +3777,7 @@ bool parse_parameter_assignment(const std::string &line,
const size_t close = find_matching_square_bracket(text, pos);
if (close == std::string::npos) {
if (error) {
*error = "unterminated indexed parameter assignment";
*error = "Unclosed expression";
}
return true;
}
@@ -3793,7 +3901,7 @@ bool parse_parameter_assignment(const std::string &line,
const size_t close = find_matching_square_bracket(text, pos);
if (close == std::string::npos) {
if (error) {
*error = "unterminated parameter assignment expression";
*error = "Unclosed expression";
}
return true;
}
@@ -4994,6 +5102,119 @@ bool validate_r_arc_reaches_end_point(const CncSimEvent &event, int plane, doubl
return true;
}
bool validate_absolute_center_arc_axis_words(const std::unordered_map<char, double> &words,
int plane,
bool absolute,
std::string *error) {
if (!absolute || words.count('R')) {
return true;
}
auto missing = [error](char axis) {
if (error) {
*error = std::string(1, axis) + " word missing in absolute center arc";
}
return false;
};
if (plane == 17 && (words.count('I') || words.count('J'))) {
if (!words.count('I')) {
return missing('I');
}
if (!words.count('J')) {
return missing('J');
}
} else if (plane == 18 && (words.count('I') || words.count('K'))) {
if (!words.count('I')) {
return missing('I');
}
if (!words.count('K')) {
return missing('K');
}
} else if (plane == 19 && (words.count('J') || words.count('K'))) {
if (!words.count('J')) {
return missing('J');
}
if (!words.count('K')) {
return missing('K');
}
}
return true;
}
bool validate_arc_format_words(const std::unordered_map<char, double> &words, std::string *error) {
const bool has_r = words.count('R') != 0;
const bool has_ijk = words.count('I') || words.count('J') || words.count('K');
if (has_r && has_ijk) {
if (error) {
*error = "Mixed radius ijk format for arc";
}
return false;
}
if (!has_r && !has_ijk) {
if (error) {
*error = "R i j k words all missing for arc";
}
return false;
}
return true;
}
bool validate_arc_plane_center_words(const std::unordered_map<char, double> &words,
int plane,
std::string *error) {
if (plane == 17 && words.count('K')) {
if (error) {
*error = "K word given for arc in xy plane";
}
return false;
}
if (plane == 18 && words.count('J')) {
if (error) {
*error = "J word given for arc in xz plane";
}
return false;
}
if (plane == 19 && words.count('I')) {
if (error) {
*error = "I word given for arc in yz plane";
}
return false;
}
return true;
}
bool validate_r_arc_endpoint_words(const std::unordered_map<char, double> &words,
const ParsedWordList &parsed,
int plane,
std::string *error) {
if (!words.count('R')) {
return true;
}
if (plane == 17) {
if (!words.count('X') && !words.count('Y') &&
!parsed.radius_flag && !parsed.theta_flag) {
if (error) {
*error = "X and y words missing for arc in xy plane";
}
return false;
}
} else if (plane == 18) {
if (!words.count('X') && !words.count('Z')) {
if (error) {
*error = "X and z words missing for arc in xz plane";
}
return false;
}
} else if (plane == 19) {
if (!words.count('Y') && !words.count('Z')) {
if (error) {
*error = "Y and z words missing for arc in yz plane";
}
return false;
}
}
return true;
}
bool modal_motion_is_threading(int modal_motion) {
return modal_motion == kModalMotionG33 ||
modal_motion == kModalMotionG331;
@@ -5101,7 +5322,7 @@ void emit_tool_length_offset(CanonEventSink &sink, const CncSimPose &offset, int
event.version = 1;
event.type = CNC_SIM_EVENT_COMMENT;
event.line = line;
event.reserved = 430;
event.reserved = kToolLengthOffsetEvent;
event.start = offset;
sink.emit_raw(event);
}
@@ -5645,6 +5866,13 @@ int SmokeGcodeParser::parse(const char *program, size_t program_len, std::string
}
refresh_readonly_named_parameters(line_number);
if (cutter_comp_on_ &&
line_reads_current_position_parameter(line, call_stack, parameters, named_parameters)) {
if (error) {
*error = "Cannot read current position with cutter radius compensation on";
}
return -1;
}
std::string parameter_error;
std::vector<PendingParameterAssignment> pending_assignments;
if (!extract_parameter_assignments(line,
@@ -5726,7 +5954,7 @@ int SmokeGcodeParser::parse(const char *program, size_t program_len, std::string
}
std::vector<SimulatorGcodeControlAction> simulator_control_actions;
if (!has_keyword_oword &&
parse_simulator_gcode_control_line(line, &simulator_control_actions)) {
parse_simulator_gcode_control_line(line, sink_.block_delete(), &simulator_control_actions)) {
bool simulator_only_line = true;
for (const auto &action : simulator_control_actions) {
emit_simulator_gcode_control_action(sink_, action, line_number);
@@ -6578,7 +6806,8 @@ int SmokeGcodeParser::parse(const char *program, size_t program_len, std::string
if (l == 0) {
if (spindle_tool_id_ > 0) {
if (error) {
*error = "G10 L0 not allowed with loaded tool";
*error = "G10 L0 not allowed with loaded tool <" +
std::to_string(spindle_tool_id_) + ">";
}
return -1;
}
@@ -6761,21 +6990,30 @@ int SmokeGcodeParser::parse(const char *program, size_t program_len, std::string
if (explicit_comp) {
if (!words.count('D')) {
if (error) {
*error = "G41.1/G42.1 requires a D word";
*error = std::string(cutter_comp_left ? "G41.1" : "G42.1") +
" with no D word";
}
return -1;
}
if (words.count('L') && sink_.plane() != 18) {
if (error) {
*error = "G41.1/G42.1 with L word requires G18";
*error = std::string(cutter_comp_left ? "G41.1" : "G42.1") +
" with L word, but plane is not G18";
}
return -1;
}
} else if (words.count('D')) {
const double d_value = words.at('D');
if (std::fabs(d_value - std::round(d_value)) > 0.0001 || d_value < 0.0) {
if (std::fabs(d_value - std::round(d_value)) > 0.0001) {
if (error) {
*error = "G41/G42 requires a non-negative whole-number D word";
*error = std::string(cutter_comp_left ? "G41" : "G42") +
" requires D word to be a whole number";
}
return -1;
}
if (d_value < 0.0) {
if (error) {
*error = "Negative d word tool radius index used";
}
return -1;
}
@@ -7626,6 +7864,16 @@ int SmokeGcodeParser::parse(const char *program, size_t program_len, std::string
break;
}
const int spindle = selected_spindle;
if ((m == 3 || m == 4 || m == 5) &&
parsed_word_list.dollar_flag &&
!smoke_spindle_selector_valid_for_speed(parsed_word_list.dollar_value)) {
if (error) {
*error = "Spindle ($) number out of range in M" + std::to_string(m) +
" Command\nnum_spindles =" + std::to_string(kSmokeNumSpindles) +
". $=" + std::to_string(rounded_value(parsed_word_list.dollar_value)) + "\n";
}
return -1;
}
if (m == 3) {
sink_.start_spindle(spindle, 1, line_number);
if (stop_if_callback_aborted(sink_, error)) {
@@ -7944,17 +8192,17 @@ int SmokeGcodeParser::parse(const char *program, size_t program_len, std::string
return -1;
}
} else if (m == 428) {
sink_.switch_kinematics(1, true, line_number);
sink_.switch_kinematics(1, line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
} else if (m == 429) {
sink_.switch_kinematics(0, false, line_number);
sink_.switch_kinematics(0, line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
} else if (m == 430) {
sink_.switch_kinematics(2, true, line_number);
sink_.switch_kinematics(2, line_number);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
@@ -8077,8 +8325,15 @@ int SmokeGcodeParser::parse(const char *program, size_t program_len, std::string
}
return -1;
}
const bool arc_center_or_radius_words =
(modal_motion == 2 || modal_motion == 3) &&
(words.count('I') || words.count('J') || words.count('K') ||
(words.count('R') && !has_m_code(word_list, 19)));
const bool current_line_requests_motion =
has_axis_words || parsed_word_list.radius_flag || parsed_word_list.theta_flag;
has_axis_words ||
parsed_word_list.radius_flag ||
parsed_word_list.theta_flag ||
arc_center_or_radius_words;
const bool explicit_threading_motion =
has_g_code(word_list, 33.0) || has_g_code(word_list, 33.1) || has_g_code(word_list, 76.0);
if ((current_line_requests_motion || explicit_threading_motion) && !has_axis_words &&
@@ -8723,11 +8978,23 @@ int SmokeGcodeParser::parse(const char *program, size_t program_len, std::string
}
return -1;
}
if (!validate_arc_format_words(words, error)) {
return -1;
}
if (!validate_arc_plane_center_words(words, sink_.plane(), error)) {
return -1;
}
if (!validate_r_arc_endpoint_words(words, parsed_word_list, sink_.plane(), error)) {
return -1;
}
event.arc_turns = modal_motion == 2 ? -1 : 1;
if (words.count('R') &&
!validate_r_arc_reaches_end_point(event, sink_.plane(), words.at('R') * sink_.unit_scale(), error)) {
return -1;
}
if (!validate_absolute_center_arc_axis_words(words, sink_.plane(), arc_absolute_, error)) {
return -1;
}
set_arc_center(&event, words, sink_.plane(), sink_.unit_scale(), arc_absolute_);
if (!words.count('R') &&
!validate_arc_center_radius_consistency(event, sink_.plane(), sink_.unit_scale(), error)) {
@@ -8964,7 +9231,7 @@ int SmokeGcodeParser::parse(const char *program, size_t program_len, std::string
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}
emit_comment_state(sink_, line_number, 970);
emit_comment_state(sink_, line_number, kConstantRpmSpindleModeEvent);
if (stop_if_callback_aborted(sink_, error)) {
return -1;
}