314 lines
9.5 KiB
C++
314 lines
9.5 KiB
C++
#include "can_ros_package/can_ros_node.h"
|
|
#include <iostream>
|
|
#include <thread>
|
|
#include <chrono>
|
|
|
|
namespace can_ros_package {
|
|
|
|
CanDriver::CanDriver()
|
|
: is_running_(false),
|
|
device_type_(VCI_USBCAN2),
|
|
device_ind_(0),
|
|
can1_connected_(false),
|
|
can2_connected_(false),
|
|
reconnect_interval_ms_(600),
|
|
max_reconnect_attempts_(5),
|
|
new_data_available_(false) {
|
|
// 初始化互斥锁和原子变量
|
|
}
|
|
|
|
CanDriver::~CanDriver() {
|
|
stop();
|
|
}
|
|
|
|
bool CanDriver::init() {
|
|
// 打开设备
|
|
if (VCI_OpenDevice(device_type_, device_ind_, 0) != STATUS_OK) {
|
|
std::cerr << "Failed to open CAN device" << std::endl;
|
|
return false;
|
|
}
|
|
|
|
// 读取设备信息
|
|
if (VCI_ReadBoardInfo(device_type_, device_ind_, &board_info_) != STATUS_OK) {
|
|
std::cerr << "Failed to read board info" << std::endl;
|
|
VCI_CloseDevice(device_type_, device_ind_);
|
|
return false;
|
|
}
|
|
|
|
// 分别初始化两路CAN通道
|
|
bool can1_init = initCanChannel(0);
|
|
bool can2_init = initCanChannel(1);
|
|
|
|
can1_connected_ = can1_init;
|
|
can2_connected_ = can2_init;
|
|
|
|
if (!can1_init && !can2_init) {
|
|
VCI_CloseDevice(device_type_, device_ind_);
|
|
return false;
|
|
}
|
|
|
|
is_running_ = true;
|
|
new_data_available_ = false;
|
|
|
|
// 启动接收线程
|
|
receive_thread_ = std::thread(&CanDriver::receiveLoop, this);
|
|
|
|
return true;
|
|
}
|
|
|
|
// 波特率 Timing0 (BTR0) Timing1 (BTR1) 说明
|
|
// 1 Mbps 0x00 0x14 高速通信
|
|
// 500 Kbps 0x00 0x1C 最常用配置
|
|
// 250 Kbps 0x01 0x1C 中等速度通信
|
|
// 125 Kbps 0x03 0x1C 低速通信，你的原始配置
|
|
// 50 Kbps 0x09 0x1C 长距离或低干扰环境
|
|
// 10 Kbps 0x13 0x22 非常低速，特殊场景
|
|
bool CanDriver::initCanChannel(int channel) {
|
|
VCI_INIT_CONFIG config;
|
|
config.AccCode = 0;
|
|
config.AccMask = 0xFFFFFFFF;
|
|
config.Filter = 1; // 接收所有帧
|
|
|
|
if(channel == 0){
|
|
// 设置1000Kbps波特率
|
|
config.Timing0 = 0x00;
|
|
config.Timing1 = 0x14;
|
|
} else if(channel == 1) {
|
|
// 设置250Kbps波特率
|
|
config.Timing0 = 0x01;
|
|
config.Timing1 = 0x1C;
|
|
}
|
|
|
|
config.Mode = 0; // 正常模式
|
|
|
|
if (VCI_InitCAN(device_type_, device_ind_, channel, &config) != STATUS_OK) {
|
|
std::cerr << "Failed to initialize CAN channel " << channel << std::endl;
|
|
return false;
|
|
}
|
|
|
|
// 启动前短暂延迟
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
|
|
if (VCI_StartCAN(device_type_, device_ind_, channel) != STATUS_OK) {
|
|
std::cerr << "Failed to start CAN channel " << channel << std::endl;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool CanDriver::sendToCan1(VCI_CAN_OBJ& msg) {
|
|
if (!can1_connected_) {
|
|
return false;
|
|
}
|
|
|
|
std::lock_guard<std::mutex> send_lock(send_mutex_);
|
|
std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
|
|
|
|
int result = VCI_Transmit(device_type_, device_ind_, 0, &msg, 1);
|
|
|
|
if (result != STATUS_OK) {
|
|
std::cerr << "Failed to send to CAN1, error code: " << result << std::endl;
|
|
can1_connected_ = false;
|
|
std::thread(&CanDriver::reconnect, this, 0).detach();
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool CanDriver::sendToCan2(VCI_CAN_OBJ& msg) {
|
|
if (!can2_connected_) {
|
|
return false;
|
|
}
|
|
|
|
std::lock_guard<std::mutex> send_lock(send_mutex_);
|
|
std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
|
|
|
|
int result = VCI_Transmit(device_type_, device_ind_, 1, &msg, 1);
|
|
|
|
if (result != STATUS_OK) {
|
|
std::cerr << "Failed to send to CAN2, error code: " << result << std::endl;
|
|
can2_connected_ = false;
|
|
std::thread(&CanDriver::reconnect, this, 1).detach();
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void CanDriver::receiveLoop() {
|
|
VCI_CAN_OBJ rec[3000];
|
|
int reclen = 0;
|
|
|
|
while (is_running_) {
|
|
// 接收CAN1数据
|
|
if (can1_connected_) {
|
|
{
|
|
std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
|
|
reclen = VCI_Receive(device_type_, device_ind_, 0, rec, 3000, 0); // 0ms超时实现非阻塞读取
|
|
}
|
|
|
|
if (reclen < 0) {
|
|
std::cerr << "Error receiving from CAN1, error code: " << reclen << std::endl;
|
|
can1_connected_ = false;
|
|
std::thread(&CanDriver::reconnect, this, 0).detach();
|
|
} else if (reclen > 0) {
|
|
std::lock_guard<std::mutex> receive_lock(receive_mutex_);
|
|
for (int j = 0; j < reclen; j++) {
|
|
can_msgs_.push_back(rec[j]);
|
|
}
|
|
new_data_available_ = true;
|
|
receive_cv_.notify_one();
|
|
}
|
|
}
|
|
|
|
// 接收CAN2数据
|
|
if (can2_connected_) {
|
|
{
|
|
std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
|
|
reclen = VCI_Receive(device_type_, device_ind_, 1, rec, 3000, 0); // 0ms超时实现非阻塞读取
|
|
}
|
|
|
|
if (reclen < 0) {
|
|
std::cerr << "Error receiving from CAN2, error code: " << reclen << std::endl;
|
|
can2_connected_ = false;
|
|
std::thread(&CanDriver::reconnect, this, 1).detach();
|
|
} else if (reclen > 0) {
|
|
std::lock_guard<std::mutex> receive_lock(receive_mutex_);
|
|
for (int j = 0; j < reclen; j++) {
|
|
can_msgs_.push_back(rec[j]);
|
|
}
|
|
new_data_available_ = true;
|
|
receive_cv_.notify_one();
|
|
}
|
|
}
|
|
|
|
// 短暂休眠，避免CPU占用过高
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
}
|
|
}
|
|
|
|
std::vector<VCI_CAN_OBJ> CanDriver::getReceivedMessages() {
|
|
std::unique_lock<std::mutex> lock(receive_mutex_);
|
|
|
|
// 等待新数据或超时
|
|
if (!new_data_available_) {
|
|
receive_cv_.wait_for(lock, std::chrono::milliseconds(10));
|
|
}
|
|
|
|
std::vector<VCI_CAN_OBJ> result = can_msgs_;
|
|
can_msgs_.clear();
|
|
new_data_available_ = false;
|
|
return result;
|
|
}
|
|
|
|
void CanDriver::reconnect(int channel) {
|
|
std::string channel_name = (channel == 0) ? "CAN1" : "CAN2";
|
|
int attempt = 0;
|
|
|
|
while (is_running_ && attempt < max_reconnect_attempts_ &&
|
|
!(channel == 0 ? can1_connected_ : can2_connected_))
|
|
{
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(reconnect_interval_ms_));
|
|
|
|
if (tryReconnectChannel(channel)) {
|
|
ROS_INFO_STREAM("Successfully reconnected to " << channel_name);
|
|
break;
|
|
} else {
|
|
ROS_WARN_STREAM("Reconnect attempt " << (attempt+1)
|
|
<< "/" << max_reconnect_attempts_
|
|
<< " failed for " << channel_name);
|
|
attempt++;
|
|
}
|
|
}
|
|
|
|
if (attempt >= max_reconnect_attempts_) {
|
|
ROS_ERROR_STREAM("Giving up on reconnecting " << channel_name
|
|
<< " after " << max_reconnect_attempts_ << " attempts");
|
|
}
|
|
}
|
|
|
|
bool CanDriver::tryReconnectChannel(int channel) {
|
|
std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
|
|
std::string channel_name = (channel == 0) ? "CAN1" : "CAN2";
|
|
|
|
// 1. 重置通道
|
|
if (is_running_) {
|
|
VCI_ResetCAN(device_type_, device_ind_, channel);
|
|
VCI_ClearBuffer(device_type_, device_ind_, channel);
|
|
}
|
|
|
|
// 2. 重新初始化通道
|
|
VCI_INIT_CONFIG config;
|
|
config.AccCode = 0;
|
|
config.AccMask = 0xFFFFFFFF;
|
|
config.Filter = 1; // 接收所有帧
|
|
|
|
if(channel == 0){
|
|
// 设置1000Kbps波特率
|
|
config.Timing0 = 0x00;
|
|
config.Timing1 = 0x1C;
|
|
} else if(channel == 1) {
|
|
// 设置250Kbps波特率
|
|
config.Timing0 = 0x01;
|
|
config.Timing1 = 0x1C;
|
|
}
|
|
|
|
config.Mode = 0; // 正常模式
|
|
|
|
if (VCI_InitCAN(device_type_, device_ind_, channel, &config) != STATUS_OK) {
|
|
return false;
|
|
}
|
|
|
|
// 启动前短暂延迟
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
|
|
if (VCI_StartCAN(device_type_, device_ind_, channel) != STATUS_OK) {
|
|
return false;
|
|
}
|
|
|
|
// 3. 更新连接状态
|
|
if (channel == 0) {
|
|
can1_connected_ = true;
|
|
} else {
|
|
can2_connected_ = true;
|
|
}
|
|
|
|
// 启动后延迟确保稳定
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
return true;
|
|
}
|
|
|
|
void CanDriver::stop() {
|
|
if (is_running_) {
|
|
is_running_ = false;
|
|
can1_connected_ = false;
|
|
can2_connected_ = false;
|
|
|
|
// 通知接收线程退出
|
|
{
|
|
std::lock_guard<std::mutex> lock(receive_mutex_);
|
|
receive_cv_.notify_all();
|
|
}
|
|
|
|
if (receive_thread_.joinable()) {
|
|
receive_thread_.join();
|
|
}
|
|
|
|
// 复位并关闭两路CAN通道
|
|
VCI_ResetCAN(device_type_, device_ind_, 0);
|
|
VCI_ResetCAN(device_type_, device_ind_, 1);
|
|
VCI_CloseDevice(device_type_, device_ind_);
|
|
}
|
|
}
|
|
|
|
bool CanDriver::isCan1Connected() {
|
|
return can1_connected_;
|
|
}
|
|
|
|
bool CanDriver::isCan2Connected() {
|
|
return can2_connected_;
|
|
}
|
|
|
|
} // namespace can_ros_package
|