cxh/sweeper_e800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Auto commit at 2025-06-13 10:10:04

Browse Source
This commit is contained in:
cxh 2025-06-13 10:10:04 +08:00
parent dd790c3400
commit 86830ed7ee
2 changed files with 149 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

214
src/mc/src/can_driver.cpp
View File

@ -1,15 +1,18 @@
#include "mc/can_driver.h"
#include "mc/controlcan.h"
#include <cstring>
#include <stdexcept>
#include <iostream>
#include <thread>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <iostream>
#include <linux/can.h>
#include <linux/can/raw.h>
#include "mc/controlcan.h"
#include <net/if.h>
#include <stdexcept>
#include <sys/ioctl.h>
#include <thread>
#include <unistd.h>
#include <mutex>
std::mutex canMutex;
DWORD deviceType = VCI_USBCAN2; // 或 VCI_USBCAN_2E_U 等
DWORD deviceIndex = 0; // 通常为 0（第一个设备）
@ -17,133 +20,140 @@ DWORD channelIndex = 0; // 通道号：0 表示 CAN1，1 表示 CAN2
CANDriver::CANDriver() = default;
CANDriver::~CANDriver()
{
close();
}
CANDriver::~CANDriver() { close(); }
bool CANDriver::open(const std::string &interface)
{
if (running)
return false;
if (running)
return false;
// 参数可解析 interface 设定 channelIndex，例如 interface="can0" => channelIndex=0
deviceType = VCI_USBCAN2;
deviceIndex = 0;
channelIndex = (interface == "can1") ? 1 : 0;
// 参数可解析 interface 设定 channelIndex，例如 interface="can0" =>
// channelIndex=0
deviceType = VCI_USBCAN2;
deviceIndex = 0;
channelIndex = (interface == "can1") ? 1 : 0;
if (VCI_OpenDevice(deviceType, deviceIndex, 0) != STATUS_OK)
{
std::cerr << "VCI_OpenDevice failed" << std::endl;
return false;
}
if (VCI_OpenDevice(deviceType, deviceIndex, 0) != STATUS_OK)
{
std::cerr << "VCI_OpenDevice failed" << std::endl;
return false;
}
VCI_INIT_CONFIG config{};
config.AccCode = 0;
config.AccMask = 0xFFFFFFFF;
config.Filter = 0; // 接收所有
config.Timing0 = 0x00; // 500Kbps 示例（需查文档确认 Timing）
config.Timing1 = 0x1C;
config.Mode = 0; // 正常模式
VCI_INIT_CONFIG config{};
config.AccCode = 0;
config.AccMask = 0xFFFFFFFF;
config.Filter = 0; // 接收所有
config.Timing0 = 0x00; // 500Kbps 示例（需查文档确认 Timing）
config.Timing1 = 0x1C;
config.Mode = 0; // 正常模式
if (VCI_InitCAN(deviceType, deviceIndex, channelIndex, &config) != STATUS_OK)
{
std::cerr << "VCI_InitCAN failed" << std::endl;
VCI_CloseDevice(deviceType, deviceIndex);
return false;
}
if (VCI_InitCAN(deviceType, deviceIndex, channelIndex, &config) !=
STATUS_OK)
{
std::cerr << "VCI_InitCAN failed" << std::endl;
VCI_CloseDevice(deviceType, deviceIndex);
return false;
}
if (VCI_StartCAN(deviceType, deviceIndex, channelIndex) != STATUS_OK)
{
std::cerr << "VCI_StartCAN failed" << std::endl;
VCI_CloseDevice(deviceType, deviceIndex);
return false;
}
if (VCI_StartCAN(deviceType, deviceIndex, channelIndex) != STATUS_OK)
{
std::cerr << "VCI_StartCAN failed" << std::endl;
VCI_CloseDevice(deviceType, deviceIndex);
return false;
}
if (VCI_ClearBuffer(deviceType, deviceIndex, channelIndex) != 1)
{
std::cerr << "VCI_ClearBuffer failed" << std::endl;
VCI_CloseDevice(deviceType, deviceIndex);
return false;
}
if (VCI_ClearBuffer(deviceType, deviceIndex, channelIndex) != 1)
{
std::cerr << "VCI_ClearBuffer failed" << std::endl;
VCI_CloseDevice(deviceType, deviceIndex);
return false;
}
running = true;
receiveThread = std::thread(&CANDriver::receiveThreadFunc, this);
return true;
running = true;
receiveThread = std::thread(&CANDriver::receiveThreadFunc, this);
return true;
}
void CANDriver::close()
{
if (!running)
return;
if (!running)
return;
running = false;
if (receiveThread.joinable())
receiveThread.join();
running = false;
if (receiveThread.joinable())
receiveThread.join();
// 先清空缓冲区，防止缓冲区残留数据导致重复接收
VCI_ClearBuffer(deviceType, deviceIndex, channelIndex);
// 先清空缓冲区，防止缓冲区残留数据导致重复接收
VCI_ClearBuffer(deviceType, deviceIndex, channelIndex);
VCI_ResetCAN(deviceType, deviceIndex, channelIndex);
VCI_CloseDevice(deviceType, deviceIndex);
VCI_ResetCAN(deviceType, deviceIndex, channelIndex);
VCI_CloseDevice(deviceType, deviceIndex);
}
bool CANDriver::sendFrame(const CANFrame &frame)
{
if (!running)
return false;
if (!running)
return false;
VCI_CAN_OBJ obj{};
obj.ID = frame.id;
obj.RemoteFlag = frame.rtr ? 1 : 0;
obj.ExternFlag = frame.ext ? 1 : 0;
obj.SendType = 0; // 普通发送
obj.DataLen = frame.dlc;
std::memcpy(obj.Data, frame.data, frame.dlc);
VCI_CAN_OBJ obj{};
obj.ID = frame.id;
obj.RemoteFlag = frame.rtr ? 1 : 0;
obj.ExternFlag = frame.ext ? 1 : 0;
obj.SendType = 0; // 普通发送
obj.DataLen = frame.dlc;
std::memcpy(obj.Data, frame.data, frame.dlc);
if (VCI_Transmit(deviceType, deviceIndex, channelIndex, &obj, 1) != 1)
{
std::cerr << "VCI_Transmit failed" << std::endl;
return false;
}
std::lock_guard<std::mutex> lock(canMutex); // 加锁
if (VCI_Transmit(deviceType, deviceIndex, channelIndex, &obj, 1) != 1)
{
std::cerr << "VCI_Transmit failed" << std::endl;
return false;
}
return true;
return true;
}
void CANDriver::setReceiveCallback(ReceiveCallback callback, void *userData)
{
this->callback = callback;
this->userData = userData;
this->callback = callback;
this->userData = userData;
}
void CANDriver::receiveThreadFunc()
{
while (running)
constexpr int maxFrames = 2500;
VCI_CAN_OBJ rec[maxFrames];
while (running)
{
ULONG len = 0;
{
VCI_CAN_OBJ rec[100];
ULONG len = VCI_Receive(deviceType, deviceIndex, channelIndex, rec, 100, 10);
if (len == 0)
{
std::this_thread::sleep_for(std::chrono::milliseconds(10));
continue;
}
for (ULONG i = 0; i < len && callback; ++i)
{
CANFrame frame;
frame.id = rec[i].ID;
frame.ext = rec[i].ExternFlag != 0;
frame.rtr = rec[i].RemoteFlag != 0;
frame.dlc = rec[i].DataLen;
if (frame.dlc > 8)
{
std::cerr << "Invalid CAN data length: " << (int)frame.dlc << std::endl;
frame.dlc = 8; // 或跳过此帧
}
std::memcpy(frame.data, rec[i].Data, frame.dlc);
callback(frame, userData);
}
std::lock_guard<std::mutex> lock(canMutex); // 加锁
len = VCI_Receive(deviceType, deviceIndex, channelIndex, rec, 100, 10);
}
if (len == 0)
{
std::this_thread::sleep_for(std::chrono::milliseconds(10));
continue;
}
for (ULONG i = 0; i < len && callback; ++i)
{
CANFrame frame;
frame.id = rec[i].ID;
frame.ext = rec[i].ExternFlag != 0;
frame.rtr = rec[i].RemoteFlag != 0;
frame.dlc = rec[i].DataLen;
if (frame.dlc > 8)
{
std::cerr << "Invalid CAN data length: " << (int)frame.dlc << std::endl;
frame.dlc = 8; // 或跳过此帧
}
std::memcpy(frame.data, rec[i].Data, frame.dlc);
callback(frame, userData);
}
}
}

69
src/mc/src/mc.cpp
View File

@ -1,12 +1,12 @@
#include <cstdio>
#include <iostream>
#include <iomanip>
#include "mc/can_driver.h"
#include "mc/can_struct.h"
#include "mc/get_config.h"
#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/can_frame.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include <cstdio>
#include <iomanip>
#include <iostream>
namespace sweeperMsg = sweeper_interfaces::msg;
@ -47,7 +47,8 @@ struct ControlCache
return false;
auto now = std::chrono::steady_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_update_time);
auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(
now - last_update_time);
if (duration.count() > 100) // ms超时
{
@ -108,12 +109,14 @@ void receiveHandler(const CANFrame &frame, void *userData)
if (frame.id > 0x7FF)
ss << std::hex << std::uppercase << frame.id;
else
ss << std::hex << std::uppercase << std::setw(5) << std::setfill(' ') << frame.id;
ss << std::hex << std::uppercase << std::setw(5) << std::setfill(' ')
<< frame.id;
ss << " Data: ";
for (int i = 0; i < frame.dlc; ++i)
{
ss << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " ";
ss << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i]
<< " ";
}
RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str());
@ -164,33 +167,34 @@ void VCUWakeUp()
void setupTimers(rclcpp::Node::SharedPtr node)
{
// VCU 唤醒帧发送定时器，1Hz
static rclcpp::TimerBase::SharedPtr timer_wakeup = node->create_wall_timer(
std::chrono::seconds(1), [node]()
{
if (!vcu_awake.load())
{
RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame...");
static rclcpp::TimerBase::SharedPtr timer_wakeup =
node->create_wall_timer(std::chrono::seconds(1), [node]()
{
if (!vcu_awake.load()) {
RCLCPP_INFO(node->get_logger(),
"[TIMER][VCU] Not awake, sending wake-up frame...");
VCUWakeUp();
} });
// vcu报文 watchdog 检查，200ms
static rclcpp::TimerBase::SharedPtr timer_estop_watchdog = node->create_wall_timer(
std::chrono::milliseconds(200), [node]()
{
static rclcpp::TimerBase::SharedPtr timer_estop_watchdog =
node->create_wall_timer(std::chrono::milliseconds(200), [node]()
{
auto now = node->now();
auto elapsed = now - last_vcu_msg_time;
if (vcu_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5))
{
RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] message timeout, resetting wake-up state.");
if (vcu_msg_received.load() &&
elapsed > rclcpp::Duration::from_seconds(0.5)) {
RCLCPP_WARN(node->get_logger(),
"[TIMER][VCU] message timeout, resetting wake-up state.");
vcu_msg_received.store(false);
vcu_awake.store(false);
} });
// MCU控制，50Hz
static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer(
std::chrono::milliseconds(20), []()
{
static rclcpp::TimerBase::SharedPtr timer_mcu =
node->create_wall_timer(std::chrono::milliseconds(20), []()
{
sweeperMsg::McCtrl msg = get_safe_control();
mcu_cmd.setEnabled(true);
@ -201,9 +205,9 @@ void setupTimers(rclcpp::Node::SharedPtr node)
canctl.sendFrame(mcu_cmd.toFrame()); });
// EPS 控制，20Hz
static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer(
std::chrono::milliseconds(50), []()
{
static rclcpp::TimerBase::SharedPtr timer_eps =
node->create_wall_timer(std::chrono::milliseconds(50), []()
{
sweeperMsg::McCtrl msg = get_safe_control();
eps_cmd.setCenterCmd(0);
@ -214,9 +218,9 @@ void setupTimers(rclcpp::Node::SharedPtr node)
canctl.sendFrame(eps_cmd.toFrame()); });
// VCU 控制，10Hz
static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer(
std::chrono::milliseconds(100), []()
{
static rclcpp::TimerBase::SharedPtr timer_vcu =
node->create_wall_timer(std::chrono::milliseconds(100), []()
{
sweeperMsg::McCtrl msg = get_safe_control();
vcu1_cmd.setLeftLight(msg.left_light);
@ -231,9 +235,8 @@ void setupTimers(rclcpp::Node::SharedPtr node)
vcu2_cmd.setSpray(msg.spray);
vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl);
// canctl.sendFrame(vcu1_cmd.toFrame());
// canctl.sendFrame(vcu2_cmd.toFrame());
});
canctl.sendFrame(vcu1_cmd.toFrame());
canctl.sendFrame(vcu2_cmd.toFrame()); });
}
int main(int argc, char **argv)
@ -246,7 +249,8 @@ int main(int argc, char **argv)
RCLCPP_INFO(node->get_logger(), "Starting mc package...");
// 创建一个 Publisher
auto can_publisher = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
auto can_publisher =
node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
// 创建 Subscriber（收控制指令）
auto subscriber = node->create_subscription<sweeperMsg::McCtrl>(
@ -262,7 +266,8 @@ int main(int argc, char **argv)
if (!canctl.open(mc_config.can_dev))
{
RCLCPP_ERROR(node->get_logger(), "Failed to open CAN interface: %s", mc_config.can_dev.c_str());
RCLCPP_ERROR(node->get_logger(), "Failed to open CAN interface: %s",
mc_config.can_dev.c_str());
return -1;
}