add EYOU motor control

cmvr-es/common/config/biohead_config/biohead_config.pb.txt

@@ -0,0 +1,27 @@
+bioheads {
+  id: "bio_head"
+  serial: "/dev/ttyUSB0"
+  ctrlFreq: 50
+  enable: false
+
+  eyebrow {
+    serial: "64:0~3"
+    offest: "90 90 90 90"
+    jLmtUp: "90 170 155 110"
+    jLmtLow: "20 77 90 20"
+  }
+
+  eye {
+    serial: "64:4~9"
+    offest: "90 90 90 90 90 90"
+    jLmtUp: "90 150 165 90 120 115"
+    jLmtLow: "20 90 90 25 70 75"
+  }
+
+  mouth {
+    serial: "65:0~9"
+    offest: "90 90 90 90 90 90 90 90 90 90"
+    jLmtUp: "150 110 130 140 100 105 110 125 90 95"
+    jLmtLow: "70 30 80 80 65 55 45 80 85 90"
+  }
+}

cmvr-es/common/config/camera_config/camera_config.pb.txt

@@ -27,7 +27,7 @@ realsense_cameras {
   align_mode: ALIGN_MODE_COLOR
   buffer_size: 30
   sync: false
-  enable: true
+  enable: false
 }
 
 realsense_cameras {

cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt

@@ -1,6 +1,6 @@
 rh56dftp_dexhands {
     id: "hand1"
-    ip: "192.168.1.213"
+    ip: "192.168.1.223"
     port: 6000
     poll_interval_ms: 10
     enable: false
@@ -11,7 +11,7 @@ rh56dftp_dexhands {
     ip: "192.168.1.224"
     port: 6000
     poll_interval_ms: 10
-    enable: true
+    enable: false
 }
 
 px_6ax_gen3 {
@@ -32,5 +32,5 @@ px_6ax_gen3 {
     sensor_name: "Paxini Gen3末端压力"
     polling_read_mode: PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE
     auto_calibrate: false
-    enable: true
+    enable: false
 }

cmvr-es/common/config/monitor_config/monitor_config.pb.txt

@@ -0,0 +1,25 @@
+# 监控管理器配置
+monitor_manager {
+  disk_monitor {
+    id: "file_monitor"
+    freq: 1
+    enable: false
+
+    folders {
+      fileDir: "/home/share/assets/audio"
+      maxVolume: 1000
+    }
+    folders {
+      fileDir: "/home/share/assets/image"
+      maxVolume: 1000
+    }
+    folders {
+      fileDir: "/home/share/assets/video"
+      maxVolume: 1000
+    }
+    folders {
+      fileDir: "../log"
+      maxVolume: 1000
+    }
+  }
+}

cmvr-es/common/config/robot_config/robot_config.pb.txt

@@ -0,0 +1,36 @@
+ethercat_robots {
+  id: "ec_robot_01"
+  dof: 6
+  urdf: "/home/cmvr/Projects/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
+  baseLink: "PELVIS_S"
+  jointNames: "L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
+  linkNames: "PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM"
+  bufferSize: 100
+  verbose: true
+  updFreq: 1000
+  enable: true
+
+  # EtherCAT主站配置
+  master {
+    interface: "enp1s0"
+    cycle_time_us: 1000
+    enable: true
+  }
+
+  # 电机配置列表
+  motors {
+    slave: 1
+    jointName: "L_SHOULDER_P"
+    limitQLb: -3.14
+    limitQUb: 3.14
+    limitQd: 3.0
+  }
+
+  motors {
+      slave: 2
+      jointName: "L_SHOULDER_R"
+      limitQLb: -3.14
+      limitQUb: 3.14
+      limitQd: 3.0
+    }
+}

cmvr-es/common/config/server_config/server_config.pb.txt

@@ -0,0 +1,3 @@
+grpc_server {
+    port: "50052"
+}

cmvr-es/common/utils/config_helper/include/config_helper.h

@@ -10,6 +10,9 @@
 #include "cmvr/config/microphone_config/microphone_config.pb.h"
 #include "cmvr/config/speaker_config/speaker_conifg.pb.h"
 #include "cmvr/config/touch_screen_app_config/touch_screen_app_config.pb.h"
+#include "cmvr/config/biohead_config/biohead_config.pb.h"
+#include "cmvr/config/robot_config/robot_config.pb.h"
+#include "cmvr/config/server_config/server_config.pb.h"
 
 #define GET_CONFIG(file, para)                                             \
     ([&]() -> bool {                                                       \
@@ -82,6 +85,20 @@ namespace cmvr
             return SET_CONFIG(config, touch_screen_app_config_file);
         }
 
+        static bool getHeadConfig(config::HeadConfig& config)
+        {
+            return GET_CONFIG(biohead_config_file, config);
+        }
+
+        static bool getRobotConfig(config::RobotConfig& config)
+        {
+            return GET_CONFIG(robot_config_file, config);
+        }
+
+        static bool getServerConfig(config::ServerConfig& config)
+        {
+            return GET_CONFIG(server_config_file, config);
+        }
     private:
         // Make macros able to call these (macros call ::cmvr::ConfigHelper::xxx)
         template <class T>

cmvr-es/common/utils/config_helper/include/config_setting.h

@@ -6,3 +6,6 @@ DECLARE_string(dexhand_config_file);
 DECLARE_string(microphone_config_file);
 DECLARE_string(speaker_config_file);
 DECLARE_string(touch_screen_app_config_file);
+DECLARE_string(biohead_config_file);
+DECLARE_string(robot_config_file);
+DECLARE_string(server_config_file);

cmvr-es/common/utils/config_helper/src/config_setting.cpp

@@ -88,3 +88,15 @@ DEFINE_string(speaker_config_file,
 DEFINE_string(touch_screen_app_config_file,
              basePath() + "touch_screen_app_config/touch_screen_app_config.pb.txt",
               "The configuration file for TouchScreenApp");
+
+DEFINE_string(biohead_config_file,
+             basePath() + "biohead_config/biohead_config.pb.txt",
+              "The configuration file for BioHead");
+
+DEFINE_string(robot_config_file,
+             basePath() + "robot_config/robot_config.pb.txt",
+              "The configuration file for Robot");
+
+DEFINE_string(server_config_file,
+             basePath() + "server_config/server_config.pb.txt",
+              "The configuration file for Server");

cmvr-es/common/utils/ffmpeg/include/FrameData.h

@@ -0,0 +1,53 @@
+// FrameData.h
+#pragma once
+
+#include <opencv2/opencv.hpp>
+
+extern "C" {
+#include <libavformat/avformat.h>
+#include <libavcodec/avcodec.h>
+}
+
+namespace ffmpeg {
+
+    struct FrameData {
+        AVFrame* av_frame = nullptr;      // 编码用帧（YUV格式）
+        cv::Mat rgb_image;                 // OpenCV用图像（BGR格式）
+        int64_t pts = 0;                  // 时间戳
+        int64_t frame_index = 0;          // 帧序号
+
+        FrameData() = default;
+
+        ~FrameData() {
+            if (av_frame) {
+                av_frame_free(&av_frame);
+            }
+        }
+
+        // 禁止拷贝
+        FrameData(const FrameData&) = delete;
+        FrameData& operator=(const FrameData&) = delete;
+
+        // 允许移动
+        FrameData(FrameData&& other) noexcept
+            : av_frame(other.av_frame),
+              rgb_image(std::move(other.rgb_image)),
+              pts(other.pts),
+              frame_index(other.frame_index) {
+            other.av_frame = nullptr;
+        }
+
+        FrameData& operator=(FrameData&& other) noexcept {
+            if (this != &other) {
+                if (av_frame) av_frame_free(&av_frame);
+                av_frame = other.av_frame;
+                rgb_image = std::move(other.rgb_image);
+                pts = other.pts;
+                frame_index = other.frame_index;
+                other.av_frame = nullptr;
+            }
+            return *this;
+        }
+    };
+
+} // namespace ffmpeg

cmvr-es/data_center/CMakeLists.txt

@@ -11,6 +11,7 @@ target_link_libraries(data_center PRIVATE
         cmvr_es::utils
         cmvr_es::device::canbus
         cmvr_es::device::ti5motor
+		cmvr_es::device::eyoumotor
         protobuf
 		glog
 )

cmvr-es/data_center/include/motors_info.h

@@ -23,8 +23,9 @@ namespace cmvr {
 
     public:
         ~MotorsInfo(){};
-
         void init(const XmlNode &cfg);
+        void init(const cmvr::config::HumanRobotConfig& config);
+        void init(const cmvr::config::EthercatRobotConfig& config);
 
         static MotorsInfo *getInstance() {
             static MotorsInfo instance;

cmvr-es/data_center/src/motors_info.cpp

@@ -3,6 +3,8 @@
 //
 
 #include "data_center/include/motors_info.h"
+
+#include "motor/eyou_motor/eyou_motor.h"
 #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
 #include "motor/ti5_motor/ti5_motor.h"
 
@@ -151,6 +153,152 @@ void MotorsInfo::init(const XmlNode &cfg) {
     LOG(INFO) << "All enabled motors initialized successfully.";
 }
 
+void MotorsInfo::init(const cmvr::config::HumanRobotConfig& config) {
+    const auto& can_manager = config.can_manager();
+
+    // 读取enable状态
+    left_arm_enabled_ = can_manager.left_arm().enable();
+    right_arm_enabled_ = can_manager.right_arm().enable();
+    waist_enabled_ = can_manager.waist().enable();
+    head_enabled_ = can_manager.head().enable();
+
+    // 局部变量存储电机配置
+    std::vector<cmvr::config::MotorConfig> l_motors_cfg;
+    std::vector<cmvr::config::MotorConfig> r_motors_cfg;
+    std::vector<cmvr::config::MotorConfig> waist_motors_cfg;
+    std::vector<cmvr::config::MotorConfig> head_motors_cfg;
+
+    // 左臂
+    if (left_arm_enabled_) {
+        const auto& left_arm = can_manager.left_arm();
+        for (const auto& motor : left_arm.motors()) {
+            l_motors_cfg.push_back(motor);
+        }
+        l_can_client_ = std::make_shared<SocketCanClientRaw>(left_arm);
+        l_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail>>();
+        l_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail>>();
+        l_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail>>();
+    }
+
+    // 右臂
+    if (right_arm_enabled_) {
+        const auto& right_arm = can_manager.right_arm();
+        for (const auto& motor : right_arm.motors()) {
+            r_motors_cfg.push_back(motor);
+        }
+        r_can_client_ = std::make_shared<SocketCanClientRaw>(right_arm);
+        r_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail>>();
+        r_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail>>();
+        r_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail>>();
+    }
+
+    // 腰部
+    if (waist_enabled_) {
+        const auto& waist = can_manager.waist();
+        for (const auto& motor : waist.motors()) {
+            waist_motors_cfg.push_back(motor);
+        }
+        waist_can_client_ = std::make_shared<SocketCanClientRaw>(waist);
+        waist_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail>>();
+        waist_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail>>();
+        waist_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail>>();
+    }
+
+    // 头部
+    if (head_enabled_) {
+        const auto& head = can_manager.head();
+        for (const auto& motor : head.motors()) {
+            head_motors_cfg.push_back(motor);
+        }
+        head_can_client_ = std::make_shared<SocketCanClientRaw>(head);
+        head_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail>>();
+        head_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail>>();
+        head_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail>>();
+    }
+
+    // 初始化 limbs
+    struct Limb {
+        std::string name;
+        bool enabled;
+        std::shared_ptr<AbstractCanbus> client;
+        std::shared_ptr<CanSender<msgs::RobotDetail>> sender;
+        std::shared_ptr<CanReceiver<msgs::RobotDetail>> receiver;
+        std::shared_ptr<MessageManager<msgs::RobotDetail>> message_manager;
+        std::vector<cmvr::config::MotorConfig> motor_cfgs;
+    };
+
+    std::vector<Limb> limbs{
+        {"WAIST", waist_enabled_, waist_can_client_, waist_can_sender_, waist_can_receiver_, waist_message_manager_, waist_motors_cfg},
+        {"LEFT_ARM", left_arm_enabled_, l_can_client_, l_can_sender_, l_can_receiver_, l_message_manager_, l_motors_cfg},
+        {"RIGHT_ARM", right_arm_enabled_, r_can_client_, r_can_sender_, r_can_receiver_, r_message_manager_, r_motors_cfg},
+        {"HEAD", head_enabled_, head_can_client_, head_can_sender_, head_can_receiver_, head_message_manager_, head_motors_cfg}
+    };
+
+    motor_manager_ = std::make_shared<MotorManager>();
+    std::vector<std::future<void>> tasks;
+
+    for (auto &limb : limbs) {
+        if (!limb.enabled) continue;
+
+        if (limb.client) limb.client->init();
+
+        if (limb.sender && limb.receiver && limb.client) {
+            auto ret = limb.sender->Init(limb.client.get(), false);
+            if (ret != ErrorCode::OK)
+                LOG(ERROR) << "Failed to init " << limb.name << " CAN sender.";
+
+            ret = limb.receiver->Init(limb.client.get(), limb.message_manager.get(), false);
+            if (ret != ErrorCode::OK)
+                LOG(ERROR) << "Failed to init " << limb.name << " CAN receiver.";
+
+            limb.client->start();
+            ret = limb.sender->Start();
+            if (ret != ErrorCode::OK)
+                LOG(ERROR) << "Failed to start " << limb.name << " CAN sender.";
+
+            ret = limb.receiver->Start();
+            if (ret != ErrorCode::OK)
+                LOG(ERROR) << "Failed to start " << limb.name << " CAN receiver.";
+        }
+
+        std::shared_ptr<Ti5MotorCanopenProtocol> protocol = nullptr;
+        if (limb.sender && limb.message_manager) {
+            protocol = std::make_shared<Ti5MotorCanopenProtocol>(limb.sender, limb.message_manager);
+        }
+
+        if (!limb.motor_cfgs.empty()) {
+            tasks.push_back(std::async(std::launch::async, [this, protocol, &limb] {
+                LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing " << limb.name << " motors...";
+                for (const auto& motor_cfg : limb.motor_cfgs) {
+                    auto motor = std::make_shared<Ti5Motor>(motor_cfg);
+                    if (protocol) motor->setProtocol(protocol);
+                    motor->init();
+                    motor_manager_->addMotor(motor);
+                }
+            }));
+        }
+    }
+
+    for (auto& task : tasks) task.get();
+    LOG(INFO) << "All enabled motors initialized successfully.";
+}
+
+
+void MotorsInfo::init(const cmvr::config::EthercatRobotConfig& config)
+{
+    if (!motor_manager_)
+    {
+        motor_manager_ = std::make_shared<MotorManager>();
+    }
+    auto  protocol = std::make_shared<EYouEthercatProtocol>(config.master().interface());
+    for (const auto& motor_cfg : config.motors()) {
+        auto motor = std::make_shared<EYouMotor>(motor_cfg);
+        if (protocol) motor->setProtocol(protocol);
+        motor->init();
+        motor_manager_->addMotor(motor);
+    }
+}
+
 bool MotorsInfo::getJointsQ(const std::vector<std::string> &joints_name,
                                 std::unordered_map<std::string, double> &joint_qs) const {
     auto res{true};

cmvr-es/device_manager/src/device_factory.cpp

@@ -184,6 +184,10 @@ template std::shared_ptr<AbstractDexHand> DeviceFactory::createFromConfig<Abstra
 template std::shared_ptr<AbstractDexHand> DeviceFactory::createFromConfig<AbstractDexHand>(const config::PX6AXGen3& cfg);
 template std::shared_ptr<AbstractMicrophone> DeviceFactory::createFromConfig<AbstractMicrophone>(const config::FFMpegMicroPhoneConfig& cfg);
 template std::shared_ptr<AbstractSpeaker> DeviceFactory::createFromConfig<AbstractSpeaker>(const config::FFMpegSpeakerConfig& cfg);
+template std::shared_ptr<AbstractBiohead> DeviceFactory::createFromConfig<AbstractBiohead>(const config::BioHeadConfig& cfg);
+template std::shared_ptr<AbstractRobot> DeviceFactory::createFromConfig<AbstractRobot>(const config::HumanRobotConfig& cfg);
+template std::shared_ptr<AbstractRobot> DeviceFactory::createFromConfig<AbstractRobot>(const config::EthercatRobotConfig& cfg);
+
 template <typename DeviceType, typename ConfigType>
 std::shared_ptr<DeviceType> DeviceFactory::createFromConfig(const ConfigType& cfg)
 {
@@ -244,6 +248,38 @@ std::shared_ptr<DeviceType> DeviceFactory::createFromConfig(const ConfigType& cf
                 throw runtime_error("[DeviceFactory]: Unsupported microphone device type");
             }
         }
+        else if constexpr (std::is_same_v<DeviceType, AbstractBiohead>)
+        {
+            if constexpr (std::is_same_v<ConfigType, config::BioHeadConfig>)
+            {
+                return std::make_shared<BioHeadRobot>(cfg);
+            }
+            else
+            {
+                LOG(ERROR) << "[DeviceFactory]: Unsupported microphone device type ";
+                throw runtime_error("[DeviceFactory]: Unsupported microphone device type");
+            }
+        }
+        else if constexpr (std::is_same_v<DeviceType, AbstractRobot>)
+        {
+            if constexpr (std::is_same_v<ConfigType, config::HumanRobotConfig>)
+            {
+                auto motos_info = MotorsInfo::getInstance();
+                motos_info->init(cfg);
+                return std::make_shared<HumanoidRobot<14>>(cfg);
+            }
+            else if constexpr (std::is_same_v<ConfigType, config::EthercatRobotConfig>)
+            {
+                auto motos_info = MotorsInfo::getInstance();
+                motos_info->init(cfg);
+                return std::make_shared<HumanoidRobot<14>>(cfg);
+            }
+            else
+            {
+                LOG(ERROR) << "[DeviceFactory]: Unsupported microphone device type ";
+                throw runtime_error("[DeviceFactory]: Unsupported microphone device type");
+            }
+        }
         else {
             LOG(ERROR) << "[DeviceFactory]: Unsupported device type";
             throw runtime_error("[DeviceFactory]: Unsupported device type");
@@ -253,3 +289,4 @@ std::shared_ptr<DeviceType> DeviceFactory::createFromConfig(const ConfigType& cf
         return nullptr;
     }
 }
+

cmvr-es/device_manager/src/device_manager.cpp

@@ -312,6 +312,24 @@ void DeviceManager::init_devices_() {
             LOG(INFO) << "[DeviceManager]: Init Robot " << id << " Success";
         }
 
+        // EYOU的先用新版的
+        config::RobotConfig robot_config;
+        ConfigHelper::getRobotConfig(robot_config);
+        for (int i = 0; i < robot_config.ethercat_robots().size(); i++) {
+            auto robot = robot_config.ethercat_robots(i);
+            if (!robot.enable())
+                continue;
+            auto id = robot.id();
+            if (devices_.count(id)) {
+                LOG(ERROR) << "[DeviceManager]: Duplicate Robot Device ID" << id;
+                throw runtime_error("[DeviceManager]: Duplicate Robot Device ID" + id);
+            }
+            std::cout << "DexHand[" << i << "]: " << id << std::endl;
+            auto device = dev_factory_->createFromConfig<AbstractRobot,config::EthercatRobotConfig>(robot);
+            devices_[id] = device;
+            std::get<std::shared_ptr<AbstractRobot>>(devices_[id])->init();
+        }
+
         config::MicroPhoneConfig micro_phone_config;
         ConfigHelper::getMicroPhonesConfig(micro_phone_config);
         for (int i = 0; i < micro_phone_config.ffmpeg_microphones().size(); i++) {
@@ -346,22 +364,21 @@ void DeviceManager::init_devices_() {
             std::get<std::shared_ptr<AbstractSpeaker>>(devices_[id])->init();
         }
 
-
+        config::HeadConfig head_cfg;
-        auto biohead_node = dmgr_node.getChild("BioHead");
+        ConfigHelper::getHeadConfig(head_cfg);
-        for (auto &node: biohead_node.getChildren()){
+        for (int i = 0; i < head_cfg.bioheads().size(); i++) {
-            string id = node.getAttrString("id");
+            auto head = head_cfg.bioheads(i);
-            if (devices_.count(id)){
+            if (!head.enable())
-                LOG(ERROR) << "[DeviceManager]: Duplicate AbstractBiohead Device ID" << id;
+                continue;
-                throw runtime_error("[DeviceManager]: Duplicate AbstractBiohead Device ID" + id);
+            auto id = head.id();
-            }
+            if (devices_.count(id)) {
-            auto device = dev_factory_->create<AbstractBiohead>(node);
+                LOG(ERROR) << "[DeviceManager]: Duplicate Biohead Device ID" << id;
-            if (device == nullptr){
+                throw runtime_error("[DeviceManager]: Duplicate Biohead Device ID" + id);
-                throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id);
             }
+            std::cout << "Biohead[" << i << "]: " << id << std::endl;
+            auto device = dev_factory_->createFromConfig<AbstractBiohead,config::BioHeadConfig>(head);
             devices_[id] = device;
             std::get<std::shared_ptr<AbstractBiohead>>(devices_[id])->init();
-            LOG(INFO) << "[BioHead]: Init biohead " << id << " Success";
-
         }
     }
     catch (const exception& e) {

cmvr-es/devices/biohead/abstract_biohead.h

@@ -58,6 +58,7 @@ namespace cmvr::device {
     // 抽象头部类
     class AbstractBiohead : public AbstractDevice {
     public:
+        AbstractBiohead() = default;
         explicit AbstractBiohead(const XmlNode &config) : AbstractDevice(config) {}
         ~AbstractBiohead() override = default;
 

cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h

@@ -7,7 +7,7 @@
 #include <string>
 #include <memory>
 #include <mutex>
-
+#include "cmvr/config/biohead_config/biohead_config.pb.h"
 namespace cmvr::device {
 
     struct ServoChannel {
@@ -18,6 +18,7 @@ namespace cmvr::device {
     class BioHeadRobot : public AbstractBiohead {
     public:
         explicit BioHeadRobot(const XmlNode &config);
+        BioHeadRobot(const cmvr::config::BioHeadConfig& config);
         ~BioHeadRobot() override = default;
 
         // 接口实现
@@ -66,7 +67,7 @@ namespace cmvr::device {
         std::shared_ptr<std::thread> speak_thread_;
         std::atomic<bool> speak_running_{false};
 
-
+        cmvr::config::BioHeadConfig config_;
 
     };
 

cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp

@@ -26,6 +26,219 @@ BioHeadRobot::BioHeadRobot(const XmlNode &config) : AbstractBiohead(config) {
 
 }
 
+BioHeadRobot::BioHeadRobot(const cmvr::config::BioHeadConfig& config) : config_(config) {
+    // 从config中读取串口参数
+    port_name_ = config_.serial();
+
+    serial_ = std::make_shared<SerialPort>();
+    if (!serial_->open(port_name_, 115200)) {
+        LOG(ERROR) << "[BioHeadRobot] Failed to open serial port: " << port_name_;
+    } else {
+        serial_->wakeupESP32(port_name_);
+        LOG(INFO) << "[BioHeadRobot] Serial port opened and ESP32 woken up.";
+    }
+
+    // 解析眉毛配置
+    if (config_.has_eyebrow()) {
+        const auto& eyebrow = config_.eyebrow();
+        std::string serial_str = eyebrow.serial();
+        std::string offset_str = eyebrow.offest();
+        std::string min_angles_str = eyebrow.jlmtlow();
+        std::string max_angles_str = eyebrow.jlmtup();
+
+        // 解析偏移角
+        std::vector<double> offsets;
+        std::istringstream oss(offset_str);
+        double val;
+        while (oss >> val) offsets.push_back(val);
+
+        // 解析角度限制
+        std::vector<double> min_angles;
+        if (!min_angles_str.empty()) {
+            std::istringstream min_ss(min_angles_str);
+            while (min_ss >> val) min_angles.push_back(val);
+        }
+
+        std::vector<double> max_angles;
+        if (!max_angles_str.empty()) {
+            std::istringstream max_ss(max_angles_str);
+            while (max_ss >> val) max_angles.push_back(val);
+        }
+
+        // 解析 channel，如 64:0~3
+        size_t colon = serial_str.find(':');
+        if (colon != std::string::npos) {
+            uint8_t addr = std::stoi(serial_str.substr(0, colon));
+            std::string range = serial_str.substr(colon + 1);
+            size_t tilde = range.find('~');
+            if (tilde != std::string::npos) {
+                int ch_start = std::stoi(range.substr(0, tilde));
+                int ch_end = std::stoi(range.substr(tilde + 1));
+                int channel_count = ch_end - ch_start + 1;
+
+                // 添加舵机通道
+                for (int ch = ch_start; ch <= ch_end; ++ch) {
+                    channels_.push_back({addr, static_cast<uint8_t>(ch)});
+                }
+
+                // 添加角度限制
+                if (min_angles.size() == 1) {
+                    min_angles_.insert(min_angles_.end(), channel_count, min_angles[0]);
+                } else if (min_angles.size() >= channel_count) {
+                    min_angles_.insert(min_angles_.end(), min_angles.begin(), min_angles.begin() + channel_count);
+                } else {
+                    min_angles_.insert(min_angles_.end(), channel_count, 0.0);
+                }
+
+                if (max_angles.size() == 1) {
+                    max_angles_.insert(max_angles_.end(), channel_count, max_angles[0]);
+                } else if (max_angles.size() >= channel_count) {
+                    max_angles_.insert(max_angles_.end(), max_angles.begin(), max_angles.begin() + channel_count);
+                } else {
+                    max_angles_.insert(max_angles_.end(), channel_count, 180.0);
+                }
+            }
+        }
+    }
+
+    // 解析眼睛配置
+    if (config_.has_eye()) {
+        const auto& eye = config_.eye();
+        std::string serial_str = eye.serial();
+        std::string offset_str = eye.offest();
+        std::string min_angles_str = eye.jlmtlow();
+        std::string max_angles_str = eye.jlmtup();
+
+        // 解析偏移角
+        std::vector<double> offsets;
+        std::istringstream oss(offset_str);
+        double val;
+        while (oss >> val) offsets.push_back(val);
+
+        // 解析角度限制
+        std::vector<double> min_angles;
+        if (!min_angles_str.empty()) {
+            std::istringstream min_ss(min_angles_str);
+            while (min_ss >> val) min_angles.push_back(val);
+        }
+
+        std::vector<double> max_angles;
+        if (!max_angles_str.empty()) {
+            std::istringstream max_ss(max_angles_str);
+            while (max_ss >> val) max_angles.push_back(val);
+        }
+
+        // 解析 channel，如 64:4~9
+        size_t colon = serial_str.find(':');
+        if (colon != std::string::npos) {
+            uint8_t addr = std::stoi(serial_str.substr(0, colon));
+            std::string range = serial_str.substr(colon + 1);
+            size_t tilde = range.find('~');
+            if (tilde != std::string::npos) {
+                int ch_start = std::stoi(range.substr(0, tilde));
+                int ch_end = std::stoi(range.substr(tilde + 1));
+                int channel_count = ch_end - ch_start + 1;
+
+                // 添加舵机通道
+                for (int ch = ch_start; ch <= ch_end; ++ch) {
+                    channels_.push_back({addr, static_cast<uint8_t>(ch)});
+                }
+
+                // 添加角度限制
+                if (min_angles.size() == 1) {
+                    min_angles_.insert(min_angles_.end(), channel_count, min_angles[0]);
+                } else if (min_angles.size() >= channel_count) {
+                    min_angles_.insert(min_angles_.end(), min_angles.begin(), min_angles.begin() + channel_count);
+                } else {
+                    min_angles_.insert(min_angles_.end(), channel_count, 0.0);
+                }
+
+                if (max_angles.size() == 1) {
+                    max_angles_.insert(max_angles_.end(), channel_count, max_angles[0]);
+                } else if (max_angles.size() >= channel_count) {
+                    max_angles_.insert(max_angles_.end(), max_angles.begin(), max_angles.begin() + channel_count);
+                } else {
+                    max_angles_.insert(max_angles_.end(), channel_count, 180.0);
+                }
+            }
+        }
+    }
+
+    // 解析嘴巴配置
+    if (config_.has_mouth()) {
+        const auto& mouth = config_.mouth();
+        std::string serial_str = mouth.serial();
+        std::string offset_str = mouth.offest();
+        std::string min_angles_str = mouth.jlmtlow();
+        std::string max_angles_str = mouth.jlmtup();
+
+        // 解析偏移角
+        std::vector<double> offsets;
+        std::istringstream oss(offset_str);
+        double val;
+        while (oss >> val) offsets.push_back(val);
+
+        // 解析角度限制
+        std::vector<double> min_angles;
+        if (!min_angles_str.empty()) {
+            std::istringstream min_ss(min_angles_str);
+            while (min_ss >> val) min_angles.push_back(val);
+        }
+
+        std::vector<double> max_angles;
+        if (!max_angles_str.empty()) {
+            std::istringstream max_ss(max_angles_str);
+            while (max_ss >> val) max_angles.push_back(val);
+        }
+
+        // 解析 channel，如 65:0~9
+        size_t colon = serial_str.find(':');
+        if (colon != std::string::npos) {
+            uint8_t addr = std::stoi(serial_str.substr(0, colon));
+            std::string range = serial_str.substr(colon + 1);
+            size_t tilde = range.find('~');
+            if (tilde != std::string::npos) {
+                int ch_start = std::stoi(range.substr(0, tilde));
+                int ch_end = std::stoi(range.substr(tilde + 1));
+                int channel_count = ch_end - ch_start + 1;
+
+                // 添加舵机通道
+                for (int ch = ch_start; ch <= ch_end; ++ch) {
+                    channels_.push_back({addr, static_cast<uint8_t>(ch)});
+                }
+
+                // 添加角度限制
+                if (min_angles.size() == 1) {
+                    min_angles_.insert(min_angles_.end(), channel_count, min_angles[0]);
+                } else if (min_angles.size() >= channel_count) {
+                    min_angles_.insert(min_angles_.end(), min_angles.begin(), min_angles.begin() + channel_count);
+                } else {
+                    min_angles_.insert(min_angles_.end(), channel_count, 0.0);
+                }
+
+                if (max_angles.size() == 1) {
+                    max_angles_.insert(max_angles_.end(), channel_count, max_angles[0]);
+                } else if (max_angles.size() >= channel_count) {
+                    max_angles_.insert(max_angles_.end(), max_angles.begin(), max_angles.begin() + channel_count);
+                } else {
+                    max_angles_.insert(max_angles_.end(), channel_count, 180.0);
+                }
+            }
+        }
+    }
+
+    // 确保所有向量大小一致
+    if (min_angles_.size() != channels_.size()) {
+        min_angles_.resize(channels_.size(), 0.0);
+    }
+
+    if (max_angles_.size() != channels_.size()) {
+        max_angles_.resize(channels_.size(), 180.0);
+    }
+
+    current_joints_.resize(channels_.size(), 90.0);
+    last_joints_ = current_joints_;
+}
 
 
 void BioHeadRobot::parseXmlConfig(const XmlNode &node) {

cmvr-es/devices/canbus/abstract_canbus.h

@@ -54,6 +54,7 @@ namespace cmvr::device {
 
     class AbstractCanbus : public AbstractDevice {
     public:
+        AbstractCanbus() = default;
         AbstractCanbus(const XmlNode &cfg) : AbstractDevice(cfg) {}
         ~AbstractCanbus() {}
         /**

cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc

@@ -35,6 +35,17 @@ namespace cmvr {
             //   LOG(ERROR) << "[SocketCanClientRaw] ([SocketCanClientRaw]): Failed to parse XML: " << e.what();
         }
 
+        SocketCanClientRaw::SocketCanClientRaw(const cmvr::config::CanDeviceConfig& config):config_(config)
+        {
+            auto channel_id = config_.channelid();
+            port_ = static_cast<CANCardParameter::CANChannelId>(channel_id);
+            interface_ = CANCardParameter::NATIVE;
+
+            enable_can_err_check_ = false;
+
+
+        }
+
 
         void SocketCanClientRaw::init() {
             // if (!parameter.has_channel_id()) {

cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h

@@ -26,6 +26,7 @@
 #include "gflags/gflags.h"
 #include "../../abstract_canbus.h"
 #include  "canbus/common/canbus_consts.h"
+#include "cmvr/config/robot_config/robot_config.pb.h"
 
 namespace cmvr {
     namespace device {
@@ -36,6 +37,7 @@ namespace cmvr {
         class SocketCanClientRaw final : public AbstractCanbus {
         public:
             explicit SocketCanClientRaw(const XmlNode &cfg);
+            SocketCanClientRaw(const cmvr::config::CanDeviceConfig& config);
             ~SocketCanClientRaw();
             void init() override;
             void start() override;
@@ -73,6 +75,8 @@ namespace cmvr {
 
             //
             bool enable_can_err_check_{false};
+
+            cmvr::config::CanDeviceConfig config_;
         };
     }
 }

cmvr-es/devices/motor/CMakeLists.txt

@@ -1,5 +1,5 @@
 add_subdirectory(ti5_motor)
-
+add_subdirectory(eyou_motor)
 # --------------------------------------------------------
 # Unit test
 # --------------------------------------------------------

cmvr-es/devices/motor/abstract_motor.h

@@ -41,6 +41,7 @@ namespace cmvr::device{
         } JointStatus;
 
     public:
+        AbstractMotor() = default;
         explicit AbstractMotor(const XmlNode &config): AbstractDevice(config) {}
         AbstractMotor(const XmlNode &config,uint8_t node_id): AbstractDevice(config) ,node_id_(node_id) {}
         ~AbstractMotor() override = default;

cmvr-es/devices/motor/eyou_motor/CMakeLists.txt

@@ -0,0 +1,21 @@
+add_library(eyoumotor SHARED
+        ${CMAKE_CURRENT_SOURCE_DIR}/ethercat/protocol/ec_xml_parser.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/ethercat/protocol/ec_master.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/ethercat/eyou_motor_ethercat_protocol.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/eyou_motor.cpp
+
+)
+
+
+target_include_directories(eyoumotor PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+add_library(cmvr_es::device::eyoumotor ALIAS eyoumotor)
+
+target_link_libraries(eyoumotor
+        PRIVATE
+        -lsoem
+        protobuf
+        glog
+)
+
+install(TARGETS eyoumotor LIBRARY DESTINATION lib)

cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.cpp

@@ -0,0 +1,747 @@
+//
+// Created by cmvr on 2026/3/30.
+//
+
+#include "eyou_motor_ethercat_protocol.h"
+#include "canbus/canopen/register.h"
+#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.h"
+#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.h"
+#include "motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.h"
+#include "canbus/canopen/nmt_response_protocol.h"
+
+#include <cmath>
+#include <thread>
+#include <chrono>
+
+using namespace cmvr::device;
+
+// 辅助函数：获取操作模式字符串
+static const char* getOpModeString(int8_t mode) {
+    switch (mode) {
+        case 1: return "Profile Position (PP)";
+        case 2: return "Velocity (VL)";
+        case 3: return "Profile Velocity (PV)";
+        case 4: return "Profile Torque (PT)";
+        case 6: return "Homing (HM)";
+        case 8: return "Cyclic Sync Position (CSP)";
+        case 9: return "Cyclic Sync Velocity (CSV)";
+        case 10: return "Cyclic Sync Torque (CST)";
+        default: return "Unknown";
+    }
+}
+
+// 构造函数
+EYouEthercatProtocol::EYouEthercatProtocol(std::string ifName, const std::string& xmlPath)
+    : interface_name_(ifName),
+      xml_config_path_(xmlPath),
+      initialized_(false),
+      slave_count_(0),
+      use_fixed_pdo_(xmlPath.empty()) {
+
+    comm_proto = CommProto::ETHERCAT;
+
+    try {
+        // 初始化message_manager
+        message_manager_ = std::make_shared<device::MessageManager<msgs::RobotDetail>>();
+
+        // 创建 EcMaster 实例
+        master_ = std::make_unique<EcMaster>();
+
+        // 初始化主站
+        int cycleTimeUs = 1000;  // 1ms周期
+        int slaveCount = master_->init(interface_name_.c_str(), cycleTimeUs);
+
+        if (slaveCount < 0) {
+            throw std::runtime_error("[EYouEthercatProtocol] master init failed");
+        }
+
+        slave_count_ = slaveCount;
+
+        EcSlaveConfig config = createFixedSlaveConfig(1);
+        std::vector<uint16_t> rxPdos = config.availableRxPdos;
+        std::vector<uint16_t> txPdos = config.availableTxPdos;
+
+        if (rxPdos.empty() || txPdos.empty()) {
+           LOG(ERROR) << "[EYouEthercatProtocol] No available PDOs found\n";
+            throw std::runtime_error("[EYouEthercatProtocol] No available PDOs found ");
+        }
+        const EcPdo* rxPdo = &config.modules[0].rxPdos[0];
+        const EcPdo* txPdo = &config.modules[0].txPdos[0];
+        for (int slaveId = 1; slaveId <= slave_count_; slaveId++) {
+            if (master_->configurePdo(slaveId, rxPdo, txPdo) != 0) {
+                throw std::runtime_error("[EYouEthercatProtocol] PDO configuration failed for slave " + std::to_string(slaveId));
+            }
+        }
+        master_->setupMappingAndDc();
+
+        // 等待配置生效
+        std::this_thread::sleep_for(std::chrono::milliseconds(500));
+        // 进入OP状态
+        if (master_->gotoOperational() != 0) {
+            throw std::runtime_error("[EYouEthercatProtocol] OPERATIONAL state failed");
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(500));
+        if (!master_->isOperational()) {
+            LOG(WARNING) << "[EYouEthercatProtocol] Master reports not operational";
+        } else {
+            LOG(INFO) << "[EYouEthercatProtocol] EtherCAT in OPERATIONAL state";
+        }
+
+        // 初始化从站状态
+        for (int i = 1; i <= slave_count_; i++) {
+            node_states_[i] = NodeState();
+            uint16_t status = master_->getStatusWord(i);
+            LOG(INFO) << "[EYouEthercatProtocol] Slave " << i << " initial status word: 0x" << std::hex << status << std::dec;
+        }
+
+        initialized_ = true;
+        LOG(INFO) << "[EYouEthercatProtocol] Initialized successfully";
+
+        // 添加NMT响应协议
+        for (int i = 1; i <= slave_count_; i++) {
+            message_manager_->AddRecvProtocolData<NmtResponseProtocol<cmvr::msgs::RobotDetail>, false>(i);
+        }
+
+    } catch (const std::exception& e) {
+        LOG(ERROR) << "[EYouEthercatProtocol] Constructor failed: " << e.what();
+        throw std::runtime_error(e.what());
+    }
+}
+
+// 析构函数
+EYouEthercatProtocol::~EYouEthercatProtocol() {
+    if (master_) {
+        // 失能所有电机
+        for (auto& node : node_states_) {
+            if (node.second.enabled) {
+                master_->disable(node.first);
+                LOG(INFO) << "[EYouEthercatProtocol] Disabled motor " << node.first;
+            }
+        }
+
+        master_->shutdown();
+        LOG(INFO) << "[EYouEthercatProtocol] Master shutdown successfully";
+    }
+}
+
+// 执行电机使能序列
+bool EYouEthercatProtocol::performEnableSequence(uint8_t node_id) {
+    if (node_id > slave_count_) {
+        LOG(ERROR) << "[EYouEthercatProtocol] Node " << node_id << " out of range";
+        return false;
+    }
+    // 读取状态字
+    uint16_t status = master_->getStatusWord(node_id);
+    LOG(INFO) << "[EYouEthercatProtocol] Node " << node_id << " status: 0x" << std::hex << status << std::dec;
+
+    // 如果处于故障状态，先复位
+    if (status & 0x0008) {
+        LOG(INFO) << "[EYouEthercatProtocol] Fault detected, resetting...";
+        master_->faultReset(node_id);
+        std::this_thread::sleep_for(std::chrono::milliseconds(200));
+        status = master_->getStatusWord(node_id);
+    }
+
+    // 如果处于 Switch On Disabled 状态，执行使能序列
+    if (status & 0x0040) {
+        LOG(INFO) << "[EYouEthercatProtocol] Node " << (int)node_id << " in Switch On Disabled, enabling...";
+
+        // 获取当前位置并同步目标位置（防止抖动）
+        int32_t actualPos = master_->getActualPosition(node_id);
+        master_->setTargetPosition(node_id, actualPos);
+        master_->setModeOfOperation(node_id,EcMaster::EcOperationMode_ProfilePosition);
+        // Shutdown
+        master_->setControlWord(node_id, 0x06);
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+        // Switch On
+        master_->setControlWord(node_id, 0x07);
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+        // Enable Operation
+        master_->setControlWord(node_id, 0x0F);
+        std::this_thread::sleep_for(std::chrono::milliseconds(200));
+
+        status = master_->getStatusWord(node_id);
+    }
+
+    // 验证使能成功
+    bool enabled = (status & 0x0004) != 0;
+    if (enabled) {
+        LOG(INFO) << "[EYouEthercatProtocol] Node " << node_id << " enabled successfully";
+    } else {
+        LOG(ERROR) << "[EYouEthercatProtocol] Node " << node_id << " enable failed, status: 0x"
+                   << std::hex << status << std::dec;
+    }
+
+    return enabled;
+}
+
+// 操作模式转换 (msgs::RunMode -> EcOperationMode)
+int8_t EYouEthercatProtocol::runModeToEcMode(msgs::RunMode mode) {
+    switch (mode) {
+        case msgs::RUN_MODE_PROFILE_POSITION:
+            return EcMaster::EcOperationMode_ProfilePosition;      // 1
+
+        case msgs::RUN_MODE_PROFILE_VELOCITY:
+            return EcMaster::EcOperationMode_ProfileVelocity;      // 3
+
+        case msgs::RUN_MODE_TORQUE:   // 需要确认是否有这个枚举
+            return EcMaster::EcOperationMode_ProfileTorque;        // 4
+
+        case msgs::RUN_MODE_INTERPOLATED_POSITION:
+            return EcMaster::EcOperationMode_InterpolatedPosition; // 7
+
+        case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
+            return EcMaster::EcOperationMode_CyclicSyncPosition;   // 8
+
+        case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY:
+            return EcMaster::EcOperationMode_CyclicSyncVelocity;   // 9
+
+        case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT:   // 名称统一
+            return EcMaster::EcOperationMode_CyclicSyncTorque;     // 10
+
+        default:
+            return 0;
+    }
+}
+
+// 操作模式转换 (EcOperationMode -> msgs::RunMode)
+cmvr::msgs::RunMode EYouEthercatProtocol::ecModeToRunMode(int8_t mode) {
+    switch (mode) {
+        case EcMaster::EcOperationMode_ProfilePosition:
+            return msgs::RUN_MODE_PROFILE_POSITION;
+
+        case EcMaster::EcOperationMode_ProfileVelocity:
+            return msgs::RUN_MODE_PROFILE_VELOCITY;
+
+        case EcMaster::EcOperationMode_ProfileTorque:
+            return msgs::RUN_MODE_TORQUE;  // 需要确认枚举存在
+
+        case EcMaster::EcOperationMode_InterpolatedPosition:
+            return msgs::RUN_MODE_INTERPOLATED_POSITION;
+
+        case EcMaster::EcOperationMode_CyclicSyncPosition:
+            return msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
+
+        case EcMaster::EcOperationMode_CyclicSyncVelocity:
+            return msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY;
+
+        case EcMaster::EcOperationMode_CyclicSyncTorque:
+            return msgs::RUN_MODE_CYCLIC_SYNC_CURRENT;
+
+        default:
+            return msgs::RUN_MODE_UNSPECIFIED;
+    }
+}
+
+// 初始化节点
+bool EYouEthercatProtocol::initNode(uint8_t node_id) {
+    try {
+        if (node_id > slave_count_) {
+            LOG(ERROR) << "[EYouEthercatProtocol] Node " << node_id << " out of range (max: " << slave_count_ << ")";
+            return false;
+        }
+
+        // 执行使能序列
+        if (!performEnableSequence(node_id)) {
+            LOG(ERROR) << "[EYouEthercatProtocol] Failed to enable node " << node_id;
+            return false;
+        }
+
+        node_states_[node_id].initialized = true;
+        node_states_[node_id].enabled = true;
+        node_states_[node_id].current_mode = msgs::RUN_MODE_PROFILE_POSITION;
+
+        LOG(INFO) << "[EYouEthercatProtocol] Node " << node_id << " initialized successfully";
+        return true;
+
+    } catch (const std::exception& e) {
+        LOG(ERROR) << "[EYouEthercatProtocol] Failed to init node " << node_id << ": " << e.what();
+        return false;
+    }
+}
+
+// 设置运行模式
+void EYouEthercatProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
+    
+
+    int8_t ec_mode = runModeToEcMode(mode);
+    master_->setModeOfOperation(node_id, ec_mode);
+
+    // 验证模式设置
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+    int8_t actual_mode = master_->getModeOfOperationDisplay(node_id);
+
+    if (actual_mode == ec_mode) {
+        node_states_[node_id].current_mode = mode;
+        LOG(INFO) << "[EYouEthercatProtocol] Node " << (int)node_id << " mode set to "
+                  << getOpModeString(ec_mode);
+    } else {
+        LOG(WARNING) << "[EYouEthercatProtocol] Node " << node_id << " mode set failed, "
+                     << "expected " << getOpModeString(ec_mode)
+                     << ", got " << getOpModeString(actual_mode);
+    }
+}
+
+// 设置目标位置和速度
+void EYouEthercatProtocol::setTarget(uint8_t node_id, double angle_rad, double vel) {
+    auto pos_cmd = angleToPos(angle_rad);
+    auto speed = static_cast<uint32_t>(std::abs(velToSpeed(vel)));
+
+    msgs::RunMode current_mode = getMode(node_id);
+
+    switch (current_mode) {
+    case msgs::RUN_MODE_PROFILE_POSITION: {
+            // 先设置轮廓速度
+            master_->setProfileVelocity(node_id, speed);
+            // 设置目标位置
+            master_->setTargetPosition(node_id, pos_cmd);
+            node_states_[node_id].target_position = pos_cmd;
+
+            // 构建控制字：使能状态 + 立即更新标志
+            uint16_t ctlword = 0x0F | 0x20;  // 0x0F使能, 0x20立即更新
+
+            // 触发位置更新
+            master_->setControlWord(node_id, ctlword);
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+
+            // 启动新位置命令
+            master_->setControlWord(node_id, ctlword | 0x10);
+            break;
+    }
+    case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
+        master_->setTargetPosition(node_id, pos_cmd);
+        node_states_[node_id].target_position = pos_cmd;
+        break;
+    default:
+        LOG(WARNING) << "[EYouEthercatProtocol] setTarget(pos,vel) called in unsupported mode for node " << node_id;
+        break;
+    }
+}
+
+// 设置目标速度
+void EYouEthercatProtocol::setTarget(uint8_t node_id, double vel) {
+    auto speed = static_cast<int32_t>(velToSpeed(vel));
+
+    
+
+    msgs::RunMode current_mode = getMode(node_id);
+
+    switch (current_mode) {
+        case msgs::RUN_MODE_VELOCITY:
+        case msgs::RUN_MODE_PROFILE_VELOCITY:
+        case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY:
+            master_->setTargetVelocity(node_id, speed);
+            break;
+        default:
+            LOG(WARNING) << "[EYouEthercatProtocol] setTarget(vel) called in unsupported mode for node " << node_id;
+            break;
+    }
+}
+
+// 设置目标位置
+void EYouEthercatProtocol::setQ(uint8_t node_id, double angle_rad) {
+    auto pos_cmd = angleToPos(angle_rad);
+
+    
+
+    msgs::RunMode current_mode = getMode(node_id);
+
+    switch (current_mode) {
+        case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
+            {
+                master_->setTargetPosition(node_id, pos_cmd);
+                node_states_[node_id].target_position = pos_cmd;
+                break;
+            }
+        case msgs::RUN_MODE_PROFILE_POSITION:
+            {
+                // 1. 设置目标位置
+                master_->setTargetPosition(node_id, pos_cmd);
+                node_states_[node_id].target_position = pos_cmd;
+
+                // 2. 构建控制字：使能状态 + 立即更新标志
+                uint16_t ctlword = 0x0F | 0x20;  // 0x0F使能, 0x20立即更新
+
+                // 3. 触发位置更新
+                master_->setControlWord(node_id, ctlword);
+                std::this_thread::sleep_for(std::chrono::milliseconds(10));
+
+                // 4. 启动新位置命令
+                master_->setControlWord(node_id, ctlword | 0x10);
+                break;
+            }
+        default:
+            {
+                LOG(WARNING) << "[EYouEthercatProtocol] setQ called in unsupported mode for node " << node_id;
+                break;
+            }
+    }
+}
+
+// 设置位置限制
+void EYouEthercatProtocol::setLimitQ(uint8_t node_id, double ub, double lb) {
+    
+
+    int32_t lb_val = angleToPos(lb);
+    int32_t ub_val = angleToPos(ub);
+
+    // 开启软件位置限制 (0x4C494D54 = "LIMT")
+    uint32_t limitEnable = 0x4C494D54;
+    if (master_->writeSdo(node_id, 0x2003, 0x00, &limitEnable, EcDataType::Uint32, 100) != 0) {
+        LOG(ERROR) << "[EYouEthercatProtocol] Failed to enable software position limit for node " << node_id;
+    }
+
+    // 设置软件位置限制
+    if (master_->writeSdo(node_id, 0x607D, 0x01, &lb_val, EcDataType::Int32, 100) != 0) {
+        LOG(ERROR) << "[EYouEthercatProtocol] Failed to set lower position limit for node " << node_id;
+    }
+
+    if (master_->writeSdo(node_id, 0x607D, 0x02, &ub_val, EcDataType::Int32, 100) != 0) {
+        LOG(ERROR) << "[EYouEthercatProtocol] Failed to set upper position limit for node " << node_id;
+    }
+
+    LOG(INFO) << "[EYouEthercatProtocol] Position limits set for node " << node_id
+              << ": [" << lb << ", " << ub << "] rad";
+}
+
+// 设置速度限制
+void EYouEthercatProtocol::setLimitQd(uint8_t node_id, double qd) {
+    
+
+    auto speed = static_cast<uint32_t>(std::abs(velToSpeed(qd)));
+
+    // 设置最大速度限制
+    if (master_->writeSdo(node_id, 0x607F, 0x00, &speed, EcDataType::Uint32, 100) != 0) {
+        LOG(ERROR) << "[EYouEthercatProtocol] Failed to set max speed limit for node " << node_id;
+    }
+
+    if (master_->writeSdo(node_id, 0x6081, 0x00, &speed, EcDataType::Uint32, 100) != 0) {
+        LOG(ERROR) << "[EYouEthercatProtocol] Failed to set profile speed for node " << node_id;
+    }
+
+    LOG(INFO) << "[EYouEthercatProtocol] Speed limit set for node " << node_id << ": " << qd << " rad/s";
+}
+
+// 设置加速度限制
+void EYouEthercatProtocol::setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd) {
+    
+
+    // u_qdd: rad/s² (最大加速度)
+    // l_qdd: rad/s² (最大减速度)
+    // 转换为电机加速度（脉冲/秒²）
+    // 圈数/秒² = 弧度/秒² / 2π
+    // 脉冲/秒² = 圈数/秒² * GearRatio * PULSE_PER_REV
+    auto accel = static_cast<uint32_t>((std::abs(u_qdd) / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV);
+    auto decel = static_cast<uint32_t>((std::abs(l_qdd) / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV);
+
+    // 通过SDO写入加速度和减速度限制
+    if (master_->writeSdo(node_id, 0x6083, 0x00, &accel, EcDataType::Uint32, 100) != 0) {
+        LOG(ERROR) << "[EYouEthercatProtocol] Failed to set acceleration limit for node " << node_id;
+    }
+
+    if (master_->writeSdo(node_id, 0x6084, 0x00, &decel, EcDataType::Uint32, 100) != 0) {
+        LOG(ERROR) << "[EYouEthercatProtocol] Failed to set deceleration limit for node " << node_id;
+    }
+
+    LOG(INFO) << "[EYouEthercatProtocol] Acceleration limits set for node " << node_id
+              << ": accel=" << u_qdd << ", decel=" << l_qdd << " rad/s²";
+}
+
+// 零位校准
+bool EYouEthercatProtocol::calibrateZeroQ(uint8_t node_id) {
+    
+
+    try {
+        int32_t pos;
+        int32_t offset = 0;
+        int32_t rev = 0;
+
+        // 读取当前位置
+        if (master_->readSdo(node_id, 0x6064, 0x00, &pos, EcDataType::Int32, 20000) != 0) {
+            LOG(ERROR) << "[EYouEthercatProtocol] read actual position failed";
+            return false;
+        }
+        LOG(INFO) << "[EYouEthercatProtocol] Actual position: " << pos;
+
+        // 读取home offset
+        if (master_->readSdo(node_id, 0x607C, 0x00, &offset, EcDataType::Int32, 20000) != 0) {
+            LOG(ERROR) << "[EYouEthercatProtocol] read home offset failed";
+            return false;
+        }
+        LOG(INFO) << "[EYouEthercatProtocol] Home offset before: " << offset;
+
+        // 先将home offset设为0
+        offset = 0;
+        if (master_->writeSdo(node_id, 0x607C, 0x00, &offset, EcDataType::Int32, 20000) != 0) {
+            LOG(ERROR) << "[EYouEthercatProtocol] write home offset 0 failed";
+            return false;
+        }
+        LOG(INFO) << "[EYouEthercatProtocol] Home offset set to 0";
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(500));
+
+        // 读取实际位置
+        if (master_->readSdo(node_id, 0x6064, 0x00, &pos, EcDataType::Int32, 20000) != 0) {
+            LOG(ERROR) << "[EYouEthercatProtocol] read actual position after offset reset failed";
+            return false;
+        }
+        LOG(INFO) << "[EYouEthercatProtocol] Actual position after offset reset: " << pos;
+
+        // 获取轴转数，超圈处理
+        if (master_->readSdo(node_id, 0x6091, 0x02, &rev, EcDataType::Int32, 20000) != 0) {
+            LOG(ERROR) << "[EYouEthercatProtocol] read gear ratio shaft revolution failed";
+            return false;
+        }
+        LOG(INFO) << "[EYouEthercatProtocol] Gear ratio shaft revolution: " << rev;
+
+        // 处理超出编码器范围的情况
+        long long r = rev;
+        int temp = static_cast<int>(r / 2);
+        if (pos > temp || pos < -temp) {
+            pos = pos % static_cast<int>(r);
+            if (pos > temp || pos < -temp) {
+                if (pos > temp) {
+                    pos = pos - static_cast<int>(r);
+                } else if (pos < -temp) {
+                    pos = pos + static_cast<int>(r);
+                }
+            }
+            LOG(WARNING) << "[EYouEthercatProtocol] Position out of encoder range, adjusted to " << pos;
+        }
+
+        // 写home offset偏移
+        offset = -pos;
+        if (master_->writeSdo(node_id, 0x607C, 0x00, &offset, EcDataType::Int32, 20000) != 0) {
+            LOG(ERROR) << "[EYouEthercatProtocol] write home offset failed";
+            return false;
+        }
+        LOG(INFO) << "[EYouEthercatProtocol] Home offset set to: " << offset;
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(500));
+
+        // 读取实际位置
+        if (master_->readSdo(node_id, 0x6064, 0x00, &pos, EcDataType::Int32, 20000) != 0) {
+            LOG(ERROR) << "[EYouEthercatProtocol] read actual position after calibration failed";
+            return false;
+        }
+        LOG(INFO) << "[EYouEthercatProtocol] Actual position after calibration: " << pos;
+
+        // 保存参数
+        uint32_t saveValue = 0x65766173;  // "evas"
+        if (master_->writeSdo(node_id, 0x1010, 0x01, &saveValue, EcDataType::Uint32, 20000) != 0) {
+            LOG(ERROR) << "[EYouEthercatProtocol] save parameters failed";
+            return false;
+        }
+        LOG(INFO) << "[EYouEthercatProtocol] Parameters saved";
+
+        LOG(INFO) << "[EYouEthercatProtocol] Motor " << node_id << " calibrated successfully";
+        return true;
+
+    } catch (const std::exception& e) {
+        LOG(ERROR) << "[EYouEthercatProtocol] Calibration failed for node " << node_id << ": " << e.what();
+        return false;
+    }
+}
+
+// 刹车（快速停机）
+void EYouEthercatProtocol::brake(uint8_t node_id) {
+    
+
+    // 发送快速停机命令
+    uint16_t ctlword = 0x0002;  // Quick Stop
+    master_->setControlWord(node_id, ctlword);
+
+    LOG(INFO) << "[EYouEthercatProtocol] Brake (quick stop) applied to motor " << node_id;
+}
+
+// 检查是否到达目标位置
+bool EYouEthercatProtocol::reachedTargetQ(uint8_t node_id) {
+    uint16_t status_word = getStatusWord(node_id);
+    // 检查状态字的bit 10（目标到达位）
+    return (status_word & 0x0400) != 0;
+}
+
+// 获取当前位置
+double EYouEthercatProtocol::getQ(uint8_t node_id) {
+    
+    int32_t position = master_->getActualPosition(node_id);
+    return posToAngle(position);
+}
+
+// 获取当前速度
+double EYouEthercatProtocol::getQd(uint8_t node_id) {
+    
+    int32_t velocity = master_->getActualVelocity(node_id);
+    return speedToVel(velocity);
+}
+
+// 设置速度
+void EYouEthercatProtocol::setQd(uint8_t node_id, double qd) {
+    auto speed = static_cast<uint32_t>(std::abs(velToSpeed(qd)));
+
+    
+
+    msgs::RunMode current_mode = getMode(node_id);
+
+    switch (current_mode) {
+        case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
+        case msgs::RUN_MODE_PROFILE_POSITION:
+            master_->setProfileVelocity(node_id, speed);
+            last_Qd_[node_id] = speed;
+            break;
+        case msgs::RUN_MODE_PROFILE_VELOCITY:
+        case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY:
+            master_->setTargetVelocity(node_id, static_cast<int32_t>(speed));
+            break;
+        default:
+            LOG(WARNING) << "[EYouEthercatProtocol] setQd called in unsupported mode for node " << node_id;
+            break;
+    }
+}
+
+// 设置加速度
+void EYouEthercatProtocol::setQdd(uint8_t node_id, double qdd) {
+    // qdd: rad/s² (加速度)
+    // 转换为电机加速度（脉冲/秒²）
+    uint32_t accel = static_cast<uint32_t>((std::abs(qdd) / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV);
+
+    if (last_Qdd_[node_id] != accel) {
+        
+        master_->setProfileAcceleration(node_id, accel);
+        master_->setProfileDeceleration(node_id, accel);
+        last_Qdd_[node_id] = accel;
+    }
+}
+
+// 力矩关闭
+void EYouEthercatProtocol::torqueOff(uint8_t node_id) {
+    
+    auto ret = master_->disable(node_id);
+    node_states_[node_id].enabled = false;
+    LOG(INFO) << "[EYouEthercatProtocol] Torque off for motor " << node_id << ", ret = " << ret;
+}
+
+// 获取运行模式
+cmvr::msgs::RunMode EYouEthercatProtocol::getMode(uint8_t node_id) {
+    
+    int8_t mode = master_->getModeOfOperationDisplay(node_id);
+    return ecModeToRunMode(mode);
+}
+
+// 配置轮廓参数
+void EYouEthercatProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) {
+    
+    master_->setProfileVelocity(node_id, speed);
+    master_->setProfileAcceleration(node_id, accel);
+    master_->setProfileDeceleration(node_id, decel);
+    LOG(INFO) << "[EYouEthercatProtocol] Profile configured for node " << node_id
+              << ": speed=" << speed << ", accel=" << accel << ", decel=" << decel;
+}
+
+// 获取力矩
+double EYouEthercatProtocol::getTorque(uint8_t node_id) {
+    
+    int16_t torque = master_->getActualTorque(node_id);
+    // 返回实际力矩值（取决于具体驱动器，这里假设是额定力矩的倍数）
+    return torque / 1000.0;
+}
+
+// 获取状态字
+uint16_t EYouEthercatProtocol::getStatusWord(uint8_t node_id) {
+    
+    uint16_t status_word = master_->getStatusWord(node_id);
+    node_states_[node_id].status_word = status_word;
+    return status_word;
+}
+
+// 检查电机是否使能
+bool EYouEthercatProtocol::isEnabled(uint8_t node_id) {
+    uint16_t status_word = getStatusWord(node_id);
+    // 检查状态字的bit 2（操作使能）和bit 3（故障）
+    return (status_word & 0x000C) == 0x0008;
+}
+
+// 检查是否有故障
+bool EYouEthercatProtocol::isFault(uint8_t node_id) {
+    uint16_t status_word = getStatusWord(node_id);
+    return (status_word & 0x0008) != 0;
+}
+
+// 故障复位
+void EYouEthercatProtocol::faultReset(uint8_t node_id) {
+    if (isFault(node_id)) {
+        
+        if (master_->faultReset(node_id) == 0) {
+            LOG(INFO) << "[EYouEthercatProtocol] Fault reset for motor " << node_id;
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        } else {
+            LOG(ERROR) << "[EYouEthercatProtocol] Failed to reset fault for motor " << node_id;
+        }
+    }
+}
+
+// 等待目标到达
+bool EYouEthercatProtocol::waitForTargetReached(uint8_t node_id, int timeout_ms) {
+    return waitUntil([this, node_id]() { return reachedTargetQ(node_id); }, timeout_ms);
+}
+
+// 获取RobotDetail数据
+std::unique_ptr<cmvr::msgs::RobotDetail> EYouEthercatProtocol::GetRobotDetail() {
+    if (message_manager_) {
+        auto data_ptr = std::make_unique<msgs::RobotDetail>();
+        message_manager_->GetSensorData(data_ptr.get());
+        return data_ptr;
+    }
+    return std::make_unique<msgs::RobotDetail>();
+}
+
+// 辅助函数实现（弧度制版本）
+double EYouEthercatProtocol::posToAngle(int32_t pos) const {
+    // 脉冲数 -> 弧度
+    // 圈数 = 脉冲数 / (GearRatio * PULSE_PER_REV)
+    // 弧度 = 圈数 * 2π
+    return (pos * 2.0 * M_PI) / (GearRatio * PULSE_PER_REV);
+}
+
+int32_t EYouEthercatProtocol::angleToPos(double angle) const {
+    // 弧度 -> 脉冲数
+    // 圈数 = 弧度 / 2π
+    // 脉冲数 = 圈数 * GearRatio * PULSE_PER_REV
+    return static_cast<int32_t>((angle / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV);
+}
+
+double EYouEthercatProtocol::velToSpeed(double vel) const {
+    // vel: rad/s (角速度)
+    // 输出：电机速度（脉冲/秒）
+    // 圈数/秒 = vel / 2π
+    // 脉冲/秒 = 圈数/秒 * GearRatio * PULSE_PER_REV
+    return (vel / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV;
+}
+
+double EYouEthercatProtocol::speedToVel(int32_t speed) const {
+    // speed: 电机速度（脉冲/秒）
+    // 输出：rad/s
+    // 圈数/秒 = speed / (GearRatio * PULSE_PER_REV)
+    // 弧度/秒 = 圈数/秒 * 2π
+    return (speed * 2.0 * M_PI) / (GearRatio * PULSE_PER_REV);
+}
+
+bool EYouEthercatProtocol::waitUntil(std::function<bool()> condition, int timeout_ms) {
+    auto start = std::chrono::steady_clock::now();
+    while (!condition()) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        if (std::chrono::steady_clock::now() - start > std::chrono::milliseconds(timeout_ms)) {
+            return false;
+        }
+    }
+    return true;
+}
+
+void EYouEthercatProtocol::updateNodeState(uint8_t node_id) {
+    
+    // 更新状态字
+    node_states_[node_id].status_word = master_->getStatusWord(node_id);
+    // 可以根据需要更新其他状态信息
+}

cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h

@@ -0,0 +1,125 @@
+//
+// Created by cmvr on 2026/3/30.
+//
+
+#pragma once
+#include "cmvr/msgs/motor.pb.h"
+#include "motor/motor_protocol_interface.h"
+#include "../../../canbus/abstract_canbus.h"
+#include "canbus/can_comm/can_receiver.h"
+#include "canbus/can_comm/can_sender.h"
+#include "canbus/can_comm/message_manager.h"
+#include "cmvr/msgs/error_code.pb.h"
+#include "cmvr/msgs/robot_detail.pb.h"
+
+// 替换原有的 eu_ethercat 头文件
+#include "protocol/ec_master.h"
+#include "protocol/ec_xml_parser.h"
+#include "protocol/ec_pdo_config.h"
+
+#include <cmath>
+#include <unordered_map>
+#include <functional>
+#include <memory>
+#include <chrono>
+#include <thread>
+#include <mutex>
+
+namespace cmvr {
+    namespace device {
+
+        // 节点状态结构体
+        struct NodeState {
+            bool enabled = false;
+            bool initialized = false;
+            msgs::RunMode current_mode = msgs::RUN_MODE_UNSPECIFIED;
+            uint16_t status_word = 0;
+            int32_t position_offset = 0;
+            int32_t target_position = 0;
+        };
+
+        class EYouEthercatProtocol : public MotorProtocolInterface {
+        public:
+            explicit EYouEthercatProtocol(std::string ifName, const std::string& xmlPath = "");
+            ~EYouEthercatProtocol() override;
+
+            // 基础接口实现
+            bool initNode(uint8_t node_id) override;
+            void setMode(uint8_t node_id, msgs::RunMode mode) override;
+            void setTarget(uint8_t node_id, double angle_rad, double vel) override;
+            void setTarget(uint8_t node_id, double vel) override;
+            void setQ(uint8_t node_id, double angle_rad) override;
+            void setLimitQ(uint8_t node_id, double ub, double lb) override;
+            void setLimitQd(uint8_t node_id, double qd) override;
+            void setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd) override;
+            bool calibrateZeroQ(uint8_t node_id) override;
+            void brake(uint8_t node_id) override;
+            bool reachedTargetQ(uint8_t node_id) override;
+
+            double getQ(uint8_t node_id) override;
+            double getQd(uint8_t node_id) override;
+
+            void setQd(uint8_t node_id, double qd) override;
+            void setQdd(uint8_t node_id, double qdd) override;
+
+            void torqueOff(uint8_t node_id) override;
+            msgs::RunMode getMode(uint8_t node_id) override;
+
+            // 配置接口
+            void configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel);
+
+            // 扩展功能接口
+            double getTorque(uint8_t node_id);
+            uint16_t getStatusWord(uint8_t node_id);
+            bool isEnabled(uint8_t node_id);
+            bool isFault(uint8_t node_id);
+            void faultReset(uint8_t node_id);
+            bool waitForTargetReached(uint8_t node_id, int timeout_ms);
+
+            // 数据获取接口
+            std::unique_ptr<msgs::RobotDetail> GetRobotDetail();
+
+            int getSlaveCount() const { return slave_count_; }
+
+        private:
+            // 常量定义
+            static constexpr double GearRatio = 101.0;
+            static constexpr double RADTODEG = 180.0 / M_PI;
+            static constexpr double PULSE_PER_REV = 65536.0;
+
+            // 辅助函数
+            double posToAngle(int32_t pos) const;
+            int32_t angleToPos(double angle) const;
+            double velToSpeed(double vel) const;
+            double speedToVel(int32_t speed) const;
+
+            bool waitUntil(std::function<bool()> condition, int timeout_ms);
+            void updateNodeState(uint8_t node_id);
+            bool performEnableSequence(uint8_t node_id);
+
+            int8_t runModeToEcMode(msgs::RunMode mode);
+            msgs::RunMode ecModeToRunMode(int8_t mode);
+
+            // 成员变量
+            std::unordered_map<uint8_t, NodeState> node_states_;
+            std::unordered_map<uint8_t, double> last_Qd_;
+            std::unordered_map<uint8_t, uint32_t> last_Qdd_;
+
+            std::shared_ptr<device::CanSender<msgs::RobotDetail>> can_sender_;
+            std::shared_ptr<device::MessageManager<msgs::RobotDetail>> message_manager_;
+
+            std::string interface_name_;
+            std::string xml_config_path_;
+            bool initialized_;
+            int slave_count_;
+
+            std::unique_ptr<EcMaster> master_;
+            std::unique_ptr<EcXmlParser> parser_;
+            EcSlaveConfig slave_config_;
+            bool use_fixed_pdo_;
+
+            std::mutex master_mutex_;
+        };
+
+    } // namespace device
+} // namespace cmvr

cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_master.h

@@ -0,0 +1,328 @@
+// ec_master.h
+#ifndef EC_MASTER_H
+#define EC_MASTER_H
+
+#include "ec_types.h"
+#include <map>
+#include <atomic>
+#include <cstring>
+#include <vector>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+#include "soem/soem.h"
+#include "soem/osal.h"
+#ifdef __cplusplus
+}
+#endif
+
+// 错误码定义
+#define EC_SUCCESS 0
+#define EC_FAILED_INIT 1
+#define EC_FAILED_NOSLAVE 2
+
+#ifndef EC_TIMEOUTMON
+#define EC_TIMEOUTMON 500
+#endif
+
+#ifndef EC_TIMEOUTRET
+#define EC_TIMEOUTRET 2000
+#endif
+
+#ifndef EC_TIMEOUTSTATE
+#define EC_TIMEOUTSTATE 2000
+#endif
+
+#ifndef EC_TIMEOUTRXM
+#define EC_TIMEOUTRXM 2000
+#endif
+
+// EtherCAT 主站类
+class EcMaster {
+
+public:
+    // 操作模式枚举（符合 CiA 402 标准）
+    enum EcOperationMode : int8_t {
+        // 轮廓模式 (Profile Modes)
+        EcOperationMode_ProfilePosition = 1,   // PP 模式
+        EcOperationMode_ProfileVelocity = 3,   // PV 模式
+        EcOperationMode_ProfileTorque = 4,     // PT 模式
+
+        // 插补模式
+        EcOperationMode_InterpolatedPosition = 7,  // IP 模式
+
+        // 循环同步模式 (Cyclic Sync Modes)
+        EcOperationMode_CyclicSyncPosition = 8,    // CSP 模式
+        EcOperationMode_CyclicSyncVelocity = 9,    // CSV 模式
+        EcOperationMode_CyclicSyncTorque = 10,     // CST 模式
+    };
+
+public:
+    EcMaster();
+    ~EcMaster();
+
+    EcMaster(const EcMaster&) = delete;
+    EcMaster& operator=(const EcMaster&) = delete;
+
+    // 初始化
+    int init(const char* ifName, int cycleTimeUs = 1000);
+    void close() { shutdown(); }
+    int getSlaveCount() const { return slaveCount; }
+    bool isOperational() const { return inOp; }
+
+    // 状态控制
+    int setState(uint16_t slaveId, uint8_t targetState);
+    int preOperational(uint16_t slaveId) { return setState(slaveId, EC_STATE_PRE_OP); }
+    int safeOperational(uint16_t slaveId) { return setState(slaveId, EC_STATE_SAFE_OP); }
+    int operational(uint16_t slaveId) { return setState(slaveId, EC_STATE_OPERATIONAL); }
+    int gotoOperational();
+    void shutdown();
+
+    // PDO 配置
+    int configurePdo(uint16_t slaveId, const EcPdo* rxPdo, const EcPdo* txPdo);
+    int setupMappingAndDc();  // 配置所有从站的映射和DC
+    int setupPdoOffsets(uint16_t slaveId, const EcPdo* rxPdo, const EcPdo* txPdo);
+
+    // PDO 数据访问（高性能，零拷贝）
+    void setControlWord(uint16_t slaveId, uint16_t value);
+    uint16_t getStatusWord(uint16_t slaveId);
+    void setTargetPosition(uint16_t slaveId, int32_t value);
+    int32_t getActualPosition(uint16_t slaveId);
+    void setTargetVelocity(uint16_t slaveId, int32_t value);
+    int32_t getActualVelocity(uint16_t slaveId);
+    void setTargetTorque(uint16_t slaveId, int16_t value);
+    int16_t getActualTorque(uint16_t slaveId);
+    void setModeOfOperation(uint16_t slaveId, int8_t value);
+    int8_t getModeOfOperationDisplay(uint16_t slaveId);
+
+    void setProfileVelocity(uint16_t slaveId, uint32_t value);
+    void setProfileAcceleration(uint16_t slaveId, uint32_t value);
+    void setProfileDeceleration(uint16_t slaveId, uint32_t value);
+    int16_t getCurrentValue(uint16_t slaveId);
+    uint16_t getErrorCode(uint16_t slaveId);
+
+    // 电机控制高级接口
+    int enable(uint16_t slaveId);
+    int disable(uint16_t slaveId);
+    int faultReset(uint16_t slaveId);
+
+    // 周期操作
+    int sendOutputs() { return ecx_send_processdata(&ctx); }
+    int receiveInputs() { return ecx_receive_processdata(&ctx, EC_TIMEOUTRET); }
+
+    // 影子缓冲区同步接口
+    void syncSlaveOutputs(uint16_t slaveId);   // 同步指定从站的输出
+    void syncAllOutputs();                     // 同步所有从站的输出
+
+    // 原始数据访问
+    uint8_t* getInputs(uint16_t slaveId) {
+        auto it = inputs.find(slaveId);
+        return (it != inputs.end()) ? it->second : nullptr;
+    }
+    uint8_t* getOutputs(uint16_t slaveId) {
+        auto it = outputs.find(slaveId);
+        return (it != outputs.end()) ? it->second : nullptr;
+    }
+    int getInputSize(uint16_t slaveId) {
+        auto it = inputSizes.find(slaveId);
+        return (it != inputSizes.end()) ? it->second : 0;
+    }
+    int getOutputSize(uint16_t slaveId) {
+        auto it = outputSizes.find(slaveId);
+        return (it != outputSizes.end()) ? it->second : 0;
+    }
+
+    // SDO 读写（仅用于配置阶段）
+    int readSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, void* value, EcDataType type, int timeoutMs = 100);
+    int writeSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, void* value, EcDataType type, int timeoutMs = 100);
+
+    // SM/FMMU 配置校验（调试用）
+    void verifySmConfiguration(uint16_t slaveId);
+    void verifyAllSlavesSmConfiguration();
+
+    // 模板便捷接口
+    template<typename T>
+    int readSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, T* value, int timeoutMs = 100) {
+        EcDataType type;
+        if constexpr (std::is_same_v<T, int8_t>) type = EcDataType::Int8;
+        else if constexpr (std::is_same_v<T, uint8_t>) type = EcDataType::Uint8;
+        else if constexpr (std::is_same_v<T, int16_t>) type = EcDataType::Int16;
+        else if constexpr (std::is_same_v<T, uint16_t>) type = EcDataType::Uint16;
+        else if constexpr (std::is_same_v<T, int32_t>) type = EcDataType::Int32;
+        else if constexpr (std::is_same_v<T, uint32_t>) type = EcDataType::Uint32;
+        else if constexpr (std::is_same_v<T, float>) type = EcDataType::Real32;
+        else if constexpr (std::is_same_v<T, double>) type = EcDataType::Real64;
+        else return -1;
+        return readSdo(slaveId, index, subindex, value, type, timeoutMs);
+    }
+
+    template<typename T>
+    int writeSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, T* value, int timeoutMs = 100) {
+        EcDataType type;
+        if constexpr (std::is_same_v<T, int8_t>) type = EcDataType::Int8;
+        else if constexpr (std::is_same_v<T, uint8_t>) type = EcDataType::Uint8;
+        else if constexpr (std::is_same_v<T, int16_t>) type = EcDataType::Int16;
+        else if constexpr (std::is_same_v<T, uint16_t>) type = EcDataType::Uint16;
+        else if constexpr (std::is_same_v<T, int32_t>) type = EcDataType::Int32;
+        else if constexpr (std::is_same_v<T, uint32_t>) type = EcDataType::Uint32;
+        else if constexpr (std::is_same_v<T, float>) type = EcDataType::Real32;
+        else if constexpr (std::is_same_v<T, double>) type = EcDataType::Real64;
+        else return -1;
+        return writeSdo(slaveId, index, subindex, value, type, timeoutMs);
+    }
+
+private:
+    // ========== 影子缓冲区结构（每个从站独立） ==========
+    struct SlaveShadowBuffer {
+        uint8_t* data = nullptr;   // 影子缓冲区数据指针
+        size_t size = 0;           // 缓冲区大小
+        bool dirty = false;        // 是否有待同步的数据
+
+        SlaveShadowBuffer() = default;
+
+        explicit SlaveShadowBuffer(size_t sz) : size(sz) {
+            if (sz > 0) {
+                data = new uint8_t[sz];
+                memset(data, 0, sz);
+            }
+        }
+
+        ~SlaveShadowBuffer() {
+            delete[] data;
+            data = nullptr;
+        }
+
+        // 禁止拷贝
+        SlaveShadowBuffer(const SlaveShadowBuffer&) = delete;
+        SlaveShadowBuffer& operator=(const SlaveShadowBuffer&) = delete;
+
+        // 允许移动
+        SlaveShadowBuffer(SlaveShadowBuffer&& other) noexcept
+            : data(other.data), size(other.size), dirty(other.dirty) {
+            other.data = nullptr;
+            other.size = 0;
+            other.dirty = false;
+        }
+
+        SlaveShadowBuffer& operator=(SlaveShadowBuffer&& other) noexcept {
+            if (this != &other) {
+                delete[] data;
+                data = other.data;
+                size = other.size;
+                dirty = other.dirty;
+                other.data = nullptr;
+                other.size = 0;
+                other.dirty = false;
+            }
+            return *this;
+        }
+
+        // 重新分配缓冲区
+        void reallocate(size_t newSize) {
+            if (data) {
+                delete[] data;
+                data = nullptr;
+            }
+            size = newSize;
+            if (newSize > 0) {
+                data = new uint8_t[newSize];
+                memset(data, 0, newSize);
+            }
+            dirty = false;
+        }
+    };
+
+    // SOEM 上下文
+    ecx_contextt ctx;
+    uint8_t IOmap[4096];
+
+    // 每个从站的影子缓冲区映射
+    std::map<uint16_t, SlaveShadowBuffer> shadowBuffers;
+    std::atomic<bool> anyPdoDirty{false};  // 是否有任何影子缓冲区有更新
+
+    int slaveCount = 0;
+    int expectedWKC = 0;
+    int currentgroup = 0;
+
+    // 状态标志
+    std::atomic<bool> inOp{false};
+    std::atomic<bool> isRunning{false};
+    std::atomic<int> mappingdone{0};
+    std::atomic<int> dorun{0};
+    std::atomic<int> cyclicWkcErrorCount{0};
+
+    int64_t cycleTimeNs = 1000000;
+
+    OSAL_THREAD_HANDLE cyclicThread;
+    OSAL_THREAD_HANDLE checkThread;
+
+    // PDO 数据指针映射
+    std::map<uint16_t, uint8_t*> inputs;
+    std::map<uint16_t, uint8_t*> outputs;
+    std::map<uint16_t, int> inputSizes;
+    std::map<uint16_t, int> outputSizes;
+    std::map<uint16_t, uint16_t> outputOffsets;
+
+    // PDO 偏移映射
+    std::map<uint16_t, EcPdoOffsets> pdoOffsets;
+
+    // 辅助函数
+    template<typename T>
+    void writePdo(uint16_t slaveId, uint16_t offset, T value);
+
+    template<typename T>
+    T readPdo(uint16_t slaveId, uint16_t offset);
+
+    // 获取从站的影子缓冲区
+    SlaveShadowBuffer* getShadowBuffer(uint16_t slaveId);
+
+    // 线程函数
+    static OSAL_THREAD_FUNC_RT cyclicThreadFunc(void* arg);
+    static OSAL_THREAD_FUNC checkThreadFunc(void* arg);
+    void runCyclic();
+    void runCheck();
+};
+
+// ==================== 模板实现（必须放在头文件中） ====================
+
+template<typename T>
+void EcMaster::writePdo(uint16_t slaveId, uint16_t offset, T value) {
+    if (inOp) {
+        auto* shadow = getShadowBuffer(slaveId);
+        if (shadow && shadow->data) {
+            if (offset + sizeof(T) <= shadow->size) {
+                *(T*)(shadow->data + offset) = value;
+                shadow->dirty = true;
+                anyPdoDirty = true;
+                return;
+            }
+        }
+    }
+
+    // 降级：直接写 IOmap（配置阶段使用）
+    auto outIt = outputs.find(slaveId);
+    if (outIt != outputs.end() && outIt->second) {
+        auto sizeIt = outputSizes.find(slaveId);
+        if (sizeIt != outputSizes.end() && offset + sizeof(T) <= (uint16_t)sizeIt->second) {
+            *(T*)(outIt->second + offset) = value;
+        }
+    }
+}
+
+template<typename T>
+T EcMaster::readPdo(uint16_t slaveId, uint16_t offset) {
+    auto it = pdoOffsets.find(slaveId);
+    if (it == pdoOffsets.end() || offset == 0xFFFF) return 0;
+
+    auto inIt = inputs.find(slaveId);
+    if (inIt != inputs.end() && inIt->second) {
+        auto sizeIt = inputSizes.find(slaveId);
+        if (sizeIt != inputSizes.end() && offset + sizeof(T) <= (uint16_t)sizeIt->second) {
+            return *(T*)(inIt->second + offset);
+        }
+    }
+    return 0;
+}
+
+#endif // EC_MASTER_H

cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_pdo_config.h

@@ -0,0 +1,75 @@
+// ec_pdo_config.h - 固定 PDO 配置（替代 XML 解析）
+#ifndef EC_PDO_CONFIG_H
+#define EC_PDO_CONFIG_H
+
+#include "ec_types.h"
+
+// 创建固定的 RxPDO 配置（对应 XML 中的 #x1600）
+inline EcPdo createFixedRxPdo() {
+    EcPdo rxPdo;
+    rxPdo.index = 0x1600;
+    rxPdo.mapCount = 10;      // 9个有效条目 + 1个结束标志
+    rxPdo.name = "Outputs";
+    rxPdo.isRxPdo = true;
+
+    // 按XML顺序填充 Entry
+    rxPdo.entries.push_back({0x6040, 0, 16, "Control Word", EcDataType::Uint16});
+    rxPdo.entries.push_back({0x607A, 0, 32, "Target Position", EcDataType::Int32});
+    rxPdo.entries.push_back({0x60FF, 0, 32, "Target Velocity", EcDataType::Int32});
+    rxPdo.entries.push_back({0x6071, 0, 16, "Target Torque", EcDataType::Int16});
+    rxPdo.entries.push_back({0x6083, 0, 32, "Profile Acceleration", EcDataType::Uint32});
+    rxPdo.entries.push_back({0x6084, 0, 32, "Profile Deceleration", EcDataType::Uint32});
+    rxPdo.entries.push_back({0x6081, 0, 32, "Profile Velocity", EcDataType::Uint32});
+    rxPdo.entries.push_back({0x6087, 0, 32, "Torque Slope", EcDataType::Uint32});
+    rxPdo.entries.push_back({0x6060, 0, 8, "Modes Of Operation", EcDataType::Int8});
+    rxPdo.entries.push_back({0x0000, 0, 8, "End Marker", EcDataType::Uint8});
+
+    return rxPdo;
+}
+
+// 创建固定的 TxPDO 配置（对应 XML 中的 #x1a00）
+inline EcPdo createFixedTxPdo() {
+    EcPdo txPdo;
+    txPdo.index = 0x1A00;
+    txPdo.mapCount = 8;       // 7个有效条目 + 1个结束标志
+    txPdo.name = "Inputs";
+    txPdo.isRxPdo = false;
+
+    // 按XML顺序填充 Entry
+    txPdo.entries.push_back({0x6041, 0, 16, "Status Word", EcDataType::Uint16});
+    txPdo.entries.push_back({0x6064, 0, 32, "Actual Position", EcDataType::Int32});
+    txPdo.entries.push_back({0x606C, 0, 32, "Actual Velocity", EcDataType::Int32});
+    txPdo.entries.push_back({0x6077, 0, 16, "Actual Torque", EcDataType::Int16});
+    txPdo.entries.push_back({0x6061, 0, 8, "Mode Of Operation Display", EcDataType::Int8});
+    txPdo.entries.push_back({0x603F, 0, 16, "Error Code", EcDataType::Uint16});
+    txPdo.entries.push_back({0x6078, 0, 16, "Actual Current Value", EcDataType::Int16});
+    txPdo.entries.push_back({0x0000, 0, 8, "End Marker", EcDataType::Uint8});
+
+    return txPdo;
+}
+
+// 创建完整的从站配置 - 可以直接替换 XML 解析结果
+inline EcSlaveConfig createFixedSlaveConfig(uint16_t slaveId) {
+    EcSlaveConfig config;
+    config.slaveId = slaveId;
+    config.productCode = 0x00000000;
+    config.revisionNo = 0;
+    config.name = "Axis (csv,csp,cst)";
+
+    EcModule module;
+    module.moduleIdent = 0x119800;  // 对应 XML 中的 ModuleIdent="#x119800"
+    module.name = "dynamic switchbewteen csp/csv";
+
+    module.rxPdos.push_back(createFixedRxPdo());
+    module.txPdos.push_back(createFixedTxPdo());
+
+    // 记录可用的PDO索引（对应 parser.getAvailableRxPdos/getAvailableTxPdos 的返回值）
+    config.availableRxPdos.push_back(0x1600);
+    config.availableTxPdos.push_back(0x1A00);
+
+    config.modules.push_back(module);
+
+    return config;
+}
+
+#endif // EC_PDO_CONFIG_H

cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_types.h

@@ -0,0 +1,111 @@
+// ec_types.h - 公共类型定义
+#ifndef EC_TYPES_H
+#define EC_TYPES_H
+
+#include <string>
+#include <vector>
+#include <cstdint>
+
+// 数据类型枚举
+enum class EcDataType : uint8_t {
+    Boolean = 0x01,
+    Int8 = 0x02,
+    Int16 = 0x03,
+    Int32 = 0x04,
+    Uint8 = 0x05,
+    Uint16 = 0x06,
+    Uint32 = 0x07,
+    Real32 = 0x08,
+    Real64 = 0x09
+};
+
+// 从站状态枚举
+enum class EcState : uint8_t {
+    Init = 0x01,
+    PreOp = 0x02,
+    SafeOp = 0x04,
+    Operational = 0x08
+};
+
+// 操作模式枚举
+enum class EcOperateMode : int8_t {
+    ProfilePosition = 1,
+    Velocity = 2,
+    ProfileVelocity = 3,
+    ProfileTorque = 4,
+    Homing = 6,
+    CyclicSyncPosition = 8,
+    CyclicSyncVelocity = 9,
+    CyclicSyncTorque = 10
+};
+
+// PDO 条目
+struct EcPdoEntry {
+    uint16_t index = 0;
+    uint8_t subindex = 0;
+    uint8_t bitlen = 0;
+    std::string name;
+    EcDataType dataType = EcDataType::Uint8;
+
+    uint32_t makeMapValue() const {
+        return ((uint32_t)index << 16) | ((uint32_t)subindex << 8) | bitlen;
+    }
+};
+
+// PDO
+struct EcPdo {
+    uint16_t index = 0;
+    uint8_t mapCount = 0;
+    std::vector<EcPdoEntry> entries;
+    std::string name;
+    bool isRxPdo = false;
+
+    void updateMapCount() {
+        mapCount = entries.size();
+    }
+};
+
+// 模块信息
+struct EcModule {
+    uint32_t moduleIdent = 0;
+    std::string name;
+    std::vector<EcPdo> rxPdos;
+    std::vector<EcPdo> txPdos;
+};
+
+// 从站配置信息
+struct EcSlaveConfig {
+    uint16_t slaveId = 0;
+    uint32_t productCode = 0;
+    uint32_t revisionNo = 0;
+    std::string name;
+    std::vector<EcModule> modules;
+    std::vector<uint16_t> availableRxPdos;
+    std::vector<uint16_t> availableTxPdos;
+};
+
+// PDO 偏移映射
+// ec_types.h 或 ec_master.h 中
+struct EcPdoOffsets {
+    // RxPDO 偏移量 (主站 -> 从站)
+    uint16_t controlWord = 0xFFFF;           // 0x6040
+    uint16_t targetPosition = 0xFFFF;        // 0x607A
+    uint16_t targetVelocity = 0xFFFF;        // 0x60FF
+    uint16_t targetTorque = 0xFFFF;          // 0x6071
+    uint16_t profileAcceleration = 0xFFFF;   // 0x6083 (PP模式)
+    uint16_t profileDeceleration = 0xFFFF;   // 0x6084 (PP模式)
+    uint16_t profileVelocity = 0xFFFF;       // 0x6081 (PP模式)
+    uint16_t torqueSlope = 0xFFFF;           // 0x6087 (PT模式)
+    uint16_t modeOfOperation = 0xFFFF;       // 0x6060
+
+    // TxPDO 偏移量 (从站 -> 主站)
+    uint16_t statusWord = 0xFFFF;            // 0x6041
+    uint16_t actualPosition = 0xFFFF;        // 0x6064
+    uint16_t actualVelocity = 0xFFFF;        // 0x606C
+    uint16_t actualTorque = 0xFFFF;          // 0x6077
+    uint16_t modeOfOperationDisplay = 0xFFFF;// 0x6061
+    uint16_t errorCode = 0xFFFF;             // 0x603F
+    uint16_t actualCurrent = 0xFFFF;         // 0x6078 当前电流值
+};
+
+#endif // EC_TYPES_H

cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_xml_parser.cpp

@@ -0,0 +1,317 @@
+// ec_xml_parser.cpp
+#include "ec_xml_parser.h"
+#include "rapidxml/xml_parser.h"
+#include <cstdlib>
+#include <cstdio>
+#include <map>
+
+using namespace rapidxml;
+
+static EcDataType stringToDataType(const std::string& typeName) {
+    if (typeName == "BOOL") return EcDataType::Boolean;
+    if (typeName == "SINT") return EcDataType::Int8;
+    if (typeName == "INT") return EcDataType::Int16;
+    if (typeName == "DINT") return EcDataType::Int32;
+    if (typeName == "USINT") return EcDataType::Uint8;
+    if (typeName == "UINT") return EcDataType::Uint16;
+    if (typeName == "UDINT") return EcDataType::Uint32;
+    if (typeName == "REAL") return EcDataType::Real32;
+    return EcDataType::Uint8;
+}
+
+static uint32_t parseHexValue(const std::string& str) {
+    if (str.empty()) return 0;
+    if (str[0] == '#' && str[1] == 'x') {
+        return strtoul(str.c_str() + 2, nullptr, 16);
+    }
+    return strtoul(str.c_str(), nullptr, 16);
+}
+
+class EcXmlParser::Impl {
+public:
+    std::map<uint16_t, EcSlaveConfig> slaveConfigs;
+    std::unique_ptr<XmlNode> xmlDoc;
+
+    bool loadXmlFile(const std::string& path) {
+        try {
+            xmlDoc = std::make_unique<XmlNode>(path);
+            return true;
+        } catch (const std::exception& e) {
+            printf("Failed to load XML file: %s, error: %s\n", path.c_str(), e.what());
+            return false;
+        }
+    }
+
+    void parsePdoEntry(const XmlNode& entryNode, EcPdoEntry& entry) {
+        if (entryNode.hasChild("Index")) {
+            XmlNode idxNode = entryNode.getChild("Index");
+            entry.index = (uint16_t)parseHexValue(idxNode.getNodeValue());
+        }
+        if (entryNode.hasChild("SubIndex")) {
+            XmlNode subIdxNode = entryNode.getChild("SubIndex");
+            entry.subindex = (uint8_t)atoi(subIdxNode.getNodeValue().c_str());
+        }
+        if (entryNode.hasChild("BitLen")) {
+            XmlNode bitLenNode = entryNode.getChild("BitLen");
+            entry.bitlen = (uint8_t)atoi(bitLenNode.getNodeValue().c_str());
+        }
+        if (entryNode.hasChild("Name")) {
+            XmlNode nameEntryNode = entryNode.getChild("Name");
+            entry.name = nameEntryNode.getNodeValue();
+        }
+        if (entryNode.hasChild("DataType")) {
+            XmlNode dataTypeNode = entryNode.getChild("DataType");
+            entry.dataType = stringToDataType(dataTypeNode.getNodeValue());
+        }
+    }
+
+    void parsePdoMapping(const XmlNode& pdoNode, bool isRxPdo, EcModule& module) {
+        if (!pdoNode.hasChild("Index")) {
+            printf("Warning: PDO missing Index node, skipping\n");
+            return;
+        }
+
+        XmlNode indexNode = pdoNode.getChild("Index");
+        uint16_t index = (uint16_t)parseHexValue(indexNode.getNodeValue());
+
+        EcPdo pdo;
+        pdo.index = index;
+        pdo.isRxPdo = isRxPdo;
+
+        if (pdoNode.hasChild("Name")) {
+            XmlNode nameNode = pdoNode.getChild("Name");
+            pdo.name = nameNode.getNodeValue();
+        }
+
+        std::vector<XmlNode> entries = pdoNode.getChildren("Entry");
+        for (const auto& entryNode : entries) {
+            EcPdoEntry entry;
+            parsePdoEntry(entryNode, entry);
+            pdo.entries.push_back(entry);
+        }
+
+        pdo.updateMapCount();
+
+        if (isRxPdo) {
+            module.rxPdos.push_back(pdo);
+        } else {
+            module.txPdos.push_back(pdo);
+        }
+    }
+
+    void parseModule(const XmlNode& moduleNode, EcSlaveConfig& config) {
+        if (!moduleNode.hasChild("Type")) {
+            printf("Warning: Module missing Type node, skipping\n");
+            return;
+        }
+
+        auto typeNode = moduleNode.getChild("Type");
+        std::string moduleIdentStr = typeNode.getAttrDefault("ModuleIdent", "");
+        if (moduleIdentStr.empty()) {
+            printf("Warning: Module missing ModuleIdent attribute, skipping\n");
+            return;
+        }
+
+        EcModule module;
+        module.moduleIdent = (uint32_t)parseHexValue(moduleIdentStr);
+
+        if (moduleNode.hasChild("Name")) {
+            XmlNode nameNode = moduleNode.getChild("Name");
+            module.name = nameNode.getNodeValue();
+        }
+
+        if (moduleNode.hasChild("RxPdo")) {
+            auto rxPdoNode = moduleNode.getChild("RxPdo");
+            parsePdoMapping(rxPdoNode, true, module);
+        }
+
+        if (moduleNode.hasChild("TxPdo")) {
+            auto txPdoNode = moduleNode.getChild("TxPdo");
+            parsePdoMapping(txPdoNode, false, module);
+        }
+
+        config.modules.push_back(module);
+
+        for (const auto& pdo : module.rxPdos) {
+            config.availableRxPdos.push_back(pdo.index);
+        }
+        for (const auto& pdo : module.txPdos) {
+            config.availableTxPdos.push_back(pdo.index);
+        }
+
+        printf("  Parsed module: 0x%08X - %s\n", module.moduleIdent, module.name.c_str());
+        printf("    RxPDOs: %zu, TxPDOs: %zu\n", module.rxPdos.size(), module.txPdos.size());
+    }
+
+    bool parseSlaveConfigById(uint16_t slaveId, EcSlaveConfig& outConfig) {
+        if (!xmlDoc) return false;
+
+        XmlNode rootNode = xmlDoc->getRoot();
+        if (rootNode.getNodeName() != "EtherCATInfo") {
+            printf("Root node is not EtherCATInfo\n");
+            return false;
+        }
+
+        if (!rootNode.hasChild("Descriptions")) {
+            printf("No Descriptions node found\n");
+            return false;
+        }
+        XmlNode descNode = rootNode.getChild("Descriptions");
+
+        outConfig = EcSlaveConfig();
+        outConfig.slaveId = slaveId;
+
+        if (descNode.hasChild("Devices")) {
+            XmlNode devicesNode = descNode.getChild("Devices");
+            if (devicesNode.hasChild("Device")) {
+                XmlNode deviceNode = devicesNode.getChild("Device");
+                if (deviceNode.hasChild("Type")) {
+                    XmlNode typeNode = deviceNode.getChild("Type");
+                    std::string productCode = typeNode.getAttrDefault("ProductCode", "");
+                    if (!productCode.empty()) {
+                        outConfig.productCode = (uint32_t)parseHexValue(productCode);
+                    }
+                    std::string revision = typeNode.getAttrDefault("RevisionNo", "");
+                    if (!revision.empty()) {
+                        outConfig.revisionNo = (uint32_t)atoi(revision.c_str());
+                    }
+                }
+                if (deviceNode.hasChild("Name")) {
+                    XmlNode nameNode = deviceNode.getChild("Name");
+                    outConfig.name = nameNode.getNodeValue();
+                }
+            }
+        }
+
+        if (!descNode.hasChild("Modules")) {
+            printf("Warning: No Modules node found under Descriptions\n");
+            return !outConfig.modules.empty();
+        }
+
+        XmlNode modulesNode = descNode.getChild("Modules");
+        printf("Found Modules node under Descriptions, parsing PDO configurations...\n");
+
+        std::vector<XmlNode> moduleNodes = modulesNode.getChildren("Module");
+        if (moduleNodes.empty()) {
+            printf("Warning: No Module element found under Modules\n");
+            return !outConfig.modules.empty();
+        }
+
+        for (const auto& moduleNode : moduleNodes) {
+            parseModule(moduleNode, outConfig);
+        }
+
+        printf("Total %zu modules parsed\n", moduleNodes.size());
+        return true;
+    }
+};
+
+// ==================== EcXmlParser 公共接口 ====================
+
+EcXmlParser::EcXmlParser() : pImpl(std::make_unique<Impl>()) {}
+EcXmlParser::~EcXmlParser() = default;
+
+bool EcXmlParser::loadXmlFile(const std::string& xmlPath) {
+    return pImpl->loadXmlFile(xmlPath);
+}
+
+bool EcXmlParser::parseSlaveConfig(uint16_t slaveId, EcSlaveConfig& outConfig) {
+    if (!pImpl->parseSlaveConfigById(slaveId, outConfig)) {
+        return false;
+    }
+    pImpl->slaveConfigs[slaveId] = outConfig;
+    return true;
+}
+
+EcSlaveConfig* EcXmlParser::getSlaveConfig(uint16_t slaveId) {
+    auto it = pImpl->slaveConfigs.find(slaveId);
+    return (it != pImpl->slaveConfigs.end()) ? &it->second : nullptr;
+}
+
+const EcSlaveConfig* EcXmlParser::getSlaveConfig(uint16_t slaveId) const {
+    auto it = pImpl->slaveConfigs.find(slaveId);
+    return (it != pImpl->slaveConfigs.end()) ? &it->second : nullptr;
+}
+
+std::vector<uint16_t> EcXmlParser::getAvailableRxPdos(uint16_t slaveId) const {
+    auto it = pImpl->slaveConfigs.find(slaveId);
+    return (it != pImpl->slaveConfigs.end()) ? it->second.availableRxPdos : std::vector<uint16_t>();
+}
+
+std::vector<uint16_t> EcXmlParser::getAvailableTxPdos(uint16_t slaveId) const {
+    auto it = pImpl->slaveConfigs.find(slaveId);
+    return (it != pImpl->slaveConfigs.end()) ? it->second.availableTxPdos : std::vector<uint16_t>();
+}
+
+const EcPdo* EcXmlParser::findPdo(uint16_t slaveId, uint16_t pdoIndex, bool isRxPdo) const {
+    auto it = pImpl->slaveConfigs.find(slaveId);
+    if (it == pImpl->slaveConfigs.end()) return nullptr;
+
+    for (const auto& module : it->second.modules) {
+        const auto& pdos = isRxPdo ? module.rxPdos : module.txPdos;
+        for (const auto& pdo : pdos) {
+            if (pdo.index == pdoIndex) return &pdo;
+        }
+    }
+    return nullptr;
+}
+
+const EcModule* EcXmlParser::findModuleByIndex(uint16_t slaveId, uint16_t pdoIndex, bool isRxPdo) const {
+    auto it = pImpl->slaveConfigs.find(slaveId);
+    if (it == pImpl->slaveConfigs.end()) return nullptr;
+
+    for (const auto& module : it->second.modules) {
+        const auto& pdos = isRxPdo ? module.rxPdos : module.txPdos;
+        for (const auto& pdo : pdos) {
+            if (pdo.index == pdoIndex) return &module;
+        }
+    }
+    return nullptr;
+}
+
+const EcModule* EcXmlParser::findModuleByIdent(uint16_t slaveId, uint32_t moduleIdent) const {
+    auto it = pImpl->slaveConfigs.find(slaveId);
+    if (it == pImpl->slaveConfigs.end()) return nullptr;
+
+    for (const auto& module : it->second.modules) {
+        if (module.moduleIdent == moduleIdent) return &module;
+    }
+    return nullptr;
+}
+
+void EcXmlParser::printSlaveInfo(uint16_t slaveId) const {
+    auto it = pImpl->slaveConfigs.find(slaveId);
+    if (it == pImpl->slaveConfigs.end()) {
+        printf("Slave %d not configured\n", slaveId);
+        return;
+    }
+
+    const EcSlaveConfig& config = it->second;
+    printf("\n========== Slave %d Configuration ==========\n", slaveId);
+    printf("Name: %s\n", config.name.c_str());
+    printf("Product Code: 0x%08X\n", config.productCode);
+    printf("Revision: %u\n", config.revisionNo);
+
+    for (const auto& module : config.modules) {
+        printf("\n[Module 0x%08X] %s\n", module.moduleIdent, module.name.c_str());
+        for (const auto& pdo : module.rxPdos) {
+            printf("  RxPDO 0x%04X - %s (%d entries)\n", pdo.index, pdo.name.c_str(), pdo.mapCount);
+            for (const auto& entry : pdo.entries) {
+                printf("    -> 0x%04X:%02X (%d bits) - %s\n",
+                       entry.index, entry.subindex, entry.bitlen, entry.name.c_str());
+            }
+        }
+        for (const auto& pdo : module.txPdos) {
+            printf("  TxPDO 0x%04X - %s (%d entries)\n", pdo.index, pdo.name.c_str(), pdo.mapCount);
+            for (const auto& entry : pdo.entries) {
+                printf("    <- 0x%04X:%02X (%d bits) - %s\n",
+                       entry.index, entry.subindex, entry.bitlen, entry.name.c_str());
+            }
+        }
+    }
+    printf("==========================================\n");
+}
+
+void EcXmlParser::clear() {
+    pImpl->slaveConfigs.clear();
+}

cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_xml_parser.h

@@ -0,0 +1,41 @@
+// ec_xml_parser.h
+#ifndef EC_XML_PARSER_H
+#define EC_XML_PARSER_H
+
+#include "ec_types.h"
+#include <memory>
+#include <vector>
+#include <cstdint>
+
+// XML PDO配置解析器类（仅负责解析XML）
+class EcXmlParser {
+public:
+    EcXmlParser();
+    ~EcXmlParser();
+
+    bool loadXmlFile(const std::string& xmlPath);
+    bool parseSlaveConfig(uint16_t slaveId, EcSlaveConfig& outConfig);
+
+    // 获取已解析的配置
+    EcSlaveConfig* getSlaveConfig(uint16_t slaveId);
+    const EcSlaveConfig* getSlaveConfig(uint16_t slaveId) const;
+
+    // 查询接口
+    std::vector<uint16_t> getAvailableRxPdos(uint16_t slaveId) const;
+    std::vector<uint16_t> getAvailableTxPdos(uint16_t slaveId) const;
+    const EcPdo* findPdo(uint16_t slaveId, uint16_t pdoIndex, bool isRxPdo) const;
+    const EcModule* findModuleByIndex(uint16_t slaveId, uint16_t pdoIndex, bool isRxPdo) const;
+    const EcModule* findModuleByIdent(uint16_t slaveId, uint32_t moduleIdent) const;
+
+    // 打印信息
+    void printSlaveInfo(uint16_t slaveId) const;
+
+    // 清空所有配置
+    void clear();
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pImpl;
+};
+
+#endif // EC_XML_PARSER_H

cmvr-es/devices/motor/eyou_motor/eyou_motor.cpp

@@ -0,0 +1,5 @@
+//
+// Created by cmvr on 2026/3/30.
+//
+
+#include "eyou_motor.h"

cmvr-es/devices/motor/eyou_motor/eyou_motor.h

@@ -0,0 +1,46 @@
+//
+// Created by cmvr on 2026/3/30.
+//
+#pragma once
+#include "../abstract_motor.h"
+#include "motor/motor_protocol_interface.h"
+#include "motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h"
+
+namespace  cmvr {
+    namespace device {
+        class EYouMotor : public AbstractMotor {
+        public:
+            EYouMotor(const XmlNode &config,uint8_t node_id):AbstractMotor(config,node_id){}
+            EYouMotor(const XmlNode &config):AbstractMotor(config) {
+                info_.id = config.getAttrDefault("slave", -1);
+                info_.joint_name = config.getAttrString("jointName");
+                info_.limitQ = config.getAttrDefault("limitQ", 3.14f);
+                info_.limitQd = config.getAttrDefault("limitQd", 0.5f);
+                node_id_ = info_.id;
+            }
+
+            EYouMotor(const cmvr::config::EthercatMotorConfig& config) : AbstractMotor() {
+                info_.id = config.slave();
+                info_.joint_name = config.jointname();
+                info_.limitQ = config.limitqlb();
+                info_.limitQd = config.limitqd();
+                node_id_ = info_.id;
+            }
+            /**
+             * 电机初始化在具体的类中完成
+             */
+            void init() override {
+                if (!protocol_) {
+                    throw std::runtime_error("Protocol not set for motor");
+                }
+                if (protocol_->comm_proto == MotorProtocolInterface::CommProto::ETHERCAT ) {
+                    auto ethercat_protocol = std::dynamic_pointer_cast<EYouEthercatProtocol>(protocol_);
+                    ethercat_protocol->initNode(node_id_);
+                    ethercat_protocol->setQdd(node_id_,1);
+                }
+            }
+        };
+
+
+    }
+}

cmvr-es/devices/motor/motor_protocol_interface.h

@@ -15,7 +15,8 @@ namespace cmvr {
         public:
             enum class CommProto : uint8_t {
                 CANOPEN = 1,
-                CUSTOM = 2
+                ETHERCAT = 2,
+                CUSTOM = 3
             };
             virtual ~MotorProtocolInterface() = default;
 

cmvr-es/devices/motor/ti5_motor/ti5_motor.h

@@ -1,11 +1,12 @@
 //
 // Created by lgv on 2025/8/1.
 //
+#pragma once
 
 #include "../abstract_motor.h"
 #include "motor/motor_protocol_interface.h"
 #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
-
+#include "cmvr/config/robot_config/robot_config.pb.h"
 /**
  * class: Ti5Motor  完成电机硬件的相关工作，软件相关的在protocol 中完成
  */
@@ -22,6 +23,15 @@ namespace  cmvr {
                 node_id_ = info_.id;
             }
 
+            Ti5Motor(const cmvr::config::MotorConfig& config) : AbstractMotor() {
+                info_.id = config.id();
+                info_.joint_name = config.jointname();
+                info_.limitQ = config.limitqlb();
+                info_.limitQd = config.limitqd();
+                node_id_ = info_.id;
+            }
+
+
             /**
              * 电机初始化在具体的类中完成
              */

cmvr-es/devices/robot/abstract_robot.h

@@ -41,6 +41,7 @@ namespace cmvr::device{
 
     class AbstractRobot: public AbstractDevice {
     public:
+        AbstractRobot() = default;
         explicit AbstractRobot(const XmlNode &config): AbstractDevice(config) {}
         ~AbstractRobot() override=default;
 

cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h

@@ -35,6 +35,8 @@
 #include "planner/joint_space_planner/include/joint_space_planner_creator.h"
 #include "planner/joint_space_planner/include/joint_space_planner.h"
 #include "ik_solver/include/pinocchio_dls_ik_solver.h"
+#include "cmvr/config/robot_config/robot_config.pb.h"
+
 
 namespace cmvr::device{
 
@@ -49,12 +51,10 @@ namespace cmvr::device{
             ROBOT_ERROR
         } RobotStateMachine;
 
-//        typedef enum {
-//            PPM_MODE, PVM_MODE, PTM_MODE, CSP_MODE, CSV_MODE, CSC_MODE
-//        } RobotRunningMode;
-
     public:
         explicit HumanoidRobot(const XmlNode& cfg);
+        HumanoidRobot(const cmvr::config::HumanRobotConfig& config);
+        HumanoidRobot(const cmvr::config::EthercatRobotConfig& config);
         ~HumanoidRobot();
 
         void init() override;
@@ -232,6 +232,7 @@ namespace cmvr::device{
         std::atomic<bool> is_head_busy_{false}, is_waist_busy_{false};
 
 
+        std::string urdf_path_ = "";
     };
 
 

cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp

@@ -49,6 +49,87 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
     }
 }
 
+
+template<int DOF>
+HumanoidRobot<DOF>::HumanoidRobot(const cmvr::config::HumanRobotConfig& config) {
+    try {
+        id_ = config.id();
+        urdf_path_ = config.urdf();
+        dof_ = DOF;
+        if (!pathExists(config.urdf())) {
+            throw runtime_error("urdf file does not exist");
+        }
+        auto rcfg = cmvr::dyn::LoadRobotFromURDF(
+            config.urdf(), config.baselink());
+        m_robot_ = std::make_shared<cmvr::dyn::Robot<DOF> >(rcfg);
+
+        // 解析jointNames字符串，按逗号分隔
+        joint_names_ = splitString(config.jointnames(), ",");
+        // 解析linkNames字符串，按逗号分隔
+        link_names_ = splitString(config.linknames(), ",");
+
+        if (joint_names_.size() != dof_) {
+            throw runtime_error("joint names size mismatched with dof");
+        }
+
+        m_state_ = m_robot_->MakeState(link_names_, joint_names_);
+        m_cctrl_ = make_shared<ctrl::CartesianController<DOF> >(m_robot_);
+        upd_freq_ = config.updfreq();
+        CSP_buffer_ = make_shared<SPMCRingBuffer<JointPoint> >(config.buffersize());
+        CSV_buffer_ = make_shared<SPMCRingBuffer<JointVelocityCommand> >(config.buffersize());
+        CSC_buffer_ = make_shared<SPMCRingBuffer<JointCurrentCommand> >(config.buffersize());
+
+        upd_freq_ = 1000;
+        upd_timer_ = make_shared<FDTimer>();
+        upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000),
+                          [this] { update_state_(); });
+        rsm_.store(ROBOT_READY);
+    } catch (exception &e) {
+        LOG(ERROR) << "HumanoidRobot init failed, id=" << id_;
+        throw runtime_error(e.what());
+    }
+}
+
+template<int DOF>
+HumanoidRobot<DOF>::HumanoidRobot(const cmvr::config::EthercatRobotConfig& config) {
+    try {
+        id_ = config.id();
+        urdf_path_ = config.urdf();
+        dof_ = DOF;
+        if (!pathExists(config.urdf())) {
+            throw runtime_error("urdf file does not exist");
+        }
+        auto rcfg = cmvr::dyn::LoadRobotFromURDF(
+            config.urdf(), config.baselink());
+        m_robot_ = std::make_shared<cmvr::dyn::Robot<DOF> >(rcfg);
+
+        // 解析jointNames字符串，按逗号分隔
+        joint_names_ = splitString(config.jointnames(), ",");
+        // 解析linkNames字符串，按逗号分隔
+        link_names_ = splitString(config.linknames(), ",");
+
+        if (joint_names_.size() != dof_) {
+            throw runtime_error("joint names size mismatched with dof");
+        }
+
+        m_state_ = m_robot_->MakeState(link_names_, joint_names_);
+        m_cctrl_ = make_shared<ctrl::CartesianController<DOF> >(m_robot_);
+        upd_freq_ = config.updfreq();
+        CSP_buffer_ = make_shared<SPMCRingBuffer<JointPoint> >(config.buffersize());
+        CSV_buffer_ = make_shared<SPMCRingBuffer<JointVelocityCommand> >(config.buffersize());
+        CSC_buffer_ = make_shared<SPMCRingBuffer<JointCurrentCommand> >(config.buffersize());
+
+        upd_freq_ = 1000;
+        upd_timer_ = make_shared<FDTimer>();
+        upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000),
+                          [this] { update_state_(); });
+        rsm_.store(ROBOT_READY);
+    } catch (exception &e) {
+        LOG(ERROR) << "HumanoidRobot init failed, id=" << id_;
+        throw runtime_error(e.what());
+    }
+}
+
 template<int DOF>
 void HumanoidRobot<DOF>::init() {
     motor_manager_ = MotorsInfo::getInstance()->getMotorManager();

cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp

@@ -990,3 +990,21 @@ TEST(HumanoidRobotTest,IBVSWithRealRobotTrackedPoint) {
 
     EXPECT_GT(ok_steps, 0) << "No successful tracked-point IBVS control steps.";
 }
+
+
+TEST(HumanoidRobotTest, EYouTest) {
+    auto& dmgr = DeviceManager::getInstance();
+    auto robot_abs = dmgr.getDevice<AbstractRobot>("ec_robot_01");
+    auto robot = std::dynamic_pointer_cast<HumanoidRobot<14>>(robot_abs);
+    ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>";
+
+    std::vector<JointPoint> init_cmd = {
+        {"L_SHOULDER_P", 0.1}
+    };
+    // robot->moveJ(init_cmd, 0.1, 1.0);
+
+    // robot->calibrateZeroQ("L_SHOULDER_P");
+
+    robot->torqueOff("L_SHOULDER_P");
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+}

cmvr-es/main.cpp

@@ -9,6 +9,7 @@
 #include <glog/logging.h>
 #include "service/grpc/include/server_runner.h"
 #include "utils/base/include/logger.h"
+#include "common/utils/config_helper/include/config_helper.h"
 using namespace cmvr::service;
 int main(int argc, char* argv[]) {
     std::string config_path;

cmvr-es/monitor/abstract_monitor.h

@@ -13,6 +13,7 @@
 namespace cmvr::monitor{
     class AbstractMonitor {
     public:
+        AbstractMonitor() = default;
         explicit AbstractMonitor(const XmlNode &cfg){
             cfg_ = cfg;
             freq_ = cfg_.getAttrDefault("freq", 1);

cmvr-es/monitor/diskmonitor/include/disk_monitor.h

@@ -8,17 +8,21 @@
 #include <condition_variable>
 
 #include "../../abstract_monitor.h"
+#include "cmvr/config/monitor_config/monitor_config.pb.h"
 
 namespace cmvr::monitor {
 
     class DiskMonitor final : public AbstractMonitor{
     public:
         explicit DiskMonitor(const XmlNode& config);
+        explicit DiskMonitor(const cmvr::config::DiskMonitorConfig& config);  // 新增proto构造函数
         ~DiskMonitor();
     protected:
         bool check_() override;
         bool execution_() override;
     private:
+        void initFromProto(const cmvr::config::DiskMonitorConfig& config);  // 新增初始化方法
+
         std::unordered_map<std::string,int> folders_;
         std::vector<std::string> dirsToClean_;          // 需要清理的目录列表
         mutable std::mutex mutex_;                      // 保护共享资源的互斥锁
@@ -27,6 +31,4 @@ namespace cmvr::monitor {
     };
 }
 
-
-
 #endif //FOLDER_MONITOR_H

cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp

@@ -14,7 +14,6 @@ using namespace cmvr::monitor;
 namespace fs = std::filesystem;
 using namespace std::chrono;
 
-
 // 文件信息结构体
 struct FileInfo {
     std::string path;
@@ -38,51 +37,40 @@ bool deleteOldestFile(const std::string& directoryPath) {
     struct dirent* entry;
     struct stat fileStat;
 
-    // 遍历目录中的每个条目
     while ((entry = readdir(dir)) != nullptr) {
-        // 跳过 "." 和 ".."
         if (std::string(entry->d_name) == "." || std::string(entry->d_name) == "..") {
             continue;
         }
 
-        // 构建完整路径
         std::string filePath = directoryPath + "/" + entry->d_name;
 
-        // 获取文件状态信息
         if (stat(filePath.c_str(), &fileStat) == -1) {
             std::cerr << "Failed to get file status: " << filePath << std::endl;
             continue;
         }
 
-        // 只处理普通文件
         if (S_ISREG(fileStat.st_mode)) {
             FileInfo fileInfo;
             fileInfo.path = filePath;
-
-            // 优先使用 st_birthtime (如果可用)，否则使用 st_ctime
             #ifdef HAVE_STRUCT_STAT_ST_BIRTHTIME
                 fileInfo.creationTime = fileStat.st_birthtime;
             #else
                 fileInfo.creationTime = fileStat.st_ctime;
             #endif
-
             files.push_back(fileInfo);
         }
     }
 
     closedir(dir);
 
-    // 如果没有找到文件，返回失败
     if (files.empty()) {
         std::cout << "No files found in directory: " << directoryPath << std::endl;
         return false;
     }
 
-    // 按创建时间排序
     std::sort(files.begin(), files.end(), compareByCreationTime);
-
-    // 删除最早创建的文件
     std::string oldestFilePath = files[0].path;
+
     if (unlink(oldestFilePath.c_str()) == -1) {
         std::cerr << "Failed to delete file: " << oldestFilePath << std::endl;
         return false;
@@ -92,11 +80,11 @@ bool deleteOldestFile(const std::string& directoryPath) {
     return true;
 }
 
-
 bool pathExists(const std::string& path) {
     struct stat buffer;
     return (stat(path.c_str(), &buffer) == 0);
 }
+
 int countFilesInDirectory(const std::string& path) {
     DIR* dir = opendir(path.c_str());
     if (!dir) {
@@ -107,9 +95,7 @@ int countFilesInDirectory(const std::string& path) {
     int count = 0;
     struct dirent* entry;
 
-    // 遍历目录中的每个条目
     while ((entry = readdir(dir)) != nullptr) {
-        // 跳过 "." 和 ".."
         if (std::string(entry->d_name) != "." && std::string(entry->d_name) != "..") {
             count++;
         }
@@ -119,8 +105,8 @@ int countFilesInDirectory(const std::string& path) {
     return count;
 }
 
+// XML构造函数
 DiskMonitor::DiskMonitor(const XmlNode& config) : AbstractMonitor(config){
-
     try {
         for (auto &node: config.getChildren()){
             string fileDir = node.getAttrString("fileDir");
@@ -138,13 +124,42 @@ DiskMonitor::DiskMonitor(const XmlNode& config) : AbstractMonitor(config){
         throw runtime_error("[DiskMonitor] (DiskMonitor): Failed "+ string(e.what()));
     }
 }
+
+// Proto构造函数
+DiskMonitor::DiskMonitor(const cmvr::config::DiskMonitorConfig& config) : AbstractMonitor() {
+    try {
+        initFromProto(config);
+        is_initialized_ = true;
+        LOG(INFO) << "[DiskMonitor]: Init DiskMonitor from proto Success";
+    }
+    catch (const exception& e) {
+        LOG(ERROR) << "[DiskMonitor] (DiskMonitor): Failed " << e.what();
+        throw runtime_error("[DiskMonitor] (DiskMonitor): Failed " + string(e.what()));
+    }
+}
+
+// Proto初始化方法
+void DiskMonitor::initFromProto(const cmvr::config::DiskMonitorConfig& config) {
+    // 设置监控ID和频率（通过AbstractMonitor）
+    // 假设AbstractMonitor有setId和setFreq方法，如果没有需要添加
+    // 这里根据你的AbstractMonitor实现来调整
+
+    for (const auto& folder : config.folders()) {
+        string fileDir = folder.filedir();
+        if (!pathExists(fileDir)) {
+            throw runtime_error("[DiskMonitor] (initFromProto): path not exists: " + fileDir);
+        }
+        int maxVolume = folder.maxvolume();
+        folders_[fileDir] = maxVolume;
+    }
+}
+
 DiskMonitor::~DiskMonitor() {
     folders_.clear();
 }
+
 bool DiskMonitor::check_() {
-    // 检查当前目录下的文件数量是否超过最大值
     bool allOk = true;
-    // 遍历所有监控的文件夹
     for (const auto& folder : folders_) {
         const std::string& path = folder.first;
         size_t maxFiles = folder.second;
@@ -154,28 +169,21 @@ bool DiskMonitor::check_() {
                 continue;
             }
             int nFileCount = countFilesInDirectory(path);
-            if (nFileCount > maxFiles)
+            if (nFileCount > maxFiles) {
-            {
                 allOk = false;
                 dirsToClean_.push_back(path);
                 std::cout << "path need remove oldest file: " << path << std::endl;
             }
-
-
         } catch (const fs::filesystem_error& e) {
             std::cerr << "Filesystem error: " << e.what() << '\n';
         } catch (const std::exception& e) {
             std::cerr << "Unexpected error: " << e.what() << '\n';
         }
     }
-
     return allOk;
 }
 
-
 bool DiskMonitor::execution_() {
-
-    // 删除创建时间最早的文件
     std::lock_guard<std::mutex> lock(mutex_);
     bool success = true;
 
@@ -183,6 +191,6 @@ bool DiskMonitor::execution_() {
         deleteOldestFile(path);
     }
 
-    dirsToClean_.clear(); // 清理完成后清空列表
+    dirsToClean_.clear();
     return success;
 }

cmvr-es/monitor_manager/include/monitor_factory.h

@@ -5,8 +5,8 @@
 #ifndef MONITOR_FACTORY_H
 #define MONITOR_FACTORY_H
 
-
+#include "monitor/abstract_monitor.h"
-#include "../../monitor/abstract_monitor.h"
+#include "cmvr/config/monitor_config/monitor_config.pb.h"
 
 namespace cmvr::monitor
 {
@@ -14,14 +14,17 @@ namespace cmvr::monitor
     {
     public:
         MonitorFactory() = default;
+
+        // 原有的XML方式
         template <typename MonitorType>
         std::shared_ptr<MonitorType> create(const XmlNode& cfg);
         std::shared_ptr<AbstractMonitor> create(const XmlNode& cfg);
+
+        // 新增proto方式
+        template <typename MonitorType>
+        std::shared_ptr<MonitorType> create(const cmvr::config::DiskMonitorConfig& config);
+        std::shared_ptr<AbstractMonitor> create(const cmvr::config::DiskMonitorConfig& config);
     };
-
-
 }
 
-
-
 #endif //MONITOR_FACTORY_H

cmvr-es/monitor_manager/include/monitor_manager.h

@@ -15,23 +15,38 @@ namespace cmvr::monitor {
     public:
         MonitorManager(const MonitorManager&) = delete;
         MonitorManager& operator=(const MonitorManager&) = delete;
+
+        // 原有的XML方式
         static MonitorManager& getInstance(const XmlNode &cfg);
+
+        // 新增proto方式
+        static MonitorManager& getInstance(const cmvr::config::MonitorManagerConfig& config);
+
         static MonitorManager& getInstance();
         static void destroyInstance();
 
     private:
-        explicit MonitorManager (const XmlNode &cfg);
+        // 原有的XML方式
-        void init_monitors();
+        explicit MonitorManager(const XmlNode &cfg);
+
+        // 新增proto方式
+        explicit MonitorManager(const cmvr::config::MonitorManagerConfig& config);
+
+        void init_monitors();           // XML方式
+        void init_monitors_from_proto(); // Proto方式
+
     private:
         static std::once_flag init_flag_;
         static std::shared_ptr<MonitorManager> instance_;
 
-        XmlNode cfg_;
+        // 两种配置方式，只有一个有效
+        bool use_proto_ = false;
+        XmlNode cfg_;                                    // XML配置
+        cmvr::config::MonitorManagerConfig proto_cfg_;   // Proto配置
+
         std::shared_ptr<MonitorFactory> monitor_factory_;
         std::unordered_map<std::string, std::shared_ptr<AbstractMonitor>> monitors_;
     };
 }
 
-
-
 #endif //MONITOR_MANAGER_H

cmvr-es/monitor_manager/src/monitor_factory.cpp

@@ -3,12 +3,12 @@
 //
 
 #include "../include/monitor_factory.h"
-
 #include "../../monitor/diskmonitor/include/disk_monitor.h"
 
 using namespace std;
 using namespace cmvr::monitor;
 
+// XML方式的模板特化
 template std::shared_ptr<DiskMonitor> MonitorFactory::create<DiskMonitor>(const XmlNode&);
 
 template <typename MonitorType>
@@ -17,8 +17,30 @@ std::shared_ptr<MonitorType> MonitorFactory::create(const XmlNode& cfg) {
         return std::make_shared<DiskMonitor>(cfg);
     }
     else {
-        LOG(ERROR) << "[DeviceFactory]: Unsupported device type";
+        LOG(ERROR) << "[MonitorFactory]: Unsupported monitor type";
-        throw runtime_error("[DeviceFactory]: Unsupported device type");
+        throw runtime_error("[MonitorFactory]: Unsupported monitor type");
     }
 }
 
+// Proto方式的模板特化
+template std::shared_ptr<DiskMonitor> MonitorFactory::create<DiskMonitor>(const cmvr::config::DiskMonitorConfig&);
+
+template <typename MonitorType>
+std::shared_ptr<MonitorType> MonitorFactory::create(const cmvr::config::DiskMonitorConfig& config) {
+    if constexpr (std::is_same_v<MonitorType, DiskMonitor>) {
+        return std::make_shared<DiskMonitor>(config);
+    }
+    else {
+        LOG(ERROR) << "[MonitorFactory]: Unsupported monitor type";
+        throw runtime_error("[MonitorFactory]: Unsupported monitor type");
+    }
+}
+
+std::shared_ptr<AbstractMonitor> MonitorFactory::create(const XmlNode& cfg) {
+    // 保留原有实现
+    return nullptr;
+}
+
+std::shared_ptr<AbstractMonitor> MonitorFactory::create(const cmvr::config::DiskMonitorConfig& config) {
+    return std::make_shared<DiskMonitor>(config);
+}

cmvr-es/monitor_manager/src/monitor_manager.cpp

@@ -3,8 +3,7 @@
 //
 
 #include "../include/monitor_manager.h"
-
+#include "monitor/diskmonitor/include/disk_monitor.h"
-#include "../../monitor/diskmonitor/include/disk_monitor.h"
 
 using namespace std;
 using namespace cmvr::monitor;
@@ -12,11 +11,19 @@ using namespace cmvr::monitor;
 std::shared_ptr<MonitorManager> MonitorManager::instance_ = nullptr;
 std::once_flag MonitorManager::init_flag_;
 
-MonitorManager::MonitorManager(const XmlNode& cfg) {
+// XML方式构造函数
+MonitorManager::MonitorManager(const XmlNode& cfg) : use_proto_(false), cfg_(cfg) {
     monitor_factory_ = make_shared<MonitorFactory>();
-    cfg_ = cfg;
     init_monitors();
 }
+
+// Proto方式构造函数
+MonitorManager::MonitorManager(const cmvr::config::MonitorManagerConfig& config) : use_proto_(true), proto_cfg_(config) {
+    monitor_factory_ = make_shared<MonitorFactory>();
+    init_monitors_from_proto();
+}
+
+// XML方式单例
 MonitorManager& MonitorManager::getInstance(const XmlNode& cfg) {
     std::call_once(init_flag_, [&cfg] {
         instance_.reset(new MonitorManager(cfg));
@@ -24,9 +31,17 @@ MonitorManager& MonitorManager::getInstance(const XmlNode& cfg) {
     return *instance_;
 }
 
+// Proto方式单例
+MonitorManager& MonitorManager::getInstance(const cmvr::config::MonitorManagerConfig& config) {
+    std::call_once(init_flag_, [&config] {
+        instance_.reset(new MonitorManager(config));
+    });
+    return *instance_;
+}
+
 MonitorManager& MonitorManager::getInstance() {
     if (!instance_) {
-        throw std::runtime_error("[MonitorManager] (getInstance): MonitorManager not initialized. Call getInstance(const XmlNode&) first.");
+        throw std::runtime_error("[MonitorManager] (getInstance): MonitorManager not initialized. Call getInstance() first.");
     }
     return *instance_;
 }
@@ -35,13 +50,14 @@ void MonitorManager::destroyInstance() {
     instance_.reset();
 }
 
+// XML方式初始化
 void MonitorManager::init_monitors() {
-    try{
+    try {
         auto diskmonitor_node = cfg_.getChild("DiskMonitor");
         auto id = diskmonitor_node.getAttrString("id");
         auto monitor = monitor_factory_->create<DiskMonitor>(diskmonitor_node);
         if (monitor == nullptr) {
-            throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id);
+            throw std::runtime_error("[MonitorManager]: Factory returned nullptr for monitor ID: " + id);
         }
         monitors_[id] = monitor;
     }
@@ -49,5 +65,31 @@ void MonitorManager::init_monitors() {
         LOG(ERROR) << e.what();
         throw runtime_error(e.what());
     }
+}
 
+// Proto方式初始化
+void MonitorManager::init_monitors_from_proto() {
+    try {
+        if (!proto_cfg_.has_disk_monitor()) {
+            LOG(WARNING) << "[MonitorManager]: No disk_monitor config found";
+            return;
+        }
+
+        const auto& disk_monitor_cfg = proto_cfg_.disk_monitor();
+        if (!disk_monitor_cfg.enable()) {
+            LOG(INFO) << "[MonitorManager]: Disk monitor is disabled";
+            return;
+        }
+
+        auto id = disk_monitor_cfg.id();
+        auto monitor = monitor_factory_->create<DiskMonitor>(disk_monitor_cfg);
+        if (monitor == nullptr) {
+            throw std::runtime_error("[MonitorManager]: Factory returned nullptr for monitor ID: " + id);
+        }
+        monitors_[id] = monitor;
+    }
+    catch (const exception& e) {
+        LOG(ERROR) << e.what();
+        throw runtime_error(e.what());
+    }
 }

cmvr-es/service/grpc/include/server_runner.h

@@ -11,13 +11,14 @@
 #include <thread>
 #include <grpcpp/grpcpp.h>
 #include "rapidxml/xml_parser.h"
+#include "cmvr/config/server_config/server_config.pb.h"
 namespace cmvr::service {
 
 
     class ServerRunner {
     public:
         ServerRunner();
-        explicit ServerRunner(XmlNode cfg);
+        ServerRunner(const cmvr::config::ServerConfig& config);
         ~ServerRunner();
 
         ServerRunner(const ServerRunner&) = delete;
@@ -25,7 +26,7 @@ namespace cmvr::service {
         ServerRunner(ServerRunner&&) = delete;
         ServerRunner& operator=(ServerRunner&&) = delete;
 
-        void start(XmlNode cfg);
+        void start();
         void stop();
         void join();
 
@@ -40,7 +41,7 @@ namespace cmvr::service {
         std::condition_variable cv_;
         std::thread worker_;
 
-        XmlNode cfg_;
+        cmvr::config::ServerConfig cfg_;
         std::unique_ptr<grpc::Server> server_;
 
         std::string address_;

cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp

@@ -17,7 +17,7 @@ using google::protobuf::util::TimeUtil;
 gRPCHumanoidRobotServiceImpl::gRPCHumanoidRobotServiceImpl():dmgr_(DeviceManager::getInstance())
 {
 
-    pinocchio_ik_ = std::make_shared<PinocchioQpIKSolver>("/home/linbo/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
+    pinocchio_ik_ = std::make_shared<PinocchioQpIKSolver>("/home/cmvr/Projects/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
         ,"PELVIS_S"
         ,"R_WRIST_R_S");
     pinocchio_ik_->init();

cmvr-es/service/grpc/src/server_runner.cpp

@@ -25,23 +25,21 @@
 using namespace  cmvr::service;
 ServerRunner::ServerRunner() = default;
 
-ServerRunner::ServerRunner(XmlNode cfg) {
+ServerRunner::ServerRunner(const cmvr::config::ServerConfig& config):cfg_(config)
-    start(std::move(cfg));
+{
+    // start();
 }
-
 ServerRunner::~ServerRunner() {
     stop();
     join();
 }
 
-void ServerRunner::start(XmlNode cfg) {
+void ServerRunner::start() {
     std::unique_lock<std::mutex> lk(mtx_);
 
     if (worker_.joinable()) {
         throw std::runtime_error("ServerRunner already started");
     }
-
-    cfg_ = std::move(cfg);
     stop_requested_ = false;
     started_ = false;
     start_failed_ = false;
@@ -97,25 +95,15 @@ void ServerRunner::threadMain() {
     using namespace cmvr::monitor;
 
     try {
-        if (!cfg_.hasChild("DeviceManager")) {
+
-            throw std::runtime_error("Device Manager node not found");
-        }
 
         static std::once_flag reflection_once;
         std::call_once(reflection_once, []() {
             grpc::reflection::InitProtoReflectionServerBuilderPlugin();
         });
+        DeviceManager::getInstance();
 
-        auto dmgr_cfg = cfg_.getChild("DeviceManager");
+        std::string port = cfg_.grpc_server().port();
-        DeviceManager::getInstance(dmgr_cfg);
-
-        if (cfg_.hasChild("MonitorManager")) {
-            auto mmgr_cfg = cfg_.getChild("MonitorManager");
-            MonitorManager::getInstance(mmgr_cfg);
-        }
-
-        auto grpc_cfg = cfg_.getChild("gRPCServer");
-        std::string port = grpc_cfg.getAttrDefault("port", "50051");
         std::string local_address = "0.0.0.0:" + port;
 
         auto camera_service = std::make_unique<gRPCCameraServiceImpl>();

dependency/x86/third_party/soem/include/soem/ec_base.h

@@ -0,0 +1,42 @@
+/*
+ * This software is dual-licensed under GPLv3 and a commercial
+ * license. See the file LICENSE.md distributed with this software for
+ * full license information.
+ */
+
+/** \file
+ * \brief
+ * Headerfile for ec_base.c
+ */
+
+#ifndef _ec_base_
+#define _ec_base_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int ecx_setupdatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data);
+uint16 ecx_adddatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data);
+int ecx_BWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
+int ecx_BRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
+int ecx_APRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
+int ecx_ARMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
+int ecx_FRMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
+uint16 ecx_APRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout);
+int ecx_FPRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
+uint16 ecx_FPRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout);
+int ecx_APWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout);
+int ecx_APWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
+int ecx_FPWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout);
+int ecx_FPWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
+int ecx_LRW(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout);
+int ecx_LRD(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout);
+int ecx_LWR(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout);
+int ecx_LRWDC(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dependency/x86/third_party/soem/include/soem/ec_coe.h

@@ -0,0 +1,73 @@
+/*
+ * This software is dual-licensed under GPLv3 and a commercial
+ * license. See the file LICENSE.md distributed with this software for
+ * full license information.
+ */
+
+/** \file
+ * \brief
+ * Headerfile for ec_coe.c
+ */
+
+#ifndef _ec_coe_
+#define _ec_coe_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Storage for object description list */
+typedef struct
+{
+   /** slave number */
+   uint16 Slave;
+   /** number of entries in list */
+   uint16 Entries;
+   /** array of indexes */
+   uint16 Index[EC_MAXODLIST];
+   /** array of datatypes, see EtherCAT specification */
+   uint16 DataType[EC_MAXODLIST];
+   /** array of object codes, see EtherCAT specification */
+   uint8 ObjectCode[EC_MAXODLIST];
+   /** number of subindexes for each index */
+   uint8 MaxSub[EC_MAXODLIST];
+   /** textual description of each index */
+   char Name[EC_MAXODLIST][EC_MAXNAME + 1];
+} ec_ODlistt;
+
+/* storage for object list entry information */
+typedef struct
+{
+   /** number of entries in list */
+   uint16 Entries;
+   /** array of value infos, see EtherCAT specification */
+   uint8 ValueInfo[EC_MAXOELIST];
+   /** array of value infos, see EtherCAT specification */
+   uint16 DataType[EC_MAXOELIST];
+   /** array of bit lengths, see EtherCAT specification */
+   uint16 BitLength[EC_MAXOELIST];
+   /** array of object access bits, see EtherCAT specification */
+   uint16 ObjAccess[EC_MAXOELIST];
+   /** textual description of each index */
+   char Name[EC_MAXOELIST][EC_MAXNAME + 1];
+} ec_OElistt;
+
+void ecx_SDOerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode);
+int ecx_SDOread(ecx_contextt *context, uint16 slave, uint16 index, uint8 subindex,
+                boolean CA, int *psize, void *p, int timeout);
+int ecx_SDOwrite(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIndex,
+                 boolean CA, int psize, const void *p, int Timeout);
+int ecx_RxPDO(ecx_contextt *context, uint16 Slave, uint16 RxPDOnumber, int psize, const void *p);
+int ecx_TxPDO(ecx_contextt *context, uint16 slave, uint16 TxPDOnumber, int *psize, void *p, int timeout);
+int ecx_readPDOmap(ecx_contextt *context, uint16 Slave, uint32 *Osize, uint32 *Isize);
+int ecx_readPDOmapCA(ecx_contextt *context, uint16 Slave, int Thread_n, uint32 *Osize, uint32 *Isize);
+int ecx_readODlist(ecx_contextt *context, uint16 Slave, ec_ODlistt *pODlist);
+int ecx_readODdescription(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist);
+int ecx_readOEsingle(ecx_contextt *context, uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist);
+int ecx_readOE(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dependency/x86/third_party/soem/include/soem/ec_config.h

@@ -0,0 +1,31 @@
+/*
+ * This software is dual-licensed under GPLv3 and a commercial
+ * license. See the file LICENSE.md distributed with this software for
+ * full license information.
+ */
+
+/** \file
+ * \brief
+ * Headerfile for ec_config.c
+ */
+
+#ifndef _ec_config_
+#define _ec_config_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define EC_NODEOFFSET 0x1000
+#define EC_TEMPNODE   0xffff
+
+int ecx_config_init(ecx_contextt *context);
+int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group);
+int ecx_recover_slave(ecx_contextt *context, uint16 slave, int timeout);
+int ecx_reconfig_slave(ecx_contextt *context, uint16 slave, int timeout);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dependency/x86/third_party/soem/include/soem/ec_dc.h

@@ -0,0 +1,27 @@
+/*
+ * This software is dual-licensed under GPLv3 and a commercial
+ * license. See the file LICENSE.md distributed with this software for
+ * full license information.
+ */
+
+/** \file
+ * \brief
+ * Headerfile for ec_dc.c
+ */
+
+#ifndef _EC_ECATDC_H
+#define _EC_ECATDC_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+boolean ecx_configdc(ecx_contextt *context);
+void ecx_dcsync0(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift);
+void ecx_dcsync01(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _EC_ECATDC_H */

dependency/x86/third_party/soem/include/soem/ec_eoe.h

@@ -0,0 +1,213 @@
+/*
+ * This software is dual-licensed under GPLv3 and a commercial
+ * license. See the file LICENSE.md distributed with this software for
+ * full license information.
+ */
+
+/** \file
+ * \brief
+ * Headerfile for ec_eoe.c
+ */
+
+#ifndef _ec_eoe_
+#define _ec_eoe_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "soem/soem.h"
+
+/* use maximum size for EOE mailbox data - mbxheader and 2x frameinfo */
+#define EC_MAXEOEDATA (EC_MAXMBX - (sizeof(ec_mbxheadert) + \
+                                    sizeof(uint16_t) +      \
+                                    sizeof(uint16_t)))
+
+/** DNS length according to ETG 1000.6 */
+#define EOE_DNS_NAME_LENGTH     32
+/** Ethernet address length not including VLAN */
+#define EOE_ETHADDR_LENGTH      6
+/** IPv4 address length */
+#define EOE_IP4_LENGTH          sizeof(uint32_t)
+
+#define EOE_MAKEU32(a, b, c, d) (((uint32_t)((a) & 0xff) << 24) | \
+                                 ((uint32_t)((b) & 0xff) << 16) | \
+                                 ((uint32_t)((c) & 0xff) << 8) |  \
+                                 (uint32_t)((d) & 0xff))
+
+#if !defined(EC_BIG_ENDIAN) && defined(EC_LITTLE_ENDIAN)
+
+#define EOE_HTONS(x) ((((x) & 0x00ffUL) << 8) | (((x) & 0xff00UL) >> 8))
+#define EOE_NTOHS(x) EOE_HTONS(x)
+#define EOE_HTONL(x) ((((x) & 0x000000ffUL) << 24) | \
+                      (((x) & 0x0000ff00UL) << 8) |  \
+                      (((x) & 0x00ff0000UL) >> 8) |  \
+                      (((x) & 0xff000000UL) >> 24))
+#define EOE_NTOHL(x) EOE_HTONL(x)
+#else
+#define EOE_HTONS(x) (x)
+#define EOE_NTOHS(x) (x)
+#define EOE_HTONL(x) (x)
+#define EOE_NTOHL(x) (x)
+#endif /* !defined(EC_BIG_ENDIAN) && defined(EC_LITTLE_ENDIAN) */
+
+/** Get one byte from the 4-byte address */
+#define eoe_ip4_addr1(ipaddr) (((const uint8_t *)(&(ipaddr)->addr))[0])
+#define eoe_ip4_addr2(ipaddr) (((const uint8_t *)(&(ipaddr)->addr))[1])
+#define eoe_ip4_addr3(ipaddr) (((const uint8_t *)(&(ipaddr)->addr))[2])
+#define eoe_ip4_addr4(ipaddr) (((const uint8_t *)(&(ipaddr)->addr))[3])
+
+/** Set an IP address given by the four byte-parts */
+#define EOE_IP4_ADDR_TO_U32(ipaddr, a, b, c, d) \
+   (ipaddr)->addr = EOE_HTONL(EOE_MAKEU32(a, b, c, d))
+
+/** Header frame info 1 */
+#define EOE_HDR_FRAME_TYPE_OFFSET         0
+#define EOE_HDR_FRAME_TYPE                (0xF << 0)
+#define EOE_HDR_FRAME_TYPE_SET(x)         (((x) & 0xF) << 0)
+#define EOE_HDR_FRAME_TYPE_GET(x)         (((x) >> 0) & 0xF)
+#define EOE_HDR_FRAME_PORT_OFFSET         4
+#define EOE_HDR_FRAME_PORT                (0xF << 4)
+#define EOE_HDR_FRAME_PORT_SET(x)         ((uint16)(((x) & 0xF) << 4))
+#define EOE_HDR_FRAME_PORT_GET(x)         (((x) >> 4) & 0xF)
+#define EOE_HDR_LAST_FRAGMENT_OFFSET      8
+#define EOE_HDR_LAST_FRAGMENT             (0x1 << 8)
+#define EOE_HDR_LAST_FRAGMENT_SET(x)      (((x) & 0x1) << 8)
+#define EOE_HDR_LAST_FRAGMENT_GET(x)      (((x) >> 8) & 0x1)
+#define EOE_HDR_TIME_APPEND_OFFSET        9
+#define EOE_HDR_TIME_APPEND               (0x1 << 9)
+#define EOE_HDR_TIME_APPEND_SET(x)        (((x) & 0x1) << 9)
+#define EOE_HDR_TIME_APPEND_GET(x)        (((x) >> 9) & 0x1)
+#define EOE_HDR_TIME_REQUEST_OFFSET       10
+#define EOE_HDR_TIME_REQUEST              (0x1 << 10)
+#define EOE_HDR_TIME_REQUEST_SET(x)       (((x) & 0x1) << 10)
+#define EOE_HDR_TIME_REQUEST_GET(x)       (((x) >> 10) & 0x1)
+
+/** Header frame info 2 */
+#define EOE_HDR_FRAG_NO_OFFSET            0
+#define EOE_HDR_FRAG_NO                   (0x3F << 0)
+#define EOE_HDR_FRAG_NO_SET(x)            ((uint16)(((x) & 0x3F) << 0))
+#define EOE_HDR_FRAG_NO_GET(x)            (((x) >> 0) & 0x3F)
+#define EOE_HDR_FRAME_OFFSET_OFFSET       6
+#define EOE_HDR_FRAME_OFFSET              (0x3F << 6)
+#define EOE_HDR_FRAME_OFFSET_SET(x)       ((uint16)(((x) & 0x3F) << 6))
+#define EOE_HDR_FRAME_OFFSET_GET(x)       (((x) >> 6) & 0x3F)
+#define EOE_HDR_FRAME_NO_OFFSET           12
+#define EOE_HDR_FRAME_NO                  (0xF << 12)
+#define EOE_HDR_FRAME_NO_SET(x)           ((uint16)(((x) & 0xF) << 12))
+#define EOE_HDR_FRAME_NO_GET(x)           (((x) >> 12) & 0xF)
+
+/** EOE param */
+#define EOE_PARAM_OFFSET                  4
+#define EOE_PARAM_MAC_INCLUDE             (0x1 << 0)
+#define EOE_PARAM_IP_INCLUDE              (0x1 << 1)
+#define EOE_PARAM_SUBNET_IP_INCLUDE       (0x1 << 2)
+#define EOE_PARAM_DEFAULT_GATEWAY_INCLUDE (0x1 << 3)
+#define EOE_PARAM_DNS_IP_INCLUDE          (0x1 << 4)
+#define EOE_PARAM_DNS_NAME_INCLUDE        (0x1 << 5)
+
+/** EoE frame types */
+#define EOE_FRAG_DATA                     0
+#define EOE_INIT_RESP_TIMESTAMP           1
+#define EOE_INIT_REQ                      2 /* Spec SET IP REQ */
+#define EOE_INIT_RESP                     3 /* Spec SET IP RESP */
+#define EOE_SET_ADDR_FILTER_REQ           4
+#define EOE_SET_ADDR_FILTER_RESP          5
+#define EOE_GET_IP_PARAM_REQ              6
+#define EOE_GET_IP_PARAM_RESP             7
+#define EOE_GET_ADDR_FILTER_REQ           8
+#define EOE_GET_ADDR_FILTER_RESP          9
+
+/** EoE parameter result codes */
+#define EOE_RESULT_SUCCESS                0x0000
+#define EOE_RESULT_UNSPECIFIED_ERROR      0x0001
+#define EOE_RESULT_UNSUPPORTED_FRAME_TYPE 0x0002
+#define EOE_RESULT_NO_IP_SUPPORT          0x0201
+#define EOE_RESULT_NO_DHCP_SUPPORT        0x0202
+#define EOE_RESULT_NO_FILTER_SUPPORT      0x0401
+
+/** EOE ip4 address in network order */
+typedef struct eoe_ip4_addr
+{
+   uint32_t addr;
+} eoe_ip4_addr_t;
+
+/** EOE ethernet address */
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED eoe_ethaddr
+{
+   uint8_t addr[EOE_ETHADDR_LENGTH];
+} eoe_ethaddr_t;
+OSAL_PACKED_END
+
+/** EoE IP request structure, storage only, no need to pack */
+typedef struct eoe_param
+{
+   uint8_t mac_set : 1;
+   uint8_t ip_set : 1;
+   uint8_t subnet_set : 1;
+   uint8_t default_gateway_set : 1;
+   uint8_t dns_ip_set : 1;
+   uint8_t dns_name_set : 1;
+   eoe_ethaddr_t mac;
+   eoe_ip4_addr_t ip;
+   eoe_ip4_addr_t subnet;
+   eoe_ip4_addr_t default_gateway;
+   eoe_ip4_addr_t dns_ip;
+   char dns_name[EOE_DNS_NAME_LENGTH];
+} eoe_param_t;
+
+/** EOE structure.
+ * Used to interpret EoE mailbox packets.
+ */
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED
+{
+   ec_mbxheadert mbxheader;
+   uint16_t frameinfo1;
+   union
+   {
+      uint16_t frameinfo2;
+      uint16_t result;
+   };
+   uint8 data[EC_MAXEOEDATA];
+} ec_EOEt;
+OSAL_PACKED_END
+
+int ecx_EOEdefinehook(ecx_contextt *context, void *hook);
+int ecx_EOEsetIp(ecx_contextt *context,
+                 uint16 slave,
+                 uint8 port,
+                 eoe_param_t *ipparam,
+                 int timeout);
+int ecx_EOEgetIp(ecx_contextt *context,
+                 uint16 slave,
+                 uint8 port,
+                 eoe_param_t *ipparam,
+                 int timeout);
+int ecx_EOEsend(ecx_contextt *context,
+                uint16 slave,
+                uint8 port,
+                int psize,
+                void *p,
+                int timeout);
+int ecx_EOErecv(ecx_contextt *context,
+                uint16 slave,
+                uint8 port,
+                int *psize,
+                void *p,
+                int timeout);
+int ecx_EOEreadfragment(
+    ec_mbxbuft *MbxIn,
+    uint8 *rxfragmentno,
+    uint16 *rxframesize,
+    uint16 *rxframeoffset,
+    uint16 *rxframeno,
+    int *psize,
+    void *p);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dependency/x86/third_party/soem/include/soem/ec_foe.h

@@ -0,0 +1,27 @@
+/*
+ * This software is dual-licensed under GPLv3 and a commercial
+ * license. See the file LICENSE.md distributed with this software for
+ * full license information.
+ */
+
+/** \file
+ * \brief
+ * Headerfile for ec_foe.c
+ */
+
+#ifndef _ec_foe_
+#define _ec_foe_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int ecx_FOEdefinehook(ecx_contextt *context, void *hook);
+int ecx_FOEread(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout);
+int ecx_FOEwrite(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dependency/x86/third_party/soem/include/soem/ec_main.h

@@ -0,0 +1,624 @@
+/*
+ * This software is dual-licensed under GPLv3 and a commercial
+ * license. See the file LICENSE.md distributed with this software for
+ * full license information.
+ */
+
+/** \file
+ * \brief
+ * Headerfile for ec_main.c
+ */
+
+#ifndef _ec_main_
+#define _ec_main_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "soem/ec_options.h"
+
+typedef struct ec_adapter ec_adaptert;
+struct ec_adapter
+{
+   char name[EC_MAXLEN_ADAPTERNAME];
+   char desc[EC_MAXLEN_ADAPTERNAME];
+   ec_adaptert *next;
+};
+
+/** record for FMMU */
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED ec_fmmu
+{
+   uint32 LogStart;
+   uint16 LogLength;
+   uint8 LogStartbit;
+   uint8 LogEndbit;
+   uint16 PhysStart;
+   uint8 PhysStartBit;
+   uint8 FMMUtype;
+   uint8 FMMUactive;
+   uint8 unused1;
+   uint16 unused2;
+} ec_fmmut;
+OSAL_PACKED_END
+
+/** record for sync manager */
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED ec_sm
+{
+   uint16 StartAddr;
+   uint16 SMlength;
+   uint32 SMflags;
+} ec_smt;
+OSAL_PACKED_END
+
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED ec_state_status
+{
+   uint16 State;
+   uint16 Unused;
+   uint16 ALstatuscode;
+} ec_state_status;
+OSAL_PACKED_END
+
+/** mailbox buffer array */
+typedef uint8 ec_mbxbuft[EC_MAXMBX + 1];
+
+#define EC_MBXINENABLE (uint8 *)1
+
+typedef struct
+{
+   int listhead, listtail, listcount;
+   int mbxemptylist[EC_MBXPOOLSIZE];
+   osal_mutext *mbxmutex;
+   ec_mbxbuft mbx[EC_MBXPOOLSIZE];
+} ec_mbxpoolt;
+
+#define EC_MBXQUEUESTATE_NONE 0
+#define EC_MBXQUEUESTATE_REQ  1
+#define EC_MBXQUEUESTATE_FAIL 2
+#define EC_MBXQUEUESTATE_DONE 3
+
+typedef struct
+{
+   int listhead, listtail, listcount;
+   ec_mbxbuft *mbx[EC_MBXPOOLSIZE];
+   int mbxstate[EC_MBXPOOLSIZE];
+   int mbxremove[EC_MBXPOOLSIZE];
+   int mbxticket[EC_MBXPOOLSIZE];
+   uint16 mbxslave[EC_MBXPOOLSIZE];
+   osal_mutext *mbxmutex;
+} ec_mbxqueuet;
+
+#define ECT_MBXPROT_AOE      0x0001
+#define ECT_MBXPROT_EOE      0x0002
+#define ECT_MBXPROT_COE      0x0004
+#define ECT_MBXPROT_FOE      0x0008
+#define ECT_MBXPROT_SOE      0x0010
+#define ECT_MBXPROT_VOE      0x0020
+
+#define ECT_COEDET_SDO       0x01
+#define ECT_COEDET_SDOINFO   0x02
+#define ECT_COEDET_PDOASSIGN 0x04
+#define ECT_COEDET_PDOCONFIG 0x08
+#define ECT_COEDET_UPLOAD    0x10
+#define ECT_COEDET_SDOCA     0x20
+
+#define EC_SMENABLEMASK      0xfffeffff
+
+typedef struct ecx_context ecx_contextt;
+
+#define ECT_MBXH_NONE   0
+#define ECT_MBXH_CYCLIC 1
+#define ECT_MBXH_LOST   2
+
+/** Slave state
+ * All slave information is put in this structure. Needed for most
+ * user interaction with slaves.
+ */
+typedef struct ec_slave
+{
+   /** state of slave */
+   uint16 state;
+   /** AL status code */
+   uint16 ALstatuscode;
+   /** Configured address */
+   uint16 configadr;
+   /** Alias address */
+   uint16 aliasadr;
+   /** Manufacturer from EEprom */
+   uint32 eep_man;
+   /** ID from EEprom */
+   uint32 eep_id;
+   /** revision from EEprom */
+   uint32 eep_rev;
+   /** serial number from EEprom */
+   uint32 eep_ser;
+   /** Interface type */
+   uint16 Itype;
+   /** Device type */
+   uint16 Dtype;
+   /** output bits */
+   uint16 Obits;
+   /** output bytes, if Obits < 8 then Obytes = 0 */
+   uint32 Obytes;
+   /** output pointer in IOmap buffer */
+   uint8 *outputs;
+   /** output offset in IOmap buffer */
+   uint32 Ooffset;
+   /** startbit in first output byte */
+   uint8 Ostartbit;
+   /** input bits */
+   uint16 Ibits;
+   /** input bytes, if Ibits < 8 then Ibytes = 0 */
+   uint32 Ibytes;
+   /** input pointer in IOmap buffer */
+   uint8 *inputs;
+   /** input offset in IOmap buffer */
+   uint32 Ioffset;
+   /** startbit in first input byte */
+   uint8 Istartbit;
+   /** SM structure */
+   ec_smt SM[EC_MAXSM];
+   /** SM type 0=unused 1=MbxWr 2=MbxRd 3=Outputs 4=Inputs */
+   uint8 SMtype[EC_MAXSM];
+   /** FMMU structure */
+   ec_fmmut FMMU[EC_MAXFMMU];
+   /** FMMU0 function 0=unused 1=outputs 2=inputs 3=SM status*/
+   uint8 FMMU0func;
+   /** FMMU1 function */
+   uint8 FMMU1func;
+   /** FMMU2 function */
+   uint8 FMMU2func;
+   /** FMMU3 function */
+   uint8 FMMU3func;
+   /** length of write mailbox in bytes, if no mailbox then 0 */
+   uint16 mbx_l;
+   /** mailbox write offset */
+   uint16 mbx_wo;
+   /** length of read mailbox in bytes */
+   uint16 mbx_rl;
+   /** mailbox read offset */
+   uint16 mbx_ro;
+   /** mailbox supported protocols */
+   uint16 mbx_proto;
+   /** Counter value of mailbox link layer protocol 1..7 */
+   uint8 mbx_cnt;
+   /** has DC capability */
+   boolean hasdc;
+   /** Physical type; Ebus, EtherNet combinations */
+   uint8 ptype;
+   /** topology: 1 to 3 links */
+   uint8 topology;
+   /** active ports bitmap : ....3210 , set if respective port is active **/
+   uint8 activeports;
+   /** consumed ports bitmap : ....3210, used for internal delay measurement **/
+   uint8 consumedports;
+   /** slave number for parent, 0=master */
+   uint16 parent;
+   /** port number on parent this slave is connected to **/
+   uint8 parentport;
+   /** port number on this slave the parent is connected to **/
+   uint8 entryport;
+   /** DC receivetimes on port A */
+   int32 DCrtA;
+   /** DC receivetimes on port B */
+   int32 DCrtB;
+   /** DC receivetimes on port C */
+   int32 DCrtC;
+   /** DC receivetimes on port D */
+   int32 DCrtD;
+   /** propagation delay */
+   int32 pdelay;
+   /** next DC slave */
+   uint16 DCnext;
+   /** previous DC slave */
+   uint16 DCprevious;
+   /** DC cycle time in ns */
+   int32 DCcycle;
+   /** DC shift from clock modulus boundary */
+   int32 DCshift;
+   /** DC sync activation, 0=off, 1=on */
+   uint8 DCactive;
+   /** link to SII config */
+   uint16 SIIindex;
+   /** 1 = 8 bytes per read, 0 = 4 bytes per read */
+   uint8 eep_8byte;
+   /** 0 = eeprom to master , 1 = eeprom to PDI */
+   uint8 eep_pdi;
+   /** CoE details */
+   uint8 CoEdetails;
+   /** FoE details */
+   uint8 FoEdetails;
+   /** EoE details */
+   uint8 EoEdetails;
+   /** SoE details */
+   uint8 SoEdetails;
+   /** E-bus current */
+   int16 Ebuscurrent;
+   /** if >0 block use of LRW in processdata */
+   uint8 blockLRW;
+   /** group */
+   uint8 group;
+   /** first unused FMMU */
+   uint8 FMMUunused;
+   /** Boolean for tracking whether the slave is (not) responding, not used/set by the SOEM library */
+   boolean islost;
+   /** registered configuration function PO->SO */
+   int (*PO2SOconfig)(ecx_contextt *context, uint16 slave);
+   /** mailbox handler state, 0 = no handler, 1 = cyclic task mbx handler, 2 = slave lost */
+   int mbxhandlerstate;
+   /** mailbox handler robust mailbox protocol state */
+   int mbxrmpstate;
+   /** mailbox handler RMP extended mbx in state */
+   uint16 mbxinstateex;
+   /** pointer to CoE mailbox in buffer */
+   uint8 *coembxin;
+   /** CoE mailbox in flag, true = mailbox full */
+   boolean coembxinfull;
+   /** CoE mailbox in overrun counter */
+   int coembxoverrun;
+   /** pointer to SoE mailbox in buffer */
+   uint8 *soembxin;
+   /** SoE mailbox in flag, true = mailbox full */
+   boolean soembxinfull;
+   /** SoE mailbox in overrun counter */
+   int soembxoverrun;
+   /** pointer to FoE mailbox in buffer */
+   uint8 *foembxin;
+   /** FoE mailbox in flag, true = mailbox full */
+   boolean foembxinfull;
+   /** FoE mailbox in overrun counter */
+   int foembxoverrun;
+   /** pointer to EoE mailbox in buffer */
+   uint8 *eoembxin;
+   /** EoE mailbox in flag, true = mailbox full */
+   boolean eoembxinfull;
+   /** EoE mailbox in overrun counter */
+   int eoembxoverrun;
+   /** pointer to VoE mailbox in buffer */
+   uint8 *voembxin;
+   /** VoE mailbox in flag, true = mailbox full */
+   boolean voembxinfull;
+   /** VoE mailbox in overrun counter */
+   int voembxoverrun;
+   /** pointer to AoE mailbox in buffer */
+   uint8 *aoembxin;
+   /** AoE mailbox in flag, true = mailbox full */
+   boolean aoembxinfull;
+   /** AoE mailbox in overrun counter */
+   int aoembxoverrun;
+   /** pointer to out mailbox status register buffer */
+   uint8 *mbxstatus;
+   /** readable name */
+   char name[EC_MAXNAME + 1];
+} ec_slavet;
+
+/** for list of ethercat slave groups */
+typedef struct ec_group
+{
+   /** logical start address for this group */
+   uint32 logstartaddr;
+   /** output bytes, if Obits < 8 then Obytes = 0 */
+   uint32 Obytes;
+   /** output pointer in IOmap buffer */
+   uint8 *outputs;
+   /** input bytes, if Ibits < 8 then Ibytes = 0 */
+   uint32 Ibytes;
+   /** input pointer in IOmap buffer */
+   uint8 *inputs;
+   /** has DC capability */
+   boolean hasdc;
+   /** next DC slave */
+   uint16 DCnext;
+   /** E-bus current */
+   int16 Ebuscurrent;
+   /** if >0 block use of LRW in processdata */
+   uint8 blockLRW;
+   /** IO segments used */
+   uint16 nsegments;
+   /** 1st input segment */
+   uint16 Isegment;
+   /** Offset in input segment */
+   uint16 Ioffset;
+   /** Expected workcounter outputs */
+   uint16 outputsWKC;
+   /** Expected workcounter inputs */
+   uint16 inputsWKC;
+   /** check slave states */
+   boolean docheckstate;
+   /** IO segmentation list. Datagrams must not break SM in two. */
+   uint32 IOsegment[EC_MAXIOSEGMENTS];
+   /** pointer to out mailbox status register buffer */
+   uint8 *mbxstatus;
+   /** mailbox status register buffer length */
+   int32 mbxstatuslength;
+   /** mailbox status lookup table */
+   uint16 mbxstatuslookup[EC_MAXSLAVE];
+   /** mailbox last handled in mxbhandler */
+   uint16 lastmbxpos;
+   /** mailbox  transmit queue struct */
+   ec_mbxqueuet mbxtxqueue;
+} ec_groupt;
+
+#define ECT_ESMTRANS_IP 0x0001
+#define ECT_ESMTRANS_PS 0x0002
+#define ECT_ESMTRANS_PI 0x0004
+#define ECT_ESMTRANS_SP 0x0008
+#define ECT_ESMTRANS_SO 0x0010
+#define ECT_ESMTRANS_SI 0x0020
+#define ECT_ESMTRANS_OS 0x0040
+#define ECT_ESMTRANS_OP 0x0080
+#define ECT_ESMTRANS_OI 0x0100
+#define ECT_ESMTRANS_IB 0x0200
+#define ECT_ESMTRANS_BI 0x0400
+#define ECT_ESMTRANS_II 0x0800
+#define ECT_ESMTRANS_PP 0x1000
+#define ECT_ESMTRANS_SS 0x2000
+
+/** ENI CoE command structure */
+typedef struct ec_enicoecmd
+{
+   /** transition(s) during which command should be sent */
+   uint16 Transition;
+   /** complete access flag */
+   boolean CA;
+   /** ccs (1 = read, 2 = write) */
+   uint8 Ccs;
+   /** object index */
+   uint16 Index;
+   /** object subindex */
+   uint8 SubIdx;
+   /** timeout in us */
+   int Timeout;
+   /** size in bytes of parameter buffer */
+   int DataSize;
+   /** pointer to parameter buffer */
+   void *Data;
+} ec_enicoecmdt;
+
+/** ENI slave structure */
+typedef struct ec_enislave
+{
+   uint16 Slave;
+   uint32 VendorId;
+   uint32 ProductCode;
+   uint32 RevisionNo;
+   ec_enicoecmdt *CoECmds;
+   int CoECmdCount;
+} ec_enislavet;
+
+/** ENI structure */
+typedef struct ec_eni
+{
+   ec_enislavet *slave;
+   int slavecount;
+} ec_enit;
+
+/** SII FMMU structure */
+typedef struct ec_eepromFMMU
+{
+   uint16 Startpos;
+   uint8 nFMMU;
+   uint8 FMMU0;
+   uint8 FMMU1;
+   uint8 FMMU2;
+   uint8 FMMU3;
+} ec_eepromFMMUt;
+
+/** SII SM structure */
+typedef struct ec_eepromSM
+{
+   uint16 Startpos;
+   uint8 nSM;
+   uint16 PhStart;
+   uint16 Plength;
+   uint8 Creg;
+   uint8 Sreg; /* don't care */
+   uint8 Activate;
+   uint8 PDIctrl; /* don't care */
+} ec_eepromSMt;
+
+/** record to store rxPDO and txPDO table from eeprom */
+typedef struct ec_eepromPDO
+{
+   uint16 Startpos;
+   uint16 Length;
+   uint16 nPDO;
+   uint16 Index[EC_MAXEEPDO];
+   uint16 SyncM[EC_MAXEEPDO];
+   uint16 BitSize[EC_MAXEEPDO];
+   uint16 SMbitsize[EC_MAXSM];
+} ec_eepromPDOt;
+
+/** standard ethercat mailbox header */
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED ec_mbxheader
+{
+   uint16 length;
+   uint16 address;
+   uint8 priority;
+   uint8 mbxtype;
+} ec_mbxheadert;
+OSAL_PACKED_END
+
+/** ALstatus and ALstatus code */
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED ec_alstatus
+{
+   uint16 alstatus;
+   uint16 unused;
+   uint16 alstatuscode;
+} ec_alstatust;
+OSAL_PACKED_END
+
+/** stack structure to store segmented LRD/LWR/LRW constructs */
+typedef struct ec_idxstack
+{
+   uint8 pushed;
+   uint8 pulled;
+   uint8 idx[EC_MAXBUF];
+   void *data[EC_MAXBUF];
+   uint16 length[EC_MAXBUF];
+   uint16 dcoffset[EC_MAXBUF];
+   uint8 type[EC_MAXBUF];
+} ec_idxstackT;
+
+/** ringbuf for error storage */
+typedef struct ec_ering
+{
+   int16 head;
+   int16 tail;
+   ec_errort Error[EC_MAXELIST + 1];
+} ec_eringt;
+
+/** SyncManager Communication Type structure for CA */
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED ec_SMcommtype
+{
+   uint8 n;
+   uint8 nu1;
+   uint8 SMtype[EC_MAXSM];
+} ec_SMcommtypet;
+OSAL_PACKED_END
+
+/** SDO assign structure for CA */
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED ec_PDOassign
+{
+   uint8 n;
+   uint8 nu1;
+   uint16 index[256];
+} ec_PDOassignt;
+OSAL_PACKED_END
+
+/** SDO description structure for CA */
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED ec_PDOdesc
+{
+   uint8 n;
+   uint8 nu1;
+   uint32 PDO[256];
+} ec_PDOdesct;
+OSAL_PACKED_END
+
+/** Context structure, referenced by all ecx functions*/
+struct ecx_context
+{
+   /** @publicsection */
+   /* Network state */
+
+   /** port, may include red_port */
+   ecx_portt port;
+   /** list of detected slaves */
+   ec_slavet slavelist[EC_MAXSLAVE];
+   /** number of slaves found in configuration */
+   int slavecount;
+   /** list of groups */
+   ec_groupt grouplist[EC_MAXGROUP];
+   /** ecaterror state */
+   boolean ecaterror;
+   /** last DC time from slaves */
+   int64 DCtime;
+
+   /** @privatesection */
+   /* Internal state */
+
+   /** internal, eeprom cache buffer */
+   uint8 esibuf[EC_MAXEEPBUF];
+   /** internal, eeprom cache map */
+   uint32 esimap[EC_MAXEEPBITMAP];
+   /** internal, current slave for eeprom cache */
+   uint16 esislave;
+   /** internal, error list */
+   ec_eringt elist;
+   /** internal, processdata stack buffer info */
+   ec_idxstackT idxstack;
+   /** internal, SM buffer */
+   ec_SMcommtypet SMcommtype[EC_MAX_MAPT];
+   /** internal, PDO assign list */
+   ec_PDOassignt PDOassign[EC_MAX_MAPT];
+   /** internal, PDO description list */
+   ec_PDOdesct PDOdesc[EC_MAX_MAPT];
+   /** internal, SM list from eeprom */
+   ec_eepromSMt eepSM;
+   /** internal, FMMU list from eeprom */
+   ec_eepromFMMUt eepFMMU;
+   /** internal, mailbox pool */
+   ec_mbxpoolt mbxpool;
+
+   /** @publicsection */
+   /* Configurable settings */
+
+   /** network information hook */
+   ec_enit *ENI;
+   /** registered FoE hook */
+   int (*FOEhook)(uint16 slave, int packetnumber, int datasize);
+   /** registered EoE hook */
+   int (*EOEhook)(ecx_contextt *context, uint16 slave, void *eoembx);
+   /** flag to control legacy automatic state change or manual state change */
+   int manualstatechange;
+   /** opaque pointer to application userdata, never used by SOEM. */
+   void *userdata;
+   /** In overlapped mode, inputs will replace outputs in the incoming
+    * frame. Use this mode for TI ESC:s. Processdata is always aligned
+    * on a byte boundary. */
+   boolean overlappedMode;
+   /** Do not map each slave on a byte boundary. May result in smaller
+    * frame sizes. Has no effect in overlapped mode. */
+   boolean packedMode;
+};
+
+ec_adaptert *ec_find_adapters(void);
+void ec_free_adapters(ec_adaptert *adapter);
+uint8 ec_nextmbxcnt(uint8 cnt);
+void ec_clearmbx(ec_mbxbuft *Mbx);
+void ecx_pusherror(ecx_contextt *context, const ec_errort *Ec);
+boolean ecx_poperror(ecx_contextt *context, ec_errort *Ec);
+boolean ecx_iserror(ecx_contextt *context);
+void ecx_packeterror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, uint16 ErrorCode);
+int ecx_init(ecx_contextt *context, const char *ifname);
+int ecx_init_redundant(ecx_contextt *context, ecx_redportt *redport, const char *ifname, char *if2name);
+void ecx_close(ecx_contextt *context);
+uint8 ecx_siigetbyte(ecx_contextt *context, uint16 slave, uint16 address);
+int16 ecx_siifind(ecx_contextt *context, uint16 slave, uint16 cat);
+void ecx_siistring(ecx_contextt *context, char *str, uint16 slave, uint16 Sn);
+uint16 ecx_siiFMMU(ecx_contextt *context, uint16 slave, ec_eepromFMMUt *FMMU);
+uint16 ecx_siiSM(ecx_contextt *context, uint16 slave, ec_eepromSMt *SM);
+uint16 ecx_siiSMnext(ecx_contextt *context, uint16 slave, ec_eepromSMt *SM, uint16 n);
+uint32 ecx_siiPDO(ecx_contextt *context, uint16 slave, ec_eepromPDOt *PDO, uint8 t);
+int ecx_readstate(ecx_contextt *context);
+int ecx_writestate(ecx_contextt *context, uint16 slave);
+uint16 ecx_statecheck(ecx_contextt *context, uint16 slave, uint16 reqstate, int timeout);
+int ecx_mbxhandler(ecx_contextt *context, uint8 group, int limit);
+int ecx_mbxempty(ecx_contextt *context, uint16 slave, int timeout);
+int ecx_mbxsend(ecx_contextt *context, uint16 slave, ec_mbxbuft *mbx, int timeout);
+int ecx_mbxreceive(ecx_contextt *context, uint16 slave, ec_mbxbuft **mbx, int timeout);
+int ecx_mbxENIinitcmds(ecx_contextt *context, uint16 slave, uint16_t transition);
+void ecx_esidump(ecx_contextt *context, uint16 slave, uint8 *esibuf);
+uint32 ecx_readeeprom(ecx_contextt *context, uint16 slave, uint16 eeproma, int timeout);
+int ecx_writeeeprom(ecx_contextt *context, uint16 slave, uint16 eeproma, uint16 data, int timeout);
+int ecx_eeprom2master(ecx_contextt *context, uint16 slave);
+int ecx_eeprom2pdi(ecx_contextt *context, uint16 slave);
+uint64 ecx_readeepromAP(ecx_contextt *context, uint16 aiadr, uint16 eeproma, int timeout);
+int ecx_writeeepromAP(ecx_contextt *context, uint16 aiadr, uint16 eeproma, uint16 data, int timeout);
+uint64 ecx_readeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma, int timeout);
+int ecx_writeeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma, uint16 data, int timeout);
+void ecx_readeeprom1(ecx_contextt *context, uint16 slave, uint16 eeproma);
+uint32 ecx_readeeprom2(ecx_contextt *context, uint16 slave, int timeout);
+int ecx_receive_processdata_group(ecx_contextt *context, uint8 group, int timeout);
+int ecx_send_processdata(ecx_contextt *context);
+int ecx_receive_processdata(ecx_contextt *context, int timeout);
+int ecx_send_processdata_group(ecx_contextt *context, uint8 group);
+ec_mbxbuft *ecx_getmbx(ecx_contextt *context);
+int ecx_dropmbx(ecx_contextt *context, ec_mbxbuft *mbx);
+int ecx_initmbxpool(ecx_contextt *context);
+int ecx_initmbxqueue(ecx_contextt *context, uint8 group);
+int ecx_slavembxcyclic(ecx_contextt *context, uint16 slave);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dependency/x86/third_party/soem/include/soem/ec_options.h

@@ -0,0 +1,128 @@
+/*
+ * This software is dual-licensed under GPLv3 and a commercial
+ * license. See the file LICENSE.md distributed with this software for
+ * full license information.
+ */
+
+/** \file
+ * \brief
+ * Build options for SOEM
+ */
+
+#ifndef _ec_options_
+#define _ec_options_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Max sizes */
+
+/** standard frame buffer size in bytes */
+#define EC_BUFSIZE (EC_MAXECATFRAME)
+
+/** number of frame buffers per channel (tx, rx1 rx2) */
+#define EC_MAXBUF (16)
+
+/** size of EEPROM bitmap cache */
+#define EC_MAXEEPBITMAP (128)
+
+/** size of EEPROM cache buffer */
+#define EC_MAXEEPBUF (EC_MAXEEPBITMAP << 5)
+
+/** default group size in 2^x */
+#define EC_LOGGROUPOFFSET (16)
+
+/** max. entries in EtherCAT error list */
+#define EC_MAXELIST (64)
+
+/** max. length of readable name in slavelist and Object Description List */
+#define EC_MAXNAME (40)
+
+/** max. number of slaves in array */
+#define EC_MAXSLAVE (200)
+
+/** max. number of groups */
+#define EC_MAXGROUP (2)
+
+/** max. number of IO segments per group */
+#define EC_MAXIOSEGMENTS (64)
+
+/** max. mailbox size */
+#define EC_MAXMBX (1486)
+
+/** number of mailboxes in pool */
+#define EC_MBXPOOLSIZE (32)
+
+/** max. eeprom PDO entries */
+#define EC_MAXEEPDO (0x200)
+
+/** max. SM used */
+#define EC_MAXSM (8)
+
+/** max. FMMU used */
+#define EC_MAXFMMU (4)
+
+/** max. adapter name length */
+#define EC_MAXLEN_ADAPTERNAME (128)
+
+/** define maximum number of concurrent threads in mapping */
+#define EC_MAX_MAPT (1)
+
+/** max entries in Object Description list */
+#define EC_MAXODLIST (1024)
+
+/** max entries in Object Entry list */
+#define EC_MAXOELIST (256)
+
+/** max. length of readable SoE name */
+#define EC_SOE_MAXNAME (60)
+
+/** max. number of SoE mappings */
+#define EC_SOE_MAXMAPPING (64)
+
+/* Timeouts and retries */
+
+/** timeout value in us for tx frame to return to rx */
+#define EC_TIMEOUTRET (2000)
+
+/** timeout value in us for safe data transfer, max. triple retry */
+#define EC_TIMEOUTRET3 (EC_TIMEOUTRET * 3)
+
+/** timeout value in us for return "safe" variant (f.e. wireless) */
+#define EC_TIMEOUTSAFE (20000)
+
+/** timeout value in us for EEPROM access */
+#define EC_TIMEOUTEEP (20000)
+
+/** timeout value in us for tx mailbox cycle */
+#define EC_TIMEOUTTXM (20000)
+
+/** timeout value in us for rx mailbox cycle */
+#define EC_TIMEOUTRXM (700000)
+
+/** timeout value in us for check statechange */
+#define EC_TIMEOUTSTATE (2000000)
+
+/** default number of retries if wkc <= 0 */
+#define EC_DEFAULTRETRIES (3)
+
+/* MAC addresses */
+
+/** Primary source MAC address used for EtherCAT.
+ *
+ * This address is not the MAC address used from the NIC.  EtherCAT
+ * does not care about MAC addressing, but it is used here to
+ * differentiate the route the packet traverses through the EtherCAT
+ * segment. This is needed to find out the packet flow in redundant
+ * configurations. */
+#define EC_PRIMARY_MAC_ARRAY {0x0101, 0x0101, 0x0101}
+
+/** Secondary source MAC address used for EtherCAT. */
+#define EC_SECONDARY_MAC_ARRAY {0x0404, 0x0404, 0x0404}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dependency/x86/third_party/soem/include/soem/ec_print.h

@@ -0,0 +1,30 @@
+/*
+ * This software is dual-licensed under GPLv3 and a commercial
+ * license. See the file LICENSE.md distributed with this software for
+ * full license information.
+ */
+
+/** \file
+ * \brief
+ * Headerfile for ec_print.c
+ */
+
+#ifndef _ec_print_
+#define _ec_print_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+const char *ec_sdoerror2string(uint32 sdoerrorcode);
+char *ec_ALstatuscode2string(uint16 ALstatuscode);
+char *ec_soeerror2string(uint16 errorcode);
+char *ec_mbxerror2string(uint16 errorcode);
+char *ecx_err2string(const ec_errort Ec);
+char *ecx_elist2string(ecx_contextt *context);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dependency/x86/third_party/soem/include/soem/ec_soe.h

@@ -0,0 +1,121 @@
+/*
+ * This software is dual-licensed under GPLv3 and a commercial
+ * license. See the file LICENSE.md distributed with this software for
+ * full license information.
+ */
+
+/** \file
+ * \brief
+ * Headerfile for ec_soe.c
+ */
+
+#ifndef _ec_soe_
+#define _ec_soe_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define EC_SOE_DATASTATE_B 0x01
+#define EC_SOE_NAME_B      0x02
+#define EC_SOE_ATTRIBUTE_B 0x04
+#define EC_SOE_UNIT_B      0x08
+#define EC_SOE_MIN_B       0x10
+#define EC_SOE_MAX_B       0x20
+#define EC_SOE_VALUE_B     0x40
+#define EC_SOE_DEFAULT_B   0x80
+
+#define EC_IDN_MDTCONFIG   24
+#define EC_IDN_ATCONFIG    16
+
+/** SoE name structure */
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED
+{
+   /** current length in bytes of list */
+   uint16 currentlength;
+   /** maximum length in bytes of list */
+   uint16 maxlength;
+   char name[EC_SOE_MAXNAME];
+} ec_SoEnamet;
+OSAL_PACKED_END
+
+/** SoE list structure */
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED
+{
+   /** current length in bytes of list */
+   uint16 currentlength;
+   /** maximum length in bytes of list */
+   uint16 maxlength;
+   union
+   {
+      uint8 byte[8];
+      uint16 word[4];
+      uint32 dword[2];
+      uint64 lword[1];
+   };
+} ec_SoElistt;
+OSAL_PACKED_END
+
+/** SoE IDN mapping structure */
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED
+{
+   /** current length in bytes of list */
+   uint16 currentlength;
+   /** maximum length in bytes of list */
+   uint16 maxlength;
+   uint16 idn[EC_SOE_MAXMAPPING];
+} ec_SoEmappingt;
+OSAL_PACKED_END
+
+#define EC_SOE_LENGTH_1       0x00
+#define EC_SOE_LENGTH_2       0x01
+#define EC_SOE_LENGTH_4       0x02
+#define EC_SOE_LENGTH_8       0x03
+#define EC_SOE_TYPE_BINARY    0x00
+#define EC_SOE_TYPE_UINT      0x01
+#define EC_SOE_TYPE_INT       0x02
+#define EC_SOE_TYPE_HEX       0x03
+#define EC_SOE_TYPE_STRING    0x04
+#define EC_SOE_TYPE_IDN       0x05
+#define EC_SOE_TYPE_FLOAT     0x06
+#define EC_SOE_TYPE_PARAMETER 0x07
+
+/** SoE attribute structure */
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED
+{
+   /** evaluation factor for display purposes */
+   uint32 evafactor : 16;
+   /** length of IDN element(s) */
+   uint32 length : 2;
+   /** IDN is list */
+   uint32 list : 1;
+   /** IDN is command */
+   uint32 command : 1;
+   /** datatype */
+   uint32 datatype : 3;
+   uint32 reserved1 : 1;
+   /** decimals to display if float datatype */
+   uint32 decimals : 4;
+   /** write protected in pre-op */
+   uint32 wppreop : 1;
+   /** write protected in safe-op */
+   uint32 wpsafeop : 1;
+   /** write protected in op */
+   uint32 wpop : 1;
+   uint32 reserved2 : 1;
+} ec_SoEattributet;
+OSAL_PACKED_END
+
+int ecx_SoEread(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout);
+int ecx_SoEwrite(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout);
+int ecx_readIDNmap(ecx_contextt *context, uint16 slave, uint32 *Osize, uint32 *Isize);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dependency/x86/third_party/soem/include/soem/ec_type.h

@@ -0,0 +1,551 @@
+/*
+ * This software is dual-licensed under GPLv3 and a commercial
+ * license. See the file LICENSE.md distributed with this software for
+ * full license information.
+ */
+
+/** \file
+ * \brief
+ * General typedefs and defines for EtherCAT.
+ *
+ * Defines that could need optimisation for specific applications
+ * are the EC_TIMEOUTxxx. Assumptions for the standard settings are a
+ * standard linux PC or laptop and a wired connection to maximal 100 slaves.
+ * For use with wireless connections or lots of slaves the timeouts need
+ * increasing. For fast systems running Xenomai and RT-net or alike the
+ * timeouts need to be shorter.
+ */
+
+#ifndef _EC_TYPE_H
+#define _EC_TYPE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "osal.h"
+#include "soem/ec_options.h"
+
+/* Endianness */
+
+/** Define little endian target by default if no endian is set */
+#if !defined(EC_LITTLE_ENDIAN) && !defined(EC_BIG_ENDIAN)
+#define EC_LITTLE_ENDIAN
+#endif
+
+/* Error codes */
+
+/** return value no frame returned */
+#define EC_NOFRAME            -1
+/** return value unknown frame received */
+#define EC_OTHERFRAME         -2
+/** return value general error */
+#define EC_ERROR              -3
+/** return value too many slaves */
+#define EC_SLAVECOUNTEXCEEDED -4
+/** return value request timeout */
+#define EC_TIMEOUT            -5
+
+/* Constants */
+
+/** maximum EtherCAT frame length in bytes */
+#define EC_MAXECATFRAME       1518
+/** size of DC datagram used in first LRW frame */
+#define EC_FIRSTDCDATAGRAM    20
+/** datagram type EtherCAT */
+#define EC_ECATTYPE           0x1000
+/** maximum EtherCAT LRW frame length in bytes */
+/* MTU - Ethernet header - length - datagram header - WCK - FCS */
+#define EC_MAXLRWDATA         (EC_MAXECATFRAME - 14 - 2 - 10 - 2 - 4)
+
+/** definition for frame buffers */
+typedef uint8 ec_bufT[EC_BUFSIZE];
+
+/** ethernet header definition */
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED
+{
+   /** destination MAC */
+   uint16 da0, da1, da2;
+   /** source MAC */
+   uint16 sa0, sa1, sa2;
+   /** ethernet type */
+   uint16 etype;
+} ec_etherheadert;
+OSAL_PACKED_END
+
+/** ethernet header size */
+#define ETH_HEADERSIZE sizeof(ec_etherheadert)
+
+/** EtherCAT datagram header definition */
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED
+{
+   /** length of EtherCAT datagram */
+   uint16 elength;
+   /** EtherCAT command, see ec_cmdtype */
+   uint8 command;
+   /** index, used in SOEM for Tx to Rx recombination */
+   uint8 index;
+   /** ADP */
+   uint16 ADP;
+   /** ADO */
+   uint16 ADO;
+   /** length of data portion in datagram */
+   uint16 dlength;
+   /** interrupt, currently unused */
+   uint16 irpt;
+} ec_comt;
+OSAL_PACKED_END
+
+/** EtherCAT header size */
+#define EC_HEADERSIZE      sizeof(ec_comt)
+/** size of ec_comt.elength item in EtherCAT header */
+#define EC_ELENGTHSIZE     sizeof(uint16)
+/** offset position of command in EtherCAT header */
+#define EC_CMDOFFSET       EC_ELENGTHSIZE
+/** size of workcounter item in EtherCAT datagram */
+#define EC_WKCSIZE         sizeof(uint16)
+/** definition of datagram follows bit in ec_comt.dlength */
+#define EC_DATAGRAMFOLLOWS (1 << 15)
+
+/** Possible error codes returned. */
+typedef enum
+{
+   /** No error */
+   EC_ERR_OK = 0,
+   /** Library already initialized. */
+   EC_ERR_ALREADY_INITIALIZED,
+   /** Library not initialized. */
+   EC_ERR_NOT_INITIALIZED,
+   /** Timeout occurred during execution of the function. */
+   EC_ERR_TIMEOUT,
+   /** No slaves were found. */
+   EC_ERR_NO_SLAVES,
+   /** Function failed. */
+   EC_ERR_NOK
+} ec_err;
+
+/** Possible EtherCAT slave states */
+typedef enum
+{
+   /** No valid state. */
+   EC_STATE_NONE = 0x00,
+   /** Init state*/
+   EC_STATE_INIT = 0x01,
+   /** Pre-operational. */
+   EC_STATE_PRE_OP = 0x02,
+   /** Boot state*/
+   EC_STATE_BOOT = 0x03,
+   /** Safe-operational. */
+   EC_STATE_SAFE_OP = 0x04,
+   /** Operational */
+   EC_STATE_OPERATIONAL = 0x08,
+   /** Error or ACK error */
+   EC_STATE_ACK = 0x10,
+   EC_STATE_ERROR = 0x10
+} ec_state;
+
+/** Possible buffer states */
+typedef enum
+{
+   /** Empty */
+   EC_BUF_EMPTY = 0x00,
+   /** Allocated, but not filled */
+   EC_BUF_ALLOC = 0x01,
+   /** Transmitted */
+   EC_BUF_TX = 0x02,
+   /** Received, but not consumed */
+   EC_BUF_RCVD = 0x03,
+   /** Cycle completed */
+   EC_BUF_COMPLETE = 0x04
+} ec_bufstate;
+
+/** Ethercat data types */
+typedef enum
+{
+   ECT_BOOLEAN = 0x0001,
+   ECT_INTEGER8 = 0x0002,
+   ECT_INTEGER16 = 0x0003,
+   ECT_INTEGER32 = 0x0004,
+   ECT_UNSIGNED8 = 0x0005,
+   ECT_UNSIGNED16 = 0x0006,
+   ECT_UNSIGNED32 = 0x0007,
+   ECT_REAL32 = 0x0008,
+   ECT_VISIBLE_STRING = 0x0009,
+   ECT_OCTET_STRING = 0x000A,
+   ECT_UNICODE_STRING = 0x000B,
+   ECT_TIME_OF_DAY = 0x000C,
+   ECT_TIME_DIFFERENCE = 0x000D,
+   ECT_DOMAIN = 0x000F,
+   ECT_INTEGER24 = 0x0010,
+   ECT_REAL64 = 0x0011,
+   ECT_INTEGER64 = 0x0015,
+   ECT_UNSIGNED24 = 0x0016,
+   ECT_UNSIGNED64 = 0x001B,
+   ECT_BIT1 = 0x0030,
+   ECT_BIT2 = 0x0031,
+   ECT_BIT3 = 0x0032,
+   ECT_BIT4 = 0x0033,
+   ECT_BIT5 = 0x0034,
+   ECT_BIT6 = 0x0035,
+   ECT_BIT7 = 0x0036,
+   ECT_BIT8 = 0x0037
+} ec_datatype;
+
+/** Ethercat command types */
+typedef enum
+{
+   /** No operation */
+   EC_CMD_NOP = 0x00,
+   /** Auto Increment Read */
+   EC_CMD_APRD,
+   /** Auto Increment Write */
+   EC_CMD_APWR,
+   /** Auto Increment Read Write */
+   EC_CMD_APRW,
+   /** Configured Address Read */
+   EC_CMD_FPRD,
+   /** Configured Address Write */
+   EC_CMD_FPWR,
+   /** Configured Address Read Write */
+   EC_CMD_FPRW,
+   /** Broadcast Read */
+   EC_CMD_BRD,
+   /** Broadcast Write */
+   EC_CMD_BWR,
+   /** Broadcast Read Write */
+   EC_CMD_BRW,
+   /** Logical Memory Read */
+   EC_CMD_LRD,
+   /** Logical Memory Write */
+   EC_CMD_LWR,
+   /** Logical Memory Read Write */
+   EC_CMD_LRW,
+   /** Auto Increment Read Multiple Write */
+   EC_CMD_ARMW,
+   /** Configured Read Multiple Write */
+   EC_CMD_FRMW
+   /** Reserved */
+} ec_cmdtype;
+
+/** Ethercat EEprom command types */
+typedef enum
+{
+   /** No operation */
+   EC_ECMD_NOP = 0x0000,
+   /** Read */
+   EC_ECMD_READ = 0x0100,
+   /** Write */
+   EC_ECMD_WRITE = 0x0201,
+   /** Reload */
+   EC_ECMD_RELOAD = 0x0300
+} ec_ecmdtype;
+
+/** EEprom state machine read size */
+#define EC_ESTAT_R64   0x0040
+/** EEprom state machine busy flag */
+#define EC_ESTAT_BUSY  0x8000
+/** EEprom state machine error flag mask */
+#define EC_ESTAT_EMASK 0x7800
+/** EEprom state machine error acknowledge */
+#define EC_ESTAT_NACK  0x2000
+
+/* Ethercat SSI (Slave Information Interface) */
+
+/** Start address SII sections in Eeprom */
+#define ECT_SII_START  0x0040
+
+enum
+{
+   /** SII category strings */
+   ECT_SII_STRING = 10,
+   /** SII category general */
+   ECT_SII_GENERAL = 30,
+   /** SII category FMMU */
+   ECT_SII_FMMU = 40,
+   /** SII category SM */
+   ECT_SII_SM = 41,
+   /** SII category PDO */
+   ECT_SII_PDO = 50
+};
+
+/** Item offsets in SII general section */
+enum
+{
+   ECT_SII_MANUF = 0x0008,
+   ECT_SII_ID = 0x000a,
+   ECT_SII_REV = 0x000c,
+   ECT_SII_SER = 0x000e,
+   ECT_SII_BOOTRXMBX = 0x0014,
+   ECT_SII_BOOTTXMBX = 0x0016,
+   ECT_SII_MBXSIZE = 0x0019,
+   ECT_SII_TXMBXADR = 0x001a,
+   ECT_SII_RXMBXADR = 0x0018,
+   ECT_SII_MBXPROTO = 0x001c
+};
+
+/** Mailbox types definitions */
+enum
+{
+   /** Error mailbox type */
+   ECT_MBXT_ERR = 0x00,
+   /** ADS over EtherCAT mailbox type */
+   ECT_MBXT_AOE,
+   /** Ethernet over EtherCAT mailbox type */
+   ECT_MBXT_EOE,
+   /** CANopen over EtherCAT mailbox type */
+   ECT_MBXT_COE,
+   /** File over EtherCAT mailbox type */
+   ECT_MBXT_FOE,
+   /** Servo over EtherCAT mailbox type */
+   ECT_MBXT_SOE,
+   /** Vendor over EtherCAT mailbox type */
+   ECT_MBXT_VOE = 0x0f
+};
+
+/** CoE mailbox types */
+enum
+{
+   ECT_COES_EMERGENCY = 0x01,
+   ECT_COES_SDOREQ,
+   ECT_COES_SDORES,
+   ECT_COES_TXPDO,
+   ECT_COES_RXPDO,
+   ECT_COES_TXPDO_RR,
+   ECT_COES_RXPDO_RR,
+   ECT_COES_SDOINFO
+};
+
+/** CoE SDO commands */
+enum
+{
+   ECT_SDO_DOWN_INIT = 0x21,
+   ECT_SDO_DOWN_EXP = 0x23,
+   ECT_SDO_DOWN_INIT_CA = 0x31,
+   ECT_SDO_UP_REQ = 0x40,
+   ECT_SDO_UP_REQ_CA = 0x50,
+   ECT_SDO_SEG_UP_REQ = 0x60,
+   ECT_SDO_ABORT = 0x80
+};
+
+/** CoE Object Description commands */
+enum
+{
+   ECT_GET_ODLIST_REQ = 0x01,
+   ECT_GET_ODLIST_RES = 0x02,
+   ECT_GET_OD_REQ = 0x03,
+   ECT_GET_OD_RES = 0x04,
+   ECT_GET_OE_REQ = 0x05,
+   ECT_GET_OE_RES = 0x06,
+   ECT_SDOINFO_ERROR = 0x07
+};
+
+/** FoE opcodes */
+enum
+{
+   ECT_FOE_READ = 0x01,
+   ECT_FOE_WRITE,
+   ECT_FOE_DATA,
+   ECT_FOE_ACK,
+   ECT_FOE_ERROR,
+   ECT_FOE_BUSY
+};
+
+/** SoE opcodes */
+enum
+{
+   ECT_SOE_READREQ = 0x01,
+   ECT_SOE_READRES,
+   ECT_SOE_WRITEREQ,
+   ECT_SOE_WRITERES,
+   ECT_SOE_NOTIFICATION,
+   ECT_SOE_EMERGENCY
+};
+
+/** Ethercat registers */
+enum
+{
+   ECT_REG_TYPE = 0x0000,
+   ECT_REG_PORTDES = 0x0007,
+   ECT_REG_ESCSUP = 0x0008,
+   ECT_REG_STADR = 0x0010,
+   ECT_REG_ALIAS = 0x0012,
+   ECT_REG_DLCTL = 0x0100,
+   ECT_REG_DLPORT = 0x0101,
+   ECT_REG_DLALIAS = 0x0103,
+   ECT_REG_DLSTAT = 0x0110,
+   ECT_REG_ALCTL = 0x0120,
+   ECT_REG_ALSTAT = 0x0130,
+   ECT_REG_ALSTATCODE = 0x0134,
+   ECT_REG_PDICTL = 0x0140,
+   ECT_REG_IRQMASK = 0x0200,
+   ECT_REG_RXERR = 0x0300,
+   ECT_REG_FRXERR = 0x0308,
+   ECT_REG_EPUECNT = 0x030C,
+   ECT_REG_PECNT = 0x030D,
+   ECT_REG_PECODE = 0x030E,
+   ECT_REG_LLCNT = 0x0310,
+   ECT_REG_WDCNT = 0x0442,
+   ECT_REG_EEPCFG = 0x0500,
+   ECT_REG_EEPCTL = 0x0502,
+   ECT_REG_EEPSTAT = 0x0502,
+   ECT_REG_EEPADR = 0x0504,
+   ECT_REG_EEPDAT = 0x0508,
+   ECT_REG_FMMU0 = 0x0600,
+   ECT_REG_FMMU1 = ECT_REG_FMMU0 + 0x10,
+   ECT_REG_FMMU2 = ECT_REG_FMMU1 + 0x10,
+   ECT_REG_FMMU3 = ECT_REG_FMMU2 + 0x10,
+   ECT_REG_SM0 = 0x0800,
+   ECT_REG_SM1 = ECT_REG_SM0 + 0x08,
+   ECT_REG_SM2 = ECT_REG_SM1 + 0x08,
+   ECT_REG_SM3 = ECT_REG_SM2 + 0x08,
+   ECT_REG_SM0STAT = ECT_REG_SM0 + 0x05,
+   ECT_REG_SM1STAT = ECT_REG_SM1 + 0x05,
+   ECT_REG_SM1ACT = ECT_REG_SM1 + 0x06,
+   ECT_REG_SM1CONTR = ECT_REG_SM1 + 0x07,
+   ECT_REG_DCTIME0 = 0x0900,
+   ECT_REG_DCTIME1 = 0x0904,
+   ECT_REG_DCTIME2 = 0x0908,
+   ECT_REG_DCTIME3 = 0x090C,
+   ECT_REG_DCSYSTIME = 0x0910,
+   ECT_REG_DCSOF = 0x0918,
+   ECT_REG_DCSYSOFFSET = 0x0920,
+   ECT_REG_DCSYSDELAY = 0x0928,
+   ECT_REG_DCSYSDIFF = 0x092C,
+   ECT_REG_DCSPEEDCNT = 0x0930,
+   ECT_REG_DCTIMEFILT = 0x0934,
+   ECT_REG_DCCUC = 0x0980,
+   ECT_REG_DCSYNCACT = 0x0981,
+   ECT_REG_DCSTART0 = 0x0990,
+   ECT_REG_DCCYCLE0 = 0x09A0,
+   ECT_REG_DCCYCLE1 = 0x09A4
+};
+
+/** standard SDO Sync Manager Communication Type */
+#define ECT_SDO_SMCOMMTYPE  0x1c00
+/** standard SDO PDO assignment */
+#define ECT_SDO_PDOASSIGN   0x1c10
+/** standard SDO RxPDO assignment */
+#define ECT_SDO_RXPDOASSIGN 0x1c12
+/** standard SDO TxPDO assignment */
+#define ECT_SDO_TXPDOASSIGN 0x1c13
+
+/** Ethercat packet type */
+#define ETH_P_ECAT          0x88A4
+
+/** Error types */
+typedef enum
+{
+   EC_ERR_TYPE_SDO_ERROR = 0,
+   EC_ERR_TYPE_EMERGENCY = 1,
+   EC_ERR_TYPE_PACKET_ERROR = 3,
+   EC_ERR_TYPE_SDOINFO_ERROR = 4,
+   EC_ERR_TYPE_FOE_ERROR = 5,
+   EC_ERR_TYPE_FOE_BUF2SMALL = 6,
+   EC_ERR_TYPE_FOE_PACKETNUMBER = 7,
+   EC_ERR_TYPE_SOE_ERROR = 8,
+   EC_ERR_TYPE_MBX_ERROR = 9,
+   EC_ERR_TYPE_FOE_FILE_NOTFOUND = 10,
+   EC_ERR_TYPE_EOE_INVALID_RX_DATA = 11
+} ec_err_type;
+
+/** Struct to retrieve errors. */
+typedef struct
+{
+   /** Time at which the error was generated. */
+   ec_timet Time;
+   /** Signal bit, error set but not read */
+   boolean Signal;
+   /** Slave number that generated the error */
+   uint16 Slave;
+   /** CoE SDO index that generated the error */
+   uint16 Index;
+   /** CoE SDO subindex that generated the error */
+   uint8 SubIdx;
+   /** Type of error */
+   ec_err_type Etype;
+   union
+   {
+      /** General abortcode */
+      int32 AbortCode;
+      /** Specific error for Emergency mailbox */
+      struct
+      {
+         uint16 ErrorCode;
+         uint8 ErrorReg;
+         uint8 b1;
+         uint16 w1;
+         uint16 w2;
+      };
+   };
+} ec_errort;
+
+/** Helper macros */
+
+/** Set the count value in the Mailbox header */
+#define MBX_HDR_SET_CNT(cnt) ((uint8)((cnt) << 4))
+
+/** Macro to make a word from 2 bytes */
+#define MK_WORD(msb, lsb)    ((((uint16)(msb)) << 8) | (lsb))
+/** Macro to get hi byte of a word */
+#define HI_BYTE(w)           ((w) >> 8)
+/** Macro to get low byte of a word */
+#define LO_BYTE(w)           ((w) & 0x00ff)
+/** Macro to swap hi and low byte of a word */
+#define SWAP(w)              ((((w) & 0xff00) >> 8) | (((w) & 0x00ff) << 8))
+/** Macro to get hi word of a dword */
+#define LO_WORD(l)           ((l) & 0xffff)
+/** Macro to get hi word of a dword */
+#define HI_WORD(l)           ((l) >> 16)
+
+#define get_unaligned(ptr) \
+   ({ __typeof__(*(ptr)) __tmp; memcpy(&__tmp, (ptr), sizeof(*(ptr))); __tmp; })
+
+#define put_unaligned32(val, ptr) \
+   (memcpy((ptr), &(val), 4))
+
+#define put_unaligned64(val, ptr) \
+   (memcpy((ptr), &(val), 8))
+
+#if !defined(EC_BIG_ENDIAN) && defined(EC_LITTLE_ENDIAN)
+
+#define htoes(A)  (A)
+#define htoel(A)  (A)
+#define htoell(A) (A)
+#define etohs(A)  (A)
+#define etohl(A)  (A)
+#define etohll(A) (A)
+
+#elif !defined(EC_LITTLE_ENDIAN) && defined(EC_BIG_ENDIAN)
+
+#define htoes(A) ((((uint16)(A) & 0xff00) >> 8) | \
+                  (((uint16)(A) & 0x00ff) << 8))
+#define htoel(A) ((((uint32)(A) & 0xff000000) >> 24) | \
+                  (((uint32)(A) & 0x00ff0000) >> 8) |  \
+                  (((uint32)(A) & 0x0000ff00) << 8) |  \
+                  (((uint32)(A) & 0x000000ff) << 24))
+#define htoell(A) ((((uint64)(A) & (uint64)0xff00000000000000ULL) >> 56) | \
+                   (((uint64)(A) & (uint64)0x00ff000000000000ULL) >> 40) | \
+                   (((uint64)(A) & (uint64)0x0000ff0000000000ULL) >> 24) | \
+                   (((uint64)(A) & (uint64)0x000000ff00000000ULL) >> 8) |  \
+                   (((uint64)(A) & (uint64)0x00000000ff000000ULL) << 8) |  \
+                   (((uint64)(A) & (uint64)0x0000000000ff0000ULL) << 24) | \
+                   (((uint64)(A) & (uint64)0x000000000000ff00ULL) << 40) | \
+                   (((uint64)(A) & (uint64)0x00000000000000ffULL) << 56))
+
+#define etohs  htoes
+#define etohl  htoel
+#define etohll htoell
+
+#else
+
+#error "Must define one of EC_BIG_ENDIAN or EC_LITTLE_ENDIAN"
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _EC_TYPE_H */

dependency/x86/third_party/soem/include/soem/nicdrv.h

@@ -0,0 +1,108 @@
+/*
+ * This software is dual-licensed under GPLv3 and a commercial
+ * license. See the file LICENSE.md distributed with this software for
+ * full license information.
+ */
+
+/** \file
+ * \brief
+ * Headerfile for nicdrv.c
+ */
+
+#ifndef _nicdrvh_
+#define _nicdrvh_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <pthread.h>
+
+/** pointer structure to Tx and Rx stacks */
+typedef struct
+{
+   /** socket connection used */
+   int *sock;
+   /** tx buffer */
+   ec_bufT (*txbuf)[EC_MAXBUF];
+   /** tx buffer lengths */
+   int (*txbuflength)[EC_MAXBUF];
+   /** temporary receive buffer */
+   ec_bufT *tempbuf;
+   /** rx buffers */
+   ec_bufT (*rxbuf)[EC_MAXBUF];
+   /** rx buffer status fields */
+   int (*rxbufstat)[EC_MAXBUF];
+   /** received MAC source address (middle word) */
+   int (*rxsa)[EC_MAXBUF];
+   /** number of received frames */
+   uint64 rxcnt;
+} ec_stackT;
+
+/** pointer structure to buffers for redundant port */
+typedef struct
+{
+   ec_stackT stack;
+   int sockhandle;
+   /** rx buffers */
+   ec_bufT rxbuf[EC_MAXBUF];
+   /** rx buffer status */
+   int rxbufstat[EC_MAXBUF];
+   /** rx MAC source address */
+   int rxsa[EC_MAXBUF];
+   /** temporary rx buffer */
+   ec_bufT tempinbuf;
+} ecx_redportt;
+
+/** pointer structure to buffers, vars and mutexes for port instantiation */
+typedef struct
+{
+   ec_stackT stack;
+   int sockhandle;
+   /** rx buffers */
+   ec_bufT rxbuf[EC_MAXBUF];
+   /** rx buffer status */
+   int rxbufstat[EC_MAXBUF];
+   /** rx MAC source address */
+   int rxsa[EC_MAXBUF];
+   /** temporary rx buffer */
+   ec_bufT tempinbuf;
+   /** temporary rx buffer status */
+   int tempinbufs;
+   /** transmit buffers */
+   ec_bufT txbuf[EC_MAXBUF];
+   /** transmit buffer lengths */
+   int txbuflength[EC_MAXBUF];
+   /** temporary tx buffer */
+   ec_bufT txbuf2;
+   /** temporary tx buffer length */
+   int txbuflength2;
+   /** last used frame index */
+   uint8 lastidx;
+   /** current redundancy state */
+   int redstate;
+   /** pointer to redundancy port and buffers */
+   ecx_redportt *redport;
+   pthread_mutex_t getindex_mutex;
+   pthread_mutex_t tx_mutex;
+   pthread_mutex_t rx_mutex;
+} ecx_portt;
+
+extern const uint16 priMAC[3];
+extern const uint16 secMAC[3];
+
+void ec_setupheader(void *p);
+int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary);
+int ecx_closenic(ecx_portt *port);
+void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat);
+uint8 ecx_getindex(ecx_portt *port);
+int ecx_outframe(ecx_portt *port, uint8 idx, int sock);
+int ecx_outframe_red(ecx_portt *port, uint8 idx);
+int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout);
+int ecx_srconfirm(ecx_portt *port, uint8 idx, int timeout);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dependency/x86/third_party/soem/include/soem/osal.h

@@ -0,0 +1,222 @@
+/*
+ * This software is dual-licensed under GPLv3 and a commercial
+ * license. See the file LICENSE.md distributed with this software for
+ * full license information.
+ */
+
+#ifndef _osal_
+#define _osal_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "osal_defs.h"
+#include <stdint.h>
+#include <stddef.h>
+
+/* General types */
+#ifndef TRUE
+#define TRUE 1
+#endif
+#ifndef FALSE
+#define FALSE 0
+#endif
+typedef uint8_t boolean;
+typedef int8_t int8;
+typedef int16_t int16;
+typedef int32_t int32;
+typedef uint8_t uint8;
+typedef uint16_t uint16;
+typedef uint32_t uint32;
+typedef int64_t int64;
+typedef uint64_t uint64;
+typedef float float32;
+typedef double float64;
+
+typedef struct osal_timer
+{
+   ec_timet stop_time;
+} osal_timert;
+
+/**
+ * @brief Returns monotonic time from some unspecified moment in the
+ * past.
+ *
+ * This time must be strictly increasing. It is used for time
+ * intervals measurement.
+ *
+ * @param ts Pointer to an ec_timet structure where the time will be
+ * stored.
+ */
+void osal_get_monotonic_time(ec_timet *ts);
+
+/**
+ * @brief Returns the current time.
+ *
+ * This time is used to set the initial EtherCAT network DC time and
+ * for logging purposes.
+ *
+ * @return ec_timet containing the current time.
+ */
+ec_timet osal_current_time(void);
+
+/**
+ * @brief Calculates the difference between two timestamps.
+ *
+ * @param start Pointer to the start timestamp.
+ * @param end Pointer to the end timestamp.
+ * @param diff Pointer to an ec_timet structure where the difference
+ * will be stored.
+ */
+void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff);
+
+/**
+ * @brief Starts the timer with a specified timeout.
+ *
+ * @param self Pointer to the timer object.
+ * @param timeout_usec Timeout in microseconds.
+ */
+void osal_timer_start(osal_timert *self, uint32 timeout_usec);
+
+/**
+ * @brief Checks if the timer has expired.
+ *
+ * @param self Pointer to the timer object.
+ * @return True if the timer is expired, false otherwise.
+ */
+boolean osal_timer_is_expired(osal_timert *self);
+
+/**
+ * @brief Sleeps for a specified duration in microseconds.
+ *
+ * @param usec Duration in microseconds.
+ * @return 0 on success, -1 on failure.
+ */
+int osal_usleep(uint32 usec);
+
+/**
+ * @brief Sleeps until the specified monotonic time.
+ *
+ * @param ts Pointer to an ec_timet structure representing the
+ * absolute time to sleep until.
+ * @return 0 on success, -1 on failure.
+ */
+int osal_monotonic_sleep(ec_timet *ts);
+
+/**
+ * @brief Allocates memory of the specified size.
+ *
+ * @param size Size in bytes to allocate.
+ * @return Pointer to the allocated memory or NULL on failure.
+ */
+void *osal_malloc(size_t size);
+
+/**
+ * @brief Frees the allocated memory.
+ *
+ * @param ptr Pointer to the memory to free.
+ */
+void osal_free(void *ptr);
+
+/**
+ * @brief Creates a new thread.
+ *
+ * @param thandle Pointer to the thread handle which will store the
+ * thread ID.
+ * @param stacksize Size of the stack for the new thread.
+ * @param func Pointer to the function to execute in the new thread.
+ * @param param Pointer to parameters to pass to the thread function.
+ * @return 1 on success, 0 on failure.
+ */
+int osal_thread_create(void *thandle, int stacksize, void *func, void *param);
+
+/**
+ * @brief Creates a new real-time thread.
+ *
+ * @param thandle Pointer to the thread handle which will store the
+ * thread ID.
+ * @param stacksize Size of the stack for the new thread.
+ * @param func Pointer to the function to execute in the new thread.
+ * @param param Pointer to parameters to pass to the thread function.
+ * @return 1 on success, 0 on failure.
+ */
+int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param);
+
+/**
+ * @brief Creates a mutex.
+ *
+ * @return Pointer to the created mutex or NULL on failure.
+ */
+void *osal_mutex_create(void);
+
+/**
+ * @brief Destroys a mutex.
+ *
+ * @param mutex Pointer to the mutex to destroy.
+ */
+void osal_mutex_destroy(void *mutex);
+
+/**
+ * @brief Locks the mutex.
+ *
+ * @param mutex Pointer to the mutex to lock.
+ */
+void osal_mutex_lock(void *mutex);
+
+/**
+ * @brief Unlocks the mutex.
+ *
+ * @param mutex Pointer to the mutex to unlock.
+ */
+void osal_mutex_unlock(void *mutex);
+
+#ifndef osal_timespec_from_usec
+#define osal_timespec_from_usec(usec, result)      \
+   do                                              \
+   {                                               \
+      (result)->tv_sec = usec / 1000000;           \
+      (result)->tv_nsec = (usec % 1000000) * 1000; \
+   } while (0)
+#endif
+
+#ifndef osal_timespeccmp
+#define osal_timespeccmp(a, b, CMP)      \
+   (((a)->tv_sec == (b)->tv_sec)         \
+        ? ((a)->tv_nsec CMP(b)->tv_nsec) \
+        : ((a)->tv_sec CMP(b)->tv_sec))
+#endif
+
+#ifndef osal_timespecadd
+#define osal_timespecadd(a, b, result)                 \
+   do                                                  \
+   {                                                   \
+      (result)->tv_sec = (a)->tv_sec + (b)->tv_sec;    \
+      (result)->tv_nsec = (a)->tv_nsec + (b)->tv_nsec; \
+      if ((result)->tv_nsec >= 1000000000)             \
+      {                                                \
+         ++(result)->tv_sec;                           \
+         (result)->tv_nsec -= 1000000000;              \
+      }                                                \
+   } while (0)
+#endif
+
+#ifndef osal_timespecsub
+#define osal_timespecsub(a, b, result)                 \
+   do                                                  \
+   {                                                   \
+      (result)->tv_sec = (a)->tv_sec - (b)->tv_sec;    \
+      (result)->tv_nsec = (a)->tv_nsec - (b)->tv_nsec; \
+      if ((result)->tv_nsec < 0)                       \
+      {                                                \
+         --(result)->tv_sec;                           \
+         (result)->tv_nsec += 1000000000;              \
+      }                                                \
+   } while (0)
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dependency/x86/third_party/soem/include/soem/osal_defs.h

@@ -0,0 +1,46 @@
+/*
+ * This software is dual-licensed under GPLv3 and a commercial
+ * license. See the file LICENSE.md distributed with this software for
+ * full license information.
+ */
+
+#ifndef _osal_defs_
+#define _osal_defs_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <pthread.h>
+#include <sys/time.h>
+
+// define if debug printf is needed
+#ifdef EC_DEBUG
+#include <stdio.h>
+#define EC_PRINT printf
+#else
+#define EC_PRINT(...) \
+   do                 \
+   {                  \
+   } while (0)
+#endif
+
+#ifndef OSAL_PACKED
+#define OSAL_PACKED_BEGIN
+#define OSAL_PACKED __attribute__((__packed__))
+#define OSAL_PACKED_END
+#endif
+
+#define ec_timet            struct timespec
+
+#define OSAL_THREAD_HANDLE  pthread_t *
+#define OSAL_THREAD_FUNC    void
+#define OSAL_THREAD_FUNC_RT void
+
+#define osal_mutext         pthread_mutex_t
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dependency/x86/third_party/soem/include/soem/soem.h

@@ -0,0 +1,28 @@
+/*
+ * This software is dual-licensed under GPLv3 and a commercial
+ * license. See the file LICENSE.md distributed with this software for
+ * full license information.
+ */
+
+/** \file
+ * \brief
+ * SOEM main include file
+ */
+
+#ifndef _SOEM_H
+#define _SOEM_H
+
+#include "soem/ec_options.h"
+#include "soem/ec_type.h"
+#include "nicdrv.h"
+#include "soem/ec_base.h"
+#include "soem/ec_main.h"
+#include "soem/ec_dc.h"
+#include "soem/ec_coe.h"
+#include "soem/ec_foe.h"
+#include "soem/ec_soe.h"
+#include "soem/ec_eoe.h"
+#include "soem/ec_config.h"
+#include "soem/ec_print.h"
+
+#endif /* _SOEM_H */

protos/cmvr/config/biohead_config/biohead_config.proto

@@ -0,0 +1,44 @@
+// biohead.proto
+syntax = "proto3";
+
+package cmvr.config;
+
+// 眉毛配置
+message EyeBrowConfig {
+    string serial = 1;           // 如 "64:0~3"
+    string offest = 2;           // 偏移量，如 "90 90 90 90"
+    string jLmtUp = 3;           // 上限，如 "90 170 155 110"
+    string jLmtLow = 4;          // 下限，如 "20 77 90 20"
+}
+
+// 眼睛配置
+message EyeConfig {
+    string serial = 1;           // 如 "64:4~9"
+    string offest = 2;           // 偏移量，如 "90 90 90 90 90 90"
+    string jLmtUp = 3;           // 上限，如 "90 150 165 90 120 115"
+    string jLmtLow = 4;          // 下限，如 "20 90 90 25 70 75"
+}
+
+// 嘴巴配置
+message MouthConfig {
+    string serial = 1;           // 如 "65:0~9"
+    string offest = 2;           // 偏移量，如 "90 90 90 90 90 90 90 90 90 90"
+    string jLmtUp = 3;           // 上限，如 "150 110 130 140 100 105 110 125 90 95"
+    string jLmtLow = 4;          // 下限，如 "70 30 80 80 65 55 45 80 85 90"
+}
+
+// BioHead配置
+message BioHeadConfig {
+    string id = 1;               // 设备ID，如 "bio_head"
+    string serial = 2;          // 串口路径，如 "/dev/ttyUSB0"
+    int32 ctrlFreq = 3;         // 控制频率，如 50
+
+    EyeBrowConfig eyebrow = 4;   // 眉毛配置
+    EyeConfig eye = 5;           // 眼睛配置
+    MouthConfig mouth = 6;       // 嘴巴配置
+    bool enable = 7;             // 是否启用
+}
+
+message HeadConfig {
+    repeated BioHeadConfig bioheads = 3;
+}

protos/cmvr/config/monitor_config/monitor_config.proto

@@ -0,0 +1,22 @@
+syntax = "proto3";
+
+package cmvr.config;
+
+// 文件夹监控配置
+message FolderConfig {
+    string fileDir = 1;      // 文件夹路径
+    int32 maxVolume = 2;     // 最大容量（MB）
+}
+
+// 磁盘监控配置
+message DiskMonitorConfig {
+    string id = 1;           // 监控器ID，如 "file_monitor"
+    int32 freq = 2;          // 监控频率（Hz）
+    repeated FolderConfig folders = 3;  // 要监控的文件夹列表
+    bool enable = 4;         // 是否启用
+}
+
+// 监控管理器配置
+message MonitorManagerConfig {
+    DiskMonitorConfig disk_monitor = 1;  // 磁盘监控配置
+}

protos/cmvr/config/robot_config/robot_config.proto

@@ -0,0 +1,88 @@
+syntax = "proto3";
+
+package cmvr.config;
+
+// 电机配置
+message MotorConfig {
+    int32 id = 1;                // 电机ID
+    string jointName = 2;        // 关节名称
+    float limitQLb = 3;          // 位置下限
+    float limitQUb = 4;          // 位置上限
+    float limitQd = 5;           // 速度限制
+}
+
+// 通用的CAN设备配置（手臂、头部、腰部都可以复用）
+message CanDeviceConfig {
+    string id = 1;               // CAN ID
+    string devId = 2;            // 设备ID
+    int32 channelId = 3;         // 通道ID
+    bool enable = 4;             // 是否启用
+    string toolFrame = 5;        // 工具坐标系（手臂需要，头/腰可选）
+    repeated MotorConfig motors = 6;  // 电机列表
+}
+
+// CAN管理器配置
+message CanManagerConfig {
+    string id = 1;               // CAN管理器ID
+    string devId = 2;            // 设备ID
+    CanDeviceConfig left_arm = 3;    // 左臂CAN
+    CanDeviceConfig right_arm = 4;   // 右臂CAN
+    CanDeviceConfig head = 5;        // 头部CAN
+    CanDeviceConfig waist = 6;       // 腰部CAN
+}
+
+// 人形机器人配置
+message HumanRobotConfig {
+    string id = 1;               // 机器人ID，如 "hc01"
+    int32 dof = 2;               // 自由度，如 14
+    string urdf = 3;             // URDF文件路径
+    string baseLink = 4;         // 基座link名称，如 "PELVIS_S"
+    string jointNames = 5;       // 关节名称列表，逗号分隔
+    string linkNames = 6;        // link名称列表，逗号分隔
+    int32 bufferSize = 7;        // 缓冲区大小，如 50
+    bool verbose = 8;            // 是否详细输出
+    int32 updFreq = 9;           // 更新频率，如 500
+    CanManagerConfig can_manager = 10;  // CAN管理器配置
+    bool enable = 11;            // 是否启用
+}
+
+// EtherCAT主站配置
+message EthercatMasterConfig {
+    string interface = 1;        // 网络接口，如 "eth0"
+    int32 cycle_time_us = 2;     // 周期时间（微秒），默认1000
+    bool enable = 3;             // 是否启用
+}
+
+// EtherCAT电机驱动配置
+message EthercatMotorConfig {
+    int32 slave = 1;             // 从站ID
+    string jointName = 2;        // 关节名称
+    float limitQLb = 3;          // 位置下限
+    float limitQUb = 4;          // 位置上限
+    float limitQd = 5;           // 速度限制
+}
+
+// EtherCAT机器人配置
+message EthercatRobotConfig {
+    string id = 1;               // 机器人ID
+    int32 dof = 2;               // 自由度
+    string urdf = 3;             // URDF文件路径
+    string baseLink = 4;         // 基座link名称
+    string jointNames = 5;       // 关节名称列表，逗号分隔
+    string linkNames = 6;        // link名称列表，逗号分隔
+    int32 bufferSize = 7;        // 缓冲区大小
+    bool verbose = 8;            // 是否详细输出
+    int32 updFreq = 9;           // 更新频率（Hz）
+    bool enable = 10;            // 是否启用
+
+    EthercatMasterConfig master = 11;      // EtherCAT主站配置
+    repeated EthercatMotorConfig motors = 12;  // 电机配置列表
+}
+
+
+
+// 主配置
+message RobotConfig {
+    repeated HumanRobotConfig humanrobots = 1;
+    repeated EthercatRobotConfig ethercat_robots = 2; // 新增的EtherCAT机器人
+}

protos/cmvr/config/server_config/server_config.proto

@@ -0,0 +1,10 @@
+syntax = "proto3";
+package cmvr.config;
+
+message GrpcServerConfig{
+    string port = 1;
+}
+
+message ServerConfig{
+    GrpcServerConfig grpc_server = 1;
+}

request.txt

@@ -33,5 +33,6 @@ third_party/modbus/3.1.11
 third_party/visp/3.7.0
 third_party/mainif/0.0.5
 third_party/matplotplusplus/1.2.0
+third_party/soem