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

40
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,17 +20,15 @@ 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;
// 参数可解析 interface 设定 channelIndex，例如 interface="can0" => channelIndex=0
// 参数可解析 interface 设定 channelIndex，例如 interface="can0" =>
// channelIndex=0
deviceType = VCI_USBCAN2;
deviceIndex = 0;
channelIndex = (interface == "can1") ? 1 : 0;
@ -46,7 +47,8 @@ bool CANDriver::open(const std::string &interface)
config.Timing1 = 0x1C;
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);
@ -101,6 +103,7 @@ bool CANDriver::sendFrame(const CANFrame &frame)
obj.DataLen = frame.dlc;
std::memcpy(obj.Data, frame.data, frame.dlc);
std::lock_guard<std::mutex> lock(canMutex); // 加锁
if (VCI_Transmit(deviceType, deviceIndex, channelIndex, &obj, 1) != 1)
{
std::cerr << "VCI_Transmit failed" << std::endl;
@ -118,10 +121,17 @@ void CANDriver::setReceiveCallback(ReceiveCallback callback, void *userData)
void CANDriver::receiveThreadFunc()
{
constexpr int maxFrames = 2500;
VCI_CAN_OBJ rec[maxFrames];
while (running)
{
VCI_CAN_OBJ rec[100];
ULONG len = VCI_Receive(deviceType, deviceIndex, channelIndex, rec, 100, 10);
ULONG len = 0;
{
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));

59
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,32 +167,33 @@ 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]()
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...");
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();
@ -201,8 +205,8 @@ 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();
@ -214,8 +218,8 @@ 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();
@ -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;
}