Initial commit from DCSP - 2025/12/30 17:02:38
This commit is contained in:
xb
636
ComHS.cpp
Normal file
636
ComHS.cpp
Normal file
@@ -0,0 +1,636 @@
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <cstring>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <termios.h>
|
||||
#include <pthread.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <signal.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "ComHS.h"
|
||||
#include "SimMsg.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
// 全局变量定义
|
||||
static int fd_telecontrol = -1; // 遥控串口 通信机发送给服务端
|
||||
static int fd_telemetry = -1; // 遥测串口 服务端发送给通信机
|
||||
|
||||
S_Comm_telemetry_data_t S_TELE; // 维护遥测数据缓存，供状态查询使用
|
||||
uint8_t cmderPadding[1] = {0xA5}; // 用于填充指令
|
||||
uint8_t S_COMM_ON_OFF = 0;
|
||||
|
||||
uint8_t scommHeaderErrCnt = 0; // 帧头错误
|
||||
uint8_t scommResIDErrCnt = 0; // 应答帧ID错误
|
||||
uint8_t scommCheckErrCnt = 0; // 帧校验错误
|
||||
uint8_t scommUartResetCnt = 0; // 无应答串口复位计数
|
||||
uint8_t sendCmderCnt = 0; // 向通信机发送控制指令计数
|
||||
uint8_t sendCmderSuccessCnt = 0; // 通信机控制指令执行成功应答计数
|
||||
uint8_t sendCmderErrCnt = 0; // 通信机控制指令错误应答计数
|
||||
uint8_t sendTelemCnt = 0; // 向通信机发送遥测帧计数
|
||||
uint8_t sendTelemSuccessCnt = 0; // 接收通信机遥测帧应答计数
|
||||
uint8_t sendGetSelfTelemCmderCnt = 0; // 发送查询通信机工程遥测计数
|
||||
|
||||
unsigned char S_Telec1_Data[512] = {0}; // 遥控
|
||||
unsigned char S_Telem_Data[512] = {0}; // 遥测
|
||||
|
||||
// ComHS相关全局变量
|
||||
SimMsg* ComHS_part = nullptr;
|
||||
string servername = "Com_Hardware_Service";
|
||||
string topic_name_cmd = "Command";
|
||||
string topic_name_tlm = "Telemetry";
|
||||
const char* fastdds_dest = "Com_Service";
|
||||
|
||||
// 运行控制
|
||||
static volatile int g_running = 1;
|
||||
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)
|
||||
{
|
||||
std::cout << msg << std::endl;
|
||||
}
|
||||
|
||||
// 初始化ComHS
|
||||
void ComHS_init(uint8_t domainid, std::string appname)
|
||||
{
|
||||
std::vector<std::string> parameters;
|
||||
string expression = "dest = '" + servername + "'";
|
||||
|
||||
if (nullptr == ComHS_part)
|
||||
{
|
||||
ComHS_part = new SimMsg(domainid, 3000, appname, ComHSWriteLog);
|
||||
ComHS_part->create_pub(topic_name_cmd);
|
||||
ComHS_part->create_pub(topic_name_tlm);
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
// 通信硬件服务化遥控发布
|
||||
void ComHS_command_Pub(uint8_t* data, string dest, uint16_t len)
|
||||
{
|
||||
if (ComHS_part)
|
||||
{
|
||||
ComHS_part->publish(topic_name_cmd, "Com_Hardware_Service", dest, "command", data, len);
|
||||
}
|
||||
}
|
||||
|
||||
// 通信硬件服务化遥测发布
|
||||
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)
|
||||
{
|
||||
|
||||
if (S_COMM_ON_OFF != 1)
|
||||
{
|
||||
cout << "[ERROR] S_COMM is not available" << endl;
|
||||
return;
|
||||
}
|
||||
|
||||
if (data.empty())
|
||||
{
|
||||
cout << "[ERROR] No command data provided" << endl;
|
||||
return;
|
||||
}
|
||||
|
||||
// 直接发送给通信机
|
||||
uint8_t *cmd_data = data.data();
|
||||
uint16_t cmd_len = data.size();
|
||||
|
||||
cout << "[INFO] Sending command to S_COMM: " << data.size() << " bytes" << endl;
|
||||
|
||||
// 发送控制指令
|
||||
send_S_COMM_Cmder(cmd_data, cmd_len);
|
||||
}
|
||||
|
||||
// ComHS遥测回调函数
|
||||
void telemetry_callback(std::string src, std::string dest, std::string type,
|
||||
std::string reserve1, std::string reserve2,
|
||||
std::vector<uint8_t>& data)
|
||||
{
|
||||
|
||||
if (S_COMM_ON_OFF != 1)
|
||||
{
|
||||
cout << "[ERROR] S_COMM is not available" << endl;
|
||||
return;
|
||||
}
|
||||
|
||||
if (data.empty())
|
||||
{
|
||||
cout << "[ERROR] No telemetry data provided" << endl;
|
||||
return;
|
||||
}
|
||||
|
||||
// 解析为Multi_EPDU_packet_t结构体
|
||||
if (data.size() >= sizeof(Multi_EPDU_packet_t))
|
||||
{
|
||||
Multi_EPDU_packet_t *down_pkt = (Multi_EPDU_packet_t *)data.data();
|
||||
|
||||
// 通过通信机发送下行遥测
|
||||
Send_Telemetry_From_S((uint8_t *)down_pkt, sizeof(Multi_EPDU_packet_t));
|
||||
cout << "[INFO] Downlink telemetry sent via S_COMM" << endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "[ERROR] Telemetry data too small: " << data.size()
|
||||
<< " bytes, need at least " << sizeof(Multi_EPDU_packet_t) << " bytes" << endl;
|
||||
}
|
||||
}
|
||||
|
||||
// 串口初始化
|
||||
int scomm_uart_init(const char *dev_telec, const char *dev_telem, int baudrate)
|
||||
{
|
||||
// 初始化遥控串口
|
||||
fd_telecontrol = open(dev_telec, O_RDWR | O_NOCTTY | O_NDELAY);
|
||||
if (fd_telecontrol < 0)
|
||||
{
|
||||
perror("open telecontrol serial failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 设置为非阻塞模式
|
||||
fcntl(fd_telecontrol, F_SETFL, 0);
|
||||
|
||||
// 初始化遥测串口
|
||||
fd_telemetry = open(dev_telem, O_RDWR | O_NOCTTY | O_NDELAY);
|
||||
if (fd_telemetry < 0)
|
||||
{
|
||||
perror("open telemetry serial failed");
|
||||
close(fd_telecontrol);
|
||||
fd_telecontrol = -1;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 设置为非阻塞模式
|
||||
fcntl(fd_telemetry, F_SETFL, 0);
|
||||
|
||||
// 设置两个串口参数
|
||||
int fds[] = {fd_telecontrol, fd_telemetry};
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
struct termios tty;
|
||||
if (tcgetattr(fds[i], &tty) != 0)
|
||||
{
|
||||
perror("tcgetattr failed");
|
||||
continue;
|
||||
}
|
||||
|
||||
// 设置波特率
|
||||
speed_t speed;
|
||||
switch(baudrate) {
|
||||
case 9600: speed = B9600; break;
|
||||
case 19200: speed = B19200; break;
|
||||
case 38400: speed = B38400; break;
|
||||
case 57600: speed = B57600; break;
|
||||
case 115200: speed = B115200; break;
|
||||
default: speed = B115200; break;
|
||||
}
|
||||
|
||||
cfsetospeed(&tty, speed);
|
||||
cfsetispeed(&tty, speed);
|
||||
|
||||
// 8N1: 8位数据位，无奇偶校验，1位停止位
|
||||
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 | IUCLC); // 关闭输入转换
|
||||
|
||||
// 设置输出模式
|
||||
tty.c_oflag &= ~OPOST; // 原始输出
|
||||
tty.c_oflag &= ~ONLCR; // 不转换换行
|
||||
|
||||
// 设置超时和最小读取字符数
|
||||
tty.c_cc[VMIN] = 0; // 非阻塞
|
||||
tty.c_cc[VTIME] = 5; // 0.5秒超时
|
||||
|
||||
if (tcsetattr(fds[i], TCSANOW, &tty) != 0)
|
||||
{
|
||||
perror("tcsetattr failed");
|
||||
}
|
||||
|
||||
// 清空缓冲区
|
||||
tcflush(fds[i], TCIOFLUSH);
|
||||
}
|
||||
|
||||
cout << "[OK] S_COMM UART init telec=" << dev_telec
|
||||
<< " telem=" << dev_telem << " baud=" << baudrate << endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
// S遥测串口发送
|
||||
void S_UART_TELEMETRY_SEND(uint8_t cmd, uint16_t len, uint8_t *nums)
|
||||
{
|
||||
uint8_t tele_check = 0;
|
||||
uint16_t i;
|
||||
memset(S_Telem_Data, 0, 512);
|
||||
|
||||
S_Telem_Data[0] = 0xEB;
|
||||
S_Telem_Data[1] = 0x90;
|
||||
S_Telem_Data[2] = cmd;
|
||||
S_Telem_Data[3] = (unsigned char)len >> 8;
|
||||
S_Telem_Data[4] = (unsigned char)len;
|
||||
|
||||
for (i = 0; i < len; i++)
|
||||
{
|
||||
S_Telem_Data[i + 5] = *(nums + i);
|
||||
}
|
||||
|
||||
for (i = 2; i < (len + 5); i++)
|
||||
{
|
||||
tele_check += S_Telem_Data[i];
|
||||
}
|
||||
|
||||
S_Telem_Data[len + 5] = tele_check;
|
||||
|
||||
if (fd_telemetry >= 0)
|
||||
{
|
||||
int written = write(fd_telemetry, S_Telem_Data, (len + 6));
|
||||
if (written < 0)
|
||||
{
|
||||
perror("[ERROR] S_COMM write failed");
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("[INFO] S_COMM sent command: 0x%02X, len: %d, bytes: %d\n", cmd, len, written);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 向S测控发送控制指令
|
||||
void send_S_COMM_Cmder(uint8_t *cmd, uint16_t len)
|
||||
{
|
||||
sendCmderCnt++;
|
||||
|
||||
uint8_t kc = 0;
|
||||
uint8_t cmd_buff[64] = {0};
|
||||
|
||||
cmd_buff[0] = 0x11;
|
||||
cmd_buff[1] = 0x1A;
|
||||
cmd_buff[2] = 0xC0;
|
||||
cmd_buff[3] = 0x00;
|
||||
|
||||
cmd_buff[4] = (uint8_t)(len + 1) >> 8; // 根据高层协议来,实际上是+2减1
|
||||
cmd_buff[5] = (uint8_t)len + 1;
|
||||
|
||||
for (kc = 0; kc < len + 2; kc++)
|
||||
cmd_buff[kc + 6] = cmd[kc];
|
||||
|
||||
S_UART_TELEMETRY_SEND(TELECONTROL_CMD, len + 8, cmd_buff);
|
||||
}
|
||||
|
||||
// 获取S测控常规遥测
|
||||
void Get_S_COMM_Telemetry_Data(void)
|
||||
{
|
||||
if (S_COMM_ON_OFF == 1)
|
||||
{
|
||||
S_UART_TELEMETRY_SEND(CON_TELEMETRY, 1, cmderPadding);
|
||||
printf("[INFO] S_COMM query telemetry sent\n");
|
||||
if (sendGetSelfTelemCmderCnt++ >= 10)
|
||||
{
|
||||
sendGetSelfTelemCmderCnt = 0;
|
||||
printf("[WARN] S_COMM telemetry no response, reset UART\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 获取上行遥控指令
|
||||
void Get_S_COMM_UP_CMD(void)
|
||||
{
|
||||
if (S_COMM_ON_OFF == 1)
|
||||
{
|
||||
S_UART_TELEMETRY_SEND(QUERY_TELECMD, 1, cmderPadding);
|
||||
printf("[INFO] S_COMM query uplink command sent\n");
|
||||
}
|
||||
}
|
||||
|
||||
// 通过S测控发送下行遥测
|
||||
void Send_Telemetry_From_S(uint8_t *tele, uint16_t len)
|
||||
{
|
||||
sendTelemCnt++;
|
||||
S_UART_TELEMETRY_SEND(SEND_TELEMETRY, len, tele);
|
||||
printf("[INFO] S_COMM send telemetry from S, len: %d\n", len);
|
||||
}
|
||||
|
||||
// 接收数据处理函数
|
||||
int process_received_data(uint8_t *data, uint16_t size)
|
||||
{
|
||||
if (size < 6)
|
||||
{
|
||||
printf("[ERROR] S_COMM received data too short: %d bytes\n", size);
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint16_t i = 0;
|
||||
uint8_t cmd = 0;
|
||||
uint16_t frame_len = 0;
|
||||
uint8_t checksum0 = 0, checksum1 = 0;
|
||||
|
||||
memcpy(S_Telec1_Data, data, size);
|
||||
|
||||
if ((S_Telec1_Data[0] == 0xEB) && (S_Telec1_Data[1] == 0x90))
|
||||
{
|
||||
cmd = S_Telec1_Data[2];
|
||||
frame_len = (uint16_t)(S_Telec1_Data[3] << 8) | (S_Telec1_Data[4]);
|
||||
checksum0 = S_Telec1_Data[frame_len + 5];
|
||||
|
||||
// 检查帧长度是否合理
|
||||
if (frame_len > 500)
|
||||
{
|
||||
printf("[ERROR] S_COMM frame length too large: %d\n", frame_len);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 检查实际接收到的数据是否足够
|
||||
if (size < (frame_len + 6))
|
||||
{
|
||||
printf("[ERROR] S_COMM incomplete frame: need %d bytes, got %d\n",
|
||||
frame_len + 6, size);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 计算校验和
|
||||
checksum1 = 0;
|
||||
for (i = 2; i < (frame_len + 5); i++)
|
||||
{
|
||||
checksum1 += S_Telec1_Data[i];
|
||||
}
|
||||
|
||||
if (checksum1 != checksum0)
|
||||
{
|
||||
scommCheckErrCnt++;
|
||||
printf("[ERROR] S_COMM checksum error: calc=0x%02X recv=0x%02X\n", checksum1, checksum0);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 根据命令类型处理
|
||||
switch (cmd)
|
||||
{
|
||||
case 0xF0: // 常规遥测数据
|
||||
printf("[INFO] S_COMM telemetry data received, frame_len=%d\n", frame_len);
|
||||
if (size >= (13 + frame_len - 10))
|
||||
{
|
||||
// 维护遥测数据缓存
|
||||
memcpy((uint8_t *)&S_TELE, (uint8_t *)&S_Telec1_Data[13], frame_len - 10);
|
||||
if (sendGetSelfTelemCmderCnt > 0)
|
||||
sendGetSelfTelemCmderCnt--;
|
||||
|
||||
// 通过ComHS发布遥测数据
|
||||
if (frame_len - 10 > 0)
|
||||
{
|
||||
ComHS_telemetry_Pub((uint8_t *)&S_TELE, fastdds_dest, sizeof(S_Comm_telemetry_data_t));
|
||||
printf("[INFO] S_COMM telemetry published via ComHS\n");
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case 0xE1: // 遥控指令应答
|
||||
if ((S_Telec1_Data[5] == 0xAA) && (S_Telec1_Data[6] == 0x8C))
|
||||
{
|
||||
sendCmderSuccessCnt++;
|
||||
printf("[INFO] S_COMM command success\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
sendCmderErrCnt++;
|
||||
printf("[ERROR] S_COMM command error\n");
|
||||
}
|
||||
break;
|
||||
|
||||
case 0xB4: // 上行遥控指令
|
||||
printf("[INFO] S_COMM uplink command received, frame_len=%d\n", frame_len);
|
||||
if (size >= 9 && frame_len >= 4)
|
||||
{
|
||||
uint16_t total_data_len = frame_len;
|
||||
|
||||
if (total_data_len > 4)
|
||||
{
|
||||
ComHS_command_Pub(&S_Telec1_Data[5], fastdds_dest, total_data_len);
|
||||
printf("[INFO] S_COMM uplink command published, len=%d bytes\n", total_data_len);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("[INFO] No actual data in uplink command\n");
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case 0xA5: // 发送遥测确认
|
||||
if ((S_Telec1_Data[5] == 0xAA) && (S_Telec1_Data[6] == 0x50))
|
||||
{
|
||||
sendTelemSuccessCnt = 1;
|
||||
printf("[INFO] S_COMM telemetry sent successfully\n");
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
scommResIDErrCnt++;
|
||||
printf("[ERROR] S_COMM unknown command ID: 0x%02X\n", cmd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0; // 处理成功
|
||||
}
|
||||
else
|
||||
{
|
||||
scommHeaderErrCnt++;
|
||||
printf("[ERROR] S_COMM frame header error: 0x%02X 0x%02X\n",
|
||||
S_Telec1_Data[0], S_Telec1_Data[1]);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
// 读取通信机响应数据
|
||||
int read_scomm_response(void)
|
||||
{
|
||||
if (fd_telecontrol < 0)
|
||||
return -1;
|
||||
|
||||
uint8_t buffer[512];
|
||||
int total_read = 0;
|
||||
|
||||
// 非阻塞读取所有可用数据
|
||||
while (g_running)
|
||||
{
|
||||
int bytes = read(fd_telecontrol, buffer + total_read, sizeof(buffer) - total_read);
|
||||
if (bytes > 0)
|
||||
{
|
||||
total_read += bytes;
|
||||
printf("[INFO] S_COMM received %d bytes response, total: %d\n", bytes, total_read);
|
||||
}
|
||||
else if (bytes < 0)
|
||||
{
|
||||
if (errno == EAGAIN || errno == EWOULDBLOCK)
|
||||
{
|
||||
// 无数据可读，退出循环
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
perror("[ERROR] S_COMM read error");
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// read返回0，通常表示文件结束
|
||||
break;
|
||||
}
|
||||
|
||||
// 检查是否读取了足够的数据
|
||||
if (total_read >= sizeof(buffer))
|
||||
{
|
||||
printf("[WARN] S_COMM buffer full\n");
|
||||
break;
|
||||
}
|
||||
|
||||
// 短暂延时，避免CPU占用过高
|
||||
usleep(1000);
|
||||
}
|
||||
|
||||
if (total_read > 0)
|
||||
{
|
||||
// 处理接收到的数据
|
||||
process_received_data(buffer, total_read);
|
||||
}
|
||||
|
||||
return total_read;
|
||||
}
|
||||
|
||||
// 定时查询遥测数据线程
|
||||
static void *telemetry_query_thread(void *arg)
|
||||
{
|
||||
while (g_running)
|
||||
{
|
||||
if (S_COMM_ON_OFF == 1)
|
||||
{
|
||||
// 定时查询遥测数据
|
||||
Get_S_COMM_Telemetry_Data();
|
||||
}
|
||||
|
||||
// 每2秒查询一次
|
||||
sleep(2);
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// 监听串口数据线程
|
||||
static void *serial_monitor_thread(void *arg)
|
||||
{
|
||||
while (g_running)
|
||||
{
|
||||
if (S_COMM_ON_OFF == 1 && fd_telecontrol >= 0)
|
||||
{
|
||||
// 读取串口响应
|
||||
read_scomm_response();
|
||||
}
|
||||
|
||||
// 短暂延时
|
||||
usleep(10000); // 10ms
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// 启动SCOMM服务
|
||||
void start_scomm_service(const char *dev_telec, const char *dev_telem, int baudrate)
|
||||
{
|
||||
// 设置信号处理
|
||||
signal(SIGINT, signal_handler);
|
||||
signal(SIGTERM, signal_handler);
|
||||
|
||||
// 初始化串口
|
||||
if (scomm_uart_init(dev_telec, dev_telem, baudrate) < 0)
|
||||
{
|
||||
S_COMM_ON_OFF = 0;
|
||||
cerr << "[WARN] S_COMM UART init failed. Service will start but communication will fail." << endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
S_COMM_ON_OFF = 1;
|
||||
}
|
||||
|
||||
// 初始化ComHS通信
|
||||
ComHS_init(0, "S_Comm_Hardware_Service");
|
||||
|
||||
// 创建线程
|
||||
if (pthread_create(&telemetry_thread, NULL, telemetry_query_thread, NULL) != 0)
|
||||
{
|
||||
perror("[ERROR] Failed to create telemetry query thread");
|
||||
}
|
||||
|
||||
if (pthread_create(&monitor_thread, NULL, serial_monitor_thread, NULL) != 0)
|
||||
{
|
||||
perror("[ERROR] Failed to create serial monitor thread");
|
||||
}
|
||||
}
|
||||
|
||||
// 停止SCOMM服务
|
||||
void stop_scomm_service(void)
|
||||
{
|
||||
g_running = 0;
|
||||
|
||||
// 等待线程结束
|
||||
if (telemetry_thread)
|
||||
{
|
||||
pthread_join(telemetry_thread, NULL);
|
||||
}
|
||||
if (monitor_thread)
|
||||
{
|
||||
pthread_join(monitor_thread, NULL);
|
||||
}
|
||||
|
||||
// 关闭串口
|
||||
if (fd_telecontrol >= 0)
|
||||
{
|
||||
close(fd_telecontrol);
|
||||
fd_telecontrol = -1;
|
||||
}
|
||||
if (fd_telemetry >= 0)
|
||||
{
|
||||
close(fd_telemetry);
|
||||
fd_telemetry = -1;
|
||||
}
|
||||
|
||||
// 清理ComHS资源
|
||||
if (ComHS_part)
|
||||
{
|
||||
delete ComHS_part;
|
||||
ComHS_part = nullptr;
|
||||
}
|
||||
|
||||
cout << "[INFO] S_COMM Hardware Service stopped" << endl;
|
||||
}
|
||||
Reference in New Issue
Block a user