631 lines
17 KiB
C++
631 lines
17 KiB
C++
#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)
|
||
{
|
||
g_running = 0;
|
||
}
|
||
|
||
// ComHS日志函数
|
||
void ComHSWriteLog(const string &msg)
|
||
{
|
||
cout << msg << endl;
|
||
}
|
||
|
||
// 初始化ComHS的fastdds
|
||
void ComHS_init(uint8_t domainid, string appname)
|
||
{
|
||
vector<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);
|
||
|
||
}
|
||
}
|
||
|
||
// 通信硬件服务化遥控发布
|
||
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);
|
||
}
|
||
}
|
||
|
||
// ComHS命令回调函数
|
||
void command_callback(string src, string dest, string type,
|
||
string reserve1, string reserve2,
|
||
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(string src, string dest, string type,
|
||
string reserve1, string reserve2,
|
||
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;
|
||
}
|
||
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_UP_CMD();
|
||
}
|
||
|
||
// 每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;
|
||
} |