对齐 LinuxCNC 解释器行为覆盖

core/src/canon_event_sink.cpp

@@ -1,7 +1,23 @@
 #include "canon_event_sink.h"
 
+#include <cmath>
+
 #include "rtcp_kinematics.h"
 
+namespace {
+
+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);
+    const double s = std::sin(radians);
+    const double nx = *x * c - *y * s;
+    const double ny = *x * s + *y * c;
+    *x = nx;
+    *y = ny;
+}
+
+} // namespace
+
 void CanonEventSink::reset() {
     position_ = {};
     plane_ = 17;
@@ -14,6 +30,7 @@ void CanonEventSink::reset() {
     probe_position_ = {};
     probe_tripped_ = false;
     tool_length_offset_ = {};
+    xy_rotation_degrees_ = 0.0;
     callback_status_ = 0;
     kinematics_type_ = 1;
     feed_override_ = false;
@@ -62,6 +79,22 @@ void CanonEventSink::set_tool_length_offset(const CncSimPose &offset) {
     tool_length_offset_ = offset;
 }
 
+void CanonEventSink::apply_tool_length_offset(const CncSimPose &offset) {
+    double dx = tool_length_offset_.x - offset.x;
+    double dy = tool_length_offset_.y - offset.y;
+    rotate_xy(&dx, &dy, -xy_rotation_degrees_);
+    position_.x += dx;
+    position_.y += dy;
+    position_.z += tool_length_offset_.z - offset.z;
+    position_.a += tool_length_offset_.a - offset.a;
+    position_.b += tool_length_offset_.b - offset.b;
+    position_.c += tool_length_offset_.c - offset.c;
+    position_.u += tool_length_offset_.u - offset.u;
+    position_.v += tool_length_offset_.v - offset.v;
+    position_.w += tool_length_offset_.w - offset.w;
+    tool_length_offset_ = offset;
+}
+
 void CanonEventSink::set_rtcp_state(bool enabled, int h_code, int line) {
     rtcp_enabled_ = enabled;
     rtcp_h_code_ = enabled ? h_code : 0;
@@ -99,6 +132,7 @@ void CanonEventSink::set_g92_offset(const CncSimPose &offset, int line) {
 }
 
 void CanonEventSink::set_xy_rotation(double angle_degrees, int line) {
+    xy_rotation_degrees_ = angle_degrees;
     CncSimEvent event = base_event(CNC_SIM_EVENT_SET_XY_ROTATION, line);
     event.feed = angle_degrees;
     emit(event);
@@ -138,8 +172,13 @@ void CanonEventSink::set_feed_mode(int spindle, int mode, int line) {
 }
 
 void CanonEventSink::set_spindle_speed(double spindle, int line) {
-    spindle_ = spindle;
+    set_spindle_speed(0, spindle, line);
+}
+
+void CanonEventSink::set_spindle_speed(int spindle, double speed, int line) {
+    spindle_ = speed;
     CncSimEvent event = base_event(CNC_SIM_EVENT_SET_SPINDLE, line);
+    event.tool = spindle;
     event.spindle = spindle_;
     event.reserved = spindle_direction_;
     emit(event);
@@ -229,16 +268,26 @@ void CanonEventSink::set_override_control(int control, bool enabled, int spindle
 }
 
 void CanonEventSink::start_spindle(int direction, int line) {
+    start_spindle(0, direction, line);
+}
+
+void CanonEventSink::start_spindle(int spindle, int direction, int line) {
     spindle_direction_ = direction < 0 ? 2 : 1;
     CncSimEvent event = base_event(CNC_SIM_EVENT_SET_SPINDLE, line);
+    event.tool = spindle;
     event.spindle = spindle_;
     event.reserved = spindle_direction_;
     emit(event);
 }
 
 void CanonEventSink::stop_spindle(int line) {
+    stop_spindle(0, line);
+}
+
+void CanonEventSink::stop_spindle(int spindle, int line) {
     spindle_direction_ = 0;
     CncSimEvent event = base_event(CNC_SIM_EVENT_SET_SPINDLE, line);
+    event.tool = spindle;
     event.spindle = 0.0;
     event.reserved = spindle_direction_;
     emit(event);

core/src/canon_event_sink.h

@@ -13,6 +13,7 @@ public:
     void configure_rtcp(bool enabled, double tool_length);
     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 set_g5x_offset(int index, const CncSimPose &offset, int line);
@@ -25,6 +26,7 @@ public:
     void set_feed_rate(double feed, int line);
     void set_feed_mode(int spindle, int mode, int line);
     void set_spindle_speed(double spindle, int line);
+    void set_spindle_speed(int spindle, double speed, int line);
     void set_spindle_mode(int spindle, double mode, int line);
     void set_speed_feed_sync(int spindle, double feed_per_revolution, bool velocity_mode, bool enabled, int line);
     void orient_spindle(int spindle, double orientation, int mode, int line);
@@ -34,7 +36,9 @@ public:
     void wait_input(int index, int input_type, int wait_type, double timeout, int line);
     void set_override_control(int control, bool enabled, int spindle, int line);
     void start_spindle(int direction, int line);
+    void start_spindle(int spindle, int direction, int line);
     void stop_spindle(int line);
+    void stop_spindle(int spindle, int line);
     void select_tool(int tool);
     void change_tool(int line);
     void straight_traverse(int line, const CncSimPose &end);
@@ -92,6 +96,7 @@ private:
     double rtcp_tool_length_ = 0.0;
     int rtcp_h_code_ = 0;
     CncSimPose tool_length_offset_{};
+    double xy_rotation_degrees_ = 0.0;
     int kinematics_type_ = 1;
     bool feed_override_ = false;
     bool speed_override_ = false;

core/src/linuxcnc_canon_bridge.cpp

@@ -50,6 +50,12 @@ int plane_to_g_code(CANON_PLANE plane) {
         return 18;
     case CANON_PLANE::YZ:
         return 19;
+    case CANON_PLANE::UV:
+        return 171;
+    case CANON_PLANE::UW:
+        return 181;
+    case CANON_PLANE::VW:
+        return 191;
     default:
         return 17;
     }
@@ -73,6 +79,12 @@ CANON_PLANE g_code_to_plane(int plane) {
         return CANON_PLANE::XZ;
     case 19:
         return CANON_PLANE::YZ;
+    case 171:
+        return CANON_PLANE::UV;
+    case 181:
+        return CANON_PLANE::UW;
+    case 191:
+        return CANON_PLANE::VW;
     case 17:
     default:
         return CANON_PLANE::XY;

core/src/linuxcnc_tooldata_fixture.cpp

@@ -2,6 +2,8 @@
 
 #include "emc/tooldata/tooldata.hh"
 
+#include <cstdlib>
+
 void cnc_sim_init_minimal_linuxcnc_tooldata() {
     static bool created = false;
     if (!created) {
@@ -22,5 +24,29 @@ void cnc_sim_init_minimal_linuxcnc_tooldata() {
     tool.diameter = 6.0;
     tool.offset.tran.z = 12.5;
     tooldata_put(tool, 1);
-    tooldata_last_index_set(1);
+
+    CANON_TOOL_TABLE thread_tool = tooldata_entry_init();
+    thread_tool.toolno = 4;
+    thread_tool.pocketno = 4;
+    thread_tool.diameter = 1.0;
+    thread_tool.offset.tran.z = 0.7;
+    tooldata_put(thread_tool, 4);
+
+    if (const char *spindle_tool = std::getenv("CNC_SIM_TOOL_IN_SPINDLE")) {
+        const int tool_id = std::atoi(spindle_tool);
+        if (tool_id == 1) {
+            CANON_TOOL_TABLE spindle_tooldata = tool;
+            spindle_tooldata.pocketno = 0;
+            spindle_tooldata.frontangle = 12.0;
+            spindle_tooldata.backangle = 34.0;
+            spindle_tooldata.orientation = 5;
+            tooldata_put(spindle_tooldata, 0);
+        } else if (tool_id == 4) {
+            CANON_TOOL_TABLE spindle_tooldata = thread_tool;
+            spindle_tooldata.pocketno = 0;
+            tooldata_put(spindle_tooldata, 0);
+        }
+    }
+
+    tooldata_last_index_set(4);
 }

core/src/smoke_gcode_parser.h

@@ -17,9 +17,24 @@ private:
     bool arc_absolute_ = false;
     bool lathe_diameter_mode_ = false;
     bool cutter_comp_on_ = false;
+    bool cutter_comp_exit_pending_ = false;
+    bool cutter_comp_has_prev_vector_ = false;
+    bool cutter_comp_has_prev_prev_vector_ = false;
+    double cutter_comp_radius_ = 0.0;
+    int spindle_tool_id_ = 0;
+    CncSimPose cutter_comp_prev_vector_{};
+    CncSimPose cutter_comp_prev_prev_vector_{};
+    int cutter_comp_mode_ = 40;
+    bool spindle_css_mode_ = false;
+    bool tool_selected_ = false;
+    bool nurbs_g5_collecting_ = false;
+    int nurbs_g5_control_points_ = 0;
+    int nurbs_g5_order_ = 3;
+    bool nurbs_g6_collecting_ = false;
     int feed_mode_ = 0;
     int canned_cycle_ = 0;
     bool canned_return_to_initial_ = false;
+    bool g92_applied_ = false;
     bool canned_has_r_ = false;
     bool canned_has_z_ = false;
     bool canned_has_p_ = false;
@@ -36,6 +51,8 @@ private:
     double canned_i_ = 0.0;
     double canned_j_ = 0.0;
     double canned_k_ = 0.0;
+    double m66_result_ = 0.0;
     double oword_return_value_ = 0.0;
     bool oword_value_returned_ = false;
+    int selected_tool_id_ = 0;
 };