get interface HW address

本文介绍两种获取设备MAC地址的方法:一种是在用户空间通过ioctl系统调用;另一种是在内核空间直接利用net_device结构体。这两种方法均适用于Linux环境。

1. in userspace use ioctl

/*************some error in code***********************/

static int __init getmac_init()
{
        int sock_id;
        struct ifreq ifr;
        char *macstr = "";
        unsigned char *mac = "";

        memset(&ifr,0,sizeof(struct ifreq));
        sock_id = socket(PF_INET, SOCK_STREAM, 0);
        if(sock_id == -1){
                return -1;
        }
        sprintf(ifr.ifr_name,"eth0");

        if(ioctl(sock_id, SIOCGIFHWADDR, &ifr) == 0){
                memcpy(mac,ifr.ifr_hwaddr.sa_data,6);
                sprintf(macstr,"%02X:%02X:%02X:%02X:%02X:%02X",mac[0],mac[1],mac[2],mac[3],mac[4],mac[5]);
        }
        printk("the mac address is:/n %s /n",macstr);

        sock_release(sock_id);

        return 0;
}

/************************************/

 

2.in kernel space use struct net_device

/****************/

#if LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,23)
#define DEV_GET_BY_NAME(name) dev_get_by_name(name);
#else  
#define DEV_GET_BY_NAME(name) dev_get_by_name(&init_net, name);
#endif

 

struct net_device *dev = NULL;

unsigned char *name = "eth0"

dev = DEV_GET_BY_NAME(name);

pirntk("HW address   %02X:%02X:%02X:%02X:%02X:%02X/n",

                                 dev->dev_addr[0],dev->dev_addr[1],dev->dev_addr[2],

                                 dev->dev_addr[3],dev->dev_addr[4],dev->dev_addr[5])

/***************/

解析代码: static void netlink_callback(AvahiNetlink *nl, struct nlmsghdr *n, void* userdata) { AvahiInterfaceMonitor *m = userdata; /* This routine is called for every RTNETLINK response packet */ assert(m); assert(n); assert(m->osdep.netlink == nl); if (n->nlmsg_type == RTM_NEWLINK) { /* A new interface appeared or an existing one has been modified */ struct ifinfomsg *ifinfomsg = NLMSG_DATA(n); AvahiHwInterface *hw; struct rtattr *a = NULL; size_t l; /* A (superfluous?) sanity check */ if (ifinfomsg->ifi_family != AF_UNSPEC) return; /* Check whether there already is an AvahiHwInterface object * for this link, so that we can update its data. Note that * Netlink sends us an RTM_NEWLINK not only when a new * interface appears, but when it changes, too */ if (!(hw = avahi_interface_monitor_get_hw_interface(m, ifinfomsg->ifi_index))) /* No object found, so let's create a new * one. avahi_hw_interface_new() will call * avahi_interface_new() internally twice for IPv4 and * IPv6, so there is no need for us to do that * ourselves */ if (!(hw = avahi_hw_interface_new(m, (AvahiIfIndex) ifinfomsg->ifi_index))) return; /* OOM */ /* Check whether the flags of this interface are OK for us */ hw->flags_ok = (ifinfomsg->ifi_flags & IFF_UP) && (!m->server->config.use_iff_running || (ifinfomsg->ifi_flags & IFF_RUNNING)) && ((ifinfomsg->ifi_flags & IFF_LOOPBACK) || (ifinfomsg->ifi_flags & IFF_MULTICAST)) && (m->server->config.allow_point_to_point || !(ifinfomsg->ifi_flags & IFF_POINTOPOINT)); /* Handle interface attributes */ l = NLMSG_PAYLOAD(n, sizeof(struct ifinfomsg)); a = IFLA_RTA(ifinfomsg); while (RTA_OK(a, l)) { switch(a->rta_type) { case IFLA_IFNAME: /* Fill in interface name */ avahi_free(hw->name); hw->name = avahi_strndup(RTA_DATA(a), RTA_PAYLOAD(a)); break; case IFLA_MTU: /* Fill in MTU */ assert(RTA_PAYLOAD(a) == sizeof(unsigned int)); hw->mtu = *((unsigned int*) RTA_DATA(a)); break; case IFLA_ADDRESS: /* Fill in hardware (MAC) address */ hw->mac_address_size = RTA_PAYLOAD(a); if (hw->mac_address_size > AVAHI_MAC_ADDRESS_MAX) hw->mac_address_size = AVAHI_MAC_ADDRESS_MAX; memcpy(hw->mac_address, RTA_DATA(a), hw->mac_address_size); break; default: ; } a = RTA_NEXT(a, l); } /* Check whether this interface is now "relevant" for us. If * it is Avahi will start to announce its records on this * interface and send out queries for subscribed records on * it */ avahi_hw_interface_check_relevant(hw); /* Update any associated RRs of this interface. (i.e. the * _workstation._tcp record containing the MAC address) */ avahi_hw_interface_update_rrs(hw, 0); } else if (n->nlmsg_type == RTM_DELLINK) { /* An interface has been removed */ struct ifinfomsg *ifinfomsg = NLMSG_DATA(n); AvahiHwInterface *hw; /* A (superfluous?) sanity check */ if (ifinfomsg->ifi_family != AF_UNSPEC) return; /* Get a reference to our AvahiHwInterface object of this interface */ if (!(hw = avahi_interface_monitor_get_hw_interface(m, (AvahiIfIndex) ifinfomsg->ifi_index))) return; /* Free our object */ avahi_hw_interface_free(hw, 0); } else if (n->nlmsg_type == RTM_NEWADDR || n->nlmsg_type == RTM_DELADDR) { /* An address has been added, modified or removed */ struct ifaddrmsg *ifaddrmsg = NLMSG_DATA(n); AvahiInterface *i; struct rtattr *a = NULL; size_t l; AvahiAddress raddr, rlocal, *r; int raddr_valid = 0, rlocal_valid = 0; /* We are only interested in IPv4 and IPv6 */ if (ifaddrmsg->ifa_family != AF_INET && ifaddrmsg->ifa_family != AF_INET6) return; /* Try to get a reference to our AvahiInterface object for the * interface this address is assigned to. If ther is no object * for this interface, we ignore this address. */ if (!(i = avahi_interface_monitor_get_interface(m, (AvahiIfIndex) ifaddrmsg->ifa_index, avahi_af_to_proto(ifaddrmsg->ifa_family)))) return; /* Fill in address family for our new address */ rlocal.proto = raddr.proto = avahi_af_to_proto(ifaddrmsg->ifa_family); l = NLMSG_PAYLOAD(n, sizeof(struct ifaddrmsg)); a = IFA_RTA(ifaddrmsg); while (RTA_OK(a, l)) { switch(a->rta_type) { case IFA_LOCAL: if ((rlocal.proto == AVAHI_PROTO_INET6 && RTA_PAYLOAD(a) != 16) || (rlocal.proto == AVAHI_PROTO_INET && RTA_PAYLOAD(a) != 4)) return; memcpy(rlocal.data.data, RTA_DATA(a), RTA_PAYLOAD(a)); rlocal_valid = 1; break; case IFA_ADDRESS: /* Fill in local address data. Usually this is * preferable over IFA_ADDRESS if both are set, * since this refers to the local address of a PPP * link while IFA_ADDRESS refers to the other * end. */ if ((raddr.proto == AVAHI_PROTO_INET6 && RTA_PAYLOAD(a) != 16) || (raddr.proto == AVAHI_PROTO_INET && RTA_PAYLOAD(a) != 4)) return; memcpy(raddr.data.data, RTA_DATA(a), RTA_PAYLOAD(a)); raddr_valid = 1; break; default: ; } a = RTA_NEXT(a, l); } /* If there was no adress attached to this message, let's quit. */ if (rlocal_valid) r = &rlocal; else if (raddr_valid) r = &raddr; else return; if (n->nlmsg_type == RTM_NEWADDR) { AvahiInterfaceAddress *addr; /* This address is new or has been modified, so let's get an object for it */ if (!(addr = avahi_interface_monitor_get_address(m, i, r))) /* Mmm, no object existing yet, so let's create a new one */ if (!(addr = avahi_interface_address_new(m, i, r, ifaddrmsg->ifa_prefixlen))) return; /* OOM */ /* Update the scope field for the address */ addr->global_scope = ifaddrmsg->ifa_scope == RT_SCOPE_UNIVERSE || ifaddrmsg->ifa_scope == RT_SCOPE_SITE; addr->deprecated = !!(ifaddrmsg->ifa_flags & IFA_F_DEPRECATED); } else { AvahiInterfaceAddress *addr; assert(n->nlmsg_type == RTM_DELADDR); /* Try to get a reference to our AvahiInterfaceAddress object for this address */ if (!(addr = avahi_interface_monitor_get_address(m, i, r))) return; /* And free it */ avahi_interface_address_free(addr); } /* Avahi only considers interfaces with at least one address * attached relevant. Since we migh have added or removed an * address, let's have it check again whether the interface is * now relevant */ avahi_interface_check_relevant(i); /* Update any associated RRs, like A or AAAA for our new/removed address */ avahi_interface_update_rrs(i, 0); } else if (n->nlmsg_type == NLMSG_DONE) { /* This wild dump request ended, so let's see what we do next */ if (m->osdep.list == LIST_IFACE) { /* Mmmm, interfaces have been wild dumped already, so * let's go on with wild dumping the addresses */ if (netlink_list_items(m->osdep.netlink, RTM_GETADDR, &m->osdep.query_addr_seq) < 0) { avahi_log_warn("NETLINK: Failed to list addrs: %s", strerror(errno)); m->osdep.list = LIST_DONE; } else /* Update state information */ m->osdep.list = LIST_ADDR; } else /* We're done. Tell avahi_interface_monitor_sync() to finish. */ m->osdep.list = LIST_DONE; if (m->osdep.list == LIST_DONE) { /* Only after this boolean variable has been set, Avahi * will start to announce or browse on all interfaces. It * is originaly set to 0, which means that relevancy * checks and RR updates are disabled during the wild * dumps. */ m->list_complete = 1; /* So let's check if any interfaces are relevant now */ avahi_interface_monitor_check_relevant(m); /* And update all RRs attached to any interface */ avahi_interface_monitor_update_rrs(m, 0); /* Tell the user that the wild dump is complete */ avahi_log_info("Network interface enumeration completed."); } } else if (n->nlmsg_type == NLMSG_ERROR && (n->nlmsg_seq == m->osdep.query_link_seq || n->nlmsg_seq == m->osdep.query_addr_seq)) { struct nlmsgerr *e = NLMSG_DATA (n); /* Some kind of error happened. Let's just tell the user and * ignore it otherwise */ if (e->error) avahi_log_warn("NETLINK: Failed to browse: %s", strerror(-e->error)); } }
09-05
xyg@xyg-T6AD:~$ cd ~/ros2_ws colcon build --packages-select pca9685_hardware source install/setup.bash Starting >>> pca9685_hardware --- stderr: pca9685_hardware /home/xyg/ros2_ws/src/pca9685_hardware/src/pca9685_system_hardware.cpp:9:1: error: ‘CallbackReturn’ does not name a type 9 | CallbackReturn Pca9685SystemHardware::on_init(const hardware_interface::HardwareInfo& info) | ^~~~~~~~~~~~~~ /home/xyg/ros2_ws/src/pca9685_hardware/src/pca9685_system_hardware.cpp: In member function ‘virtual std::vector<hardware_interface::StateInterface> pca9685_hardware::Pca9685SystemHardware::export_state_interfaces()’: /home/xyg/ros2_ws/src/pca9685_hardware/src/pca9685_system_hardware.cpp:44:37: error: ‘HW_IF_POSITION’ is not a member of ‘hardware_interface’ 44 | hardware_interface::HW_IF_POSITION, | ^~~~~~~~~~~~~~ /home/xyg/ros2_ws/src/pca9685_hardware/src/pca9685_system_hardware.cpp: In member function ‘virtual std::vector<hardware_interface::CommandInterface> pca9685_hardware::Pca9685SystemHardware::export_command_interfaces()’: /home/xyg/ros2_ws/src/pca9685_hardware/src/pca9685_system_hardware.cpp:59:37: error: ‘HW_IF_POSITION’ is not a member of ‘hardware_interface’ 59 | hardware_interface::HW_IF_POSITION, | ^~~~~~~~~~~~~~ /home/xyg/ros2_ws/src/pca9685_hardware/src/pca9685_system_hardware.cpp: At global scope: /home/xyg/ros2_ws/src/pca9685_hardware/src/pca9685_system_hardware.cpp:67:1: error: ‘CallbackReturn’ does not name a type 67 | CallbackReturn Pca9685SystemHardware::on_activate(const rclcpp_lifecycle::State& previous_state) | ^~~~~~~~~~~~~~ /home/xyg/ros2_ws/src/pca9685_hardware/src/pca9685_system_hardware.cpp:126:1: error: ‘CallbackReturn’ does not name a type 126 | CallbackReturn Pca9685SystemHardware::on_deactivate(const rclcpp_lifecycle::State& previous_state) | ^~~~~~~~~~~~~~ /home/xyg/ros2_ws/src/pca9685_hardware/src/pca9685_system_hardware.cpp: In member function ‘virtual hardware_interface::return_type pca9685_hardware::Pca9685SystemHardware::read(const rclcpp::Time&, const rclcpp::Duration&)’: /home/xyg/ros2_ws/src/pca9685_hardware/src/pca9685_system_hardware.cpp:152:81: warning: unused parameter ‘time’ [-Wunused-parameter] 152 | return_type Pca9685SystemHardware::read(const rclcpp::Time& time, const rclcpp::Duration& period) | ~~~~~~~~~~~~~~~~~~~~^~~~ /home/xyg/ros2_ws/src/pca9685_hardware/src/pca9685_system_hardware.cpp:152:111: warning: unused parameter ‘period’ [-Wunused-parameter] 152 | ardware::read(const rclcpp::Time& time, const rclcpp::Duration& period) | ~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~ /home/xyg/ros2_ws/src/pca9685_hardware/src/pca9685_system_hardware.cpp: In member function ‘virtual hardware_interface::return_type pca9685_hardware::Pca9685SystemHardware::write(const rclcpp::Time&, const rclcpp::Duration&)’: /home/xyg/ros2_ws/src/pca9685_hardware/src/pca9685_system_hardware.cpp:159:82: warning: unused parameter ‘time’ [-Wunused-parameter] 159 | eturn_type Pca9685SystemHardware::write(const rclcpp::Time& time, const rclcpp::Duration& period) | ~~~~~~~~~~~~~~~~~~~~^~~~ /home/xyg/ros2_ws/src/pca9685_hardware/src/pca9685_system_hardware.cpp:159:112: warning: unused parameter ‘period’ [-Wunused-parameter] 159 | rdware::write(const rclcpp::Time& time, const rclcpp::Duration& period) | ~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~ gmake[2]: *** [CMakeFiles/pca9685_hardware.dir/build.make:76:CMakeFiles/pca9685_hardware.dir/src/pca9685_system_hardware.cpp.o] 错误 1 gmake[1]: *** [CMakeFiles/Makefile2:137:CMakeFiles/pca9685_hardware.dir/all] 错误 2 gmake: *** [Makefile:146:all] 错误 2 --- Failed <<< pca9685_hardware [2.25s, exited with code 2] Summary: 0 packages finished [2.39s] 1 package failed: pca9685_hardware 1 package had stderr output: pca9685_hardware not found: "/home/xyg/ros2_ws/install/pca9685_hardware/share/pca9685_hardware/local_setup.bash" xyg@xyg-T6AD:~/ros2_ws$ ros2 control list_hardware_components | grep PCA9685 # 应输出: pca9685_hardware/Pca9685SystemHardware [INFO] [1750735736.192545351] [_ros2cli_16699]: waiting for service /controller_manager/list_hardware_components to become available... [WARN] [1750735746.210548769] [_ros2cli_16699]: Could not contact service /controller_manager/list_hardware_components [INFO] [1750735746.210767288] [_ros2cli_16699]: waiting for service /controller_manager/list_hardware_components to become available... ^C 以下分别是pca9685_params.yaml:pca9685_hardware: ros__parameters: hardware: plugin: "pca9685_hardware/Pca9685SystemHardware" i2c_bus: "i2c-1" # 移除/dev/前缀,代码中会自动添加 i2c_address: "0x33" # I2C地址保持不变 channel_mapping: "5,7,9,11,13,15" # 使用逗号分隔的字符串格式 joints: - joint1 # 关节名称与机器人结构对应 - joint2 - joint3 - joint4 - joint5 - joint6然后是pca9685_system_hardware.hpp:#ifndef PCA9685_HARDWARE__PCA9685_SYSTEM_HARDWARE_HPP_ #define PCA9685_HARDWARE__PCA9685_SYSTEM_HARDWARE_HPP_ #include <hardware_interface/types/hardware_interface_return_values.hpp> #include <hardware_interface/system_interface.hpp> #include <rclcpp_lifecycle/state.hpp> #include <rclcpp/logger.hpp> #include <memory> #include <vector> #include <string> #include <fcntl.h> #include <linux/i2c-dev.h> #include <unistd.h> namespace pca9685_hardware { class Pca9685SystemHardware : public hardware_interface::SystemInterface { public: using CallbackReturn = hardware_interface::CallbackReturn; using ReturnType = hardware_interface::return_type; CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override; std::vector<hardware_interface::StateInterface> export_state_interfaces() override; std::vector<hardware_interface::CommandInterface> export_command_interfaces() override; CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; ReturnType read(const rclcpp::Time& time, const rclcpp::Duration& period) override; ReturnType write(const rclcpp::Time& time, const rclcpp::Duration& period) override; private: int i2c_fd_; std::string i2c_bus_; int i2c_address_; std::vector<int> channel_mapping_; std::vector<double> hw_commands_; std::vector<double> hw_states_; bool write_byte_data(int reg, uint8_t value); bool write_word_data(int reg, uint16_t value); uint8_t read_byte_data(int reg); }; } // namespace pca9685_hardware #endif // PCA9685_HARDWARE__PCA9685_SYSTEM_HARDWARE_HPP_ 然后是:pca9685_hardware_plugin.xml:<library path="libpca9685_hardware"> <class name="pca9685_hardware/Pca9685SystemHardware" type="pca9685_hardware::Pca9685SystemHardware" base_class_type="hardware_interface::SystemInterface"> <description>PCA9685 PWM控制器硬件接口</description> </class> </library> 还有:pca9685_system_hardware.cpp:#include "pca9685_hardware/pca9685_system_hardware.hpp" #include <hardware_interface/types/hardware_interface_return_values.hpp> #include <rclcpp/rclcpp.hpp> #include <cmath> namespace pca9685_hardware { CallbackReturn Pca9685SystemHardware::on_init(const hardware_interface::HardwareInfo& info) { if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } try { i2c_bus_ = info.hardware_parameters.at("i2c_bus"); i2c_address_ = std::stoi(info.hardware_parameters.at("i2c_address"), nullptr, 16); std::string channel_str = info.hardware_parameters.at("channel_mapping"); std::istringstream iss(channel_str); std::string token; while (std::getline(iss, token, ',')) { channel_mapping_.push_back(std::stoi(token)); } hw_commands_.resize(info.joints.size(), 0.0); hw_states_.resize(info.joints.size(), 0.0); return CallbackReturn::SUCCESS; } catch (const std::exception& e) { RCLCPP_ERROR(rclcpp::get_logger("pca9685_hardware"), "初始化失败: %s", e.what()); return CallbackReturn::ERROR; } } std::vector<hardware_interface::StateInterface> Pca9685SystemHardware::export_state_interfaces() { std::vector<hardware_interface::StateInterface> state_interfaces; for (size_t i = 0; i < hw_states_.size(); i++) { state_interfaces.emplace_back( hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i] ) ); } return state_interfaces; } std::vector<hardware_interface::CommandInterface> Pca9685SystemHardware::export_command_interfaces() { std::vector<hardware_interface::CommandInterface> command_interfaces; for (size_t i = 0; i < hw_commands_.size(); i++) { command_interfaces.emplace_back( hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i] ) ); } return command_interfaces; } CallbackReturn Pca9685SystemHardware::on_activate(const rclcpp_lifecycle::State& previous_state) { RCLCPP_INFO(rclcpp::get_logger("pca9685_hardware"), "激活硬件接口"); try { std::string device_path = "/dev/" + i2c_bus_; i2c_fd_ = open(device_path.c_str(), O_RDWR); if (i2c_fd_ < 0) { RCLCPP_ERROR(rclcpp::get_logger("pca9685_hardware"), "无法打开I2C设备: %s", device_path.c_str()); return CallbackReturn::ERROR; } if (ioctl(i2c_fd_, I2C_SLAVE, i2c_address_) < 0) { RCLCPP_ERROR(rclcpp::get_logger("pca9685_hardware"), "无法设置I2C地址: 0x%02X", i2c_address_); close(i2c_fd_); return CallbackReturn::ERROR; } // 初始化PCA9685 if (!write_byte_data(0x00, 0x01)) { // 复位 return CallbackReturn::ERROR; } usleep(50000); // 设置为普通模式 if (!write_byte_data(0x00, 0x00)) { return CallbackReturn::ERROR; } // 设置PWM频率为50Hz (20ms周期) uint8_t prescale = static_cast<uint8_t>(25000000.0 / (4096.0 * 50.0) - 1); uint8_t old_mode = read_byte_data(0x00); uint8_t new_mode = (old_mode & 0x7F) | 0x10; // 进入睡眠模式 if (!write_byte_data(0x00, new_mode)) { return CallbackReturn::ERROR; } if (!write_byte_data(0xFE, prescale)) { return CallbackReturn::ERROR; } if (!write_byte_data(0x00, old_mode)) { return CallbackReturn::ERROR; } usleep(50000); if (!write_byte_data(0x00, old_mode | 0x80)) { // 启用自动递增 return CallbackReturn::ERROR; } RCLCPP_INFO(rclcpp::get_logger("pca9685_hardware"), "硬件接口激活成功"); return CallbackReturn::SUCCESS; } catch (const std::exception& e) { RCLCPP_ERROR(rclcpp::get_logger("pca9685_hardware"), "激活失败: %s", e.what()); if (i2c_fd_ >= 0) { close(i2c_fd_); } return CallbackReturn::ERROR; } } CallbackReturn Pca9685SystemHardware::on_deactivate(const rclcpp_lifecycle::State& previous_state) { RCLCPP_INFO(rclcpp::get_logger("pca9685_hardware"), "停用硬件接口"); try { // 关闭所有PWM通道 for (size_t i = 0; i < 16; i++) { if (!write_word_data(0x06 + 4 * i, 0)) { RCLCPP_WARN(rclcpp::get_logger("pca9685_hardware"), "关闭通道 %zu 失败", i); } } if (i2c_fd_ >= 0) { close(i2c_fd_); i2c_fd_ = -1; } RCLCPP_INFO(rclcpp::get_logger("pca9685_hardware"), "硬件接口停用成功"); return CallbackReturn::SUCCESS; } catch (const std::exception& e) { RCLCPP_ERROR(rclcpp::get_logger("pca9685_hardware"), "停用失败: %s", e.what()); return CallbackReturn::ERROR; } } hardware_interface::return_type Pca9685SystemHardware::read(const rclcpp::Time& time, const rclcpp::Duration& period) { // 舵机通常没有位置反馈,所以我们将命令值作为状态返回 hw_states_ = hw_commands_; return ReturnType::OK; } hardware_interface::return_type Pca9685SystemHardware::write(const rclcpp::Time& time, const rclcpp::Duration& period) { try { for (size_t i = 0; i < hw_commands_.size(); i++) { if (i >= channel_mapping_.size()) { RCLCPP_WARN(rclcpp::get_logger("pca9685_hardware"), "通道映射越界: %zu", i); continue; } // 将弧度转换为角度 (0-180度) double angle_rad = hw_commands_[i]; double angle_deg = std::fmod(std::fmod(angle_rad, 2 * M_PI) + 2 * M_PI, 2 * M_PI); angle_deg = std::clamp(angle_deg * 180.0 / M_PI, 0.0, 180.0); // 计算PWM脉冲宽度 (500-2500us对应0-180度) uint16_t pulse_width = static_cast<uint16_t>(500 + (angle_deg / 180.0) * (2500 - 500)); uint16_t pwm_value = static_cast<uint16_t>(pulse_width * 4096.0 / 20000.0); // 设置PWM输出 int channel = channel_mapping_[i]; if (channel < 0 || channel > 15) { RCLCPP_WARN(rclcpp::get_logger("pca9685_hardware"), "无效通道: %d", channel); continue; } // 设置PWM值 (0为关闭时间,pwm_value为开启时间) if (!write_word_data(0x06 + 4 * channel, 0)) { RCLCPP_WARN(rclcpp::get_logger("pca9685_hardware"), "设置通道 %d 关闭时间失败", channel); } if (!write_word_data(0x08 + 4 * channel, pwm_value)) { RCLCPP_WARN(rclcpp::get_logger("pca9685_hardware"), "设置通道 %d 开启时间失败", channel); } } return ReturnType::OK; } catch (const std::exception& e) { RCLCPP_ERROR(rclcpp::get_logger("pca9685_hardware"), "写入失败: %s", e.what()); return ReturnType::ERROR; } } bool Pca9685SystemHardware::write_byte_data(int reg, uint8_t value) { uint8_t buf[2] = {static_cast<uint8_t>(reg), value}; if (::write(i2c_fd_, buf, 2) != 2) { RCLCPP_ERROR(rclcpp::get_logger("pca9685_hardware"), "I2C写入失败: reg=0x%02X, value=0x%02X", reg, value); return false; } return true; } bool Pca9685SystemHardware::write_word_data(int reg, uint16_t value) { uint8_t buf[3] = { static_cast<uint8_t>(reg), static_cast<uint8_t>(value & 0xFF), static_cast<uint8_t>((value >> 8) & 0xFF) }; if (::write(i2c_fd_, buf, 3) != 3) { RCLCPP_ERROR(rclcpp::get_logger("pca9685_hardware"), "I2C写入失败: reg=0x%02X, value=0x%04X", reg, value); return false; } return true; } uint8_t Pca9685SystemHardware::read_byte_data(int reg) { uint8_t buf = static_cast<uint8_t>(reg); if (::write(i2c_fd_, &buf, 1) != 1) { RCLCPP_ERROR(rclcpp::get_logger("pca9685_hardware"), "I2C写入寄存器失败: reg=0x%02X", reg); return 0; } if (::read(i2c_fd_, &buf, 1) != 1) { RCLCPP_ERROR(rclcpp::get_logger("pca9685_hardware"), "I2C读取失败: reg=0x%02X", reg); return 0; } return buf; } } // namespace pca9685_hardware #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(pca9685_hardware::Pca9685SystemHardware, hardware_interface::SystemInterface) CMakeLists.txt:cmake_minimum_required(VERSION 3.8) project(pca9685_hardware) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() find_package(ament_cmake REQUIRED) find_package(hardware_interface REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(pluginlib REQUIRED) add_library( ${PROJECT_NAME} SHARED src/pca9685_system_hardware.cpp ) target_include_directories( ${PROJECT_NAME} PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> $<INSTALL_INTERFACE:include> ) ament_target_dependencies( ${PROJECT_NAME} "hardware_interface" "rclcpp" "rclcpp_lifecycle" "pluginlib" ) # 导出插件描述文件(确保resource目录存在) pluginlib_export_plugin_description_file(hardware_interface resource/pca9685_hardware_plugin.xml) install( TARGETS ${PROJECT_NAME} DESTINATION lib ) install( DIRECTORY include/ DESTINATION include ) # 安装插件描述文件(明确指定resource目录) install( FILES resource/pca9685_hardware_plugin.xml DESTINATION share/${PROJECT_NAME} ) ament_package() package.xml:<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema-instance"?> <package format="3"> <name>pca9685_hardware</name> <version>0.0.1</version> <description>PCA9685 PWM Servo Driver Hardware Interface for ROS 2</description> <maintainer email="xyg@example.com">xyg</maintainer> <license>Apache License 2.0</license> <!-- 核心依赖 --> <depend>hardware_interface</depend> <depend>rclcpp</depend> <depend>pluginlib</depend> <depend>controller_manager</depend> <depend>rclcpp_lifecycle</depend> <!-- 测试依赖 --> <test_depend>ament_cmake</test_depend> <test_depend>ament_cmake_copyright</test_depend> <test_depend>ament_cmake_clang_format</test_depend> <test_depend>ament_cmake_cppcheck</test_depend> <!-- 移除所有Python相关依赖 --> <!-- 原test_depend已删除: ament_flake8, ament_pep257, python3-pytest --> <export> <!-- 改为ament_cmake编译类型 --> <build_type>ament_cmake</build_type> <!-- 硬件接口插件描述文件 --> <hardware_interface plugin="${prefix}/resource/plugin_description.xml" /> </export> </package> 以上是各个文件内容以及启动日志报错内容
06-25
static u8 * pppoe_build_rewrite (vnet_main_t * vnm, u32 sw_if_index, vnet_link_t link_type, const void *dst_address) { pppoe_main_t *pem = &pppoe_main; pppoe_session_t *t; vnet_hw_interface_t *hi; vnet_sw_interface_t *si; vnet_sw_interface_t *sup_si; pppoe_header_t *pppoe; u32 session_id; u8 *rw = 0; session_id = pem->session_index_by_sw_if_index[sw_if_index]; t = pool_elt_at_index (pem->sessions, session_id); int len = sizeof (pppoe_header_t) + sizeof (ethernet_header_t); si = vnet_get_sw_interface (vnm, t->encap_if_index); sup_si = vnet_get_sup_sw_interface(vnm, t->encap_if_index); hi = vnet_get_hw_interface (vnm, sup_si->hw_if_index); if (si->type == VNET_SW_INTERFACE_TYPE_SUB) { if (si->sub.eth.flags.one_tag == 1) { len += sizeof (ethernet_vlan_header_t); } } else if (hi->dev_class_index == dsa_device_class.index) { len += sizeof (ethernet_vlan_header_t); } vec_validate_aligned (rw, len - 1, CLIB_CACHE_LINE_BYTES); ethernet_header_t *eth_hdr = (ethernet_header_t *) rw; eth_hdr->type = clib_host_to_net_u16 (ETHERNET_TYPE_PPPOE_SESSION); pppoe = (pppoe_header_t *) (eth_hdr + 1); if (si->type == VNET_SW_INTERFACE_TYPE_SUB) { if (si->sub.eth.flags.one_tag == 1) { //eth_hdr->type = clib_host_to_net_u16 (ETHERNET_TYPE_VLAN); eth_hdr->type = clib_host_to_net_u16 (get_outter_tag_type(si->sw_if_index)); ethernet_vlan_header_t *vlan = (ethernet_vlan_header_t *) (eth_hdr + 1); vlan->type = clib_host_to_net_u16 (ETHERNET_TYPE_PPPOE_SESSION); vlan->priority_cfi_and_id = clib_host_to_net_u16 (si->sub.eth.outer_vlan_id); pppoe = (pppoe_header_t *) (vlan + 1); } si = vnet_get_sw_interface (vnm, si->sup_sw_if_index); } else if (hi->dev_class_index == dsa_device_class.index) { u32 tag = get_full_tag_for_interface(si->sw_if_index); u32 *p = (u32*)(&eth_hdr->type); *p = tag; *(u16 *)(p + 1) = clib_host_to_net_u16 (ETHERNET_TYPE_PPPOE_SESSION); pppoe = (pppoe_header_t *) ((char*)p + 6); } // set the right mac addresses clib_memcpy (eth_hdr->src_address, hi->hw_address, 6); clib_memcpy (eth_hdr->dst_address, t->client_mac, 6); pppoe->ver_type = PPPOE_VER_TYPE; pppoe->code = 0; pppoe->session_id = clib_host_to_net_u16 (t->session_id); pppoe->length = 0; /* To be filled in at run-time */ switch (link_type) { case VNET_LINK_IP4: pppoe->ppp_proto = clib_host_to_net_u16 (PPP_PROTOCOL_ip4); break; case VNET_LINK_IP6: pppoe->ppp_proto = clib_host_to_net_u16 (PPP_PROTOCOL_ip6); break; default: break; } return rw; } 这段代码有什么用
11-20
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值