From a75b5cf358afb9c2a5b63de48ecf4e358bf72c1e Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Sat, 27 Apr 2024 10:25:57 +0000 Subject: [PATCH] Fixes SDO --- ethercat_driver/src/ethercat_driver.cpp | 764 ++++++++++-------- .../include/ethercat_interface/ec_master.hpp | 292 +++---- ethercat_interface/src/ec_master.cpp | 756 +++++++++-------- 3 files changed, 921 insertions(+), 891 deletions(-) diff --git a/ethercat_driver/src/ethercat_driver.cpp b/ethercat_driver/src/ethercat_driver.cpp index ae4cb980..04422bc8 100644 --- a/ethercat_driver/src/ethercat_driver.cpp +++ b/ethercat_driver/src/ethercat_driver.cpp @@ -21,419 +21,465 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" -namespace ethercat_driver -{ -CallbackReturn EthercatDriver::on_init( - const hardware_interface::HardwareInfo & info) -{ - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { - return CallbackReturn::ERROR; - } +namespace ethercat_driver { + CallbackReturn + EthercatDriver::on_init(const hardware_interface::HardwareInfo &info) { + if (hardware_interface::SystemInterface::on_init(info) != + CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } - hw_joint_states_.resize(info_.joints.size()); - for (uint j = 0; j < info_.joints.size(); j++) { - hw_joint_states_[j].resize( - info_.joints[j].state_interfaces.size(), - std::numeric_limits::quiet_NaN()); - } - hw_sensor_states_.resize(info_.sensors.size()); - for (uint s = 0; s < info_.sensors.size(); s++) { - hw_sensor_states_[s].resize( - info_.sensors[s].state_interfaces.size(), - std::numeric_limits::quiet_NaN()); - } - hw_gpio_states_.resize(info_.gpios.size()); - for (uint g = 0; g < info_.gpios.size(); g++) { - hw_gpio_states_[g].resize( - info_.gpios[g].state_interfaces.size(), - std::numeric_limits::quiet_NaN()); - } - hw_joint_commands_.resize(info_.joints.size()); - for (uint j = 0; j < info_.joints.size(); j++) { - hw_joint_commands_[j].resize( - info_.joints[j].command_interfaces.size(), - std::numeric_limits::quiet_NaN()); - } - hw_sensor_commands_.resize(info_.sensors.size()); - for (uint s = 0; s < info_.sensors.size(); s++) { - hw_sensor_commands_[s].resize( - info_.sensors[s].command_interfaces.size(), - std::numeric_limits::quiet_NaN()); - } - hw_gpio_commands_.resize(info_.gpios.size()); - for (uint g = 0; g < info_.gpios.size(); g++) { - hw_gpio_commands_[g].resize( - info_.gpios[g].command_interfaces.size(), - std::numeric_limits::quiet_NaN()); - } + hw_joint_states_.resize(info_.joints.size()); + for (uint j = 0; j < info_.joints.size(); j++) { + hw_joint_states_[j].resize( + info_.joints[j].state_interfaces.size(), + std::numeric_limits::quiet_NaN() + ); + } + hw_sensor_states_.resize(info_.sensors.size()); + for (uint s = 0; s < info_.sensors.size(); s++) { + hw_sensor_states_[s].resize( + info_.sensors[s].state_interfaces.size(), + std::numeric_limits::quiet_NaN() + ); + } + hw_gpio_states_.resize(info_.gpios.size()); + for (uint g = 0; g < info_.gpios.size(); g++) { + hw_gpio_states_[g].resize( + info_.gpios[g].state_interfaces.size(), + std::numeric_limits::quiet_NaN() + ); + } + hw_joint_commands_.resize(info_.joints.size()); + for (uint j = 0; j < info_.joints.size(); j++) { + hw_joint_commands_[j].resize( + info_.joints[j].command_interfaces.size(), + std::numeric_limits::quiet_NaN() + ); + } + hw_sensor_commands_.resize(info_.sensors.size()); + for (uint s = 0; s < info_.sensors.size(); s++) { + hw_sensor_commands_[s].resize( + info_.sensors[s].command_interfaces.size(), + std::numeric_limits::quiet_NaN() + ); + } + hw_gpio_commands_.resize(info_.gpios.size()); + for (uint g = 0; g < info_.gpios.size(); g++) { + hw_gpio_commands_[g].resize( + info_.gpios[g].command_interfaces.size(), + std::numeric_limits::quiet_NaN() + ); + } - for (uint j = 0; j < info_.joints.size(); j++) { - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joint: %s", info_.joints[j].name.c_str()); - // check all joints for EC modules and load into ec_modules_ - auto module_params = getEcModuleParam(info_.original_xml, info_.joints[j].name, "joint"); - ec_module_parameters_.insert( - ec_module_parameters_.end(), module_params.begin(), module_params.end()); - for (auto i = 0ul; i < module_params.size(); i++) { - for (auto k = 0ul; k < info_.joints[j].state_interfaces.size(); k++) { - module_params[i]["state_interface/" + - info_.joints[j].state_interfaces[k].name] = std::to_string(k); - } - for (auto k = 0ul; k < info_.joints[j].command_interfaces.size(); k++) { - module_params[i]["command_interface/" + - info_.joints[j].command_interfaces[k].name] = std::to_string(k); - } - try { - auto module = ec_loader_.createSharedInstance(module_params[i].at("plugin")); - if (!module->setupSlave( - module_params[i], &hw_joint_states_[j], &hw_joint_commands_[j], info_.joints[j].name)) - { + for (uint j = 0; j < info_.joints.size(); j++) { + RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), "joint: %s", + info_.joints[j].name.c_str() + ); + // check all joints for EC modules and load into ec_modules_ + auto module_params = + getEcModuleParam(info_.original_xml, info_.joints[j].name, "joint"); + ec_module_parameters_.insert( + ec_module_parameters_.end(), module_params.begin(), + module_params.end() + ); + for (auto i = 0ul; i < module_params.size(); i++) { + for (auto k = 0ul; k < info_.joints[j].state_interfaces.size(); k++) { + module_params[i] + ["state_interface/" + + info_.joints[j].state_interfaces[k].name] = + std::to_string(k); + } + for (auto k = 0ul; k < info_.joints[j].command_interfaces.size(); k++) { + module_params[i] + ["command_interface/" + + info_.joints[j].command_interfaces[k].name] = + std::to_string(k); + } + try { + auto module = + ec_loader_.createSharedInstance(module_params[i].at("plugin")); + if (!module->setupSlave( + module_params[i], &hw_joint_states_[j], + &hw_joint_commands_[j], info_.joints[j].name + )) { + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), + "Setup of Joint module %li FAILED.", i + 1 + ); + return CallbackReturn::ERROR; + } + ec_modules_.push_back(module); + } catch (pluginlib::PluginlibException &ex) { RCLCPP_FATAL( - rclcpp::get_logger("EthercatDriver"), - "Setup of Joint module %li FAILED.", i + 1); - return CallbackReturn::ERROR; + rclcpp::get_logger("EthercatDriver"), + "The plugin of %s failed to load for some reason. Error: %s\n", + info_.joints[j].name.c_str(), ex.what() + ); } - ec_modules_.push_back(module); - } catch (pluginlib::PluginlibException & ex) { - RCLCPP_FATAL( - rclcpp::get_logger("EthercatDriver"), - "The plugin of %s failed to load for some reason. Error: %s\n", - info_.joints[j].name.c_str(), ex.what()); } } - } - for (uint g = 0; g < info_.gpios.size(); g++) { - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "gpio: %s", info_.gpios[g].name.c_str()); - // check all gpios for EC modules and load into ec_modules_ - auto module_params = getEcModuleParam(info_.original_xml, info_.gpios[g].name, "gpio"); - ec_module_parameters_.insert( - ec_module_parameters_.end(), module_params.begin(), module_params.end()); - for (auto i = 0ul; i < module_params.size(); i++) { - for (auto k = 0ul; k < info_.gpios[g].state_interfaces.size(); k++) { - module_params[i]["state_interface/" + - info_.gpios[g].state_interfaces[k].name] = std::to_string(k); - } - for (auto k = 0ul; k < info_.gpios[g].command_interfaces.size(); k++) { - module_params[i]["command_interface/" + - info_.gpios[g].command_interfaces[k].name] = std::to_string(k); - } - try { - auto module = ec_loader_.createSharedInstance(module_params[i].at("plugin")); - if (!module->setupSlave( - module_params[i], &hw_gpio_states_[g], &hw_gpio_commands_[g], info_.gpios[g].name)) - { + for (uint g = 0; g < info_.gpios.size(); g++) { + RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), "gpio: %s", + info_.gpios[g].name.c_str() + ); + // check all gpios for EC modules and load into ec_modules_ + auto module_params = + getEcModuleParam(info_.original_xml, info_.gpios[g].name, "gpio"); + ec_module_parameters_.insert( + ec_module_parameters_.end(), module_params.begin(), + module_params.end() + ); + for (auto i = 0ul; i < module_params.size(); i++) { + for (auto k = 0ul; k < info_.gpios[g].state_interfaces.size(); k++) { + module_params[i] + ["state_interface/" + + info_.gpios[g].state_interfaces[k].name] = + std::to_string(k); + } + for (auto k = 0ul; k < info_.gpios[g].command_interfaces.size(); k++) { + module_params[i] + ["command_interface/" + + info_.gpios[g].command_interfaces[k].name] = + std::to_string(k); + } + try { + auto module = + ec_loader_.createSharedInstance(module_params[i].at("plugin")); + if (!module->setupSlave( + module_params[i], &hw_gpio_states_[g], &hw_gpio_commands_[g], + info_.gpios[g].name + )) { + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), + "Setup of GPIO module %li FAILED.", i + 1 + ); + return CallbackReturn::ERROR; + } + ec_modules_.push_back(module); + } catch (pluginlib::PluginlibException &ex) { RCLCPP_FATAL( - rclcpp::get_logger("EthercatDriver"), - "Setup of GPIO module %li FAILED.", i + 1); - return CallbackReturn::ERROR; + rclcpp::get_logger("EthercatDriver"), + "The plugin of %s failed to load for some reason. Error: %s\n", + info_.gpios[g].name.c_str(), ex.what() + ); } - ec_modules_.push_back(module); - } catch (pluginlib::PluginlibException & ex) { - RCLCPP_FATAL( - rclcpp::get_logger("EthercatDriver"), - "The plugin of %s failed to load for some reason. Error: %s\n", - info_.gpios[g].name.c_str(), ex.what()); } } - } - for (uint s = 0; s < info_.sensors.size(); s++) { - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "sensors"); - // check all sensors for EC modules and load into ec_modules_ - auto module_params = getEcModuleParam(info_.original_xml, info_.sensors[s].name, "sensor"); - ec_module_parameters_.insert( - ec_module_parameters_.end(), module_params.begin(), module_params.end()); - for (auto i = 0ul; i < module_params.size(); i++) { - for (auto k = 0ul; k < info_.sensors[s].state_interfaces.size(); k++) { - module_params[i]["state_interface/" + - info_.sensors[s].state_interfaces[k].name] = std::to_string(k); - } - for (auto k = 0ul; k < info_.sensors[s].command_interfaces.size(); k++) { - module_params[i]["command_interface/" + - info_.sensors[s].command_interfaces[k].name] = std::to_string(k); - } - try { - auto module = ec_loader_.createSharedInstance(module_params[i].at("plugin")); - if (!module->setupSlave( - module_params[i], &hw_sensor_states_[s], &hw_sensor_commands_[s], info_.sensors[s].name)) - { + for (uint s = 0; s < info_.sensors.size(); s++) { + RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "sensors"); + // check all sensors for EC modules and load into ec_modules_ + auto module_params = + getEcModuleParam(info_.original_xml, info_.sensors[s].name, "sensor"); + ec_module_parameters_.insert( + ec_module_parameters_.end(), module_params.begin(), + module_params.end() + ); + for (auto i = 0ul; i < module_params.size(); i++) { + for (auto k = 0ul; k < info_.sensors[s].state_interfaces.size(); k++) { + module_params[i] + ["state_interface/" + + info_.sensors[s].state_interfaces[k].name] = + std::to_string(k); + } + for (auto k = 0ul; k < info_.sensors[s].command_interfaces.size(); + k++) { + module_params[i] + ["command_interface/" + + info_.sensors[s].command_interfaces[k].name] = + std::to_string(k); + } + try { + auto module = + ec_loader_.createSharedInstance(module_params[i].at("plugin")); + if (!module->setupSlave( + module_params[i], &hw_sensor_states_[s], + &hw_sensor_commands_[s], info_.sensors[s].name + )) { + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), + "Setup of Sensor module %li FAILED.", i + 1 + ); + return CallbackReturn::ERROR; + } + ec_modules_.push_back(module); + } catch (pluginlib::PluginlibException &ex) { RCLCPP_FATAL( - rclcpp::get_logger("EthercatDriver"), - "Setup of Sensor module %li FAILED.", i + 1); - return CallbackReturn::ERROR; + rclcpp::get_logger("EthercatDriver"), + "The plugin of %s failed to load for some reason. Error: %s\n", + info_.sensors[s].name.c_str(), ex.what() + ); } - ec_modules_.push_back(module); - } catch (pluginlib::PluginlibException & ex) { - RCLCPP_FATAL( - rclcpp::get_logger("EthercatDriver"), - "The plugin of %s failed to load for some reason. Error: %s\n", - info_.sensors[s].name.c_str(), ex.what()); } } + + RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), "Got %li modules", + ec_modules_.size() + ); + + return CallbackReturn::SUCCESS; } - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Got %li modules", ec_modules_.size()); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn EthercatDriver::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - return CallbackReturn::SUCCESS; -} - -std::vector -EthercatDriver::export_state_interfaces() -{ - std::vector state_interfaces; - // export joint state interface - for (uint j = 0; j < info_.joints.size(); j++) { - for (uint i = 0; i < info_.joints[j].state_interfaces.size(); i++) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - info_.joints[j].name, - info_.joints[j].state_interfaces[i].name, - &hw_joint_states_[j][i])); - } + CallbackReturn EthercatDriver::on_configure(const rclcpp_lifecycle::State + & /*previous_state*/) { + return CallbackReturn::SUCCESS; } - // export sensor state interface - for (uint s = 0; s < info_.sensors.size(); s++) { - for (uint i = 0; i < info_.sensors[s].state_interfaces.size(); i++) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - info_.sensors[s].name, - info_.sensors[s].state_interfaces[i].name, - &hw_sensor_states_[s][i])); + + std::vector + EthercatDriver::export_state_interfaces() { + std::vector state_interfaces; + // export joint state interface + for (uint j = 0; j < info_.joints.size(); j++) { + for (uint i = 0; i < info_.joints[j].state_interfaces.size(); i++) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[j].name, info_.joints[j].state_interfaces[i].name, + &hw_joint_states_[j][i] + )); + } } - } - // export gpio state interface - for (uint g = 0; g < info_.gpios.size(); g++) { - for (uint i = 0; i < info_.gpios[g].state_interfaces.size(); i++) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - info_.gpios[g].name, - info_.gpios[g].state_interfaces[i].name, - &hw_gpio_states_[g][i])); + // export sensor state interface + for (uint s = 0; s < info_.sensors.size(); s++) { + for (uint i = 0; i < info_.sensors[s].state_interfaces.size(); i++) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.sensors[s].name, info_.sensors[s].state_interfaces[i].name, + &hw_sensor_states_[s][i] + )); + } } - } - return state_interfaces; -} - -std::vector -EthercatDriver::export_command_interfaces() -{ - std::vector command_interfaces; - // export joint command interface - std::vector test; - for (uint j = 0; j < info_.joints.size(); j++) { - for (uint i = 0; i < info_.joints[j].command_interfaces.size(); i++) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - info_.joints[j].name, - info_.joints[j].command_interfaces[i].name, - &hw_joint_commands_[j][i])); + // export gpio state interface + for (uint g = 0; g < info_.gpios.size(); g++) { + for (uint i = 0; i < info_.gpios[g].state_interfaces.size(); i++) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.gpios[g].name, info_.gpios[g].state_interfaces[i].name, + &hw_gpio_states_[g][i] + )); + } } + return state_interfaces; } - // export sensor command interface - for (uint s = 0; s < info_.sensors.size(); s++) { - for (uint i = 0; i < info_.sensors[s].command_interfaces.size(); i++) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - info_.sensors[s].name, - info_.sensors[s].command_interfaces[i].name, - &hw_sensor_commands_[s][i])); + + std::vector + EthercatDriver::export_command_interfaces() { + std::vector command_interfaces; + // export joint command interface + std::vector test; + for (uint j = 0; j < info_.joints.size(); j++) { + for (uint i = 0; i < info_.joints[j].command_interfaces.size(); i++) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[j].name, info_.joints[j].command_interfaces[i].name, + &hw_joint_commands_[j][i] + )); + } } - } - // export gpio command interface - for (uint g = 0; g < info_.gpios.size(); g++) { - for (uint i = 0; i < info_.gpios[g].command_interfaces.size(); i++) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - info_.gpios[g].name, - info_.gpios[g].command_interfaces[i].name, - &hw_gpio_commands_[g][i])); + // export sensor command interface + for (uint s = 0; s < info_.sensors.size(); s++) { + for (uint i = 0; i < info_.sensors[s].command_interfaces.size(); i++) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.sensors[s].name, info_.sensors[s].command_interfaces[i].name, + &hw_sensor_commands_[s][i] + )); + } } - } - return command_interfaces; -} - -CallbackReturn EthercatDriver::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Starting ...please wait..."); - if (info_.hardware_parameters.find("control_frequency") == info_.hardware_parameters.end()) { - control_frequency_ = 100; - } else { - control_frequency_ = std::stod(info_.hardware_parameters["control_frequency"]); + // export gpio command interface + for (uint g = 0; g < info_.gpios.size(); g++) { + for (uint i = 0; i < info_.gpios[g].command_interfaces.size(); i++) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.gpios[g].name, info_.gpios[g].command_interfaces[i].name, + &hw_gpio_commands_[g][i] + )); + } + } + return command_interfaces; } - if (control_frequency_ < 0) { - RCLCPP_FATAL( - rclcpp::get_logger("EthercatDriver"), "Invalid control frequency!"); - return CallbackReturn::ERROR; - } + CallbackReturn EthercatDriver::on_activate(const rclcpp_lifecycle::State + & /*previous_state*/) { + RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), "Starting ...please wait..." + ); + if (info_.hardware_parameters.find("control_frequency") == + info_.hardware_parameters.end()) { + control_frequency_ = 100; + } else { + control_frequency_ = + std::stod(info_.hardware_parameters["control_frequency"]); + } - // start EC and wait until state operative + if (control_frequency_ < 0) { + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), "Invalid control frequency!" + ); + return CallbackReturn::ERROR; + } - master_.setCtrlFrequency(control_frequency_); + // start EC and wait until state operative - for (auto i = 0ul; i < ec_modules_.size(); i++) { - master_.addSlave( - std::stod(ec_module_parameters_[i]["alias"]), - std::stod(ec_module_parameters_[i]["position"]), - ec_modules_[i].get()); - } + master_.setCtrlFrequency(control_frequency_); - // configure SDO - for (auto i = 0ul; i < ec_modules_.size(); i++) { - for (auto & sdo : ec_modules_[i]->sdo_config) { - uint32_t abort_code; - int ret = master_.configSlaveSdo( - std::stod(ec_module_parameters_[i]["position"]), - sdo, - &abort_code + for (auto i = 0ul; i < ec_modules_.size(); i++) { + master_.addSlave( + std::stod(ec_module_parameters_[i]["alias"]), + std::stod(ec_module_parameters_[i]["position"]), ec_modules_[i].get() ); - if (ret) { - RCLCPP_INFO( - rclcpp::get_logger("EthercatDriver"), - "Failed to download config SDO for module at position %s with Error: %d", - ec_module_parameters_[i]["position"].c_str(), - abort_code + } + + // configure SDO + for (auto i = 0ul; i < ec_modules_.size(); i++) { + for (auto &sdo : ec_modules_[i]->sdo_config) { + uint32_t abort_code; + int ret = master_.configSlaveSdo( + std::stod(ec_module_parameters_[i]["alias"]), + std::stod(ec_module_parameters_[i]["position"]), sdo, &abort_code ); + if (ret) { + RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), + "Failed to download config SDO for module at position %s with " + "Error: %d", + ec_module_parameters_[i]["position"].c_str(), abort_code + ); + } } } - } - master_.activate(); - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Activated EcMaster!"); + master_.activate(); + RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Activated EcMaster!"); - // start after one second - struct timespec t; - clock_gettime(CLOCK_MONOTONIC, &t); - t.tv_sec++; + // start after one second + struct timespec t; + clock_gettime(CLOCK_MONOTONIC, &t); + t.tv_sec++; - bool running = true; - while (running) { - // wait until next shot - clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); - // update EtherCAT bus + bool running = true; + while (running) { + // wait until next shot + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); + // update EtherCAT bus - master_.update(); - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "updated!"); + master_.update(); + RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "updated!"); - // check if operational - bool isAllInit = true; - for (auto & module : ec_modules_) { - isAllInit = isAllInit && module->initialized(); - } - if (isAllInit) { - running = false; - } - // calculate next shot. carry over nanoseconds into microseconds. - t.tv_nsec += master_.getInterval(); - while (t.tv_nsec >= 1000000000) { - t.tv_nsec -= 1000000000; - t.tv_sec++; + // check if operational + bool isAllInit = true; + for (auto &module : ec_modules_) { + isAllInit = isAllInit && module->initialized(); + } + if (isAllInit) { + running = false; + } + // calculate next shot. carry over nanoseconds into microseconds. + t.tv_nsec += master_.getInterval(); + while (t.tv_nsec >= 1000000000) { + t.tv_nsec -= 1000000000; + t.tv_sec++; + } } - } - RCLCPP_INFO( - rclcpp::get_logger("EthercatDriver"), "System Successfully started!"); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn EthercatDriver::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Stopping ...please wait..."); - - // stop EC and disconnect - master_.stop(); - - RCLCPP_INFO( - rclcpp::get_logger("EthercatDriver"), "System successfully stopped!"); - - return CallbackReturn::SUCCESS; -} - -hardware_interface::return_type EthercatDriver::read( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) -{ - master_.readData(); - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type EthercatDriver::write( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) -{ - master_.writeData(); - return hardware_interface::return_type::OK; -} - -std::vector> EthercatDriver::getEcModuleParam( - std::string & urdf, - std::string component_name, - std::string component_type) -{ - // Check if everything OK with URDF string - if (urdf.empty()) { - throw std::runtime_error("empty URDF passed to robot"); - } - tinyxml2::XMLDocument doc; - if (!doc.Parse(urdf.c_str()) && doc.Error()) { - throw std::runtime_error("invalid URDF passed in to robot parser"); + RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), "System Successfully started!" + ); + + return CallbackReturn::SUCCESS; } - if (doc.Error()) { - throw std::runtime_error("invalid URDF passed in to robot parser"); + + CallbackReturn EthercatDriver::on_deactivate(const rclcpp_lifecycle::State + & /*previous_state*/) { + RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), "Stopping ...please wait..." + ); + + // stop EC and disconnect + master_.stop(); + + RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), "System successfully stopped!" + ); + + return CallbackReturn::SUCCESS; } - tinyxml2::XMLElement * robot_it = doc.RootElement(); - if (std::string("robot").compare(robot_it->Name())) { - throw std::runtime_error("the robot tag is not root element in URDF"); + hardware_interface::return_type EthercatDriver:: + read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + master_.readData(); + return hardware_interface::return_type::OK; } - const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement("ros2_control"); - if (!ros2_control_it) { - throw std::runtime_error("no ros2_control tag"); + hardware_interface::return_type EthercatDriver::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ + ) { + master_.writeData(); + return hardware_interface::return_type::OK; } - std::vector> module_params; - std::unordered_map module_param; - - while (ros2_control_it) { - const auto * ros2_control_child_it = ros2_control_it->FirstChildElement(component_type.c_str()); - while (ros2_control_child_it) { - if (!component_name.compare(ros2_control_child_it->Attribute("name"))) { - const auto * ec_module_it = ros2_control_child_it->FirstChildElement("ec_module"); - while (ec_module_it) { - module_param.clear(); - module_param["name"] = ec_module_it->Attribute("name"); - const auto * plugin_it = ec_module_it->FirstChildElement("plugin"); - if (NULL != plugin_it) { - module_param["plugin"] = plugin_it->GetText(); - } - const auto * param_it = ec_module_it->FirstChildElement("param"); - while (param_it) { - module_param[param_it->Attribute("name")] = param_it->GetText(); - param_it = param_it->NextSiblingElement("param"); + std::vector> + EthercatDriver::getEcModuleParam( + std::string &urdf, std::string component_name, std::string component_type + ) { + // Check if everything OK with URDF string + if (urdf.empty()) { + throw std::runtime_error("empty URDF passed to robot"); + } + tinyxml2::XMLDocument doc; + if (!doc.Parse(urdf.c_str()) && doc.Error()) { + throw std::runtime_error("invalid URDF passed in to robot parser"); + } + if (doc.Error()) { + throw std::runtime_error("invalid URDF passed in to robot parser"); + } + + tinyxml2::XMLElement *robot_it = doc.RootElement(); + if (std::string("robot").compare(robot_it->Name())) { + throw std::runtime_error("the robot tag is not root element in URDF"); + } + + const tinyxml2::XMLElement *ros2_control_it = + robot_it->FirstChildElement("ros2_control"); + if (!ros2_control_it) { + throw std::runtime_error("no ros2_control tag"); + } + + std::vector> module_params; + std::unordered_map module_param; + + while (ros2_control_it) { + const auto *ros2_control_child_it = + ros2_control_it->FirstChildElement(component_type.c_str()); + while (ros2_control_child_it) { + if (!component_name.compare(ros2_control_child_it->Attribute("name"))) { + const auto *ec_module_it = + ros2_control_child_it->FirstChildElement("ec_module"); + while (ec_module_it) { + module_param.clear(); + module_param["name"] = ec_module_it->Attribute("name"); + const auto *plugin_it = ec_module_it->FirstChildElement("plugin"); + if (NULL != plugin_it) { + module_param["plugin"] = plugin_it->GetText(); + } + const auto *param_it = ec_module_it->FirstChildElement("param"); + while (param_it) { + module_param[param_it->Attribute("name")] = param_it->GetText(); + param_it = param_it->NextSiblingElement("param"); + } + module_params.push_back(module_param); + ec_module_it = ec_module_it->NextSiblingElement("ec_module"); } - module_params.push_back(module_param); - ec_module_it = ec_module_it->NextSiblingElement("ec_module"); } + ros2_control_child_it = + ros2_control_child_it->NextSiblingElement(component_type.c_str()); } - ros2_control_child_it = ros2_control_child_it->NextSiblingElement(component_type.c_str()); + ros2_control_it = ros2_control_it->NextSiblingElement("ros2_control"); } - ros2_control_it = ros2_control_it->NextSiblingElement("ros2_control"); - } - return module_params; -} + return module_params; + } -} // namespace ethercat_driver +} // namespace ethercat_driver #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - ethercat_driver::EthercatDriver, hardware_interface::SystemInterface) + ethercat_driver::EthercatDriver, hardware_interface::SystemInterface +) diff --git a/ethercat_interface/include/ethercat_interface/ec_master.hpp b/ethercat_interface/include/ethercat_interface/ec_master.hpp index da037331..20de42c0 100644 --- a/ethercat_interface/include/ethercat_interface/ec_master.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_master.hpp @@ -24,154 +24,158 @@ #include #include "ethercat_interface/ec_slave.hpp" +namespace ethercat_interface { + + class EcMaster { + public: + explicit EcMaster(const int master = 0); + virtual ~EcMaster(); + + /** \brief add a slave device to the master + * alias and position can be found by running the following command + * /opt/etherlab/bin$ sudo ./ethercat slaves + * look for the "A B:C STATUS DEVICE" (e.g. B=alias, C=position) + */ + void addSlave(uint16_t alias, uint16_t position, EcSlave *slave); + + /** \brief configure slave using SDO + */ + int configSlaveSdo( + uint16_t alias, uint16_t position, SdoConfigEntry sdo_config, + uint32_t *abort_code + ); + + /** call after adding all slaves, and before update */ + void activate(); + + /** perform one EtherCAT cycle, passing the domain to the slaves */ + virtual void update(uint32_t domain = 0); + + /** run a control loop of update() and user_callback(), blocking. + * call activate and setThreadHighPriority/RealTime first. */ + typedef void (*SIMPLECAT_CONTRL_CALLBACK)(void); + virtual void run(SIMPLECAT_CONTRL_CALLBACK user_callback); + + /** stop the control loop. use within callback, or from a separate thread. + */ + virtual void stop() { + running_ = false; + } + + /** time of last ethercat update, since calling run. stops if stop called. + * returns actual time. use elapsedCycles()/frequency for discrete time + * at last update. */ + virtual double elapsedTime(); + + /** number of EtherCAT updates since calling run. */ + virtual uint64_t elapsedCycles(); + + /** add ctr-c exit callback. + * default exits the run loop and prints timing */ + typedef void (*SIMPLECAT_EXIT_CALLBACK)(int); + static void setCtrlCHandler(SIMPLECAT_EXIT_CALLBACK user_callback = NULL); + + /** set the thread to a priority of -19 + * priority range is -20 (highest) to 19 (lowest) */ + static void setThreadHighPriority(); + + /** set the thread to real time (FIFO) + * thread cannot be preempted. + * set priority as 49 (kernel and interrupts are 50) */ + static void setThreadRealTime(); + + void setCtrlFrequency(double frequency) { + interval_ = 1000000000.0 / frequency; + } + + uint32_t getInterval() { + return interval_; + } + + void readData(uint32_t domain = 0); + void writeData(uint32_t domain = 0); + + private: + /** true if running */ + volatile bool running_ = false; + + /** start and current time */ + std::chrono::time_point start_t_, curr_t_; + + // EtherCAT Control + + /** register a domain of the slave */ + struct DomainInfo; + void registerPDOInDomain( + uint16_t alias, uint16_t position, + std::vector &channel_indices, DomainInfo *domain_info, + EcSlave *slave + ); + + /** check for change in the domain state */ + void checkDomainState(uint32_t domain); + + /** check for change in the master state */ + void checkMasterState(); + + /** check for change in the slave states */ + void checkSlaveStates(); + + /** print warning message to terminal */ + static void printWarning(const std::string &message); + + /** EtherCAT master data */ + ec_master_t *master_ = NULL; + ec_master_state_t master_state_ = {}; + + /** data for a single domain */ + struct DomainInfo { + explicit DomainInfo(ec_master_t *master); + ~DomainInfo(); + + ec_domain_t *domain = NULL; + ec_domain_state_t domain_state = {}; + uint8_t *domain_pd = NULL; + + /** domain pdo registration array. + * do not modify after active(), or may invalidate */ + std::vector domain_regs; + + /** slave's pdo entries in the domain */ + struct Entry { + EcSlave *slave = NULL; + int num_pdos = 0; + uint32_t *offset = NULL; + uint32_t *bit_position = NULL; + }; + + std::vector entries; + }; + + /** map from domain index to domain info */ + std::map domain_info_; + + /** data needed to check slave state */ + struct SlaveInfo { + EcSlave *slave = NULL; + ec_slave_config_t *config = NULL; + ec_slave_config_state_t config_state = {0}; + uint16_t alias; + uint16_t position; + }; -namespace ethercat_interface -{ + std::vector slave_info_; -class EcMaster -{ -public: - explicit EcMaster(const int master = 0); - virtual ~EcMaster(); + /** counter of control loops */ + uint64_t update_counter_ = 0; - /** \brief add a slave device to the master - * alias and position can be found by running the following command - * /opt/etherlab/bin$ sudo ./ethercat slaves - * look for the "A B:C STATUS DEVICE" (e.g. B=alias, C=position) - */ - void addSlave(uint16_t alias, uint16_t position, EcSlave * slave); + /** frequency to check for master or slave state change. + * state checked every frequency_ control loops */ + uint32_t check_state_frequency_ = 10; - /** \brief configure slave using SDO - */ - int configSlaveSdo(uint16_t slave_position, SdoConfigEntry sdo_config, uint32_t * abort_code); - - /** call after adding all slaves, and before update */ - void activate(); - - /** perform one EtherCAT cycle, passing the domain to the slaves */ - virtual void update(uint32_t domain = 0); - - /** run a control loop of update() and user_callback(), blocking. - * call activate and setThreadHighPriority/RealTime first. */ - typedef void (* SIMPLECAT_CONTRL_CALLBACK)(void); - virtual void run(SIMPLECAT_CONTRL_CALLBACK user_callback); - - /** stop the control loop. use within callback, or from a separate thread. */ - virtual void stop() {running_ = false;} - - /** time of last ethercat update, since calling run. stops if stop called. - * returns actual time. use elapsedCycles()/frequency for discrete time at last update. */ - virtual double elapsedTime(); - - /** number of EtherCAT updates since calling run. */ - virtual uint64_t elapsedCycles(); - - /** add ctr-c exit callback. - * default exits the run loop and prints timing */ - typedef void (* SIMPLECAT_EXIT_CALLBACK)(int); - static void setCtrlCHandler(SIMPLECAT_EXIT_CALLBACK user_callback = NULL); - - /** set the thread to a priority of -19 - * priority range is -20 (highest) to 19 (lowest) */ - static void setThreadHighPriority(); - - /** set the thread to real time (FIFO) - * thread cannot be preempted. - * set priority as 49 (kernel and interrupts are 50) */ - static void setThreadRealTime(); - - void setCtrlFrequency(double frequency) - { - interval_ = 1000000000.0 / frequency; - } - - uint32_t getInterval() {return interval_;} - - void readData(uint32_t domain = 0); - void writeData(uint32_t domain = 0); - -private: - /** true if running */ - volatile bool running_ = false; - - /** start and current time */ - std::chrono::time_point start_t_, curr_t_; - - // EtherCAT Control - - /** register a domain of the slave */ - struct DomainInfo; - void registerPDOInDomain( - uint16_t alias, uint16_t position, - std::vector & channel_indices, - DomainInfo * domain_info, - EcSlave * slave); - - /** check for change in the domain state */ - void checkDomainState(uint32_t domain); - - /** check for change in the master state */ - void checkMasterState(); - - /** check for change in the slave states */ - void checkSlaveStates(); - - /** print warning message to terminal */ - static void printWarning(const std::string & message); - - /** EtherCAT master data */ - ec_master_t * master_ = NULL; - ec_master_state_t master_state_ = {}; - - /** data for a single domain */ - struct DomainInfo - { - explicit DomainInfo(ec_master_t * master); - ~DomainInfo(); - - ec_domain_t * domain = NULL; - ec_domain_state_t domain_state = {}; - uint8_t * domain_pd = NULL; - - /** domain pdo registration array. - * do not modify after active(), or may invalidate */ - std::vector domain_regs; - - /** slave's pdo entries in the domain */ - struct Entry - { - EcSlave * slave = NULL; - int num_pdos = 0; - uint32_t * offset = NULL; - uint32_t * bit_position = NULL; - }; - - std::vector entries; + uint32_t interval_; }; - /** map from domain index to domain info */ - std::map domain_info_; - - /** data needed to check slave state */ - struct SlaveInfo - { - EcSlave * slave = NULL; - ec_slave_config_t * config = NULL; - ec_slave_config_state_t config_state = {0}; - }; - - std::vector slave_info_; - - /** counter of control loops */ - uint64_t update_counter_ = 0; - - /** frequency to check for master or slave state change. - * state checked every frequency_ control loops */ - uint32_t check_state_frequency_ = 10; - - uint32_t interval_; -}; - -} // namespace ethercat_interface +} // namespace ethercat_interface -#endif // ETHERCAT_INTERFACE__EC_MASTER_HPP_ +#endif // ETHERCAT_INTERFACE__EC_MASTER_HPP_ diff --git a/ethercat_interface/src/ec_master.cpp b/ethercat_interface/src/ec_master.cpp index 31e58e11..576f0014 100644 --- a/ethercat_interface/src/ec_master.cpp +++ b/ethercat_interface/src/ec_master.cpp @@ -26,464 +26,444 @@ #include #include -#define EC_NEWTIMEVAL2NANO(TV) \ +#define EC_NEWTIMEVAL2NANO(TV) \ (((TV).tv_sec - 946684800ULL) * 1000000000ULL + (TV).tv_nsec) -namespace ethercat_interface -{ +namespace ethercat_interface { -EcMaster::DomainInfo::DomainInfo(ec_master_t * master) -{ - domain = ecrt_master_create_domain(master); - if (domain == NULL) { - printWarning("Failed to create domain"); - return; + EcMaster::DomainInfo::DomainInfo(ec_master_t *master) { + domain = ecrt_master_create_domain(master); + if (domain == NULL) { + printWarning("Failed to create domain"); + return; + } + + const ec_pdo_entry_reg_t empty = {0}; + domain_regs.push_back(empty); } - const ec_pdo_entry_reg_t empty = {0}; - domain_regs.push_back(empty); -} + EcMaster::DomainInfo::~DomainInfo() { + for (Entry &entry : entries) { + delete[] entry.offset; + delete[] entry.bit_position; + } + } + EcMaster::EcMaster(const int master) { + master_ = ecrt_request_master(master); + if (master_ == NULL) { + printWarning("Failed to obtain master."); + return; + } + interval_ = 0; + } -EcMaster::DomainInfo::~DomainInfo() -{ - for (Entry & entry : entries) { - delete[] entry.offset; - delete[] entry.bit_position; + EcMaster::~EcMaster() { + for (SlaveInfo &slave : slave_info_) { + // + } + for (auto &domain : domain_info_) { + delete domain.second; + } } -} + void EcMaster::addSlave(uint16_t alias, uint16_t position, EcSlave *slave) { + // configure slave in master + + SlaveInfo slave_info; + slave_info.slave = slave; + slave_info.alias = alias; + slave_info.position = position; + slave_info.config = ecrt_master_slave_config( + master_, alias, position, slave->vendor_id_, slave->product_id_ + ); + if (slave_info.config == NULL) { + printWarning("Add slave. Failed to get slave configuration."); + return; + } -EcMaster::EcMaster(const int master) -{ - master_ = ecrt_request_master(master); - if (master_ == NULL) { - printWarning("Failed to obtain master."); - return; - } - interval_ = 0; -} + // check and setup dc -EcMaster::~EcMaster() -{ - for (SlaveInfo & slave : slave_info_) { - // - } - for (auto & domain : domain_info_) { - delete domain.second; - } -} - -void EcMaster::addSlave(uint16_t alias, uint16_t position, EcSlave * slave) -{ - // configure slave in master - - SlaveInfo slave_info; - slave_info.slave = slave; - slave_info.config = ecrt_master_slave_config( - master_, alias, position, - slave->vendor_id_, - slave->product_id_); - if (slave_info.config == NULL) { - printWarning("Add slave. Failed to get slave configuration."); - return; - } + if (slave->assign_activate_dc_sync()) { + struct timespec t; + clock_gettime(CLOCK_MONOTONIC, &t); + ecrt_master_application_time(master_, EC_NEWTIMEVAL2NANO(t)); + ecrt_slave_config_dc( + slave_info.config, slave->assign_activate_dc_sync(), interval_, + interval_ - (t.tv_nsec % (interval_)), 0, 0 + ); + } - // check and setup dc + slave_info_.push_back(slave_info); + + // check if slave has pdos + size_t num_syncs = slave->syncSize(); + const ec_sync_info_t *syncs = slave->syncs(); + if (num_syncs > 0) { + // configure pdos in slave + int pdos_status = + ecrt_slave_config_pdos(slave_info.config, num_syncs, syncs); + if (pdos_status) { + printWarning("Add slave. Failed to configure PDOs"); + return; + } + } else { + printWarning( + "Add slave. Sync size is zero for " + std::to_string(alias) + ":" + + std::to_string(position) + ); + } - if (slave->assign_activate_dc_sync()) { - struct timespec t; - clock_gettime(CLOCK_MONOTONIC, &t); - ecrt_master_application_time(master_, EC_NEWTIMEVAL2NANO(t)); - ecrt_slave_config_dc( - slave_info.config, - slave->assign_activate_dc_sync(), - interval_, - interval_ - (t.tv_nsec % (interval_)), - 0, - 0); + // check if slave registered any pdos for the domain + EcSlave::DomainMap domain_map; + slave->domains(domain_map); + for (auto &iter : domain_map) { + // get the domain info, create if necessary + uint32_t domain_index = iter.first; + DomainInfo *domain_info = domain_info_[domain_index]; + if (domain_info == NULL) { + domain_info = new DomainInfo(master_); + domain_info_[domain_index] = domain_info; + } + + registerPDOInDomain(alias, position, iter.second, domain_info, slave); + } } - slave_info_.push_back(slave_info); - - // check if slave has pdos - size_t num_syncs = slave->syncSize(); - const ec_sync_info_t * syncs = slave->syncs(); - if (num_syncs > 0) { - // configure pdos in slave - int pdos_status = ecrt_slave_config_pdos(slave_info.config, num_syncs, syncs); - if (pdos_status) { - printWarning("Add slave. Failed to configure PDOs"); - return; + int EcMaster::configSlaveSdo( + uint16_t alias, uint16_t position, SdoConfigEntry sdo_config, + uint32_t *abort_code + ) { + uint8_t buffer[8]; + sdo_config.buffer_write(buffer); + // Get the slave config + auto slave = std::find_if( + slave_info_.begin(), slave_info_.end(), + [alias, position](const SlaveInfo &slave_info) { + return slave_info.alias == alias && slave_info.position == position; + } + ); + int ret = 0; + if (slave != slave_info_.end()) { + ret = ecrt_slave_config_sdo( + slave->config, sdo_config.index, sdo_config.sub_index, buffer, + sdo_config.data_size() + ); // ecrt_master_sdo_download( } - } else { - printWarning( - "Add slave. Sync size is zero for " + - std::to_string(alias) + ":" + std::to_string(position)); + // master_, slave_position, sdo_config.index, sdo_config.sub_index, + // buffer, sdo_config.data_size(), abort_code + //); + return ret; } - // check if slave registered any pdos for the domain - EcSlave::DomainMap domain_map; - slave->domains(domain_map); - for (auto & iter : domain_map) { - // get the domain info, create if necessary - uint32_t domain_index = iter.first; - DomainInfo * domain_info = domain_info_[domain_index]; - if (domain_info == NULL) { - domain_info = new DomainInfo(master_); - domain_info_[domain_index] = domain_info; + void EcMaster::registerPDOInDomain( + uint16_t alias, uint16_t position, std::vector &channel_indices, + DomainInfo *domain_info, EcSlave *slave + ) { + // expand the size of the domain + uint32_t num_pdo_regs = channel_indices.size(); + size_t start_index = + domain_info->domain_regs.size() - 1; // empty element at end + domain_info->domain_regs.resize( + domain_info->domain_regs.size() + num_pdo_regs + ); + + // create a new entry in the domain + DomainInfo::Entry domain_entry; + domain_entry.slave = slave; + domain_entry.num_pdos = num_pdo_regs; + domain_entry.offset = new uint32_t[num_pdo_regs]; + domain_entry.bit_position = new uint32_t[num_pdo_regs]; + domain_info->entries.push_back(domain_entry); + + EcSlave::DomainMap domain_map; + slave->domains(domain_map); + + // add to array of pdos registrations + const ec_pdo_entry_info_t *pdo_regs = slave->channels(); + for (size_t i = 0; i < num_pdo_regs; ++i) { + // create pdo entry in the domain + ec_pdo_entry_reg_t &pdo_reg = domain_info->domain_regs[start_index + i]; + pdo_reg.alias = alias; + pdo_reg.position = position; + pdo_reg.vendor_id = slave->vendor_id_; + pdo_reg.product_code = slave->product_id_; + pdo_reg.index = pdo_regs[channel_indices[i]].index; + pdo_reg.subindex = pdo_regs[channel_indices[i]].subindex; + pdo_reg.offset = &(domain_entry.offset[i]); + pdo_reg.bit_position = &(domain_entry.bit_position[i]); + + // print the domain pdo entry + // std::cout << "{" << pdo_reg.alias << ", " << pdo_reg.position; + // std::cout << ", 0x" << std::hex << pdo_reg.vendor_id; + // std::cout << ", 0x" << std::hex << pdo_reg.product_code; + // std::cout << ", 0x" << std::hex << pdo_reg.index; + // std::cout << ", 0x" << std::hex << static_cast(pdo_reg.subindex); + // std::cout << "}" << std::dec << std::endl; } - registerPDOInDomain( - alias, position, - iter.second, domain_info, - slave); - } -} - -int EcMaster::configSlaveSdo( - uint16_t slave_position, SdoConfigEntry sdo_config, - uint32_t * abort_code) -{ - uint8_t buffer[8]; - sdo_config.buffer_write(buffer); - int ret = ecrt_master_sdo_download( - master_, - slave_position, - sdo_config.index, - sdo_config.sub_index, - buffer, - sdo_config.data_size(), - abort_code - ); - return ret; -} - -void EcMaster::registerPDOInDomain( - uint16_t alias, uint16_t position, - std::vector & channel_indices, - DomainInfo * domain_info, - EcSlave * slave) -{ - // expand the size of the domain - uint32_t num_pdo_regs = channel_indices.size(); - size_t start_index = domain_info->domain_regs.size() - 1; // empty element at end - domain_info->domain_regs.resize(domain_info->domain_regs.size() + num_pdo_regs); - - // create a new entry in the domain - DomainInfo::Entry domain_entry; - domain_entry.slave = slave; - domain_entry.num_pdos = num_pdo_regs; - domain_entry.offset = new uint32_t[num_pdo_regs]; - domain_entry.bit_position = new uint32_t[num_pdo_regs]; - domain_info->entries.push_back(domain_entry); - - EcSlave::DomainMap domain_map; - slave->domains(domain_map); - - // add to array of pdos registrations - const ec_pdo_entry_info_t * pdo_regs = slave->channels(); - for (size_t i = 0; i < num_pdo_regs; ++i) { - // create pdo entry in the domain - ec_pdo_entry_reg_t & pdo_reg = domain_info->domain_regs[start_index + i]; - pdo_reg.alias = alias; - pdo_reg.position = position; - pdo_reg.vendor_id = slave->vendor_id_; - pdo_reg.product_code = slave->product_id_; - pdo_reg.index = pdo_regs[channel_indices[i]].index; - pdo_reg.subindex = pdo_regs[channel_indices[i]].subindex; - pdo_reg.offset = &(domain_entry.offset[i]); - pdo_reg.bit_position = &(domain_entry.bit_position[i]); - - - // print the domain pdo entry - // std::cout << "{" << pdo_reg.alias << ", " << pdo_reg.position; - // std::cout << ", 0x" << std::hex << pdo_reg.vendor_id; - // std::cout << ", 0x" << std::hex << pdo_reg.product_code; - // std::cout << ", 0x" << std::hex << pdo_reg.index; - // std::cout << ", 0x" << std::hex << static_cast(pdo_reg.subindex); - // std::cout << "}" << std::dec << std::endl; + // set the last element to null + ec_pdo_entry_reg_t empty = {0}; + domain_info->domain_regs.back() = empty; } - // set the last element to null - ec_pdo_entry_reg_t empty = {0}; - domain_info->domain_regs.back() = empty; -} - -void EcMaster::activate() -{ - // register domain - for (auto & iter : domain_info_) { - DomainInfo * domain_info = iter.second; - bool domain_status = ecrt_domain_reg_pdo_entry_list( - domain_info->domain, - &(domain_info->domain_regs[0])); - if (domain_status) { - printWarning("Activate. Failed to register domain PDO entries."); - return; + void EcMaster::activate() { + // register domain + for (auto &iter : domain_info_) { + DomainInfo *domain_info = iter.second; + bool domain_status = ecrt_domain_reg_pdo_entry_list( + domain_info->domain, &(domain_info->domain_regs[0]) + ); + if (domain_status) { + printWarning("Activate. Failed to register domain PDO entries."); + return; + } } - } - // set application time - struct timespec t; - clock_gettime(CLOCK_MONOTONIC, &t); - ecrt_master_application_time(master_, EC_NEWTIMEVAL2NANO(t)); - - // activate master - bool activate_status = ecrt_master_activate(master_); - if (activate_status) { - printWarning("Activate. Failed to activate master."); - return; - } + // set application time + struct timespec t; + clock_gettime(CLOCK_MONOTONIC, &t); + ecrt_master_application_time(master_, EC_NEWTIMEVAL2NANO(t)); - // retrieve domain data - for (auto & iter : domain_info_) { - DomainInfo * domain_info = iter.second; - domain_info->domain_pd = ecrt_domain_data(domain_info->domain); - if (domain_info->domain_pd == NULL) { - printWarning("Activate. Failed to retrieve domain process data."); + // activate master + bool activate_status = ecrt_master_activate(master_); + if (activate_status) { + printWarning("Activate. Failed to activate master."); return; } + + // retrieve domain data + for (auto &iter : domain_info_) { + DomainInfo *domain_info = iter.second; + domain_info->domain_pd = ecrt_domain_data(domain_info->domain); + if (domain_info->domain_pd == NULL) { + printWarning("Activate. Failed to retrieve domain process data."); + return; + } + } } -} -void EcMaster::update(uint32_t domain) -{ - // receive process data - ecrt_master_receive(master_); + void EcMaster::update(uint32_t domain) { + // receive process data + ecrt_master_receive(master_); - DomainInfo * domain_info = domain_info_[domain]; + DomainInfo *domain_info = domain_info_[domain]; - ecrt_domain_process(domain_info->domain); + ecrt_domain_process(domain_info->domain); - // check process data state (optional) - checkDomainState(domain); + // check process data state (optional) + checkDomainState(domain); - // check for master and slave state change - if (update_counter_ % check_state_frequency_ == 0) { - checkMasterState(); - checkSlaveStates(); - } + // check for master and slave state change + if (update_counter_ % check_state_frequency_ == 0) { + checkMasterState(); + checkSlaveStates(); + } - // read and write process data - for (DomainInfo::Entry & entry : domain_info->entries) { - for (int i = 0; i < entry.num_pdos; ++i) { - (entry.slave)->processData(i, domain_info->domain_pd + entry.offset[i]); + // read and write process data + for (DomainInfo::Entry &entry : domain_info->entries) { + for (int i = 0; i < entry.num_pdos; ++i) { + (entry.slave)->processData(i, domain_info->domain_pd + entry.offset[i]); + } } - } - struct timespec t; + struct timespec t; - clock_gettime(CLOCK_REALTIME, &t); - ecrt_master_application_time(master_, EC_NEWTIMEVAL2NANO(t)); - ecrt_master_sync_reference_clock(master_); - ecrt_master_sync_slave_clocks(master_); + clock_gettime(CLOCK_REALTIME, &t); + ecrt_master_application_time(master_, EC_NEWTIMEVAL2NANO(t)); + ecrt_master_sync_reference_clock(master_); + ecrt_master_sync_slave_clocks(master_); - // send process data - ecrt_domain_queue(domain_info->domain); - ecrt_master_send(master_); + // send process data + ecrt_domain_queue(domain_info->domain); + ecrt_master_send(master_); - ++update_counter_; -} + ++update_counter_; + } -void EcMaster::readData(uint32_t domain) -{ - // receive process data - ecrt_master_receive(master_); + void EcMaster::readData(uint32_t domain) { + // receive process data + ecrt_master_receive(master_); - DomainInfo * domain_info = domain_info_[domain]; + DomainInfo *domain_info = domain_info_[domain]; - ecrt_domain_process(domain_info->domain); + ecrt_domain_process(domain_info->domain); - // check process data state (optional) - checkDomainState(domain); + // check process data state (optional) + checkDomainState(domain); - // check for master and slave state change - if (update_counter_ % check_state_frequency_ == 0) { - checkMasterState(); - checkSlaveStates(); - } + // check for master and slave state change + if (update_counter_ % check_state_frequency_ == 0) { + checkMasterState(); + checkSlaveStates(); + } - // read and write process data - for (DomainInfo::Entry & entry : domain_info->entries) { - for (int i = 0; i < entry.num_pdos; ++i) { - (entry.slave)->processData(i, domain_info->domain_pd + entry.offset[i]); + // read and write process data + for (DomainInfo::Entry &entry : domain_info->entries) { + for (int i = 0; i < entry.num_pdos; ++i) { + (entry.slave)->processData(i, domain_info->domain_pd + entry.offset[i]); + } } - } - ++update_counter_; -} + ++update_counter_; + } -void EcMaster::writeData(uint32_t domain) -{ - DomainInfo * domain_info = domain_info_[domain]; - // read and write process data - for (DomainInfo::Entry & entry : domain_info->entries) { - for (int i = 0; i < entry.num_pdos; ++i) { - (entry.slave)->processData(i, domain_info->domain_pd + entry.offset[i]); + void EcMaster::writeData(uint32_t domain) { + DomainInfo *domain_info = domain_info_[domain]; + // read and write process data + for (DomainInfo::Entry &entry : domain_info->entries) { + for (int i = 0; i < entry.num_pdos; ++i) { + (entry.slave)->processData(i, domain_info->domain_pd + entry.offset[i]); + } } + + struct timespec t; + + clock_gettime(CLOCK_REALTIME, &t); + ecrt_master_application_time(master_, EC_NEWTIMEVAL2NANO(t)); + ecrt_master_sync_reference_clock(master_); + ecrt_master_sync_slave_clocks(master_); + + // send process data + ecrt_domain_queue(domain_info->domain); + ecrt_master_send(master_); } - struct timespec t; - - clock_gettime(CLOCK_REALTIME, &t); - ecrt_master_application_time(master_, EC_NEWTIMEVAL2NANO(t)); - ecrt_master_sync_reference_clock(master_); - ecrt_master_sync_slave_clocks(master_); - - // send process data - ecrt_domain_queue(domain_info->domain); - ecrt_master_send(master_); -} - -void EcMaster::setCtrlCHandler(SIMPLECAT_EXIT_CALLBACK user_callback) -{ - // ctrl c handler - struct sigaction sigIntHandler; - sigIntHandler.sa_handler = user_callback; - sigemptyset(&sigIntHandler.sa_mask); - sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); -} - -void EcMaster::run(SIMPLECAT_CONTRL_CALLBACK user_callback) -{ - // start after one second - struct timespec t; - clock_gettime(CLOCK_MONOTONIC, &t); - t.tv_sec++; - - running_ = true; - start_t_ = std::chrono::system_clock::now(); - while (running_) { - // wait until next shot - clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); - - // update EtherCAT bus - this->update(); - - // get actual time - curr_t_ = std::chrono::system_clock::now(); - - // user callback - user_callback(); - - // calculate next shot. carry over nanoseconds into microseconds. - t.tv_nsec += interval_; - while (t.tv_nsec >= 1000000000) { - t.tv_nsec -= 1000000000; - t.tv_sec++; - } + void EcMaster::setCtrlCHandler(SIMPLECAT_EXIT_CALLBACK user_callback) { + // ctrl c handler + struct sigaction sigIntHandler; + sigIntHandler.sa_handler = user_callback; + sigemptyset(&sigIntHandler.sa_mask); + sigIntHandler.sa_flags = 0; + sigaction(SIGINT, &sigIntHandler, NULL); } -} - -double EcMaster::elapsedTime() -{ - std::chrono::duration elapsed_seconds = curr_t_ - start_t_; - return elapsed_seconds.count() - 1.0; // started after 1 second -} - -uint64_t EcMaster::elapsedCycles() -{ - return update_counter_; -} - -void EcMaster::setThreadHighPriority() -{ - pid_t pid = getpid(); - int priority_status = setpriority(PRIO_PROCESS, pid, -19); - if (priority_status) { - printWarning("setThreadHighPriority. Failed to set priority."); - return; + + void EcMaster::run(SIMPLECAT_CONTRL_CALLBACK user_callback) { + // start after one second + struct timespec t; + clock_gettime(CLOCK_MONOTONIC, &t); + t.tv_sec++; + + running_ = true; + start_t_ = std::chrono::system_clock::now(); + while (running_) { + // wait until next shot + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); + + // update EtherCAT bus + this->update(); + + // get actual time + curr_t_ = std::chrono::system_clock::now(); + + // user callback + user_callback(); + + // calculate next shot. carry over nanoseconds into microseconds. + t.tv_nsec += interval_; + while (t.tv_nsec >= 1000000000) { + t.tv_nsec -= 1000000000; + t.tv_sec++; + } + } } -} - -void EcMaster::setThreadRealTime() -{ - /* Declare ourself as a real time task, priority 49. - PRREMPT_RT uses priority 50 - for kernel tasklets and interrupt handler by default */ - struct sched_param param; - param.sched_priority = 49; - // pthread_t this_thread = pthread_self(); - if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { - perror("sched_setscheduler failed"); - exit(-1); + + double EcMaster::elapsedTime() { + std::chrono::duration elapsed_seconds = curr_t_ - start_t_; + return elapsed_seconds.count() - 1.0; // started after 1 second } - /* Lock memory */ - if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { - perror("mlockall failed"); - exit(-2); + uint64_t EcMaster::elapsedCycles() { + return update_counter_; } - /* Pre-fault our stack - 8*1024 is the maximum stack size - which is guaranteed safe to access without faulting */ - int MAX_SAFE_STACK = 8 * 1024; - unsigned char dummy[MAX_SAFE_STACK]; - memset(dummy, 0, MAX_SAFE_STACK); -} + void EcMaster::setThreadHighPriority() { + pid_t pid = getpid(); + int priority_status = setpriority(PRIO_PROCESS, pid, -19); + if (priority_status) { + printWarning("setThreadHighPriority. Failed to set priority."); + return; + } + } -void EcMaster::checkDomainState(uint32_t domain) -{ - DomainInfo * domain_info = domain_info_[domain]; + void EcMaster::setThreadRealTime() { + /* Declare ourself as a real time task, priority 49. + PRREMPT_RT uses priority 50 + for kernel tasklets and interrupt handler by default */ + struct sched_param param; + param.sched_priority = 49; + // pthread_t this_thread = pthread_self(); + if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { + perror("sched_setscheduler failed"); + exit(-1); + } - ec_domain_state_t ds; - ecrt_domain_state(domain_info->domain, &ds); + /* Lock memory */ + if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { + perror("mlockall failed"); + exit(-2); + } - if (ds.working_counter != domain_info->domain_state.working_counter) { - printf("Domain: WC %u.\n", ds.working_counter); + /* Pre-fault our stack + 8*1024 is the maximum stack size + which is guaranteed safe to access without faulting */ + int MAX_SAFE_STACK = 8 * 1024; + unsigned char dummy[MAX_SAFE_STACK]; + memset(dummy, 0, MAX_SAFE_STACK); } - if (ds.wc_state != domain_info->domain_state.wc_state) { - printf("Domain: State %u.\n", ds.wc_state); - } - domain_info->domain_state = ds; -} + void EcMaster::checkDomainState(uint32_t domain) { + DomainInfo *domain_info = domain_info_[domain]; -void EcMaster::checkMasterState() -{ - ec_master_state_t ms; - ecrt_master_state(master_, &ms); + ec_domain_state_t ds; + ecrt_domain_state(domain_info->domain, &ds); - if (ms.slaves_responding != master_state_.slaves_responding) { - printf("%u slave(s).\n", ms.slaves_responding); - } - if (ms.al_states != master_state_.al_states) { - printf("Master AL states: 0x%02X.\n", ms.al_states); - } - if (ms.link_up != master_state_.link_up) { - printf("Link is %s.\n", ms.link_up ? "up" : "down"); + if (ds.working_counter != domain_info->domain_state.working_counter) { + printf("Domain: WC %u.\n", ds.working_counter); + } + if (ds.wc_state != domain_info->domain_state.wc_state) { + printf("Domain: State %u.\n", ds.wc_state); + } + domain_info->domain_state = ds; } - master_state_ = ms; -} + void EcMaster::checkMasterState() { + ec_master_state_t ms; + ecrt_master_state(master_, &ms); -void EcMaster::checkSlaveStates() -{ - for (SlaveInfo & slave : slave_info_) { - ec_slave_config_state_t s; - ecrt_slave_config_state(slave.config, &s); - - if (s.al_state != slave.config_state.al_state) { - // this spams the terminal at initialization. - printf("Slave: State 0x%02X.\n", s.al_state); + if (ms.slaves_responding != master_state_.slaves_responding) { + printf("%u slave(s).\n", ms.slaves_responding); } - if (s.online != slave.config_state.online) { - printf("Slave: %s.\n", s.online ? "online" : "offline"); + if (ms.al_states != master_state_.al_states) { + printf("Master AL states: 0x%02X.\n", ms.al_states); } - if (s.operational != slave.config_state.operational) { - printf("Slave: %soperational.\n", s.operational ? "" : "Not "); - slave.slave->set_state_is_operational(s.operational ? true : false); + if (ms.link_up != master_state_.link_up) { + printf("Link is %s.\n", ms.link_up ? "up" : "down"); } - slave.config_state = s; + master_state_ = ms; } -} + void EcMaster::checkSlaveStates() { + for (SlaveInfo &slave : slave_info_) { + ec_slave_config_state_t s; + ecrt_slave_config_state(slave.config, &s); + + if (s.al_state != slave.config_state.al_state) { + // this spams the terminal at initialization. + printf("Slave: State 0x%02X.\n", s.al_state); + } + if (s.online != slave.config_state.online) { + printf("Slave: %s.\n", s.online ? "online" : "offline"); + } + if (s.operational != slave.config_state.operational) { + printf("Slave: %soperational.\n", s.operational ? "" : "Not "); + slave.slave->set_state_is_operational(s.operational ? true : false); + } + slave.config_state = s; + } + } -void EcMaster::printWarning(const std::string & message) -{ - std::cout << "WARNING. Master. " << message << std::endl; -} + void EcMaster::printWarning(const std::string &message) { + std::cout << "WARNING. Master. " << message << std::endl; + } -} // namespace ethercat_interface +} // namespace ethercat_interface