VID 透传
This commit is contained in:
parent
3391fa4d60
commit
a5bd401f9c
108
src/base/mc/VID透传.md
Normal file
108
src/base/mc/VID透传.md
Normal file
@ -0,0 +1,108 @@
|
||||
# CAN 协议定义 — VID 透传报文
|
||||
|
||||
## 1 报文基本信息
|
||||
|
||||
| 项目 | 内容 |
|
||||
|-----|-----|
|
||||
| 报文名称 | VID_Transmit |
|
||||
| 报文功能 | 透传设备 VID |
|
||||
| CAN ID | 0x180 |
|
||||
| 帧类型 | Standard Frame |
|
||||
| DLC | 8 |
|
||||
| 发送方式 | 按需发送 |
|
||||
| 发送节点 | MCU |
|
||||
| 接收节点 | VCU / 上位机 |
|
||||
|
||||
---
|
||||
|
||||
## 2 数据格式
|
||||
|
||||
| Byte | 信号名称 | 类型 | 说明 |
|
||||
|-----|----------|------|------|
|
||||
| Byte0 | VID_CHAR1 | ASCII | VID 第1个字符 |
|
||||
| Byte1 | VID_CHAR2 | ASCII | VID 第2个字符 |
|
||||
| Byte2 | VID_CHAR3 | ASCII | VID 第3个字符 |
|
||||
| Byte3 | VID_CHAR4 | ASCII | VID 第4个字符 |
|
||||
| Byte4 | VID_CHAR5 | ASCII | VID 第5个字符 |
|
||||
| Byte5 | VID_CHAR6 | ASCII | VID 第6个字符 |
|
||||
| Byte6 | VID_CHAR7 | ASCII | VID 第7个字符 |
|
||||
| Byte7 | XOR_CHECK | uint8 | Byte0~Byte6 异或校验 |
|
||||
|
||||
---
|
||||
|
||||
## 3 校验规则
|
||||
|
||||
XOR 校验计算方式：
|
||||
|
||||
`CHECK = Byte0 ^ Byte1 ^ Byte2 ^ Byte3 ^ Byte4 ^ Byte5 ^ Byte6`
|
||||
|
||||
校验值存放在：
|
||||
|
||||
`Byte7`
|
||||
|
||||
接收端校验规则：
|
||||
|
||||
`(Byte0 ^ Byte1 ^ Byte2 ^ Byte3 ^ Byte4 ^ Byte5 ^ Byte6) == Byte7`
|
||||
|
||||
成立则数据有效。
|
||||
|
||||
---
|
||||
|
||||
## 4 数据编码说明
|
||||
|
||||
VID 使用 **7 位 ASCII 字符**编码。
|
||||
|
||||
常见字符范围：
|
||||
|
||||
| 字符 | ASCII HEX |
|
||||
|-----|-----------|
|
||||
| 0 ~ 9 | 0x30 ~ 0x39 |
|
||||
| A ~ Z | 0x41 ~ 0x5A |
|
||||
|
||||
---
|
||||
|
||||
## 5 报文示例
|
||||
|
||||
示例 VID：
|
||||
|
||||
`V060003`
|
||||
|
||||
ASCII 编码：
|
||||
|
||||
`56 30 36 30 30 30 33`
|
||||
|
||||
计算 XOR：
|
||||
|
||||
`56 ^ 30 ^ 36 ^ 30 ^ 30 ^ 30 ^ 33 = 03`
|
||||
|
||||
最终 CAN 报文：
|
||||
|
||||
| ID | DLC | DATA |
|
||||
|----|-----|------|
|
||||
| 0x180 | 8 | 56 30 36 30 30 30 33 03 |
|
||||
|
||||
---
|
||||
|
||||
## 6 接收端解析流程
|
||||
|
||||
1. 接收 CAN 报文
|
||||
2. 计算 Byte0~Byte6 XOR
|
||||
3. 与 Byte7 比较
|
||||
4. 校验通过则解析 ASCII 字符串
|
||||
|
||||
字符串提取：
|
||||
|
||||
`VID = Byte0~Byte6`
|
||||
|
||||
示例：
|
||||
|
||||
`V060003`
|
||||
|
||||
---
|
||||
|
||||
## 7 约束说明
|
||||
|
||||
- VID 固定 **7 字符**
|
||||
- 不包含字符串结束符 `\0`
|
||||
- 必须为 **ASCII 可打印字符**
|
||||
- CAN DLC 固定为 **8**
|
||||
@ -1,6 +1,10 @@
|
||||
#pragma once
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "mc/can_driver.h"
|
||||
#include <functional>
|
||||
|
||||
// 注册所有定时器任务
|
||||
void setupTimers(rclcpp::Node::SharedPtr node, CANDriver &canctl);
|
||||
|
||||
// 注册 VID 发送定时器任务
|
||||
void setupVidTimer(rclcpp::Node::SharedPtr node, std::function<void()> callback);
|
||||
|
||||
114
src/base/mc/include/mc/vid_transmit.hpp
Normal file
114
src/base/mc/include/mc/vid_transmit.hpp
Normal file
@ -0,0 +1,114 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
|
||||
#include "logger/logger.h"
|
||||
#include "mc/can_driver.h"
|
||||
|
||||
namespace vid_transmit
|
||||
{
|
||||
|
||||
constexpr uint32_t VID_CAN_ID = 0x180;
|
||||
constexpr uint8_t VID_DLC = 8;
|
||||
constexpr size_t VID_LENGTH = 7;
|
||||
|
||||
/**
|
||||
* @brief 计算 XOR 校验值
|
||||
* @param data 数据指针（至少 7 字节）
|
||||
* @return 校验值
|
||||
*/
|
||||
inline uint8_t calculate_xor_check(const uint8_t* data)
|
||||
{
|
||||
uint8_t check = 0;
|
||||
for (size_t i = 0; i < VID_LENGTH; ++i)
|
||||
{
|
||||
check ^= data[i];
|
||||
}
|
||||
return check;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 验证 VID 字符串格式
|
||||
* @param vid VID 字符串
|
||||
* @return 是否有效
|
||||
*/
|
||||
inline bool validate_vid(const std::string& vid)
|
||||
{
|
||||
if (vid.length() != VID_LENGTH)
|
||||
{
|
||||
LOG_WARN("VID length invalid: expected %zu, got %zu", VID_LENGTH, vid.length());
|
||||
return false;
|
||||
}
|
||||
|
||||
for (char c : vid)
|
||||
{
|
||||
// 检查是否为 7 位 ASCII 可打印字符
|
||||
if (static_cast<uint8_t>(c) > 0x7F || c < 0x20)
|
||||
{
|
||||
LOG_WARN("VID contains invalid character: 0x%02X", static_cast<uint8_t>(c));
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 构建 VID CAN 帧
|
||||
* @param vid VID 字符串（7个ASCII字符）
|
||||
* @param frame 输出的 CAN 帧
|
||||
* @return 是否成功
|
||||
*/
|
||||
inline bool build_vid_frame(const std::string& vid, CANFrame& frame)
|
||||
{
|
||||
if (!validate_vid(vid))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
frame.id = VID_CAN_ID;
|
||||
frame.dlc = VID_DLC;
|
||||
frame.ext = false;
|
||||
frame.rtr = false;
|
||||
|
||||
// 复制 VID ASCII 字符
|
||||
std::memcpy(frame.data, vid.data(), VID_LENGTH);
|
||||
|
||||
// 计算并填充校验值
|
||||
frame.data[7] = calculate_xor_check(frame.data);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 发送 VID 到 CAN 总线
|
||||
* @param can_driver CAN 驱动对象引用
|
||||
* @param vid VID 字符串
|
||||
* @return 是否成功发送
|
||||
*/
|
||||
inline bool send_vid_to_can(CANDriver& can_driver, const std::string& vid)
|
||||
{
|
||||
CANFrame frame;
|
||||
|
||||
if (!build_vid_frame(vid, frame))
|
||||
{
|
||||
LOG_ERROR("Failed to build VID frame for VID: %s", vid.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!can_driver.sendFrame(frame))
|
||||
{
|
||||
LOG_ERROR("Failed to send VID frame to CAN bus");
|
||||
return false;
|
||||
}
|
||||
|
||||
LOG_INFO("VID sent to CAN bus: %s (CAN ID: 0x%03X, Data: %02X %02X %02X %02X %02X %02X %02X %02X)", vid.c_str(),
|
||||
frame.id, frame.data[0], frame.data[1], frame.data[2], frame.data[3], frame.data[4], frame.data[5],
|
||||
frame.data[6], frame.data[7]);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace vid_transmit
|
||||
@ -6,13 +6,42 @@
|
||||
#include "mc/control_cache.hpp"
|
||||
#include "mc/get_config.h"
|
||||
#include "mc/timer_tasks.hpp"
|
||||
#include "mc/vid_transmit.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sweeper_interfaces/msg/can_frame.hpp"
|
||||
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
|
||||
#include "sweeper_interfaces/msg/vehicle_identity.hpp"
|
||||
|
||||
namespace sweeperMsg = sweeper_interfaces::msg;
|
||||
|
||||
CANDriver canctl;
|
||||
std::string current_vid;
|
||||
bool vid_sent = false;
|
||||
|
||||
void vehicleIdentityCallback(const sweeperMsg::VehicleIdentity::SharedPtr msg)
|
||||
{
|
||||
if (msg->ready && !msg->vid.empty() && msg->vid != current_vid)
|
||||
{
|
||||
LOG_INFO("Received new VID: %s", msg->vid.c_str());
|
||||
current_vid = msg->vid;
|
||||
vid_sent = false; // 新 VID 需要重新发送
|
||||
}
|
||||
}
|
||||
|
||||
void send_vid_task()
|
||||
{
|
||||
if (!vid_sent && !current_vid.empty())
|
||||
{
|
||||
if (vid_transmit::send_vid_to_can(canctl, current_vid))
|
||||
{
|
||||
vid_sent = true; // 成功发送后标记
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG_WARN("Failed to send VID, will retry");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg)
|
||||
{
|
||||
@ -51,6 +80,7 @@ int main(int argc, char** argv)
|
||||
|
||||
auto pub = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
|
||||
auto sub = node->create_subscription<sweeperMsg::McCtrl>("mc_ctrl", 10, mcCtrlCallback);
|
||||
auto vid_sub = node->create_subscription<sweeperMsg::VehicleIdentity>("/vehicle/identity", rclcpp::QoS(1).transient_local().reliable(), vehicleIdentityCallback);
|
||||
|
||||
Config mc_config;
|
||||
load_config(mc_config);
|
||||
@ -68,6 +98,7 @@ int main(int argc, char** argv)
|
||||
canctl.setReceiveCallback(receiveHandler, context.get());
|
||||
|
||||
setupTimers(node, canctl);
|
||||
setupVidTimer(node, send_vid_task);
|
||||
|
||||
rclcpp::on_shutdown([&]() { canctl.close(); });
|
||||
rclcpp::spin(node);
|
||||
|
||||
@ -87,6 +87,16 @@ void bmsTimerTask(CANDriver& canctl)
|
||||
canctl.sendFrame(bms_query_0x101.toFrame());
|
||||
}
|
||||
|
||||
// 注册 VID 发送定时器任务
|
||||
void setupVidTimer(rclcpp::Node::SharedPtr node, std::function<void()> callback)
|
||||
{
|
||||
// 每秒检查一次 VID 是否需要发送
|
||||
static auto timer_vid =
|
||||
node->create_wall_timer(std::chrono::milliseconds(1000), callback);
|
||||
|
||||
LOG_INFO("[TIMER] VID transmit timer setup completed");
|
||||
}
|
||||
|
||||
// 注册所有定时器任务
|
||||
void setupTimers(rclcpp::Node::SharedPtr node, CANDriver& canctl)
|
||||
{
|
||||
|
||||
Loading…
Reference in New Issue
Block a user