rs_save

#include "manager/node_manager.hpp" #include <rs_driver/macro/version.hpp> #include <signal.h> #ifdef ROS_FOUND #include <ros/ros.h> #include <ros/package.h> #elif ROS2_FOUND #include <rclcpp/rclcpp.hpp> #endif using namespace robosense::lidar; #ifdef ROS2_FOUND std::mutex g_mtx; std::condition_variable g_cv; #endif static void sigHandler(int sig) { RS_MSG << "RoboSense-LiDAR-Driver is stopping....." << RS_REND; #ifdef ROS_FOUND ros::shutdown(); #elif ROS2_FOUND g_cv.notify_all(); #endif } /////////////////////////////////////////////////////////////////////////////////////////////// static std::string pcd_name_prefix = ""; void SetPCDNamePrefix(const std::string& prefix) { RS_INFO << "------------------------------------------------------" << RS_REND; RS_INFO << "Set PCD Name Prefix: " << prefix << RS_REND; pcd_name_prefix = prefix; RS_INFO << "------------------------------------------------------" << RS_REND; } const std::string& GetPCDNamePrefix() { // RS_INFO << "Use PCD Name Prefix: " << pcd_name_prefix << RS_REND; return pcd_name_prefix; } /////////////////////////////////////////////////////////////////////////////////////////////// static std::string pcd_file_path = ""; void SetPCDFilePath(const std::string& path) { RS_INFO << "------------------------------------------------------" << RS_REND; RS_INFO << "Set PCD Dir: " << path << RS_REND; pcd_file_path = path; RS_INFO << "------------------------------------------------------" << RS_REND; } const std::string& GetPCDFilePath() { // RS_INFO << "Use PCD Dir: " << pcd_file_path << RS_REND; return pcd_file_path; } /////////////////////////////////////////////////////////////////////////////////////////////// void StringSplit(const std::string& str, const char split_char, std::vector<std::string>& res) { std::istringstream iss(str); std::string token; while(getline(iss, token, split_char)) { res.push_back(std::move(token)); } } const std::size_t LIDAR_INDEX_IN_PCAP_FILE = 4; /////////////////////////////////////////////////////////////////////////////////////////////// int main(int argc, char** argv) { signal(SIGINT, sigHandler); ///< bind ctrl+c signal with the sigHandler function RS_TITLE << "********************************************************" << RS_REND; RS_TITLE << "********** **********" << RS_REND; RS_TITLE << "********** RSLidar_SDK Version: v" << RSLIDAR_VERSION_MAJOR << "." << RSLIDAR_VERSION_MINOR << "." << RSLIDAR_VERSION_PATCH << " **********" << RS_REND; RS_TITLE << "********** **********" << RS_REND; RS_TITLE << "********************************************************" << RS_REND; #ifdef ROS_FOUND ros::init(argc, argv, "rslidar_sdk_node", ros::init_options::NoSigintHandler); #elif ROS2_FOUND rclcpp::init(argc, argv); #endif std::string config_path; #ifdef RUN_IN_ROS_WORKSPACE config_path = ros::package::getPath("rslidar_sdk"); #else config_path = (std::string)PROJECT_PATH; #endif config_path += "/config/"; #ifdef ROS_FOUND ros::NodeHandle priv_hh("~"); std::string pcap_file; std::string dest_pcd_dir; priv_hh.param("pcap_file", pcap_file, std::string("")); priv_hh.param("dest_pcd_dir", dest_pcd_dir, std::string("")); #elif ROS2_FOUND std::shared_ptr<rclcpp::Node> nd = rclcpp::Node::make_shared("param_handle"); std::string pcap_file = nd->declare_parameter<std::string>("pcap_file", ""); std::string dest_pcd_dir = nd->declare_parameter<std::string>("dest_pcd_dir", ""); #endif // #if defined(ROS_FOUND) || defined(ROS2_FOUND) // if (!path.empty()) // { // config_path = path; // } // #endif std::string lidar_name = ""; std::vector<std::string> path_nodes{}; StringSplit(pcap_file, '/', path_nodes); std::vector<std::string> filename_segs{}; std::vector<std::string> token_res{}; if (path_nodes.size() > 0) { StringSplit(path_nodes[path_nodes.size()-1], '.', filename_segs); if (filename_segs.size() > 0) { StringSplit(filename_segs[0], '_', token_res); } else { RS_ERROR << "The lidar name in pcap file is not specified: [" << pcap_file << "] is wrong. Please check." << RS_REND; return -1; } } if (token_res.size() <= LIDAR_INDEX_IN_PCAP_FILE) { RS_ERROR << "The lidar name in pcap file is not specified: [" << pcap_file << "] is wrong. Please check." << RS_REND; return -1; } else { lidar_name = token_res[LIDAR_INDEX_IN_PCAP_FILE]; std::stringstream ss; for (std::size_t i = 0; i <= LIDAR_INDEX_IN_PCAP_FILE; i++) { ss << token_res[i] << "_"; } SetPCDNamePrefix(ss.str()); } std::stringstream ss; ss << "config-" << lidar_name << ".yaml"; config_path = config_path + ss.str(); if (dest_pcd_dir.empty()) { RS_ERROR << "The destination pcd output dir is not specified: [" << dest_pcd_dir << "] is wrong. Please check." << RS_REND; return -1; } else { SetPCDFilePath(dest_pcd_dir); } SourceDriverPCAPPath::SetPCAPFilePath(pcap_file); YAML::Node config; try { config = YAML::LoadFile(config_path); RS_INFO << "--------------------------------------------------------" << RS_REND; RS_INFO << "Config loaded from PATH:" << RS_REND; RS_INFO << config_path << RS_REND; RS_INFO << "--------------------------------------------------------" << RS_REND; } catch (...) { RS_ERROR << "The format of config file " << config_path << " is wrong. Please check (e.g. indentation)." << RS_REND; return -1; } std::shared_ptr<NodeManager> demo_ptr = std::make_shared<NodeManager>(); demo_ptr->init(config); demo_ptr->start(); RS_MSG << "RoboSense-LiDAR-Driver is running....." << RS_REND; #ifdef ROS_FOUND ros::spin(); #elif ROS2_FOUND std::unique_lock<std::mutex> lck(g_mtx); g_cv.wait(lck); #endif return 0; }
06-21
// RS485 接收函数 void RS485_RX() { // 计算接收数据的CRC校验值 uint16_t R485_RxCRCCalc = ModbusCRCCalc(Rs485_RxPacket, Rs485_RxDataLen - 2); uint16_t R485_RxCRC = (Rs485_RxPacket[Rs485_RxDataLen - 2] << 8) | Rs485_RxPacket[Rs485_RxDataLen - 1]; IWDG_Feed(); // CRC校验判定 if (R485_RxCRCCalc == R485_RxCRC) // 如果CRC校验正确 { OLED_ShowString(3, 1, "OK"); // 显示校验成功 if (Rs485_RxPacket[0] == Rs485_Slaveaddr) // 判断从机地址是否正确 { if (Rs485_RxPacket[1] == 0x03) // 功能码03(读寄存器) { RS485_TX(Rs485_RxPacket, Rs485_RxPacket[3], Rs485_RxPacket[5]); // 发送响应数据 Flash_save(); } else if (Rs485_RxPacket[1] == 0x06) // 功能码06(写寄存器) { uint16_t reg_addr = (Rs485_RxPacket[2] << 8) | Rs485_RxPacket[3]; uint16_t write_value = (Rs485_RxPacket[4] << 8) | Rs485_RxPacket[5]; // 构造响应帧到发送缓冲区 Rs485_TxPacket[0] = Rs485_Slaveaddr; Rs485_TxPacket[1] = 0x06; Rs485_TxPacket[2] = Rs485_RxPacket[2]; // 寄存器地址高字节 Rs485_TxPacket[3] = Rs485_RxPacket[3]; // 寄存器地址低字节 Rs485_TxPacket[4] = Rs485_RxPacket[4]; // 写入值高字节 Rs485_TxPacket[5] = Rs485_RxPacket[5]; // 写入值低字节 // 处理不同寄存器写入 switch (reg_addr) { case 0x00: // 修改从机地址 originalAddr = Rs485_RxPacket[0]; Rs485_Slaveaddr = Rs485_RxPacket[5]; // 更新从机地址 Rs485_RxPacket[0] = originalAddr; Rs485_Slaveaddr = write_value & 0xFF; // 确保地址为1字节 break; case 0x01: wd = Rs485_RxPacket[5]; // 更新温度上限 wd = write_value & 0xFF; // 确保地址为1字节 break; case 0x03: sd = Rs485_RxPacket[5]; // 更新温度上限 sd = write_value & 0xFF; // 确保地址为1字节 break; case 0x02: wwd = Rs485_RxPacket[5]; // 更新温度xia限 wwd = write_value & 0xFF; // 确保地址为1字节 break; case 0x04: ssd = Rs485_RxPacket[5]; // 更新shi度xia限 ssd = write_value & 0xFF; // 确保地址为1字节 break; default: // 非法地址,发送错误响应 Rs485_TxPacket[0] = Rs485_Slaveaddr; Rs485_TxPacket[1] = 0x86; // 功能码 + 0x80(错误标志) Rs485_TxPacket[2] = 0x02; // 异常码(地址无效) uint16_t crc = ModbusCRCCalc(Rs485_TxPacket, 3); Rs485_TxPacket[3] = crc >> 8; Rs485_TxPacket[4] = crc & 0xFF; Rs485_SendArray(Rs485_TxPacket, 5); // 发送错误响应 IWDG_Feed(); return; // 提前退出 } Flash_save(); // 计算并发送成功响应 uint16_t crc = ModbusCRCCalc(Rs485_TxPacket, 6); Rs485_TxPacket[6] = crc >> 8; Rs485_TxPacket[7] = crc & 0xFF; Rs485_SendArray(Rs485_TxPacket, 8); // 发送8字节响应 } } } Rs485_RxDataLen = 0; // 清零接收数据包长度 IWDG_Feed(); // 喂狗操作 }帮我找问题,为什么主机发送读485的时候通讯失败,但是写485的时候通讯正常?
03-11
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值