xb/build
0
0
Fork 0
Code Issues Activity
Files
b6fe48ba79328c7f9563d4e2cfc8573222953bde
build/hook-rs422_kk.cpp

737 lines
22 KiB
C++
Raw Normal View History

Initial commit from DCSP - 2026/1/5 17:32:54
2026-01-05 17:32:54 +08:00
/*******************************************************************************
* hook_rs422.cpp
*
* FPGA扩展RS422串口外挂程序
* 功能：监听/dev/pts/25串口，处理数据并执行相应操作
*
* 1. 将产生的虚拟遥测写入/dev/pts/12串口
* 2. 将收到的上行指令写入/dev/pts/12串口
* 3. 将从串口得到的数据通过DDS接口发送出去
*
* 使用方式: ./Com_kk <发送串口> <接收串口>
* 示例: ./Com_kk /dev/pts/12 /dev/pts/25
******************************************************************************/
#include "hook-rs422_kk.h"
#include "SimMsg.h"
#include <iostream>
#include <cstring>
#include <vector>
#include <thread>
#include <mutex>
#include <atomic>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <errno.h>
#include <poll.h>
#include <signal.h>
#include <time.h>
using namespace std;
// 全局变量定义
volatile int keep_running = 1;
uint8_t scomm_enable = 1;
// DDS相关全局变量
SimMsg* simMsg = nullptr;
string pkgname = "";
string topic_name_o2_can = "o2-can";
string topic_name_o1_can = "o1-can";
string topic_name_kk = "comu-kk";
// 互斥锁保护共享数据
std::mutex g_data_mutex;
// 本地参数声明
uint8_t tm_buffer[256] = { 0 };
S_Comm_telemetry_data_t s_tele = { 0 };
uint8_t isReceiveCmder = 0;
uint8_t rec_cmder_fromdds[255] = { 0 };
uint16_t rec_len_fromdds = 0;
string Msgfrom = "O2-A";
//====================================== 串口管理 ======================================//
// Socat虚拟串口结构
typedef struct {
int fd; // 文件描述符
bool enabled; // 使能标志
char device_path[256]; // 串口设备路径
const char* port_name; // 串口名称（用于日志标识）
const char* port_role; // 串口角色
} SocatPort;
// 串口枚举定义
enum SocatPortId {
PORT_TX = 0, // 发送串口 (argv[1])
PORT_RX = 1 // 接收串口 (argv[2])
};
// 串口实例
static SocatPort socat_ports[2] = {
{ -1, false, "", "发送串口", "发送" },
{ -1, false, "", "接收串口", "接收" }
};
// 线程相关
static pthread_t listen_thread;
//====================================== 函数声明 ======================================//
// 日志函数
void WriteLog(const string &msg);
// DDS接口函数
void pkginit(uint8_t domainid, string appname, string pkgname_);
void PkgToO2_AkkDataPub(uint8_t* o2_kk_data, uint16_t len);
void PkgToO_1kkDataPub(uint8_t* o1_kk_data, uint16_t len);
void pkgSubO_1KKData(message_func_t MessageCallback);
void PkgSubO2_AKkData(message_func_t MessageCallback);
// 数据处理函数
void cjb_rs422_kk_receive(string src, string dest, string type, string reserve1,
string reserve2, vector<uint8_t>& data);
void process_tm_buffer(uint8_t* buffer, uint16_t length);
void generate_s_tele(uint8_t* tele);
uint8_t generate_cmder(uint8_t* tele);
// 串口管理函数
int init_socat_serial(SocatPortId port_id, const char* device_path);
int read_from_serial(SocatPortId port_id, uint8_t* buffer, uint16_t buffer_size);
void forward_to_socat(uint8_t* data, uint16_t len, SocatPortId port_id);
void cleanup_socat_serial(SocatPortId port_id);
void cleanup_all_socat_serial();
// 线程函数
void* listen_serial_thread(void* arg);
int start_listen_thread();
void stop_listen_thread();
// 主程序接口
void hook_rs422_kk(const char* tx_port, const char* rx_port);
void set_scomm_enable(uint8_t s_en);
void cleanup_hook_rs422();
// 信号处理
void signal_handler(int signum);
//====================================== 日志函数 ======================================//
void WriteLog(const string &msg) {
time_t now = time(0);
char buf[80];
strftime(buf, sizeof(buf), "%Y-%m-%d %H:%M:%S", localtime(&now));
cout << "[" << buf << "] " << msg << endl;
}
//====================================== FastDDS函数 ======================================//
void pkginit(uint8_t domainid, string appname, string pkgname_) {
if (simMsg == nullptr) {
WriteLog("初始化FastDDS通信...");
simMsg = new SimMsg(domainid, 3000, appname, WriteLog);
simMsg->create_pub(topic_name_o2_can);
simMsg->create_pub(topic_name_o1_can);
simMsg->create_pub(topic_name_kk);
pkgname = pkgname_;
WriteLog("FastDDS初始化完成: domainid=" + to_string(domainid) +
", appname=" + appname + ", pkgname=" + pkgname_);
}
}
void PkgToO2_AkkDataPub(uint8_t* o2_kk_data, uint16_t len) {
if (!simMsg || !scomm_enable) return;
simMsg->publish(topic_name_kk, pkgname, "O2-A", "", o2_kk_data, len);
string hex_str = "发送到O2-A DDS(" + to_string(len) + "字节): ";
for (int i = 0; i < (len < 10 ? len : 10); i++) {
char buf[4];
snprintf(buf, sizeof(buf), "%02X ", o2_kk_data[i]);
hex_str += buf;
}
if (len > 10) hex_str += "...";
WriteLog(hex_str);
}
void PkgToO_1kkDataPub(uint8_t* o1_kk_data, uint16_t len) {
if (!simMsg || !scomm_enable) return;
simMsg->publish(topic_name_kk, pkgname, "O-1", "", o1_kk_data, len);
string hex_str = "发送到O-1 DDS(" + to_string(len) + "字节): ";
for (int i = 0; i < (len < 10 ? len : 10); i++) {
char buf[4];
snprintf(buf, sizeof(buf), "%02X ", o1_kk_data[i]);
hex_str += buf;
}
if (len > 10) hex_str += "...";
WriteLog(hex_str);
}
void pkgSubO_1KKData(message_func_t MessageCallback) {
if (!simMsg) return;
vector<string> parameters;
string expression = "src = 'O-1'";
simMsg->create_sub(topic_name_kk,
static_cast<message_func_t>(MessageCallback),
expression, parameters);
WriteLog("订阅O-1数据成功");
}
void PkgSubO2_AKkData(message_func_t MessageCallback) {
if (!simMsg) return;
vector<string> parameters;
string expression = "src = 'O2-A'";
simMsg->create_sub(topic_name_kk,
static_cast<message_func_t>(MessageCallback),
expression, parameters);
WriteLog("订阅O2-A数据成功");
}
//====================================== DDS接收回调 ======================================//
void cjb_rs422_kk_receive(string src, string dest, string type, string reserve1,
string reserve2, vector<uint8_t>& data) {
if (data.empty()) {
WriteLog("警告：收到空数据");
return;
}
WriteLog("收到来自 " + src + " 的数据，大小: " + to_string(data.size()) + " 字节");
if (dest == "CJ") {
lock_guard<mutex> lock(g_data_mutex);
Msgfrom = src;
uint8_t checksum = 0;
uint16_t frameLen = 0;
// 安全复制数据
uint16_t copy_size = (data.size() < sizeof(rec_cmder_fromdds)) ?
data.size() : sizeof(rec_cmder_fromdds) - 1;
for (size_t kc = 0; kc < copy_size; kc++) {
if (kc == 5 || kc == 6) continue;
if (frameLen < sizeof(rec_cmder_fromdds)) {
rec_cmder_fromdds[frameLen++] = data[kc];
}
}
rec_len_fromdds = frameLen;
// 更新指令ID为遥控指令查询ID 0x4B
if (rec_len_fromdds > 2) {
rec_cmder_fromdds[2] = 0xB4;
}
// 调整长度字段
if (rec_len_fromdds > 4) {
if (rec_cmder_fromdds[4] >= 0x02) {
rec_cmder_fromdds[4] -= 0x02;
} else {
rec_cmder_fromdds[4] = 0;
}
}
// 重新计算校验和
for (int kc = 2; kc < rec_len_fromdds - 1; kc++) {
checksum += rec_cmder_fromdds[kc];
}
if (rec_len_fromdds > 0) {
rec_cmder_fromdds[rec_len_fromdds - 1] = checksum;
}
isReceiveCmder = 1;
WriteLog("已存储空空遥控指令，长度: " + to_string(rec_len_fromdds) + " 字节");
// 打印指令内容
string hex_str = "指令内容: ";
int print_len = (rec_len_fromdds < 20 ? rec_len_fromdds : 20);
for (int i = 0; i < print_len; i++) {
char buf[4];
snprintf(buf, sizeof(buf), "%02X ", rec_cmder_fromdds[i]);
hex_str += buf;
}
if (rec_len_fromdds > 20) hex_str += "...";
WriteLog(hex_str);
} else {
WriteLog("非CJ目标的数据，忽略");
}
}
//====================================== 串口管理 ======================================//
int init_socat_serial(SocatPortId port_id, const char* device_path) {
if (port_id < 0 || port_id >= 2) {
WriteLog("错误：无效的串口ID");
return -1;
}
SocatPort* port = &socat_ports[port_id];
// 更新设备路径
if (device_path && strlen(device_path) > 0) {
strncpy(port->device_path, device_path, sizeof(port->device_path) - 1);
port->device_path[sizeof(port->device_path) - 1] = '\0';
}
if (strlen(port->device_path) == 0) {
WriteLog("错误：串口" + string(port->port_role) + "路径为空");
return -1;
}
// 打开串口
port->fd = open(port->device_path, O_RDWR | O_NOCTTY);
if (port->fd < 0) {
WriteLog("错误：无法打开" + string(port->port_role) + " " +
string(port->device_path) + " (错误: " + strerror(errno) + ")");
port->enabled = false;
return -1;
}
// 配置串口参数
struct termios tty;
memset(&tty, 0, sizeof(tty));
if (tcgetattr(port->fd, &tty) != 0) {
WriteLog("错误：无法获取" + string(port->port_role) + "属性");
close(port->fd);
port->fd = -1;
port->enabled = false;
return -1;
}
// 设置波特率 115200
cfsetospeed(&tty, B115200);
cfsetispeed(&tty, B115200);
// 8N1配置
tty.c_cflag &= ~PARENB; // 无奇偶校验
tty.c_cflag &= ~CSTOPB; // 1位停止位
tty.c_cflag &= ~CSIZE;
tty.c_cflag |= CS8; // 8位数据位
tty.c_cflag |= CREAD | CLOCAL; // 启用接收，忽略调制解调器状态
// 关闭硬件流控
tty.c_cflag &= ~CRTSCTS;
// 输入模式
tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // 非规范模式
tty.c_iflag &= ~(IXON | IXOFF | IXANY); // 关闭软件流控
tty.c_iflag &= ~(INLCR | ICRNL | IGNCR);
// 输出模式
tty.c_oflag &= ~OPOST; // 原始输出
tty.c_oflag &= ~ONLCR;
// 超时设置
if (port_id == PORT_RX) {
tty.c_cc[VMIN] = 1; // 至少读取1个字节
tty.c_cc[VTIME] = 10; // 1秒超时
} else {
tty.c_cc[VMIN] = 0; // 非阻塞
tty.c_cc[VTIME] = 5; // 0.5秒超时
}
if (tcsetattr(port->fd, TCSANOW, &tty) != 0) {
WriteLog("错误：无法设置" + string(port->port_role) + "属性");
close(port->fd);
port->fd = -1;
port->enabled = false;
return -1;
}
// 清空缓冲区
tcflush(port->fd, TCIOFLUSH);
port->enabled = true;
WriteLog("成功初始化" + string(port->port_role) + "：" +
string(port->device_path) + " (" + string(port->port_name) + ")");
return 0;
}
int read_from_serial(SocatPortId port_id, uint8_t* buffer, uint16_t buffer_size) {
if (port_id < 0 || port_id >= 2 || buffer == nullptr || buffer_size == 0) {
return -1;
}
SocatPort* port = &socat_ports[port_id];
if (!port->enabled || port->fd < 0) {
return -1;
}
// 使用poll检查是否有数据可读
struct pollfd fds[1];
fds[0].fd = port->fd;
fds[0].events = POLLIN;
int ret = poll(fds, 1, 100); // 100ms超时
if (ret > 0 && (fds[0].revents & POLLIN)) {
ssize_t bytes_read = read(port->fd, buffer, buffer_size);
if (bytes_read > 0) {
string hex_str = "从" + string(port->port_role) + "(" +
string(port->device_path) + ")收到数据(" +
to_string(bytes_read) + "字节): ";
for (int i = 0; i < (bytes_read < 8 ? bytes_read : 8); i++) {
char buf[4];
snprintf(buf, sizeof(buf), "%02X ", buffer[i]);
hex_str += buf;
}
if (bytes_read > 8) hex_str += "...";
WriteLog(hex_str);
return bytes_read;
} else if (bytes_read == 0) {
return 0;
} else {
if (errno != EAGAIN && errno != EWOULDBLOCK) {
WriteLog("错误：读取" + string(port->port_role) + "数据失败: " +
string(strerror(errno)));
}
return -1;
}
}
return 0; // 没有数据可读
}
void forward_to_socat(uint8_t* data, uint16_t len, SocatPortId port_id) {
if (port_id < 0 || port_id >= 2) return;
SocatPort* port = &socat_ports[port_id];
if (!port->enabled || port->fd < 0 || len == 0) return;
// 打印发送的数据
string hex_str = "向" + string(port->port_role) + "(" +
string(port->device_path) + ")发送数据(" +
to_string(len) + "字节): ";
for (int i = 0; i < (len < 8 ? len : 8); i++) {
char buf[4];
snprintf(buf, sizeof(buf), "%02X ", data[i]);
hex_str += buf;
}
if (len > 8) hex_str += "...";
WriteLog(hex_str);
// 写入数据
ssize_t written = write(port->fd, data, len);
if (written != len) {
WriteLog("警告：未能完全写入所有数据到" + string(port->port_role) +
" (期望" + to_string(len) + "字节，实际" + to_string(written) + "字节)");
}
// 确保数据发送完成
tcdrain(port->fd);
}
void cleanup_socat_serial(SocatPortId port_id) {
if (port_id < 0 || port_id >= 2) return;
SocatPort* port = &socat_ports[port_id];
port->enabled = false;
if (port->fd >= 0) {
close(port->fd);
port->fd = -1;
WriteLog("清理" + string(port->port_role) + "：" + string(port->device_path));
}
}
void cleanup_all_socat_serial() {
for (int i = 0; i < 2; i++) {
cleanup_socat_serial(static_cast<SocatPortId>(i));
}
}
//====================================== 数据处理 ======================================//
void process_tm_buffer(uint8_t* buffer, uint16_t length) {
if (length < 3) {
WriteLog("警告：接收数据长度不足");
return;
}
char type_str[10];
snprintf(type_str, sizeof(type_str), "0x%02X", buffer[2]);
WriteLog("处理指令类型: " + string(type_str));
uint8_t scomm[56];
uint8_t cmder[255];
uint8_t cmder_len;
switch (buffer[2]) {
case 0x0F: // 取常规遥测数据
WriteLog("指令0x0F: 取常规遥测数据");
generate_s_tele(scomm);
WriteLog("发送常规遥测数据到发送串口");
forward_to_socat(scomm, 56, PORT_TX);
break;
case 0x1E: // 遥控指令
WriteLog("指令0x1E: 遥控指令（本地处理）");
break;
case 0x4B: // 取主星空空的遥控指令
WriteLog("指令0x4B: 请求空空遥控指令");
{
lock_guard<mutex> lock(g_data_mutex);
cmder_len = generate_cmder(cmder);
}
if (cmder_len != 0) {
WriteLog("发送空空遥控指令到发送串口");
forward_to_socat(cmder, cmder_len, PORT_TX);
} else {
WriteLog("没有可用的空空遥控指令，返回默认响应");
forward_to_socat(cmder, 6, PORT_TX);
}
break;
case 0x5A: // 发送下行遥测
WriteLog("指令0x5A: 发送下行遥测到DDS");
if (Msgfrom == "O2-A") {
if (scomm_enable) {
PkgToO2_AkkDataPub(buffer, length);
WriteLog("发送到O2-A DDS");
}
} else if (Msgfrom == "O-1") {
if (scomm_enable) {
PkgToO_1kkDataPub(buffer, length);
WriteLog("发送到O-1 DDS");
}
} else {
if (scomm_enable) {
PkgToO2_AkkDataPub(buffer, length);
WriteLog("发送到默认O2-A DDS");
}
}
break;
default:
char unknown_str[10];
snprintf(unknown_str, sizeof(unknown_str), "0x%02X", buffer[2]);
WriteLog("未知指令类型: " + string(unknown_str));
WriteLog("数据将转发到发送串口");
forward_to_socat(buffer, length, PORT_TX);
break;
}
}
void generate_s_tele(uint8_t* tele) {
static uint8_t packet_cnt = 0;
uint8_t checksum = 0;
s_tele.packet_cnt_1 = packet_cnt++;
s_tele.para2.trans = 1;
// 填充示例数据
s_tele.snrInfoChannel = 80;
s_tele.receive_RSSI = 60;
tele[0] = 0xEB; tele[1] = 0x90; tele[2] = 0xF0;
tele[3] = 0x00; tele[4] = 0x32;
tele[5] = 0x00; tele[6] = 0x30; tele[7] = 0x01;
tele[8] = 0xA6; tele[9] = 0xC0;
tele[10] = packet_cnt; tele[11] = 0x00; tele[12] = 0x29;
memcpy(&tele[13], &s_tele, sizeof(S_Comm_telemetry_data_t));
for (int kc = 2; kc < 55; kc++)
checksum += tele[kc];
tele[55] = checksum;
WriteLog("生成遥测数据包，包计数: " + to_string(packet_cnt));
}
uint8_t generate_cmder(uint8_t* tele) {
if (isReceiveCmder) {
memcpy(tele, rec_cmder_fromdds, rec_len_fromdds);
isReceiveCmder = 0;
memset(rec_cmder_fromdds, 0, 255);
return rec_len_fromdds;
} else {
tele[0] = 0xEB; tele[1] = 0x90; tele[2] = 0xB4;
tele[3] = 0x00; tele[4] = 0x00; tele[5] = 0xB4;
return 6;
}
}
//====================================== 监听线程 ======================================//
void* listen_serial_thread(void* arg) {
WriteLog("串口监听线程启动，监听接收串口");
uint8_t read_buffer[256];
while (keep_running) {
int bytes_read = read_from_serial(PORT_RX, read_buffer, sizeof(read_buffer));
if (bytes_read > 0) {
process_tm_buffer(read_buffer, bytes_read);
} else if (bytes_read < 0) {
usleep(100000); // 100ms
} else {
usleep(10000); // 10ms
}
}
WriteLog("串口监听线程停止");
return NULL;
}
int start_listen_thread() {
if (socat_ports[PORT_RX].fd < 0) {
WriteLog("错误：接收串口未初始化");
return -1;
}
keep_running = 1;
int ret = pthread_create(&listen_thread, NULL, listen_serial_thread, NULL);
if (ret != 0) {
WriteLog("错误：无法创建监听线程");
return -1;
}
return 0;
}
void stop_listen_thread() {
keep_running = 0;
if (listen_thread) {
pthread_join(listen_thread, NULL);
listen_thread = 0;
}
}
//====================================== 主程序接口 ======================================//
void hook_rs422_kk(const char* tx_port, const char* rx_port) {
WriteLog("初始化RS422通信模块");
WriteLog("发送串口: " + string(tx_port ? tx_port : "未指定"));
WriteLog("接收串口: " + string(rx_port ? rx_port : "未指定"));
// 初始化串口
int success_count = 0;
if (init_socat_serial(PORT_TX, tx_port) == 0) {
success_count++;
}
if (init_socat_serial(PORT_RX, rx_port) == 0) {
success_count++;
}
if (success_count >= 2) {
WriteLog("成功初始化所有串口");
} else if (success_count >= 1) {
WriteLog("警告：只初始化了 " + to_string(success_count) + " 个串口");
WriteLog("错误：无法正常工作，至少需要两个串口");
return;
} else {
WriteLog("错误：未能初始化任何串口");
return;
}
// 初始化DDS
pkginit(1, "rs422_app", "hook_rs422");
// 订阅DDS消息
PkgSubO2_AKkData(cjb_rs422_kk_receive);
pkgSubO_1KKData(cjb_rs422_kk_receive);
WriteLog("已订阅O2-A和O-1数据");
// 启动监听线程
if (start_listen_thread() == 0) {
WriteLog("串口监听线程已启动");
} else {
WriteLog("错误：未能启动串口监听线程");
}
}
void set_scomm_enable(uint8_t s_en) {
scomm_enable = s_en;
WriteLog("设置通信使能: " + to_string(s_en));
}
void cleanup_hook_rs422() {
WriteLog("清理RS422通信模块资源");
stop_listen_thread();
cleanup_all_socat_serial();
if (simMsg) {
delete simMsg;
simMsg = nullptr;
WriteLog("清理DDS资源");
}
}
//====================================== 信号处理 ======================================//
void signal_handler(int signum) {
keep_running = 0;
}
//====================================== 主函数 ======================================//
int main(int argc, char* argv[]) {
WriteLog("========================================");
WriteLog("RS422通信程序启动");
WriteLog("使用方式: " + string(argv[0]) + " <发送串口> <接收串口>");
WriteLog("示例: " + string(argv[0]) + " /dev/pts/12 /dev/pts/25");
WriteLog("========================================");
// 检查参数
if (argc < 3) {
WriteLog("错误：参数不足");
WriteLog("请指定发送串口和接收串口:");
WriteLog(" " + string(argv[0]) + " /dev/pts/12 /dev/pts/25");
WriteLog("或使用默认值:");
WriteLog(" " + string(argv[0]) + " /dev/pts/12 /dev/pts/25");
return 1;
}
const char* tx_port = argv[1];
const char* rx_port = argv[2];
// 设置信号处理
signal(SIGINT, signal_handler);
signal(SIGTERM, signal_handler);
signal(SIGQUIT, signal_handler);
// 启动RS422通信模块
hook_rs422_kk(tx_port, rx_port);
// 主循环
while (keep_running) {
sleep(1);
static int counter = 0;
if (counter++ % 60 == 0) {
string tx_path = socat_ports[PORT_TX].device_path;
string rx_path = socat_ports[PORT_RX].device_path;
WriteLog("程序运行中... 发送串口:" + tx_path + " 接收串口:" + rx_path);
}
}
// 清理资源
cleanup_hook_rs422();
return 0;
}
Reference in New Issue Copy Permalink