diff --git a/cmvr-es/common/config/biohead_config/biohead_config.pb.txt b/cmvr-es/common/config/biohead_config/biohead_config.pb.txt new file mode 100644 index 00000000..b2880e64 --- /dev/null +++ b/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" + } +} \ No newline at end of file diff --git a/cmvr-es/common/config/camera_config/camera_config.pb.txt b/cmvr-es/common/config/camera_config/camera_config.pb.txt index 18b02c30..f35f4018 100644 --- a/cmvr-es/common/config/camera_config/camera_config.pb.txt +++ b/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 { diff --git a/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt b/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt index e6783dd2..4a38bd2f 100644 --- a/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt +++ b/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 } diff --git a/cmvr-es/common/config/monitor_config/monitor_config.pb.txt b/cmvr-es/common/config/monitor_config/monitor_config.pb.txt new file mode 100644 index 00000000..d5350f3c --- /dev/null +++ b/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 + } + } +} \ No newline at end of file diff --git a/cmvr-es/common/config/robot_config/robot_config.pb.txt b/cmvr-es/common/config/robot_config/robot_config.pb.txt new file mode 100644 index 00000000..9e2a9327 --- /dev/null +++ b/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 + } +} \ No newline at end of file diff --git a/cmvr-es/common/config/server_config/server_config.pb.txt b/cmvr-es/common/config/server_config/server_config.pb.txt new file mode 100644 index 00000000..78599a06 --- /dev/null +++ b/cmvr-es/common/config/server_config/server_config.pb.txt @@ -0,0 +1,3 @@ +grpc_server { + port: "50052" +} \ No newline at end of file diff --git a/cmvr-es/common/utils/config_helper/include/config_helper.h b/cmvr-es/common/utils/config_helper/include/config_helper.h index 7d189128..445af379 100644 --- a/cmvr-es/common/utils/config_helper/include/config_helper.h +++ b/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 diff --git a/cmvr-es/common/utils/config_helper/include/config_setting.h b/cmvr-es/common/utils/config_helper/include/config_setting.h index 5d58fb9e..caedf533 100644 --- a/cmvr-es/common/utils/config_helper/include/config_setting.h +++ b/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); diff --git a/cmvr-es/common/utils/config_helper/src/config_setting.cpp b/cmvr-es/common/utils/config_helper/src/config_setting.cpp index 9e44092d..528819f5 100644 --- a/cmvr-es/common/utils/config_helper/src/config_setting.cpp +++ b/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"); diff --git a/cmvr-es/common/utils/ffmpeg/include/FrameData.h b/cmvr-es/common/utils/ffmpeg/include/FrameData.h new file mode 100644 index 00000000..cd69682a --- /dev/null +++ b/cmvr-es/common/utils/ffmpeg/include/FrameData.h @@ -0,0 +1,53 @@ +// FrameData.h +#pragma once + +#include + +extern "C" { +#include +#include +} + +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 \ No newline at end of file diff --git a/cmvr-es/data_center/CMakeLists.txt b/cmvr-es/data_center/CMakeLists.txt index d0fda580..ff66818d 100644 --- a/cmvr-es/data_center/CMakeLists.txt +++ b/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 ) diff --git a/cmvr-es/data_center/include/motors_info.h b/cmvr-es/data_center/include/motors_info.h index 56c2de04..ac892cfc 100644 --- a/cmvr-es/data_center/include/motors_info.h +++ b/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; diff --git a/cmvr-es/data_center/src/motors_info.cpp b/cmvr-es/data_center/src/motors_info.cpp index 4b6a087c..0a5af0b2 100644 --- a/cmvr-es/data_center/src/motors_info.cpp +++ b/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 l_motors_cfg; + std::vector r_motors_cfg; + std::vector waist_motors_cfg; + std::vector 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(left_arm); + l_can_sender_ = std::make_shared>(); + l_can_receiver_ = std::make_shared>(); + l_message_manager_ = std::make_shared>(); + } + + // 右臂 + 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(right_arm); + r_can_sender_ = std::make_shared>(); + r_can_receiver_ = std::make_shared>(); + r_message_manager_ = std::make_shared>(); + } + + // 腰部 + 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(waist); + waist_can_sender_ = std::make_shared>(); + waist_can_receiver_ = std::make_shared>(); + waist_message_manager_ = std::make_shared>(); + } + + // 头部 + 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(head); + head_can_sender_ = std::make_shared>(); + head_can_receiver_ = std::make_shared>(); + head_message_manager_ = std::make_shared>(); + } + + // 初始化 limbs + struct Limb { + std::string name; + bool enabled; + std::shared_ptr client; + std::shared_ptr> sender; + std::shared_ptr> receiver; + std::shared_ptr> message_manager; + std::vector motor_cfgs; + }; + + std::vector 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(); + std::vector> 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 protocol = nullptr; + if (limb.sender && limb.message_manager) { + protocol = std::make_shared(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(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(); + } + auto protocol = std::make_shared(config.master().interface()); + for (const auto& motor_cfg : config.motors()) { + auto motor = std::make_shared(motor_cfg); + if (protocol) motor->setProtocol(protocol); + motor->init(); + motor_manager_->addMotor(motor); + } +} + bool MotorsInfo::getJointsQ(const std::vector &joints_name, std::unordered_map &joint_qs) const { auto res{true}; diff --git a/cmvr-es/device_manager/src/device_factory.cpp b/cmvr-es/device_manager/src/device_factory.cpp index fba92e51..2c136ac5 100644 --- a/cmvr-es/device_manager/src/device_factory.cpp +++ b/cmvr-es/device_manager/src/device_factory.cpp @@ -184,6 +184,10 @@ template std::shared_ptr DeviceFactory::createFromConfig DeviceFactory::createFromConfig(const config::PX6AXGen3& cfg); template std::shared_ptr DeviceFactory::createFromConfig(const config::FFMpegMicroPhoneConfig& cfg); template std::shared_ptr DeviceFactory::createFromConfig(const config::FFMpegSpeakerConfig& cfg); +template std::shared_ptr DeviceFactory::createFromConfig(const config::BioHeadConfig& cfg); +template std::shared_ptr DeviceFactory::createFromConfig(const config::HumanRobotConfig& cfg); +template std::shared_ptr DeviceFactory::createFromConfig(const config::EthercatRobotConfig& cfg); + template std::shared_ptr DeviceFactory::createFromConfig(const ConfigType& cfg) { @@ -244,6 +248,38 @@ std::shared_ptr DeviceFactory::createFromConfig(const ConfigType& cf throw runtime_error("[DeviceFactory]: Unsupported microphone device type"); } } + else if constexpr (std::is_same_v) + { + if constexpr (std::is_same_v) + { + return std::make_shared(cfg); + } + else + { + LOG(ERROR) << "[DeviceFactory]: Unsupported microphone device type "; + throw runtime_error("[DeviceFactory]: Unsupported microphone device type"); + } + } + else if constexpr (std::is_same_v) + { + if constexpr (std::is_same_v) + { + auto motos_info = MotorsInfo::getInstance(); + motos_info->init(cfg); + return std::make_shared>(cfg); + } + else if constexpr (std::is_same_v) + { + auto motos_info = MotorsInfo::getInstance(); + motos_info->init(cfg); + return std::make_shared>(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 DeviceFactory::createFromConfig(const ConfigType& cf return nullptr; } } + diff --git a/cmvr-es/device_manager/src/device_manager.cpp b/cmvr-es/device_manager/src/device_manager.cpp index 805516f9..67c06de0 100644 --- a/cmvr-es/device_manager/src/device_manager.cpp +++ b/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(robot); + devices_[id] = device; + std::get>(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>(devices_[id])->init(); } - - auto biohead_node = dmgr_node.getChild("BioHead"); - for (auto &node: biohead_node.getChildren()){ - string id = node.getAttrString("id"); - if (devices_.count(id)){ - LOG(ERROR) << "[DeviceManager]: Duplicate AbstractBiohead Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate AbstractBiohead Device ID" + id); - } - auto device = dev_factory_->create(node); - if (device == nullptr){ - throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); + config::HeadConfig head_cfg; + ConfigHelper::getHeadConfig(head_cfg); + for (int i = 0; i < head_cfg.bioheads().size(); i++) { + auto head = head_cfg.bioheads(i); + if (!head.enable()) + continue; + auto id = head.id(); + if (devices_.count(id)) { + LOG(ERROR) << "[DeviceManager]: Duplicate Biohead Device ID" << id; + throw runtime_error("[DeviceManager]: Duplicate Biohead Device ID" + id); } + std::cout << "Biohead[" << i << "]: " << id << std::endl; + auto device = dev_factory_->createFromConfig(head); devices_[id] = device; std::get>(devices_[id])->init(); - LOG(INFO) << "[BioHead]: Init biohead " << id << " Success"; - } } catch (const exception& e) { diff --git a/cmvr-es/devices/biohead/abstract_biohead.h b/cmvr-es/devices/biohead/abstract_biohead.h index 93e0eb20..09b774ff 100644 --- a/cmvr-es/devices/biohead/abstract_biohead.h +++ b/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; diff --git a/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h b/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h index 52b34d5d..08c0953f 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h +++ b/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h @@ -7,7 +7,7 @@ #include #include #include - +#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 speak_thread_; std::atomic speak_running_{false}; - + cmvr::config::BioHeadConfig config_; }; diff --git a/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp b/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp index c3b67887..8ff9e3b3 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp +++ b/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(); + 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 offsets; + std::istringstream oss(offset_str); + double val; + while (oss >> val) offsets.push_back(val); + + // 解析角度限制 + std::vector 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 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(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 offsets; + std::istringstream oss(offset_str); + double val; + while (oss >> val) offsets.push_back(val); + + // 解析角度限制 + std::vector 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 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(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 offsets; + std::istringstream oss(offset_str); + double val; + while (oss >> val) offsets.push_back(val); + + // 解析角度限制 + std::vector 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 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(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) { diff --git a/cmvr-es/devices/canbus/abstract_canbus.h b/cmvr-es/devices/canbus/abstract_canbus.h index ca0399d0..6fce7f5e 100644 --- a/cmvr-es/devices/canbus/abstract_canbus.h +++ b/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() {} /** diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc index aef0af2c..28779a06 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc +++ b/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(channel_id); + interface_ = CANCardParameter::NATIVE; + + enable_can_err_check_ = false; + + + } + void SocketCanClientRaw::init() { // if (!parameter.has_channel_id()) { diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h index 3de5b42c..04d57c86 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h +++ b/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_; }; } } diff --git a/cmvr-es/devices/motor/CMakeLists.txt b/cmvr-es/devices/motor/CMakeLists.txt index 532924d2..3b7669c9 100644 --- a/cmvr-es/devices/motor/CMakeLists.txt +++ b/cmvr-es/devices/motor/CMakeLists.txt @@ -1,5 +1,5 @@ add_subdirectory(ti5_motor) - +add_subdirectory(eyou_motor) # -------------------------------------------------------- # Unit test # -------------------------------------------------------- diff --git a/cmvr-es/devices/motor/abstract_motor.h b/cmvr-es/devices/motor/abstract_motor.h index 710f88d2..bb89a415 100644 --- a/cmvr-es/devices/motor/abstract_motor.h +++ b/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; diff --git a/cmvr-es/devices/motor/eyou_motor/CMakeLists.txt b/cmvr-es/devices/motor/eyou_motor/CMakeLists.txt new file mode 100644 index 00000000..9f00953a --- /dev/null +++ b/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) \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.cpp b/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.cpp new file mode 100644 index 00000000..26636505 --- /dev/null +++ b/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 +#include +#include + +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>(); + + // 创建 EcMaster 实例 + master_ = std::make_unique(); + + // 初始化主站 + 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 rxPdos = config.availableRxPdos; + std::vector 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, 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(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(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(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((std::abs(u_qdd) / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV); + auto decel = static_cast((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(r / 2); + if (pos > temp || pos < -temp) { + pos = pos % static_cast(r); + if (pos > temp || pos < -temp) { + if (pos > temp) { + pos = pos - static_cast(r); + } else if (pos < -temp) { + pos = pos + static_cast(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(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(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((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 EYouEthercatProtocol::GetRobotDetail() { + if (message_manager_) { + auto data_ptr = std::make_unique(); + message_manager_->GetSensorData(data_ptr.get()); + return data_ptr; + } + return std::make_unique(); +} + +// 辅助函数实现（弧度制版本） +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((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 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); + // 可以根据需要更新其他状态信息 +} \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h b/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h new file mode 100644 index 00000000..2b14279c --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +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 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 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 node_states_; + std::unordered_map last_Qd_; + std::unordered_map last_Qdd_; + + std::shared_ptr> can_sender_; + std::shared_ptr> message_manager_; + + std::string interface_name_; + std::string xml_config_path_; + bool initialized_; + int slave_count_; + + std::unique_ptr master_; + std::unique_ptr parser_; + EcSlaveConfig slave_config_; + bool use_fixed_pdo_; + + std::mutex master_mutex_; + }; + + } // namespace device +} // namespace cmvr \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_master.cpp b/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_master.cpp new file mode 100644 index 00000000..afac1e5c --- /dev/null +++ b/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_master.cpp @@ -0,0 +1,1143 @@ +// ec_master.cpp +#include "ec_master.h" +#include + +static float pgain = 0.01f; +static float igain = 0.00002f; +static int64_t syncoffset = 500000; +static int64_t timeerror = 0; + +static void add_time_ns(ec_timet* ts, int64_t addtime) { + ec_timet addts; + addts.tv_nsec = addtime % 1000000000; + addts.tv_sec = (addtime - addts.tv_nsec) / 1000000000; + osal_timespecadd(ts, &addts, ts); +} + +// DC 同步 PI 计算 +static void ec_sync(int64_t reftime, int64_t cycletime, int64_t *offsettime) { + static int64_t integral = 0; + int64_t delta; + delta = (reftime - syncoffset) % cycletime; + if (delta > (cycletime / 2)) { + delta = delta - cycletime; + } + timeerror = -delta; + integral += timeerror; + *offsettime = (int64_t)((timeerror * pgain) + (integral * igain)); +} + +// ==================== 构造和析构 ==================== + +EcMaster::EcMaster() { + memset(&ctx, 0, sizeof(ctx)); + memset(IOmap, 0, sizeof(IOmap)); + + slaveCount = 0; + expectedWKC = 0; + currentgroup = 0; + inOp = false; + isRunning = false; + mappingdone = 0; + dorun = 0; + cyclicWkcErrorCount = 0; + cycleTimeNs = 1000000; + cyclicThread = nullptr; + checkThread = nullptr; + anyPdoDirty = false; +} + +EcMaster::~EcMaster() { + shutdown(); + // shadowBuffers 的析构函数会自动清理内存 +} + +// ==================== 初始化和状态控制 ==================== + +int EcMaster::init(const char* ifName, int cycleTimeUs) { + cycleTimeNs = cycleTimeUs * 1000; + + if (!ecx_init(&ctx, ifName)) { + printf("Failed to init EtherCAT on %s\n", ifName); + return -EC_FAILED_INIT; + } + + ecx_config_init(&ctx); + slaveCount = ctx.slavecount; + + if (slaveCount <= 0) { + printf("No slave found\n"); + return -EC_FAILED_NOSLAVE; + } + + printf("%d slaves found\n", slaveCount); + + isRunning = true; + mappingdone = 0; + dorun = 0; + inOp = false; + cyclicWkcErrorCount = 0; + anyPdoDirty = false; + + osal_thread_create_rt(&cyclicThread, 128000, reinterpret_cast(cyclicThreadFunc), this); + + return slaveCount; +} + +int EcMaster::setState(uint16_t slaveId, uint8_t targetState) { + if (slaveId > (uint16_t)ctx.slavecount) return -1; + + int oldManual = ctx.manualstatechange; + ctx.manualstatechange = 1; + + ctx.slavelist[slaveId].state = targetState; + ecx_writestate(&ctx, slaveId); + + int ret = ecx_statecheck(&ctx, slaveId, targetState, EC_TIMEOUTSTATE); + ctx.manualstatechange = oldManual; + + if (ret != targetState) { + printf("Slave %d failed to enter state %d (current: %d)\n", slaveId, targetState, ret); + return -1; + } + + return 0; +} + +int EcMaster::gotoOperational() { + while (!mappingdone) { + osal_usleep(100); + } + + dorun = 1; + + printf("Waiting for PDO exchange to stabilize...\n"); + osal_usleep(500000); + + // ========== 逐个处理每个从站 ========== + int allOperational = 1; + for (int i = 1; i <= ctx.slavecount; i++) { + printf("\n--- Processing slave %d ---\n", i); + + // 1. 检查并清除错误状态 + if (ctx.slavelist[i].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) { + printf(" Clearing error on slave %d\n", i); + ctx.slavelist[i].state = (EC_STATE_SAFE_OP + EC_STATE_ACK); + ecx_writestate(&ctx, i); + osal_usleep(100000); + + // 刷新状态 + ecx_readstate(&ctx); + printf(" Slave %d state after error clear: 0x%02X\n", i, ctx.slavelist[i].state); + } + + // 2. 请求进入 OP 状态 + printf(" Requesting OPERATIONAL state for slave %d...\n", i); + ctx.slavelist[i].state = EC_STATE_OPERATIONAL; + ecx_writestate(&ctx, i); + + // 3. 等待该从站进入 OP 状态 + printf(" Waiting for slave %d to enter OPERATIONAL...\n", i); + + int retry = 0; + const int maxRetry = 200; // 最多等待2秒 (200 * 10ms) + int currentState = 0; + + while (retry < maxRetry) { + osal_usleep(10000); // 10ms + ecx_readstate(&ctx); + currentState = ctx.slavelist[i].state; + + if (currentState == EC_STATE_OPERATIONAL) { + printf(" ✓ Slave %d entered OPERATIONAL after %d ms\n", i, retry * 10); + break; + } + + // 如果状态异常，提前退出 + if (currentState == EC_STATE_INIT || currentState == EC_STATE_PRE_OP) { + printf(" ✗ Slave %d regressed to state 0x%02X\n", i, currentState); + break; + } + + retry++; + } + + // 4. 检查结果 + if (currentState == EC_STATE_OPERATIONAL) { + printf(" ✓ Slave %d is OPERATIONAL\n", i); + } else { + printf(" ✗ Slave %d failed to enter OPERATIONAL, state: 0x%02X\n", i, currentState); + + // 读取错误码 + uint16_t alStatusCode = 0; + int psize = 2; + ecx_SDOread(&ctx, i, 0x0134, 0, FALSE, &psize, &alStatusCode, EC_TIMEOUTRXM); + printf(" AL Status Code: 0x%04X\n", alStatusCode); + + allOperational = 0; + } + } + + // 5. 检查是否所有从站都进入了 OP + if (!allOperational) { + printf("\n✗ Failed to enter OPERATIONAL\n"); + return -1; + } + + printf("\n✓ All slaves are OPERATIONAL\n"); + + // 6. 启动 check 线程 + printf("Starting check thread for runtime monitoring...\n"); + osal_thread_create(&checkThread, 128000, reinterpret_cast(checkThreadFunc), this); + + osal_usleep(200000); + inOp = true; + printf("EtherCAT OPERATIONAL (all slaves)\n"); + return 0; +} + +void EcMaster::shutdown() { + printf("Shutting down...\n"); + + if (inOp) { + inOp = false; + dorun = 0; + osal_usleep(100000); + } + + for (int i = 1; i <= slaveCount; i++) { + printf(" Resetting slave %d to INIT...\n", i); + ctx.slavelist[i].state = EC_STATE_INIT; + ecx_writestate(&ctx, i); + osal_usleep(50000); + ecx_statecheck(&ctx, i, EC_STATE_INIT, 500000); + } + + isRunning = false; + osal_usleep(100000); + + shadowBuffers.clear(); + + printf("Shutdown complete\n"); +} + +// ==================== 影子缓冲区管理 ==================== + +EcMaster::SlaveShadowBuffer* EcMaster::getShadowBuffer(uint16_t slaveId) { + auto it = shadowBuffers.find(slaveId); + if (it != shadowBuffers.end()) { + return &it->second; + } + return nullptr; +} + +void EcMaster::syncSlaveOutputs(uint16_t slaveId) { + auto* shadow = getShadowBuffer(slaveId); + if (!shadow || !shadow->dirty || !shadow->data) return; + + auto outIt = outputs.find(slaveId); + auto sizeIt = outputSizes.find(slaveId); + if (outIt == outputs.end() || outIt->second == nullptr) return; + if (sizeIt == outputSizes.end()) return; + + size_t copySize = std::min(shadow->size, (size_t)sizeIt->second); + memcpy(outIt->second, shadow->data, copySize); + shadow->dirty = false; +} + +void EcMaster::syncAllOutputs() { + if (!anyPdoDirty.load()) return; + + for (auto& pair : shadowBuffers) { + uint16_t slaveId = pair.first; + SlaveShadowBuffer& shadow = pair.second; + + if (!shadow.dirty || !shadow.data) continue; + + auto outIt = outputs.find(slaveId); + auto sizeIt = outputSizes.find(slaveId); + if (outIt == outputs.end() || outIt->second == nullptr) continue; + if (sizeIt == outputSizes.end()) continue; + + size_t copySize = std::min(shadow.size, (size_t)sizeIt->second); + memcpy(outIt->second, shadow.data, copySize); + shadow.dirty = false; + } + + anyPdoDirty = false; +} + +// ==================== 线程函数 ==================== + +OSAL_THREAD_FUNC_RT EcMaster::cyclicThreadFunc(void* arg) { + static_cast(arg)->runCyclic(); +} + +OSAL_THREAD_FUNC EcMaster::checkThreadFunc(void* arg) { + static_cast(arg)->runCheck(); +} + +void EcMaster::runCyclic() { + ec_timet ts; + int ht; + static int64_t toff = 0; + int cycleCount = 0; + int printInterval = 500; + + printf("[Cyclic Thread] Thread started, waiting for mappingdone...\n"); + + while (!mappingdone && isRunning) { + osal_usleep(100); + } + + if (!isRunning) { + printf("[Cyclic Thread] Exiting - master not running\n"); + return; + } + + printf("[Cyclic Thread] Mapping done, initializing timing...\n"); + + osal_get_monotonic_time(&ts); + ht = (ts.tv_nsec / 1000000) + 1; + ts.tv_nsec = ht * 1000000; + + printf("[Cyclic Thread] Entering main cyclic loop (cycleTime=%lld ns)\n", + (long long)cycleTimeNs); + + int receiveSuccessCount = 0; + int receiveFailCount = 0; + int lastPrintCycle = 0; + + ecx_send_processdata(&ctx); + + while (isRunning) { + add_time_ns(&ts, cycleTimeNs + toff); + osal_monotonic_sleep(&ts); + + cycleCount++; + + if (dorun) { + // ========== 同步所有从站的影子缓冲区到 outputs ========== + if (anyPdoDirty.load()) { + for (auto& pair : shadowBuffers) { + uint16_t slaveId = pair.first; + SlaveShadowBuffer& shadow = pair.second; + + if (!shadow.dirty || !shadow.data) continue; + + auto outIt = outputs.find(slaveId); + auto sizeIt = outputSizes.find(slaveId); + if (outIt == outputs.end() || outIt->second == nullptr) continue; + if (sizeIt == outputSizes.end()) continue; + + size_t copySize = std::min(shadow.size, (size_t)sizeIt->second); + memcpy(outIt->second, shadow.data, copySize); + shadow.dirty = false; + } + anyPdoDirty = false; + } + + ecx_send_processdata(&ctx); + + int wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET); + + if (wkc != expectedWKC) { + cyclicWkcErrorCount++; + receiveFailCount++; + if (cyclicWkcErrorCount <= 5 || cyclicWkcErrorCount % 100 == 0) { + printf("[Cyclic] WARNING: WKC mismatch! Expected=%d, Got=%d, ErrorCount=%d\n", + expectedWKC, wkc, cyclicWkcErrorCount.load()); + } + } else { + receiveSuccessCount++; + } + + if (ctx.slavelist[0].hasdc && (wkc > 0)) { + ec_sync(ctx.DCtime, cycleTimeNs, &toff); + } + + ecx_mbxhandler(&ctx, 0, 4); + + // if (cycleCount - lastPrintCycle >= printInterval) { + // lastPrintCycle = cycleCount; + // + // printf("\n========== PDO Data Exchange Status ==========\n"); + // printf("Cycle: %d | Success: %d | Fail: %d | WKC Error: %d\n", + // cycleCount, receiveSuccessCount, receiveFailCount, cyclicWkcErrorCount.load()); + // printf("DC time: %lld | toff: %lld\n", (long long)ctx.DCtime, (long long)toff); + // + // for (int slaveIdx = 1; slaveIdx <= slaveCount; slaveIdx++) { + // ec_slavet* slave = &ctx.slavelist[slaveIdx]; + // + // printf("\n[Slave %d] State: 0x%02X | AL Status: 0x%04X\n", + // slaveIdx, slave->state, slave->ALstatuscode); + // + // if (slave->inputs && slave->Ibytes > 0) { + // printf(" Inputs (%d bytes): ", slave->Ibytes); + // for (int i = 0; i < slave->Ibytes && i < 16; i++) { + // printf("%02X ", ((uint8_t*)slave->inputs)[i]); + // } + // if (slave->Ibytes > 16) printf("..."); + // printf("\n"); + // + // if (slave->Ibytes >= 2) { + // uint16_t statusWord = *(uint16_t*)slave->inputs; + // printf(" -> Status Word: 0x%04X", statusWord); + // if (statusWord == 0xFFFF) { + // printf(" (INVALID - All ones)"); + // } else if (statusWord == 0) { + // printf(" (INVALID - All zeros)"); + // } else { + // printf(" ["); + // if (statusWord & 0x0001) printf("ReadyToSwitchOn "); + // if (statusWord & 0x0002) printf("SwitchedOn "); + // if (statusWord & 0x0004) printf("OperationEnabled "); + // if (statusWord & 0x0008) printf("Fault "); + // if (statusWord & 0x0010) printf("VoltageEnabled "); + // if (statusWord & 0x0020) printf("QuickStop "); + // if (statusWord & 0x0040) printf("SwitchOnDisabled "); + // if (statusWord & 0x0080) printf("Warning "); + // printf("]"); + // } + // printf("\n"); + // + // if (slave->Ibytes >= 6) { + // int32_t actualPosition = *(int32_t*)(slave->inputs + 2); + // printf(" -> Actual Position: %d\n", actualPosition); + // } + // } + // } + // + // if (slave->outputs && slave->Obytes > 0) { + // printf(" Outputs (%d bytes): ", slave->Obytes); + // for (int i = 0; i < slave->Obytes && i < 16; i++) { + // printf("%02X ", ((uint8_t*)slave->outputs)[i]); + // } + // if (slave->Obytes > 16) printf("..."); + // printf("\n"); + // + // if (slave->Obytes >= 2) { + // uint16_t controlWord = *(uint16_t*)slave->outputs; + // printf(" <- Control Word: 0x%04X\n", controlWord); + // } + // } + // } + // + // printf("================================================\n\n"); + // + // receiveSuccessCount = 0; + // receiveFailCount = 0; + // } + + } else { + if (cycleCount % 1000 == 0) { + printf("[Cyclic] Waiting for dorun=1, cycle=%d\n", cycleCount); + } + } + } + + printf("[Cyclic Thread] Thread exiting\n"); +} + +void EcMaster::runCheck() { + while (isRunning) { + if (dorun && (cyclicWkcErrorCount > 2 || ctx.grouplist[currentgroup].docheckstate)) { + ctx.grouplist[currentgroup].docheckstate = FALSE; + ecx_readstate(&ctx); + + for (int slaveix = 1; slaveix <= ctx.slavecount; slaveix++) { + ec_slavet *slave = &ctx.slavelist[slaveix]; + + if ((slave->group == currentgroup) && (slave->state != EC_STATE_OPERATIONAL)) { + ctx.grouplist[currentgroup].docheckstate = TRUE; + + if (slave->state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) { + printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slaveix); + slave->state = (EC_STATE_SAFE_OP + EC_STATE_ACK); + ecx_writestate(&ctx, slaveix); + } + else if (slave->state == EC_STATE_SAFE_OP) { + printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slaveix); + slave->state = EC_STATE_OPERATIONAL; + if (slave->mbxhandlerstate == ECT_MBXH_LOST) + slave->mbxhandlerstate = ECT_MBXH_CYCLIC; + ecx_writestate(&ctx, slaveix); + } + else if (slave->state > EC_STATE_NONE) { + if (ecx_reconfig_slave(&ctx, slaveix, EC_TIMEOUTMON) >= EC_STATE_PRE_OP) { + slave->islost = FALSE; + printf("MESSAGE : slave %d reconfigured\n", slaveix); + } + } + else if (!slave->islost) { + ecx_statecheck(&ctx, slaveix, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); + if (slave->state == EC_STATE_NONE) { + slave->islost = TRUE; + slave->mbxhandlerstate = ECT_MBXH_LOST; + if (slave->Ibytes) { + memset(slave->inputs, 0x00, slave->Ibytes); + } + printf("ERROR : slave %d lost\n", slaveix); + } + } + } + + if (slave->islost) { + if (slave->state <= EC_STATE_INIT) { + if (ecx_recover_slave(&ctx, slaveix, EC_TIMEOUTMON)) { + slave->islost = FALSE; + printf("MESSAGE : slave %d recovered\n", slaveix); + } + } + else { + slave->islost = FALSE; + printf("MESSAGE : slave %d found\n", slaveix); + } + } + } + + if (!ctx.grouplist[currentgroup].docheckstate) { + printf("OK : all slaves resumed OPERATIONAL.\n"); + } + } + + osal_usleep(10000); + } +} + +// ==================== PDO 配置 ==================== + +int EcMaster::configurePdo(uint16_t slaveId, const EcPdo* rxPdo, const EcPdo* txPdo) { + uint8_t zero = 0; + int wkc = 0; + + // ========== 步骤1: 进入 PRE-OP ========== + printf("Step 1: Entering PRE-OP...\n"); + if (setState(slaveId, EC_STATE_PRE_OP) != 0) { + printf("Failed to enter PRE-OP\n"); + return -1; + } + printf("Slave %d in PRE-OP\n", slaveId); + osal_usleep(50000); + + // ========== 步骤2: 禁用全局 PDO 分配 ========== + printf("Step 2: Disabling global PDO assignment...\n"); + wkc = ecx_SDOwrite(&ctx, slaveId, 0x1C12, 0x00, FALSE, 1, &zero, EC_TIMEOUTRXM); + printf("ecx_SDOwrite 0x1C12, wkc = %d\n", wkc); + + wkc = ecx_SDOwrite(&ctx, slaveId, 0x1C13, 0x00, FALSE, 1, &zero, EC_TIMEOUTRXM); + printf("ecx_SDOwrite 0x1C13, wkc = %d\n", wkc); + + // ========== 计算输出大小（用于分配影子缓冲区） ========== + int outputSize = 0; + if (rxPdo) { + for (const auto& entry : rxPdo->entries) { + outputSize += (entry.bitlen + 7) / 8; + } + printf("Calculated output size: %d bytes\n", outputSize); + } + + // ========== 步骤3: 配置 RxPDO ========== + if (rxPdo && rxPdo->mapCount > 0) { + printf("Step 3: Configuring RxPDO 0x%04X\n", rxPdo->index); + + wkc = ecx_SDOwrite(&ctx, slaveId, rxPdo->index, 0x00, FALSE, 1, &zero, EC_TIMEOUTRXM); + if (wkc <= 0) { + printf("Failed to disable RxPDO 0x%04X\n", rxPdo->index); + return -1; + } + + uint8_t subidx = 1; + for (const auto& entry : rxPdo->entries) { + uint32_t mapValue = entry.makeMapValue(); + wkc = ecx_SDOwrite(&ctx, slaveId, rxPdo->index, subidx++, FALSE, 4, &mapValue, EC_TIMEOUTRXM); + if (wkc <= 0) { + printf("Failed to write RxPDO entry 0x%04X:%02X\n", entry.index, entry.subindex); + return -1; + } + printf(" Entry: 0x%04X:%02X (%d bits)\n", entry.index, entry.subindex, entry.bitlen); + } + + uint8_t mapCount = rxPdo->mapCount; + wkc = ecx_SDOwrite(&ctx, slaveId, rxPdo->index, 0x00, FALSE, 1, &mapCount, EC_TIMEOUTRXM); + if (wkc <= 0) { + printf("Failed to enable RxPDO 0x%04X\n", rxPdo->index); + return -1; + } + printf("RxPDO 0x%04X configured successfully\n", rxPdo->index); + } + + // ========== 步骤4: 配置 TxPDO ========== + if (txPdo && txPdo->mapCount > 0) { + printf("Step 4: Configuring TxPDO 0x%04X\n", txPdo->index); + + wkc = ecx_SDOwrite(&ctx, slaveId, txPdo->index, 0x00, FALSE, 1, &zero, EC_TIMEOUTRXM); + if (wkc <= 0) { + printf("Failed to disable TxPDO 0x%04X\n", txPdo->index); + return -1; + } + + uint8_t subidx = 1; + for (const auto& entry : txPdo->entries) { + uint32_t mapValue = entry.makeMapValue(); + wkc = ecx_SDOwrite(&ctx, slaveId, txPdo->index, subidx++, FALSE, 4, &mapValue, EC_TIMEOUTRXM); + if (wkc <= 0) { + printf("Failed to write TxPDO entry 0x%04X:%02X\n", entry.index, entry.subindex); + return -1; + } + printf(" Entry: 0x%04X:%02X (%d bits)\n", entry.index, entry.subindex, entry.bitlen); + } + + uint8_t mapCount = txPdo->mapCount; + wkc = ecx_SDOwrite(&ctx, slaveId, txPdo->index, 0x00, FALSE, 1, &mapCount, EC_TIMEOUTRXM); + if (wkc <= 0) { + printf("Failed to enable TxPDO 0x%04X\n", txPdo->index); + return -1; + } + printf("TxPDO 0x%04X configured successfully\n", txPdo->index); + } + + // ========== 步骤5: 应用 PDO 分配 ========== + printf("Step 5: Applying PDO assignment...\n"); + + if (rxPdo && rxPdo->mapCount > 0) { + uint8_t rpdo_map_count = 1; + wkc = ecx_SDOwrite(&ctx, slaveId, 0x1C12, 0x01, FALSE, 2, &rxPdo->index, EC_TIMEOUTRXM); + if (wkc <= 0) { + printf("Failed to write RxPDO index to 0x1C12:01\n"); + return -1; + } + + wkc = ecx_SDOwrite(&ctx, slaveId, 0x1C12, 0x00, FALSE, 1, &rpdo_map_count, EC_TIMEOUTRXM); + if (wkc <= 0) { + printf("Failed to set RxPDO count\n"); + return -1; + } + printf("RxPDO assigned to 0x1C12\n"); + } + + if (txPdo && txPdo->mapCount > 0) { + uint8_t tpdo_map_count = 1; + wkc = ecx_SDOwrite(&ctx, slaveId, 0x1C13, 0x01, FALSE, 2, &txPdo->index, EC_TIMEOUTRXM); + if (wkc <= 0) { + printf("Failed to write TxPDO index to 0x1C13:01\n"); + return -1; + } + + wkc = ecx_SDOwrite(&ctx, slaveId, 0x1C13, 0x00, FALSE, 1, &tpdo_map_count, EC_TIMEOUTRXM); + if (wkc <= 0) { + printf("Failed to set TxPDO count\n"); + return -1; + } + printf("TxPDO assigned to 0x1C13\n"); + } + + // ========== 步骤6: 为这个从站分配影子缓冲区 ========== + if (outputSize > 0) { + // 移除旧的影子缓冲区（如果存在） + auto it = shadowBuffers.find(slaveId); + if (it != shadowBuffers.end()) { + shadowBuffers.erase(it); + } + // 创建新的影子缓冲区 + shadowBuffers.emplace(std::piecewise_construct, + std::forward_as_tuple(slaveId), + std::forward_as_tuple(outputSize)); + printf("Shadow buffer allocated for slave %d: %d bytes\n", slaveId, outputSize); + } + + // ========== 步骤7: 设置 PDO 偏移 ========== + setupPdoOffsets(slaveId, rxPdo, txPdo); + + printf("PDO configuration for slave %d completed\n", slaveId); + return 0; +} + +// ========== setupMappingAndDc - 只需要做映射和DC配置 ========== +int EcMaster::setupMappingAndDc() { + printf("\n========== Step 6: Finalizing mapping and DC configuration ==========\n"); + + // 配置映射 + ctx.manualstatechange = 0; + ecx_config_map_group(&ctx, IOmap, 0); + + // 设置 mappingdone 标志 + mappingdone = 1; + + // 验证并保存所有从站的 PDO 数据指针和偏移 + for (int i = 1; i <= ctx.slavecount; i++) { + inputs[i] = ctx.slavelist[i].inputs; + outputs[i] = ctx.slavelist[i].outputs; + inputSizes[i] = ctx.slavelist[i].Ibytes; + outputSizes[i] = ctx.slavelist[i].Obytes; + + if (outputs[i]) { + outputOffsets[i] = (uint8_t*)outputs[i] - IOmap; + printf("Slave %d: Out offset=%d, size=%d bytes\n", + i, outputOffsets[i], outputSizes[i]); + } + printf("Slave %d: In=%d bytes, Out=%d bytes\n", i, inputSizes[i], outputSizes[i]); + + printf(" SM2 (Outputs): StartAddr=0x%04X, Length=%d\n", + ctx.slavelist[i].SM[2].StartAddr, ctx.slavelist[i].SM[2].SMlength); + printf(" SM3 (Inputs): StartAddr=0x%04X, Length=%d\n", + ctx.slavelist[i].SM[3].StartAddr, ctx.slavelist[i].SM[3].SMlength); + } + + expectedWKC = (ctx.grouplist[0].outputsWKC * 2) + ctx.grouplist[0].inputsWKC; + printf("Expected WKC: %d\n", expectedWKC); + + // 配置分布式时钟 + ecx_configdc(&ctx); + + // 添加所有 CoE 从站到周期邮箱处理 + for (int i = 1; i <= ctx.slavecount; i++) { + if (ctx.slavelist[i].CoEdetails > 0) { + ecx_slavembxcyclic(&ctx, i); + printf("Slave %d added to cyclic mailbox handler\n", i); + } + } + + // 验证影子缓冲区大小是否匹配实际输出大小 + for (int i = 1; i <= ctx.slavecount; i++) { + auto it = shadowBuffers.find(i); + if (it != shadowBuffers.end()) { + size_t shadowSize = it->second.size; + int actualSize = outputSizes[i]; + if (shadowSize != (size_t)actualSize) { + printf("WARNING: Slave %d shadow buffer size (%zu) != actual output size (%d)\n", + i, shadowSize, actualSize); + // 重新分配匹配的大小 + it->second = SlaveShadowBuffer(actualSize); + printf(" Reallocated shadow buffer to %d bytes\n", actualSize); + } + } else if (outputSizes[i] > 0) { + // 如果没有影子缓冲区但有输出数据，创建一个 + shadowBuffers.emplace(std::piecewise_construct, + std::forward_as_tuple(i), + std::forward_as_tuple(outputSizes[i])); + printf("Created shadow buffer for slave %d: %d bytes\n", i, outputSizes[i]); + } + } + + printf("Mapping and DC configuration completed\n"); + return 0; +} + +int EcMaster::setupPdoOffsets(uint16_t slaveId, const EcPdo* rxPdo, const EcPdo* txPdo) { + EcPdoOffsets offsets; + memset(&offsets, 0xFF, sizeof(offsets)); + + // ========== 解析 RxPDO (Outputs) ========== + if (rxPdo) { + uint16_t offset = 0; + printf("RxPDO 0x%04X mapping (%zu entries):\n", rxPdo->index, rxPdo->entries.size()); + + for (const auto& entry : rxPdo->entries) { + uint8_t bytes = (entry.bitlen + 7) / 8; + printf(" Entry 0x%04X:%02X -> offset %d (%d bytes)\n", + entry.index, entry.subindex, offset, bytes); + + switch (entry.index) { + case 0x6040: // Control Word + offsets.controlWord = offset; + printf(" *** Control Word at offset %d ***\n", offset); + break; + case 0x607A: // Target Position + offsets.targetPosition = offset; + break; + case 0x60FF: // Target Velocity + offsets.targetVelocity = offset; + break; + case 0x6071: // Target Torque + offsets.targetTorque = offset; + break; + case 0x6083: // Profile Acceleration + offsets.profileAcceleration = offset; + printf(" *** Profile Acceleration at offset %d ***\n", offset); + break; + case 0x6084: // Profile Deceleration + offsets.profileDeceleration = offset; + printf(" *** Profile Deceleration at offset %d ***\n", offset); + break; + case 0x6081: // Profile Velocity + offsets.profileVelocity = offset; + printf(" *** Profile Velocity at offset %d ***\n", offset); + break; + case 0x6087: // Torque Slope + offsets.torqueSlope = offset; + break; + case 0x6060: // Mode of Operation + offsets.modeOfOperation = offset; + break; + default: + printf(" *** Unknown entry 0x%04X:%02X ***\n", entry.index, entry.subindex); + break; + } + offset += bytes; + } + } + + // ========== 解析 TxPDO (Inputs) ========== + if (txPdo) { + uint16_t offset = 0; + printf("\nTxPDO 0x%04X mapping (%zu entries):\n", txPdo->index, txPdo->entries.size()); + + for (const auto& entry : txPdo->entries) { + uint8_t bytes = (entry.bitlen + 7) / 8; + printf(" Entry 0x%04X:%02X -> offset %d (%d bytes)\n", + entry.index, entry.subindex, offset, bytes); + + switch (entry.index) { + case 0x6041: // Status Word + offsets.statusWord = offset; + printf(" *** Status Word at offset %d ***\n", offset); + break; + case 0x6064: // Actual Position + offsets.actualPosition = offset; + break; + case 0x606C: // Actual Velocity + offsets.actualVelocity = offset; + break; + case 0x6077: // Actual Torque + offsets.actualTorque = offset; + break; + case 0x6061: // Mode of Operation Display + offsets.modeOfOperationDisplay = offset; + break; + case 0x603F: // Error Code + offsets.errorCode = offset; + break; + case 0x6078: // Actual Current + offsets.actualCurrent = offset; + printf(" *** Actual Current at offset %d ***\n", offset); + break; + default: + printf(" *** Unknown entry 0x%04X:%02X ***\n", entry.index, entry.subindex); + break; + } + offset += bytes; + } + } + + pdoOffsets[slaveId] = offsets; + + // ========== 打印摘要 ========== + printf("\n=== PDO Offset Summary ===\n"); + printf("RxPDO (Outputs):\n"); + printf(" Control Word: %d %s\n", offsets.controlWord, + offsets.controlWord == 0xFFFF ? "(INVALID)" : ""); + printf(" Target Position: %d %s\n", offsets.targetPosition, + offsets.targetPosition == 0xFFFF ? "(INVALID)" : ""); + printf(" Target Velocity: %d %s\n", offsets.targetVelocity, + offsets.targetVelocity == 0xFFFF ? "(INVALID)" : ""); + printf(" Target Torque: %d %s\n", offsets.targetTorque, + offsets.targetTorque == 0xFFFF ? "(INVALID)" : ""); + printf(" Profile Velocity: %d %s\n", offsets.profileVelocity, + offsets.profileVelocity == 0xFFFF ? "(NOT IN PDO)" : ""); + printf(" Profile Acc: %d %s\n", offsets.profileAcceleration, + offsets.profileAcceleration == 0xFFFF ? "(NOT IN PDO)" : ""); + printf(" Profile Dec: %d %s\n", offsets.profileDeceleration, + offsets.profileDeceleration == 0xFFFF ? "(NOT IN PDO)" : ""); + printf(" Mode of Operation: %d %s\n", offsets.modeOfOperation, + offsets.modeOfOperation == 0xFFFF ? "(INVALID)" : ""); + + printf("\nTxPDO (Inputs):\n"); + printf(" Status Word: %d %s\n", offsets.statusWord, + offsets.statusWord == 0xFFFF ? "(INVALID)" : ""); + printf(" Actual Position: %d %s\n", offsets.actualPosition, + offsets.actualPosition == 0xFFFF ? "(INVALID)" : ""); + printf(" Actual Velocity: %d %s\n", offsets.actualVelocity, + offsets.actualVelocity == 0xFFFF ? "(INVALID)" : ""); + printf(" Actual Torque: %d %s\n", offsets.actualTorque, + offsets.actualTorque == 0xFFFF ? "(INVALID)" : ""); + printf(" Mode Display: %d %s\n", offsets.modeOfOperationDisplay, + offsets.modeOfOperationDisplay == 0xFFFF ? "(INVALID)" : ""); + printf(" Error Code: %d %s\n", offsets.errorCode, + offsets.errorCode == 0xFFFF ? "(NOT IN PDO)" : ""); + printf(" Actual Current: %d %s\n", offsets.actualCurrent, + offsets.actualCurrent == 0xFFFF ? "(NOT IN PDO)" : ""); + printf("=================================\n"); + + if (offsets.controlWord == 0xFFFF || offsets.statusWord == 0xFFFF) { + printf("ERROR: Critical PDO offsets not configured!\n"); + return -1; + } + + return 0; +} + +// ==================== PDO 访问接口 ==================== + +void EcMaster::setControlWord(uint16_t slaveId, uint16_t value) { + if (inOp) { + writePdo(slaveId, pdoOffsets[slaveId].controlWord, value); + } else { + writeSdo(slaveId, 0x6040, 0, &value, EcDataType::Uint16, 100); + } +} + +uint16_t EcMaster::getStatusWord(uint16_t slaveId) { + if (inOp) { + return readPdo(slaveId, pdoOffsets[slaveId].statusWord); + } + uint16_t value; + readSdo(slaveId, 0x6041, 0, &value, EcDataType::Uint16, 100); + return value; +} + +void EcMaster::setTargetPosition(uint16_t slaveId, int32_t value) { + if (inOp) { + writePdo(slaveId, pdoOffsets[slaveId].targetPosition, value); + } else { + writeSdo(slaveId, 0x607A, 0, &value, EcDataType::Int32, 100); + } +} + +int32_t EcMaster::getActualPosition(uint16_t slaveId) { + if (inOp) { + return readPdo(slaveId, pdoOffsets[slaveId].actualPosition); + } + int32_t value; + readSdo(slaveId, 0x6064, 0, &value, EcDataType::Int32, 100); + return value; +} + +void EcMaster::setTargetVelocity(uint16_t slaveId, int32_t value) { + if (inOp) { + writePdo(slaveId, pdoOffsets[slaveId].targetVelocity, value); + } else { + writeSdo(slaveId, 0x60FF, 0, &value, EcDataType::Int32, 100); + } +} + +int32_t EcMaster::getActualVelocity(uint16_t slaveId) { + if (inOp) { + return readPdo(slaveId, pdoOffsets[slaveId].actualVelocity); + } + int32_t value; + readSdo(slaveId, 0x606C, 0, &value, EcDataType::Int32, 100); + return value; +} + +void EcMaster::setTargetTorque(uint16_t slaveId, int16_t value) { + if (inOp) { + writePdo(slaveId, pdoOffsets[slaveId].targetTorque, value); + } else { + writeSdo(slaveId, 0x6071, 0, &value, EcDataType::Int16, 100); + } +} + +int16_t EcMaster::getActualTorque(uint16_t slaveId) { + if (inOp) { + return readPdo(slaveId, pdoOffsets[slaveId].actualTorque); + } + int16_t value; + readSdo(slaveId, 0x6077, 0, &value, EcDataType::Int16, 100); + return value; +} + +void EcMaster::setModeOfOperation(uint16_t slaveId, int8_t value) { + if (inOp) { + writePdo(slaveId, pdoOffsets[slaveId].modeOfOperation, value); + } else { + writeSdo(slaveId, 0x6060, 0, &value, EcDataType::Int8, 100); + } +} + +int8_t EcMaster::getModeOfOperationDisplay(uint16_t slaveId) { + if (inOp) { + return readPdo(slaveId, pdoOffsets[slaveId].modeOfOperationDisplay); + } + int8_t value; + readSdo(slaveId, 0x6061, 0, &value, EcDataType::Int8, 100); + return value; +} + +void EcMaster::setProfileVelocity(uint16_t slaveId, uint32_t value) { + if (inOp) { + writePdo(slaveId, pdoOffsets[slaveId].profileVelocity, value); + } +} + +void EcMaster::setProfileAcceleration(uint16_t slaveId, uint32_t value) { + if (inOp) { + writePdo(slaveId, pdoOffsets[slaveId].profileAcceleration, value); + } +} + +void EcMaster::setProfileDeceleration(uint16_t slaveId, uint32_t value) { + if (inOp) { + writePdo(slaveId, pdoOffsets[slaveId].profileDeceleration, value); + } +} + +int16_t EcMaster::getCurrentValue(uint16_t slaveId) { + if (inOp) { + return readPdo(slaveId, pdoOffsets[slaveId].actualCurrent); + } + int16_t value; + readSdo(slaveId, 0x6078, 0, &value, EcDataType::Int16, 100); + return value; +} + +uint16_t EcMaster::getErrorCode(uint16_t slaveId) { + if (inOp) { + auto it = pdoOffsets.find(slaveId); + if (it != pdoOffsets.end() && it->second.errorCode != 0xFFFF) { + return readPdo(slaveId, it->second.errorCode); + } + } + uint16_t value = 0; + readSdo(slaveId, 0x603F, 0, &value, EcDataType::Uint16, 100); + return value; +} + +// ==================== 电机控制高级接口 ==================== + +int EcMaster::enable(uint16_t slaveId) { + if (inOp) { + setControlWord(slaveId, 0x0006); + osal_usleep(10000); + setControlWord(slaveId, 0x0007); + osal_usleep(10000); + setControlWord(slaveId, 0x000F); + } else { + uint16_t val = 0x0006; + writeSdo(slaveId, 0x6040, 0, &val, EcDataType::Uint16, 100); + osal_usleep(10000); + val = 0x0007; + writeSdo(slaveId, 0x6040, 0, &val, EcDataType::Uint16, 100); + osal_usleep(10000); + val = 0x000F; + writeSdo(slaveId, 0x6040, 0, &val, EcDataType::Uint16, 100); + } + osal_usleep(10000); + return 0; +} + +int EcMaster::disable(uint16_t slaveId) { + if (inOp) { + setControlWord(slaveId, 0x0007); + osal_usleep(10000); + setControlWord(slaveId, 0x0000); + } else { + uint16_t val = 0x0007; + writeSdo(slaveId, 0x6040, 0, &val, EcDataType::Uint16, 100); + osal_usleep(10000); + val = 0x0000; + writeSdo(slaveId, 0x6040, 0, &val, EcDataType::Uint16, 100); + } + return 0; +} + +int EcMaster::faultReset(uint16_t slaveId) { + if (inOp) { + setControlWord(slaveId, 0x0080); + osal_usleep(10000); + setControlWord(slaveId, 0x0000); + } else { + uint16_t val = 0x0080; + writeSdo(slaveId, 0x6040, 0, &val, EcDataType::Uint16, 100); + osal_usleep(10000); + val = 0x0000; + writeSdo(slaveId, 0x6040, 0, &val, EcDataType::Uint16, 100); + } + return 0; +} + +// ==================== SDO 读写 ==================== + +int EcMaster::readSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, void* value, EcDataType type, int timeoutMs) { + int size = 0; + switch (type) { + case EcDataType::Boolean: + case EcDataType::Int8: + case EcDataType::Uint8: + size = 1; break; + case EcDataType::Int16: + case EcDataType::Uint16: + size = 2; break; + case EcDataType::Int32: + case EcDataType::Uint32: + case EcDataType::Real32: + size = 4; break; + case EcDataType::Real64: + size = 8; break; + default: return -1; + } + int wkc = ecx_SDOread(&ctx, slaveId, index, subindex, FALSE, &size, value, timeoutMs); + return wkc > 0 ? 0 : -1; +} + +int EcMaster::writeSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, void* value, EcDataType type, int timeoutMs) { + int size = 0; + switch (type) { + case EcDataType::Boolean: + case EcDataType::Int8: + case EcDataType::Uint8: + size = 1; break; + case EcDataType::Int16: + case EcDataType::Uint16: + size = 2; break; + case EcDataType::Int32: + case EcDataType::Uint32: + case EcDataType::Real32: + size = 4; break; + case EcDataType::Real64: + size = 8; break; + default: return -1; + } + int wkc = ecx_SDOwrite(&ctx, slaveId, index, subindex, FALSE, size, value, timeoutMs); + return wkc > 0 ? 0 : -1; +} + +// ==================== SM/FMMU 配置校验 ==================== + +void EcMaster::verifySmConfiguration(uint16_t slaveId) { + if (slaveId > (uint16_t)ctx.slavecount) { + printf("Invalid slave ID: %d\n", slaveId); + return; + } + + ec_slavet* slave = &ctx.slavelist[slaveId]; + + printf("\n========== SM Configuration for Slave %d ==========\n", slaveId); + + printf("PDO Mapping Sizes:\n"); + printf(" Outputs (Obytes): %d bytes\n", slave->Obytes); + printf(" Inputs (Ibytes): %d bytes\n", slave->Ibytes); + + printf("\nSync Manager Configuration:\n"); + const char* sm_names[] = {"MBoxOut", "MBoxIn", "Outputs", "Inputs"}; + for (int sm = 0; sm < 4; sm++) { + printf(" SM%d (%s): StartAddr=0x%04X, Length=%d bytes\n", + sm, sm_names[sm], + slave->SM[sm].StartAddr, + slave->SM[sm].SMlength); + } + + printf("\nFMMU Configuration:\n"); + printf(" Outputs mapped to IOmap at offset: %td\n", + (uint8_t*)slave->outputs - IOmap); + printf(" Inputs mapped to IOmap at offset: %td\n", + (uint8_t*)slave->inputs - IOmap); + + // 打印影子缓冲区信息 + auto it = shadowBuffers.find(slaveId); + if (it != shadowBuffers.end()) { + printf("\nShadow Buffer:\n"); + printf(" Size: %zu bytes\n", it->second.size); + printf(" Dirty: %s\n", it->second.dirty ? "true" : "false"); + } else { + printf("\nShadow Buffer: Not allocated\n"); + } + + printf("==================================================\n"); +} + +void EcMaster::verifyAllSlavesSmConfiguration() { + for (int i = 1; i <= ctx.slavecount; i++) { + verifySmConfiguration((uint16_t)i); + } +} \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_master.h b/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_master.h new file mode 100644 index 00000000..e4b67ad0 --- /dev/null +++ b/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 +#include +#include +#include + +#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 + int readSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, T* value, int timeoutMs = 100) { + EcDataType type; + if constexpr (std::is_same_v) type = EcDataType::Int8; + else if constexpr (std::is_same_v) type = EcDataType::Uint8; + else if constexpr (std::is_same_v) type = EcDataType::Int16; + else if constexpr (std::is_same_v) type = EcDataType::Uint16; + else if constexpr (std::is_same_v) type = EcDataType::Int32; + else if constexpr (std::is_same_v) type = EcDataType::Uint32; + else if constexpr (std::is_same_v) type = EcDataType::Real32; + else if constexpr (std::is_same_v) type = EcDataType::Real64; + else return -1; + return readSdo(slaveId, index, subindex, value, type, timeoutMs); + } + + template + int writeSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, T* value, int timeoutMs = 100) { + EcDataType type; + if constexpr (std::is_same_v) type = EcDataType::Int8; + else if constexpr (std::is_same_v) type = EcDataType::Uint8; + else if constexpr (std::is_same_v) type = EcDataType::Int16; + else if constexpr (std::is_same_v) type = EcDataType::Uint16; + else if constexpr (std::is_same_v) type = EcDataType::Int32; + else if constexpr (std::is_same_v) type = EcDataType::Uint32; + else if constexpr (std::is_same_v) type = EcDataType::Real32; + else if constexpr (std::is_same_v) 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 shadowBuffers; + std::atomic anyPdoDirty{false}; // 是否有任何影子缓冲区有更新 + + int slaveCount = 0; + int expectedWKC = 0; + int currentgroup = 0; + + // 状态标志 + std::atomic inOp{false}; + std::atomic isRunning{false}; + std::atomic mappingdone{0}; + std::atomic dorun{0}; + std::atomic cyclicWkcErrorCount{0}; + + int64_t cycleTimeNs = 1000000; + + OSAL_THREAD_HANDLE cyclicThread; + OSAL_THREAD_HANDLE checkThread; + + // PDO 数据指针映射 + std::map inputs; + std::map outputs; + std::map inputSizes; + std::map outputSizes; + std::map outputOffsets; + + // PDO 偏移映射 + std::map pdoOffsets; + + // 辅助函数 + template + void writePdo(uint16_t slaveId, uint16_t offset, T value); + + template + 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 +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 +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 \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_pdo_config.h b/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_pdo_config.h new file mode 100644 index 00000000..7f8a919d --- /dev/null +++ b/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 \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_types.h b/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_types.h new file mode 100644 index 00000000..174f2460 --- /dev/null +++ b/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 +#include +#include + +// 数据类型枚举 +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 entries; + std::string name; + bool isRxPdo = false; + + void updateMapCount() { + mapCount = entries.size(); + } +}; + +// 模块信息 +struct EcModule { + uint32_t moduleIdent = 0; + std::string name; + std::vector rxPdos; + std::vector txPdos; +}; + +// 从站配置信息 +struct EcSlaveConfig { + uint16_t slaveId = 0; + uint32_t productCode = 0; + uint32_t revisionNo = 0; + std::string name; + std::vector modules; + std::vector availableRxPdos; + std::vector 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 \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_xml_parser.cpp b/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_xml_parser.cpp new file mode 100644 index 00000000..c721e3fe --- /dev/null +++ b/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 +#include +#include + +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 slaveConfigs; + std::unique_ptr xmlDoc; + + bool loadXmlFile(const std::string& path) { + try { + xmlDoc = std::make_unique(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 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 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()) {} +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 EcXmlParser::getAvailableRxPdos(uint16_t slaveId) const { + auto it = pImpl->slaveConfigs.find(slaveId); + return (it != pImpl->slaveConfigs.end()) ? it->second.availableRxPdos : std::vector(); +} + +std::vector EcXmlParser::getAvailableTxPdos(uint16_t slaveId) const { + auto it = pImpl->slaveConfigs.find(slaveId); + return (it != pImpl->slaveConfigs.end()) ? it->second.availableTxPdos : std::vector(); +} + +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(); +} \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_xml_parser.h b/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_xml_parser.h new file mode 100644 index 00000000..c7b72536 --- /dev/null +++ b/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 +#include +#include + +// 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 getAvailableRxPdos(uint16_t slaveId) const; + std::vector 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 pImpl; +}; + +#endif // EC_XML_PARSER_H \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/eyou_motor.cpp b/cmvr-es/devices/motor/eyou_motor/eyou_motor.cpp new file mode 100644 index 00000000..d9e2f043 --- /dev/null +++ b/cmvr-es/devices/motor/eyou_motor/eyou_motor.cpp @@ -0,0 +1,5 @@ +// +// Created by cmvr on 2026/3/30. +// + +#include "eyou_motor.h" \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/eyou_motor.h b/cmvr-es/devices/motor/eyou_motor/eyou_motor.h new file mode 100644 index 00000000..da4d8ae3 --- /dev/null +++ b/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(protocol_); + ethercat_protocol->initNode(node_id_); + ethercat_protocol->setQdd(node_id_,1); + } + } + }; + + + } +} diff --git a/cmvr-es/devices/motor/motor_protocol_interface.h b/cmvr-es/devices/motor/motor_protocol_interface.h index 634a715c..6b49aa06 100644 --- a/cmvr-es/devices/motor/motor_protocol_interface.h +++ b/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; diff --git a/cmvr-es/devices/motor/ti5_motor/ti5_motor.h b/cmvr-es/devices/motor/ti5_motor/ti5_motor.h index cfc9bde9..65f44ddc 100644 --- a/cmvr-es/devices/motor/ti5_motor/ti5_motor.h +++ b/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; + } + + /** * 电机初始化在具体的类中完成 */ diff --git a/cmvr-es/devices/robot/abstract_robot.h b/cmvr-es/devices/robot/abstract_robot.h index 5759c3af..91daf0ae 100644 --- a/cmvr-es/devices/robot/abstract_robot.h +++ b/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; diff --git a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h index 7770997b..9cd2d11d 100644 --- a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h +++ b/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 is_head_busy_{false}, is_waist_busy_{false}; + std::string urdf_path_ = ""; }; diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp index 0146c053..601125cc 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp @@ -49,6 +49,87 @@ HumanoidRobot::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { } } + +template +HumanoidRobot::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 >(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 >(m_robot_); + upd_freq_ = config.updfreq(); + CSP_buffer_ = make_shared >(config.buffersize()); + CSV_buffer_ = make_shared >(config.buffersize()); + CSC_buffer_ = make_shared >(config.buffersize()); + + upd_freq_ = 1000; + upd_timer_ = make_shared(); + 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 +HumanoidRobot::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 >(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 >(m_robot_); + upd_freq_ = config.updfreq(); + CSP_buffer_ = make_shared >(config.buffersize()); + CSV_buffer_ = make_shared >(config.buffersize()); + CSC_buffer_ = make_shared >(config.buffersize()); + + upd_freq_ = 1000; + upd_timer_ = make_shared(); + 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 void HumanoidRobot::init() { motor_manager_ = MotorsInfo::getInstance()->getMotorManager(); diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp index d1b47e4e..ed16d61d 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp +++ b/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("ec_robot_01"); + auto robot = std::dynamic_pointer_cast>(robot_abs); + ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>"; + + std::vector 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)); +} diff --git a/cmvr-es/main.cpp b/cmvr-es/main.cpp index ab0f76d3..24f189a5 100644 --- a/cmvr-es/main.cpp +++ b/cmvr-es/main.cpp @@ -9,6 +9,7 @@ #include #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; diff --git a/cmvr-es/monitor/abstract_monitor.h b/cmvr-es/monitor/abstract_monitor.h index 6b707e9c..244e5722 100644 --- a/cmvr-es/monitor/abstract_monitor.h +++ b/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); diff --git a/cmvr-es/monitor/diskmonitor/include/disk_monitor.h b/cmvr-es/monitor/diskmonitor/include/disk_monitor.h index 89664362..a22256b3 100644 --- a/cmvr-es/monitor/diskmonitor/include/disk_monitor.h +++ b/cmvr-es/monitor/diskmonitor/include/disk_monitor.h @@ -8,17 +8,21 @@ #include #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 folders_; std::vector dirsToClean_; // 需要清理的目录列表 mutable std::mutex mutex_; // 保护共享资源的互斥锁 @@ -27,6 +31,4 @@ namespace cmvr::monitor { }; } - - #endif //FOLDER_MONITOR_H \ No newline at end of file diff --git a/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp b/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp index 20630615..9f4497cf 100644 --- a/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp +++ b/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 lock(mutex_); bool success = true; @@ -183,6 +191,6 @@ bool DiskMonitor::execution_() { deleteOldestFile(path); } - dirsToClean_.clear(); // 清理完成后清空列表 + dirsToClean_.clear(); return success; } \ No newline at end of file diff --git a/cmvr-es/monitor_manager/include/monitor_factory.h b/cmvr-es/monitor_manager/include/monitor_factory.h index a0dbc900..cdd879a3 100644 --- a/cmvr-es/monitor_manager/include/monitor_factory.h +++ b/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 std::shared_ptr create(const XmlNode& cfg); std::shared_ptr create(const XmlNode& cfg); + + // 新增proto方式 + template + std::shared_ptr create(const cmvr::config::DiskMonitorConfig& config); + std::shared_ptr create(const cmvr::config::DiskMonitorConfig& config); }; - - } - - #endif //MONITOR_FACTORY_H \ No newline at end of file diff --git a/cmvr-es/monitor_manager/include/monitor_manager.h b/cmvr-es/monitor_manager/include/monitor_manager.h index 8c53a401..422ece1f 100644 --- a/cmvr-es/monitor_manager/include/monitor_manager.h +++ b/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); - void init_monitors(); + // 原有的XML方式 + 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 instance_; - XmlNode cfg_; + // 两种配置方式，只有一个有效 + bool use_proto_ = false; + XmlNode cfg_; // XML配置 + cmvr::config::MonitorManagerConfig proto_cfg_; // Proto配置 + std::shared_ptr monitor_factory_; std::unordered_map> monitors_; }; } - - #endif //MONITOR_MANAGER_H \ No newline at end of file diff --git a/cmvr-es/monitor_manager/src/monitor_factory.cpp b/cmvr-es/monitor_manager/src/monitor_factory.cpp index 821373e2..e770f294 100644 --- a/cmvr-es/monitor_manager/src/monitor_factory.cpp +++ b/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 MonitorFactory::create(const XmlNode&); template @@ -17,8 +17,30 @@ std::shared_ptr MonitorFactory::create(const XmlNode& cfg) { return std::make_shared(cfg); } else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type"; - throw runtime_error("[DeviceFactory]: Unsupported device type"); + LOG(ERROR) << "[MonitorFactory]: Unsupported monitor type"; + throw runtime_error("[MonitorFactory]: Unsupported monitor type"); } } +// Proto方式的模板特化 +template std::shared_ptr MonitorFactory::create(const cmvr::config::DiskMonitorConfig&); + +template +std::shared_ptr MonitorFactory::create(const cmvr::config::DiskMonitorConfig& config) { + if constexpr (std::is_same_v) { + return std::make_shared(config); + } + else { + LOG(ERROR) << "[MonitorFactory]: Unsupported monitor type"; + throw runtime_error("[MonitorFactory]: Unsupported monitor type"); + } +} + +std::shared_ptr MonitorFactory::create(const XmlNode& cfg) { + // 保留原有实现 + return nullptr; +} + +std::shared_ptr MonitorFactory::create(const cmvr::config::DiskMonitorConfig& config) { + return std::make_shared(config); +} \ No newline at end of file diff --git a/cmvr-es/monitor_manager/src/monitor_manager.cpp b/cmvr-es/monitor_manager/src/monitor_manager.cpp index 2dd9e2cf..dde9b386 100644 --- a/cmvr-es/monitor_manager/src/monitor_manager.cpp +++ b/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::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(); - cfg_ = cfg; init_monitors(); } + +// Proto方式构造函数 +MonitorManager::MonitorManager(const cmvr::config::MonitorManagerConfig& config) : use_proto_(true), proto_cfg_(config) { + monitor_factory_ = make_shared(); + 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_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(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()); + } } \ No newline at end of file diff --git a/cmvr-es/service/grpc/include/server_runner.h b/cmvr-es/service/grpc/include/server_runner.h index be8eadf8..e148c811 100644 --- a/cmvr-es/service/grpc/include/server_runner.h +++ b/cmvr-es/service/grpc/include/server_runner.h @@ -11,13 +11,14 @@ #include #include #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 server_; std::string address_; diff --git a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp index 487fe705..0c4e8469 100644 --- a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp +++ b/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("/home/linbo/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf" + pinocchio_ik_ = std::make_shared("/home/cmvr/Projects/cmvr-es/model/xiaoyan_description/dual_arm.urdf" ,"PELVIS_S" ,"R_WRIST_R_S"); pinocchio_ik_->init(); diff --git a/cmvr-es/service/grpc/src/server_runner.cpp b/cmvr-es/service/grpc/src/server_runner.cpp index dc689aff..ec04091a 100644 --- a/cmvr-es/service/grpc/src/server_runner.cpp +++ b/cmvr-es/service/grpc/src/server_runner.cpp @@ -25,23 +25,21 @@ using namespace cmvr::service; ServerRunner::ServerRunner() = default; -ServerRunner::ServerRunner(XmlNode cfg) { - start(std::move(cfg)); +ServerRunner::ServerRunner(const cmvr::config::ServerConfig& config):cfg_(config) +{ + // start(); } - ServerRunner::~ServerRunner() { stop(); join(); } -void ServerRunner::start(XmlNode cfg) { +void ServerRunner::start() { std::unique_lock 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"); - 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 port = cfg_.grpc_server().port(); std::string local_address = "0.0.0.0:" + port; auto camera_service = std::make_unique(); diff --git a/dependency/x86/third_party/soem/include/soem/ec_base.h b/dependency/x86/third_party/soem/include/soem/ec_base.h new file mode 100644 index 00000000..3eafa121 --- /dev/null +++ b/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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_coe.h b/dependency/x86/third_party/soem/include/soem/ec_coe.h new file mode 100644 index 00000000..45c889e5 --- /dev/null +++ b/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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_config.h b/dependency/x86/third_party/soem/include/soem/ec_config.h new file mode 100644 index 00000000..c0291177 --- /dev/null +++ b/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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_dc.h b/dependency/x86/third_party/soem/include/soem/ec_dc.h new file mode 100644 index 00000000..d0472dd3 --- /dev/null +++ b/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 */ diff --git a/dependency/x86/third_party/soem/include/soem/ec_eoe.h b/dependency/x86/third_party/soem/include/soem/ec_eoe.h new file mode 100644 index 00000000..27ca216e --- /dev/null +++ b/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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_foe.h b/dependency/x86/third_party/soem/include/soem/ec_foe.h new file mode 100644 index 00000000..6348b08a --- /dev/null +++ b/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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_main.h b/dependency/x86/third_party/soem/include/soem/ec_main.h new file mode 100644 index 00000000..70189802 --- /dev/null +++ b/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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_options.h b/dependency/x86/third_party/soem/include/soem/ec_options.h new file mode 100644 index 00000000..a6b3d7f9 --- /dev/null +++ b/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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_print.h b/dependency/x86/third_party/soem/include/soem/ec_print.h new file mode 100644 index 00000000..920f3dac --- /dev/null +++ b/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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_soe.h b/dependency/x86/third_party/soem/include/soem/ec_soe.h new file mode 100644 index 00000000..e6dbf3ee --- /dev/null +++ b/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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_type.h b/dependency/x86/third_party/soem/include/soem/ec_type.h new file mode 100644 index 00000000..faca1707 --- /dev/null +++ b/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 */ diff --git a/dependency/x86/third_party/soem/include/soem/nicdrv.h b/dependency/x86/third_party/soem/include/soem/nicdrv.h new file mode 100644 index 00000000..010d8efc --- /dev/null +++ b/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 + +/** 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 diff --git a/dependency/x86/third_party/soem/include/soem/osal.h b/dependency/x86/third_party/soem/include/soem/osal.h new file mode 100644 index 00000000..f88bce85 --- /dev/null +++ b/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 +#include + +/* 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 diff --git a/dependency/x86/third_party/soem/include/soem/osal_defs.h b/dependency/x86/third_party/soem/include/soem/osal_defs.h new file mode 100644 index 00000000..3d6ad511 --- /dev/null +++ b/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 +#include + +// define if debug printf is needed +#ifdef EC_DEBUG +#include +#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 diff --git a/dependency/x86/third_party/soem/include/soem/soem.h b/dependency/x86/third_party/soem/include/soem/soem.h new file mode 100644 index 00000000..e4d43a8e --- /dev/null +++ b/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 */ diff --git a/dependency/x86/third_party/soem/lib/libsoem.a b/dependency/x86/third_party/soem/lib/libsoem.a new file mode 100644 index 00000000..51103678 Binary files /dev/null and b/dependency/x86/third_party/soem/lib/libsoem.a differ diff --git a/protos/cmvr/config/biohead_config/biohead_config.proto b/protos/cmvr/config/biohead_config/biohead_config.proto new file mode 100644 index 00000000..80c7d052 --- /dev/null +++ b/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; +} \ No newline at end of file diff --git a/protos/cmvr/config/monitor_config/monitor_config.proto b/protos/cmvr/config/monitor_config/monitor_config.proto new file mode 100644 index 00000000..368c4660 --- /dev/null +++ b/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; // 磁盘监控配置 +} \ No newline at end of file diff --git a/protos/cmvr/config/robot_config/robot_config.proto b/protos/cmvr/config/robot_config/robot_config.proto new file mode 100644 index 00000000..1838f1eb --- /dev/null +++ b/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机器人 +} \ No newline at end of file diff --git a/protos/cmvr/config/server_config/server_config.proto b/protos/cmvr/config/server_config/server_config.proto new file mode 100644 index 00000000..911b12ca --- /dev/null +++ b/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; +} \ No newline at end of file diff --git a/request.txt b/request.txt index 9a4d60e2..9e9809c7 100644 --- a/request.txt +++ b/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