xb/test
0
0
Fork 0
Code Issues Activity

Auto commit from DCSP - 2026/1/4 20:30:54

Browse Source
This commit is contained in:
xb
2026-01-04 20:30:54 +08:00
parent 0ceb98ed87
commit 6980e8ade2
7 changed files with 93 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
ComHS.cpp
View File

@@ -16,8 +16,8 @@
using namespace std;
// 全局变量定义
static int fd_telecontrol = -1; // 遥控串口 通信机发送给服务端
static int fd_telemetry = -1; // 遥测串口 服务端发送给通信机
static int fd_telecontrol = -1; // 遥控串口 通信硬件服务化发送给通信机
static int fd_telemetry = -1; // 遥测串口 通信机发送给通信硬件服务化
S_Comm_telemetry_data_t S_TELE; // 维护遥测数据缓存，供状态查询使用
uint8_t cmderPadding[1] = {0xA5}; // 用于填充指令
@@ -51,20 +51,19 @@ static pthread_t telemetry_thread, monitor_thread;
// 信号处理函数
static void signal_handler(int signum)
{
cout << "[INFO] Received signal " << signum << ", shutting down..." << endl;
g_running = 0;
}
// ComHS日志函数
void ComHSWriteLog(const std::string &msg)
void ComHSWriteLog(const string &msg)
{
std::cout << msg << std::endl;
cout << msg << endl;
}
// 初始化ComHS
void ComHS_init(uint8_t domainid, std::string appname)
// 初始化ComHS的fastdds
void ComHS_init(uint8_t domainid, string appname)
{
std::vector<std::string> parameters;
vector<string> parameters;
string expression = "dest = '" + servername + "'";
if (nullptr == ComHS_part)
@@ -76,7 +75,6 @@ void ComHS_init(uint8_t domainid, std::string appname)
ComHS_part->create_sub(topic_name_cmd, command_callback, expression, parameters);
ComHS_part->create_sub(topic_name_tlm, telemetry_callback, expression, parameters);
cout << "[OK] ComHS initialized for " << appname << endl;
}
}
@@ -95,14 +93,13 @@ void ComHS_telemetry_Pub(uint8_t* data, string dest, uint16_t len)
if (ComHS_part)
{
ComHS_part->publish(topic_name_tlm, "Com_Hardware_Service", dest, "telemetry", data, len);
cout << "[INFO] Telemetry published to " << dest << ", len=" << len << endl;
}
}
// ComHS命令回调函数
void command_callback(std::string src, std::string dest, std::string type,
std::string reserve1, std::string reserve2,
std::vector<uint8_t>& data)
void command_callback(string src, string dest, string type,
string reserve1, string reserve2,
vector<uint8_t>& data)
{
if (S_COMM_ON_OFF != 1)
@@ -128,9 +125,9 @@ void command_callback(std::string src, std::string dest, std::string type,
}
// ComHS遥测回调函数
void telemetry_callback(std::string src, std::string dest, std::string type,
std::string reserve1, std::string reserve2,
std::vector<uint8_t>& data)
void telemetry_callback(string src, string dest, string type,
string reserve1, string reserve2,
vector<uint8_t>& data)
{
if (S_COMM_ON_OFF != 1)
@@ -447,7 +444,7 @@ int process_received_data(uint8_t *data, uint16_t size)
}
break;
case 0xA5: // 发送遥测确认
case 0xA5: // 遥测确认
if ((S_Telec1_Data[5] == 0xAA) && (S_Telec1_Data[6] == 0x50))
{
sendTelemSuccessCnt = 1;
@@ -515,8 +512,6 @@ int read_scomm_response(void)
printf("[WARN] S_COMM buffer full\n");
break;
}
// 短暂延时，避免CPU占用过高
usleep(1000);
}
@@ -537,7 +532,7 @@ static void *telemetry_query_thread(void *arg)
if (S_COMM_ON_OFF == 1)
{
// 定时查询遥测数据
Get_S_COMM_Telemetry_Data();
Get_S_COMM_UP_CMD();
}
// 每2秒查询一次