xb/test
0
0
Fork 0
Code Issues Activity

Initial commit from DCSP - 2025/12/30 17:02:59

Browse Source
This commit is contained in:
xb
2025-12-30 17:02:59 +08:00
commit 172fb807f6
37 changed files with 4734 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

583
CanHS.cpp Normal file
View File

@@ -0,0 +1,583 @@
#include "CanHS.h"
#include "SimMsg.h"
#include <iostream>
#include <string>
#include <vector>
#include <cstring>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <errno.h>
#include <sys/select.h>
#include <time.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>
using namespace std;
/* 全局变量定义 */
static CanServiceContext g_can_ctx = {
.fd_can = -1,
.running = false,
.send_id_on_o2 = 0x27,
.stats = {0},
.telemetry_len = 0,
.telemetry_request_pending = false,
.listener_thread = 0
};
/* FastDDS相关全局变量 */
string topic_name_cmd = "Command";
string topic_name_tlm = "Telemetry";
SimMsg* CanHS_part = nullptr;
string servername = "Can_Hardware_Service";
/* 内部函数声明 */
static bool can_device_init(const char* dev_can);
static void* background_listener_thread(void* arg);
static void background_listener(void);
static void process_can_data(uint8_t serve_type, uint8_t *rcv_data, uint8_t data_len);
static int process_multi_frame(uint8_t serve_type, uint8_t frame_seq,
uint8_t *data, uint8_t data_len);
static void reset_multi_frame_state(void);
static bool is_multi_frame_timeout(void);
static void process_telemetry_request(uint8_t *rcv_data, uint8_t data_len);
static void process_telemetry_control(uint8_t *rcv_data, uint8_t data_len);
/* ============================================================
FastDDS相关函数实现
============================================================ */
void CanHSWriteLog(const std::string &msg)
{
std::cout << msg << std::endl;
}
void CanHS_init(uint8_t domainid, std::string appname)
{
std::vector<std::string> parameters;
string expression = "dest = '" + servername + "'";
if (nullptr == CanHS_part)
{
CanHS_part = new SimMsg(domainid, 3000, appname, CanHSWriteLog);
CanHS_part->create_pub(topic_name_cmd);
CanHS_part->create_pub(topic_name_tlm);
CanHS_part->create_sub(topic_name_tlm, telemetry_callback, expression, parameters);
}
}
/* 命令发布函数 */
void CanHS_command_Pub(uint8_t* data, std::string dest, uint16_t len)
{
if (CanHS_part && data && len > 0)
{
CanHS_part->publish(topic_name_cmd, servername, dest, "command", data, len);
}
}
/* 遥测发布函数 */
void CanHS_telemetry_Pub(uint8_t* data, std::string dest, uint16_t len)
{
if (CanHS_part && data && len > 0)
{
CanHS_part->publish(topic_name_tlm, servername, dest, "telemetry", data, len);
}
}
/* 遥测回调函数 */
void telemetry_callback(std::string src, std::string dest, std::string type,
std::string reserve1, std::string reserve2,
std::vector<uint8_t>& data)
{
std::cout << "[INFO] CanHS telemetry received from " << src
<< " to " << dest << ", size=" << data.size() << std::endl;
if (data.empty())
{
std::cout << "[WARN] CanHS: Empty telemetry data received" << std::endl;
return;
}
/* 存储遥测数据 */
can_set_telemetry_data(data.data(), data.size());
/* 检查是否有遥测请求待处理 */
if (g_can_ctx.telemetry_request_pending)
{
std::cout << "[INFO] CanHS: Sending telemetry response (pending request)" << std::endl;
send_telemetry_response(data.data(), data.size());
g_can_ctx.telemetry_request_pending = false;
}
}
/* ============================================================
CAN服务核心实现 - 使用Linux SocketCAN
============================================================ */
/* 初始化与销毁 */
bool can_service_init(const char* can_device) {
/* 初始化互斥锁 */
pthread_mutex_init(&g_can_ctx.telemetry_mutex, NULL);
pthread_mutex_init(&g_can_ctx.multi_frame_mutex, NULL);
memset(&g_can_ctx.multi_frame_state, 0, sizeof(g_can_ctx.multi_frame_state));
/* 初始化CAN设备 */
if (!can_device_init(can_device)) {
std::cerr << "[ERROR] CAN device init failed: " << can_device << std::endl;
return false;
}
return true;
}
void can_service_destroy(void) {
can_service_stop();
pthread_mutex_destroy(&g_can_ctx.telemetry_mutex);
pthread_mutex_destroy(&g_can_ctx.multi_frame_mutex);
if (g_can_ctx.fd_can >= 0) {
close(g_can_ctx.fd_can);
g_can_ctx.fd_can = -1;
}
/* 清理FastDDS资源 */
if (CanHS_part) {
delete CanHS_part;
CanHS_part = nullptr;
}
std::cout << "[INFO] CAN Service destroyed" << std::endl;
}
/* 运行控制 */
void can_service_start(void) {
if (g_can_ctx.running) {
std::cout << "[WARN] CAN Service already running" << std::endl;
return;
}
g_can_ctx.running = true;
/* 启动后台监听线程 */
if (pthread_create(&g_can_ctx.listener_thread, NULL,
background_listener_thread, NULL) != 0) {
std::cerr << "[ERROR] Failed to create background listener thread" << std::endl;
g_can_ctx.running = false;
return;
}
std::cout << "[INFO] CAN Service started" << std::endl;
}
void can_service_stop(void) {
g_can_ctx.running = false;
if (g_can_ctx.listener_thread) {
pthread_join(g_can_ctx.listener_thread, NULL);
g_can_ctx.listener_thread = 0;
}
std::cout << "[INFO] CAN Service stopped" << std::endl;
}
/* 统计数据获取 */
const CanStats* can_service_get_stats(void) {
return &g_can_ctx.stats;
}
/* CAN设备初始化 - Linux SocketCAN版本 */
static bool can_device_init(const char* dev_can) {
int s;
struct sockaddr_can addr;
struct ifreq ifr;
/* 创建SocketCAN socket */
s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (s < 0) {
perror("socket");
return false;
}
strcpy(ifr.ifr_name, dev_can);
if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
perror("ioctl SIOCGIFINDEX");
close(s);
return false;
}
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
perror("bind");
close(s);
return false;
}
g_can_ctx.fd_can = s;
std::cout << "[OK] CAN SocketCAN initialized: " << dev_can << std::endl;
g_can_ctx.stats.can_reset++;
return true;
}
/* CAN单帧发送 */
int can_send_single(uint8_t serve_type, uint8_t *data, uint8_t data_len) {
if (data_len > 8 || g_can_ctx.fd_can < 0) {
return -1;
}
struct can_frame frame;
memset(&frame, 0, sizeof(struct can_frame));
frame.can_id = g_can_ctx.send_id_on_o2 << 5 | serve_type << 2 | O2_SINGLE_FRAME;
frame.can_dlc = data_len;
if (data && data_len > 0) {
memcpy(frame.data, data, data_len);
}
ssize_t ret = write(g_can_ctx.fd_can, &frame, sizeof(struct can_frame));
if (ret != sizeof(struct can_frame)) {
std::cerr << "[ERROR] CAN send failed: " << strerror(errno) << std::endl;
return -1;
}
if (serve_type == O2_NORMAL_TELEMETRY) {
g_can_ctx.stats.send_single_frame++;
}
std::cout << "[INFO] CAN single frame sent: type=0x" << std::hex << (int)serve_type
<< ", len=" << std::dec << (int)data_len << std::endl;
return 0;
}
/* CAN多帧发送 */
int can_send_multi(uint8_t serve_type, uint8_t *data, uint16_t data_len) {
if (data_len <= 8 || g_can_ctx.fd_can < 0) {
return -1;
}
struct can_frame frame;
uint16_t remain_len = data_len;
uint16_t frame_index = 0;
/* 发送首帧 */
memset(&frame, 0, sizeof(struct can_frame));
frame.can_id = g_can_ctx.send_id_on_o2 << 5 | serve_type << 2 | O2_START_FRAME;
frame.can_dlc = 8;
frame.data[0] = (uint8_t)frame_index;
uint16_t copy_len = (remain_len > 7) ? 7 : remain_len;
memcpy(&frame.data[1], data, copy_len);
if (write(g_can_ctx.fd_can, &frame, sizeof(struct can_frame)) != sizeof(struct can_frame)) {
std::cerr << "[ERROR] Failed to send start frame" << std::endl;
return -1;
}
remain_len -= copy_len;
frame_index++;
/* 发送中间帧 */
while (remain_len > 7) {
memset(&frame, 0, sizeof(struct can_frame));
frame.can_id = g_can_ctx.send_id_on_o2 << 5 | serve_type << 2 | O2_MID_FRAME;
frame.can_dlc = 8;
frame.data[0] = (uint8_t)frame_index;
memcpy(&frame.data[1], data + (frame_index * 7), 7);
if (write(g_can_ctx.fd_can, &frame, sizeof(struct can_frame)) != sizeof(struct can_frame)) {
std::cerr << "[ERROR] Failed to send middle frame " << frame_index << std::endl;
return -1;
}
remain_len -= 7;
frame_index++;
}
/* 发送末帧 */
memset(&frame, 0, sizeof(struct can_frame));
frame.can_id = g_can_ctx.send_id_on_o2 << 5 | serve_type << 2 | O2_END_FRAME;
frame.can_dlc = remain_len + 1;
frame.data[0] = (uint8_t)frame_index;
if (remain_len > 0) {
memcpy(&frame.data[1], data + (frame_index * 7), remain_len);
}
if (write(g_can_ctx.fd_can, &frame, sizeof(struct can_frame)) != sizeof(struct can_frame)) {
std::cerr << "[ERROR] Failed to send end frame" << std::endl;
return -1;
}
if (serve_type == O2_NORMAL_TELEMETRY) {
g_can_ctx.stats.send_multi_frame++;
}
std::cout << "[INFO] Multi-frame sent: " << data_len << " bytes in "
<< (frame_index + 1) << " frames, type=0x" << std::hex << (int)serve_type << std::dec << std::endl;
return 0;
}
/* 遥测数据管理 */
void can_set_telemetry_data(const uint8_t *data, size_t data_len) {
pthread_mutex_lock(&g_can_ctx.telemetry_mutex);
size_t copy_len = (data_len < sizeof(g_can_ctx.telemetry_buffer)) ?
data_len : sizeof(g_can_ctx.telemetry_buffer);
memcpy(g_can_ctx.telemetry_buffer, data, copy_len);
g_can_ctx.telemetry_len = copy_len;
std::cout << "[INFO] Telemetry data updated, size=" << copy_len << std::endl;
pthread_mutex_unlock(&g_can_ctx.telemetry_mutex);
}
bool can_get_telemetry_data(uint8_t *buffer, size_t buffer_size, size_t *data_len) {
pthread_mutex_lock(&g_can_ctx.telemetry_mutex);
if (g_can_ctx.telemetry_len == 0) {
pthread_mutex_unlock(&g_can_ctx.telemetry_mutex);
return false;
}
size_t copy_len = (g_can_ctx.telemetry_len < buffer_size) ?
g_can_ctx.telemetry_len : buffer_size;
memcpy(buffer, g_can_ctx.telemetry_buffer, copy_len);
if (data_len) *data_len = copy_len;
pthread_mutex_unlock(&g_can_ctx.telemetry_mutex);
return true;
}
/* 遥测响应发送 */
void send_telemetry_response(const uint8_t *data, size_t data_len) {
uint8_t send_data[1024];
send_data[0] = 0x02; // 遥测响应标识
size_t copy_len = (data_len < sizeof(send_data) - 1) ? data_len : sizeof(send_data) - 1;
memcpy(&send_data[1], data, copy_len);
uint16_t send_data_len = copy_len + 1;
if (send_data_len <= 8) {
can_send_single(O2_NORMAL_TELEMETRY, send_data, (uint8_t)send_data_len);
} else {
can_send_multi(O2_NORMAL_TELEMETRY, send_data, send_data_len);
}
std::cout << "[INFO] Telemetry response sent, data_len=" << copy_len
<< ", total_len=" << send_data_len << std::endl;
}
/* 数据处理函数 */
static void process_telemetry_request(uint8_t *rcv_data, uint8_t data_len) {
std::cout << "[INFO] Processing telemetry request, data=0x" << std::hex << (int)rcv_data[0]
<< ", len=" << std::dec << (int)data_len << std::endl;
if (rcv_data[0] == 0x01 && data_len == 1) {
g_can_ctx.stats.rec_normal_telemetry++;
/* 检查是否有遥测数据 */
uint8_t telemetry_buffer[1024];
size_t telemetry_len = 0;
if (can_get_telemetry_data(telemetry_buffer, sizeof(telemetry_buffer), &telemetry_len)) {
/* 有遥测数据，发送响应 */
send_telemetry_response(telemetry_buffer, telemetry_len);
} else {
/* 无遥测数据，标记待处理 */
g_can_ctx.telemetry_request_pending = true;
std::cout << "[WARN] No telemetry data available, request pending" << std::endl;
}
}
}
static void process_telemetry_control(uint8_t *rcv_data, uint8_t data_len) {
std::cout << "[INFO] Processing telemetry control, len=" << (int)data_len << std::endl;
if (data_len >= 4) { // 最小有效数据长度
g_can_ctx.stats.rec_telemetry_control++;
/* 通过FastDDS转发到通信服务 */
string dest = "Com_Service";
CanHS_command_Pub(rcv_data, dest, data_len);
std::cout << "[INFO] Telemetry control forwarded via FastDDS" << std::endl;
}
}
/* 主数据处理函数 */
static void process_can_data(uint8_t serve_type, uint8_t *rcv_data, uint8_t data_len) {
switch (serve_type) {
case O2_NORMAL_TELEMETRY:
process_telemetry_request(rcv_data, data_len);
break;
case O2_TELEMETRY_CONTROL:
process_telemetry_control(rcv_data, data_len);
break;
default:
std::cout << "[WARN] Unknown service type: 0x" << std::hex << (int)serve_type << std::dec << std::endl;
break;
}
}
/* 多帧重组 */
static bool is_multi_frame_timeout(void) {
struct timeval current_time;
gettimeofday(&current_time, NULL);
long elapsed_ms = (current_time.tv_sec - g_can_ctx.multi_frame_state.start_time.tv_sec) * 1000 +
(current_time.tv_usec - g_can_ctx.multi_frame_state.start_time.tv_usec) / 1000;
return (elapsed_ms > 10000); // 10秒超时
}
static void reset_multi_frame_state(void) {
pthread_mutex_lock(&g_can_ctx.multi_frame_mutex);
g_can_ctx.multi_frame_state.active = 0;
g_can_ctx.multi_frame_state.length = 0;
g_can_ctx.multi_frame_state.expected_frame_index = 0;
pthread_mutex_unlock(&g_can_ctx.multi_frame_mutex);
}
static int process_multi_frame(uint8_t serve_type, uint8_t frame_seq,
uint8_t *data, uint8_t data_len) {
pthread_mutex_lock(&g_can_ctx.multi_frame_mutex);
/* 检查超时 */
if (g_can_ctx.multi_frame_state.active && is_multi_frame_timeout()) {
std::cout << "[WARN] Multi-frame reassembly timeout" << std::endl;
reset_multi_frame_state();
pthread_mutex_unlock(&g_can_ctx.multi_frame_mutex);
return -1;
}
/* 如果是首帧 */
if (frame_seq == O2_START_FRAME) {
g_can_ctx.multi_frame_state.active = 1;
g_can_ctx.multi_frame_state.serve_type = serve_type;
g_can_ctx.multi_frame_state.expected_frame_index = 1;
g_can_ctx.multi_frame_state.length = 0;
gettimeofday(&g_can_ctx.multi_frame_state.start_time, NULL);
if (data_len > 1) {
memcpy(g_can_ctx.multi_frame_state.buffer, &data[1], data_len - 1);
g_can_ctx.multi_frame_state.length = data_len - 1;
}
pthread_mutex_unlock(&g_can_ctx.multi_frame_mutex);
return 0;
}
/* 如果是中间帧或末帧 */
if (g_can_ctx.multi_frame_state.active && serve_type == g_can_ctx.multi_frame_state.serve_type) {
if (data[0] == g_can_ctx.multi_frame_state.expected_frame_index) {
if (data_len > 1) {
memcpy(&g_can_ctx.multi_frame_state.buffer[g_can_ctx.multi_frame_state.length],
&data[1], data_len - 1);
g_can_ctx.multi_frame_state.length += data_len - 1;
}
g_can_ctx.multi_frame_state.expected_frame_index++;
if (frame_seq == O2_END_FRAME) {
g_can_ctx.multi_frame_state.active = 0;
uint16_t final_length = g_can_ctx.multi_frame_state.length;
pthread_mutex_unlock(&g_can_ctx.multi_frame_mutex);
return final_length; // 返回完整数据长度
}
pthread_mutex_unlock(&g_can_ctx.multi_frame_mutex);
return 0; // 需要继续接收
} else {
std::cout << "[WARN] Multi-frame sequence mismatch: expected="
<< (int)g_can_ctx.multi_frame_state.expected_frame_index
<< ", received=" << (int)data[0] << std::endl;
reset_multi_frame_state();
pthread_mutex_unlock(&g_can_ctx.multi_frame_mutex);
return -1;
}
}
pthread_mutex_unlock(&g_can_ctx.multi_frame_mutex);
return -1;
}
/* 后台监听线程 */
static void* background_listener_thread(void* arg) {
(void)arg;
background_listener();
return NULL;
}
static void background_listener(void) {
std::cout << "[INFO] CAN background listener started" << std::endl;
while (g_can_ctx.running) {
if (g_can_ctx.fd_can < 0) {
sleep(1);
continue;
}
fd_set readfds;
struct timeval timeout = {0, 100000}; // 100ms
FD_ZERO(&readfds);
FD_SET(g_can_ctx.fd_can, &readfds);
int ret = select(g_can_ctx.fd_can + 1, &readfds, NULL, NULL, &timeout);
if (ret > 0 && FD_ISSET(g_can_ctx.fd_can, &readfds)) {
struct can_frame recv_frame;
ssize_t bytes = read(g_can_ctx.fd_can, &recv_frame, sizeof(struct can_frame));
if (bytes == sizeof(struct can_frame)) {
uint8_t serve_type = (recv_frame.can_id >> 2) & 0x07;
uint8_t frame_seq = recv_frame.can_id & 0x03;
std::cout << "[INFO] CAN received: type=0x" << std::hex << (int)serve_type
<< ", seq=" << std::dec << (int)frame_seq
<< ", len=" << (int)recv_frame.can_dlc << std::endl;
/* 处理多帧数据 */
if (frame_seq != O2_SINGLE_FRAME) {
int result = process_multi_frame(serve_type, frame_seq,
recv_frame.data, recv_frame.can_dlc);
if (result > 0) {
std::cout << "[SUCCESS] Multi-frame reassembly completed: type=0x"
<< std::hex << (int)serve_type << std::dec
<< ", bytes=" << result << std::endl;
process_can_data(serve_type, g_can_ctx.multi_frame_state.buffer, result);
} else if (result == 0) {
std::cout << "[INFO] Waiting for more multi-frame data, type=0x"
<< std::hex << (int)serve_type << std::dec << std::endl;
} else {
std::cout << "[ERROR] Multi-frame reassembly failed: type=0x"
<< std::hex << (int)serve_type << std::dec << std::endl;
}
} else {
process_can_data(serve_type, recv_frame.data, recv_frame.can_dlc);
}
}
}
/* 检查多帧超时 */
pthread_mutex_lock(&g_can_ctx.multi_frame_mutex);
if (g_can_ctx.multi_frame_state.active && is_multi_frame_timeout()) {
std::cout << "[WARN] Multi-frame reassembly timeout" << std::endl;
reset_multi_frame_state();
}
pthread_mutex_unlock(&g_can_ctx.multi_frame_mutex);
usleep(10000); // 10ms
}
}