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/can_driver.h"
#include "mc/controlcan.h"
#include <cstring> #include <cstring>
#include <stdexcept>
#include <iostream>
#include <thread>
#include <fcntl.h> #include <fcntl.h>
#include <unistd.h> #include <iostream>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can.h> #include <linux/can.h>
#include <linux/can/raw.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 deviceType = VCI_USBCAN2; // 或 VCI_USBCAN_2E_U 等
DWORD deviceIndex = 0; // 通常为 0（第一个设备） DWORD deviceIndex = 0; // 通常为 0（第一个设备）
@ -17,133 +20,140 @@ DWORD channelIndex = 0; // 通道号：0 表示 CAN1，1 表示 CAN2
CANDriver::CANDriver() = default; CANDriver::CANDriver() = default;
CANDriver::~CANDriver() CANDriver::~CANDriver() { close(); }
{
close();
}
bool CANDriver::open(const std::string &interface) bool CANDriver::open(const std::string &interface)
{ {
if (running) if (running)
return false; return false;
// 参数可解析 interface 设定 channelIndex，例如 interface="can0" => channelIndex=0 // 参数可解析 interface 设定 channelIndex，例如 interface="can0" =>
deviceType = VCI_USBCAN2; // channelIndex=0
deviceIndex = 0; deviceType = VCI_USBCAN2;
channelIndex = (interface == "can1") ? 1 : 0; deviceIndex = 0;
channelIndex = (interface == "can1") ? 1 : 0;
if (VCI_OpenDevice(deviceType, deviceIndex, 0) != STATUS_OK) if (VCI_OpenDevice(deviceType, deviceIndex, 0) != STATUS_OK)
{ {
std::cerr << "VCI_OpenDevice failed" << std::endl; std::cerr << "VCI_OpenDevice failed" << std::endl;
return false; return false;
} }
VCI_INIT_CONFIG config{}; VCI_INIT_CONFIG config{};
config.AccCode = 0; config.AccCode = 0;
config.AccMask = 0xFFFFFFFF; config.AccMask = 0xFFFFFFFF;
config.Filter = 0; // 接收所有 config.Filter = 0; // 接收所有
config.Timing0 = 0x00; // 500Kbps 示例（需查文档确认 Timing） config.Timing0 = 0x00; // 500Kbps 示例（需查文档确认 Timing）
config.Timing1 = 0x1C; config.Timing1 = 0x1C;
config.Mode = 0; // 正常模式 config.Mode = 0; // 正常模式
if (VCI_InitCAN(deviceType, deviceIndex, channelIndex, &config) != STATUS_OK) if (VCI_InitCAN(deviceType, deviceIndex, channelIndex, &config) !=
{ STATUS_OK)
std::cerr << "VCI_InitCAN failed" << std::endl; {
VCI_CloseDevice(deviceType, deviceIndex); std::cerr << "VCI_InitCAN failed" << std::endl;
return false; VCI_CloseDevice(deviceType, deviceIndex);
} return false;
}
if (VCI_StartCAN(deviceType, deviceIndex, channelIndex) != STATUS_OK) if (VCI_StartCAN(deviceType, deviceIndex, channelIndex) != STATUS_OK)
{ {
std::cerr << "VCI_StartCAN failed" << std::endl; std::cerr << "VCI_StartCAN failed" << std::endl;
VCI_CloseDevice(deviceType, deviceIndex); VCI_CloseDevice(deviceType, deviceIndex);
return false; return false;
} }
if (VCI_ClearBuffer(deviceType, deviceIndex, channelIndex) != 1) if (VCI_ClearBuffer(deviceType, deviceIndex, channelIndex) != 1)
{ {
std::cerr << "VCI_ClearBuffer failed" << std::endl; std::cerr << "VCI_ClearBuffer failed" << std::endl;
VCI_CloseDevice(deviceType, deviceIndex); VCI_CloseDevice(deviceType, deviceIndex);
return false; return false;
} }
running = true; running = true;
receiveThread = std::thread(&CANDriver::receiveThreadFunc, this); receiveThread = std::thread(&CANDriver::receiveThreadFunc, this);
return true; return true;
} }
void CANDriver::close() void CANDriver::close()
{ {
if (!running) if (!running)
return; return;
running = false; running = false;
if (receiveThread.joinable()) if (receiveThread.joinable())
receiveThread.join(); receiveThread.join();
// 先清空缓冲区，防止缓冲区残留数据导致重复接收 // 先清空缓冲区，防止缓冲区残留数据导致重复接收
VCI_ClearBuffer(deviceType, deviceIndex, channelIndex); VCI_ClearBuffer(deviceType, deviceIndex, channelIndex);
VCI_ResetCAN(deviceType, deviceIndex, channelIndex); VCI_ResetCAN(deviceType, deviceIndex, channelIndex);
VCI_CloseDevice(deviceType, deviceIndex); VCI_CloseDevice(deviceType, deviceIndex);
} }
bool CANDriver::sendFrame(const CANFrame &frame) bool CANDriver::sendFrame(const CANFrame &frame)
{ {
if (!running) if (!running)
return false; return false;
VCI_CAN_OBJ obj{}; VCI_CAN_OBJ obj{};
obj.ID = frame.id; obj.ID = frame.id;
obj.RemoteFlag = frame.rtr ? 1 : 0; obj.RemoteFlag = frame.rtr ? 1 : 0;
obj.ExternFlag = frame.ext ? 1 : 0; obj.ExternFlag = frame.ext ? 1 : 0;
obj.SendType = 0; // 普通发送 obj.SendType = 0; // 普通发送
obj.DataLen = frame.dlc; obj.DataLen = frame.dlc;
std::memcpy(obj.Data, frame.data, frame.dlc); std::memcpy(obj.Data, frame.data, frame.dlc);
if (VCI_Transmit(deviceType, deviceIndex, channelIndex, &obj, 1) != 1) 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; std::cerr << "VCI_Transmit failed" << std::endl;
} return false;
}
return true; return true;
} }
void CANDriver::setReceiveCallback(ReceiveCallback callback, void *userData) void CANDriver::setReceiveCallback(ReceiveCallback callback, void *userData)
{ {
this->callback = callback; this->callback = callback;
this->userData = userData; this->userData = userData;
} }
void CANDriver::receiveThreadFunc() void CANDriver::receiveThreadFunc()
{ {
while (running) constexpr int maxFrames = 2500;
VCI_CAN_OBJ rec[maxFrames];
while (running)
{
ULONG len = 0;
{ {
VCI_CAN_OBJ rec[100]; std::lock_guard<std::mutex> lock(canMutex); // 加锁
ULONG len = VCI_Receive(deviceType, deviceIndex, channelIndex, rec, 100, 10); 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);
}
} }
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_driver.h"
#include "mc/can_struct.h" #include "mc/can_struct.h"
#include "mc/get_config.h" #include "mc/get_config.h"
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/can_frame.hpp" #include "sweeper_interfaces/msg/can_frame.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp" #include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include <cstdio>
#include <iomanip>
#include <iostream>
namespace sweeperMsg = sweeper_interfaces::msg; namespace sweeperMsg = sweeper_interfaces::msg;
@ -47,7 +47,8 @@ struct ControlCache
return false; return false;
auto now = std::chrono::steady_clock::now(); 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超时 if (duration.count() > 100) // ms超时
{ {
@ -108,12 +109,14 @@ void receiveHandler(const CANFrame &frame, void *userData)
if (frame.id > 0x7FF) if (frame.id > 0x7FF)
ss << std::hex << std::uppercase << frame.id; ss << std::hex << std::uppercase << frame.id;
else 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: "; ss << " Data: ";
for (int i = 0; i < frame.dlc; ++i) 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()); RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str());
@ -164,33 +167,34 @@ void VCUWakeUp()
void setupTimers(rclcpp::Node::SharedPtr node) void setupTimers(rclcpp::Node::SharedPtr node)
{ {
// VCU 唤醒帧发送定时器，1Hz // VCU 唤醒帧发送定时器，1Hz
static rclcpp::TimerBase::SharedPtr timer_wakeup = node->create_wall_timer( static rclcpp::TimerBase::SharedPtr timer_wakeup =
std::chrono::seconds(1), [node]() node->create_wall_timer(std::chrono::seconds(1), [node]()
{ {
if (!vcu_awake.load()) if (!vcu_awake.load()) {
{ RCLCPP_INFO(node->get_logger(),
RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame..."); "[TIMER][VCU] Not awake, sending wake-up frame...");
VCUWakeUp(); VCUWakeUp();
} }); } });
// vcu报文 watchdog 检查，200ms // vcu报文 watchdog 检查，200ms
static rclcpp::TimerBase::SharedPtr timer_estop_watchdog = node->create_wall_timer( static rclcpp::TimerBase::SharedPtr timer_estop_watchdog =
std::chrono::milliseconds(200), [node]() node->create_wall_timer(std::chrono::milliseconds(200), [node]()
{ {
auto now = node->now(); auto now = node->now();
auto elapsed = now - last_vcu_msg_time; auto elapsed = now - last_vcu_msg_time;
if (vcu_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5)) 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."); RCLCPP_WARN(node->get_logger(),
"[TIMER][VCU] message timeout, resetting wake-up state.");
vcu_msg_received.store(false); vcu_msg_received.store(false);
vcu_awake.store(false); vcu_awake.store(false);
} }); } });
// MCU控制，50Hz // MCU控制，50Hz
static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer( static rclcpp::TimerBase::SharedPtr timer_mcu =
std::chrono::milliseconds(20), []() node->create_wall_timer(std::chrono::milliseconds(20), []()
{ {
sweeperMsg::McCtrl msg = get_safe_control(); sweeperMsg::McCtrl msg = get_safe_control();
mcu_cmd.setEnabled(true); mcu_cmd.setEnabled(true);
@ -201,9 +205,9 @@ void setupTimers(rclcpp::Node::SharedPtr node)
canctl.sendFrame(mcu_cmd.toFrame()); }); canctl.sendFrame(mcu_cmd.toFrame()); });
// EPS 控制，20Hz // EPS 控制，20Hz
static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer( static rclcpp::TimerBase::SharedPtr timer_eps =
std::chrono::milliseconds(50), []() node->create_wall_timer(std::chrono::milliseconds(50), []()
{ {
sweeperMsg::McCtrl msg = get_safe_control(); sweeperMsg::McCtrl msg = get_safe_control();
eps_cmd.setCenterCmd(0); eps_cmd.setCenterCmd(0);
@ -214,9 +218,9 @@ void setupTimers(rclcpp::Node::SharedPtr node)
canctl.sendFrame(eps_cmd.toFrame()); }); canctl.sendFrame(eps_cmd.toFrame()); });
// VCU 控制，10Hz // VCU 控制，10Hz
static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer( static rclcpp::TimerBase::SharedPtr timer_vcu =
std::chrono::milliseconds(100), []() node->create_wall_timer(std::chrono::milliseconds(100), []()
{ {
sweeperMsg::McCtrl msg = get_safe_control(); sweeperMsg::McCtrl msg = get_safe_control();
vcu1_cmd.setLeftLight(msg.left_light); vcu1_cmd.setLeftLight(msg.left_light);
@ -231,9 +235,8 @@ void setupTimers(rclcpp::Node::SharedPtr node)
vcu2_cmd.setSpray(msg.spray); vcu2_cmd.setSpray(msg.spray);
vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl); vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl);
// canctl.sendFrame(vcu1_cmd.toFrame()); canctl.sendFrame(vcu1_cmd.toFrame());
// canctl.sendFrame(vcu2_cmd.toFrame()); canctl.sendFrame(vcu2_cmd.toFrame()); });
});
} }
int main(int argc, char **argv) int main(int argc, char **argv)
@ -246,7 +249,8 @@ int main(int argc, char **argv)
RCLCPP_INFO(node->get_logger(), "Starting mc package..."); RCLCPP_INFO(node->get_logger(), "Starting mc package...");
// 创建一个 Publisher // 创建一个 Publisher
auto can_publisher = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10); auto can_publisher =
node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
// 创建 Subscriber（收控制指令） // 创建 Subscriber（收控制指令）
auto subscriber = node->create_subscription<sweeperMsg::McCtrl>( auto subscriber = node->create_subscription<sweeperMsg::McCtrl>(
@ -262,7 +266,8 @@ int main(int argc, char **argv)
if (!canctl.open(mc_config.can_dev)) 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; return -1;
} }