717 lines
17 KiB
C++
717 lines
17 KiB
C++
#include <iostream>
|
||
#include <string>
|
||
#include <vector>
|
||
#include <cstring>
|
||
#include <unistd.h>
|
||
#include <fcntl.h>
|
||
#include <termios.h>
|
||
#include <signal.h>
|
||
#include <errno.h>
|
||
#include <sys/time.h>
|
||
#include <sys/select.h>
|
||
|
||
#include "ComHS.h"
|
||
#include "SimMsg.h"
|
||
|
||
using namespace std;
|
||
|
||
static std::mutex scomm_mutex;
|
||
|
||
/*
|
||
* 全局变量定义
|
||
*/
|
||
|
||
/* 串口文件描述符 */
|
||
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";
|
||
|
||
/* 运行控制 */
|
||
volatile int g_running = 1;
|
||
|
||
/* 函数前向声明 */
|
||
static int process_received_data(uint8_t *data, uint16_t size);
|
||
|
||
/*
|
||
* 信号处理函数
|
||
*/
|
||
static void signal_handler(int signum)
|
||
{
|
||
g_running = 0;
|
||
}
|
||
|
||
/*
|
||
* ComHS日志函数
|
||
*/
|
||
void ComHSWriteLog(const string &msg)
|
||
{
|
||
cout << msg << endl;
|
||
}
|
||
|
||
/*
|
||
* 初始化ComHS的fastdds
|
||
*
|
||
* @domainid: 域ID
|
||
* @appname: 应用程序名称
|
||
*/
|
||
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);
|
||
}
|
||
}
|
||
|
||
/*
|
||
* 通信硬件服务化遥控发布
|
||
*
|
||
* @data: 要发布的数据
|
||
* @dest: 目标对象
|
||
* @len: 数据长度
|
||
*/
|
||
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);
|
||
}
|
||
}
|
||
|
||
/*
|
||
* 通信硬件服务化遥测发布
|
||
*
|
||
* @data: 要发布的数据
|
||
* @dest: 目标对象
|
||
* @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命令回调函数
|
||
*
|
||
* 1、当接收到的为取遥控类型，即为取遥控指令时，触发取上行遥控
|
||
* 2、当接收到的为遥控类型，即为控制指令时，触发向串口写入控制命令
|
||
*
|
||
*/
|
||
void command_callback(string src, string dest, string type,
|
||
string reserve1, string reserve2,
|
||
vector<uint8_t> &data)
|
||
{
|
||
std::lock_guard<std::mutex> lock(scomm_mutex);
|
||
|
||
if (S_COMM_ON_OFF != 1) {
|
||
cerr << "S_COMM not available" << endl;
|
||
return;
|
||
}
|
||
|
||
if (data.empty()) {
|
||
cerr << "No command data provided" << endl;
|
||
return;
|
||
}
|
||
|
||
if (type == "request_command" ) {
|
||
if (data.size() == 1 && data[0] == 0x00) {
|
||
Get_S_COMM_UP_CMD();
|
||
}
|
||
} else if (type == "command") {
|
||
uint8_t *cmd_data = data.data();
|
||
uint16_t cmd_len = data.size();
|
||
|
||
send_S_COMM_Cmder(cmd_data, cmd_len);
|
||
}
|
||
|
||
|
||
}
|
||
|
||
/*
|
||
* ComHS遥测回调函数
|
||
*
|
||
* 1、当接收到的为命令类型，即为取遥测指令时，触发取遥测
|
||
* 2、当接收到的为遥测类型，即为需要下行的遥测时，将遥测下行
|
||
*/
|
||
void telemetry_callback(string src, string dest, string type,
|
||
string reserve1, string reserve2,
|
||
vector<uint8_t> &data)
|
||
{
|
||
std::lock_guard<std::mutex> lock(scomm_mutex);
|
||
|
||
if (S_COMM_ON_OFF != 1) {
|
||
cerr << "S_COMM not available" << endl;
|
||
return;
|
||
}
|
||
|
||
if (data.empty()) {
|
||
cerr << "No telemetry data provided" << endl;
|
||
return;
|
||
}
|
||
|
||
if (type == "request_telemetry" ) {
|
||
if (data.size() == 1 && data[0] == 0xFF) {
|
||
Get_S_COMM_Telemetry_Data();
|
||
}
|
||
} else if (type == "telemetry") {
|
||
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));
|
||
} else {
|
||
cerr << "Telemetry data too small" << endl;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*
|
||
* 串口初始化
|
||
*
|
||
* @dev_telec: 遥控串口设备路径
|
||
* @dev_telem: 遥测串口设备路径
|
||
* @baudrate: 波特率
|
||
*
|
||
* 返回值: 成功返回0，失败返回-1
|
||
*/
|
||
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 << "S_COMM UART initialized: telec=" << dev_telec
|
||
<< " telem=" << dev_telem << " baud=" << baudrate << endl;
|
||
return 0;
|
||
}
|
||
|
||
/*
|
||
* S遥测串口发送
|
||
*
|
||
* 将遥测数据进行封装，然后发送到通信机串口
|
||
*
|
||
* @cmd: 命令码
|
||
* @len: 数据长度
|
||
* @nums: 数据指针
|
||
*/
|
||
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("S_COMM write failed");
|
||
}
|
||
}
|
||
}
|
||
|
||
/*
|
||
* 超时读取
|
||
*
|
||
* @timeout_ms: 超时时间（毫秒）
|
||
* @return: 读取到的字节数，-1表示超时或错误
|
||
*/
|
||
int scomm_read_with_select(int timeout_ms)
|
||
{
|
||
if (fd_telecontrol < 0) {
|
||
cerr << "S_COMM telecontrol port not opened" << endl;
|
||
return -1;
|
||
}
|
||
|
||
fd_set read_fds;
|
||
struct timeval tv;
|
||
uint8_t buffer[512];
|
||
int total_read = 0;
|
||
|
||
// 设置超时时间
|
||
tv.tv_sec = timeout_ms / 1000;
|
||
tv.tv_usec = (timeout_ms % 1000) * 1000;
|
||
|
||
// 清空并设置文件描述符集合
|
||
FD_ZERO(&read_fds);
|
||
FD_SET(fd_telecontrol, &read_fds);
|
||
|
||
// 使用select等待数据到达
|
||
int ret = select(fd_telecontrol + 1, &read_fds, NULL, NULL, &tv);
|
||
|
||
if (ret > 0) {
|
||
// 有数据可读
|
||
if (FD_ISSET(fd_telecontrol, &read_fds)) {
|
||
// 读取所有可用数据
|
||
while (total_read < (int)sizeof(buffer)) {
|
||
int bytes = read(fd_telecontrol, buffer + total_read,
|
||
sizeof(buffer) - total_read);
|
||
if (bytes > 0) {
|
||
total_read += bytes;
|
||
|
||
// 尝试处理数据
|
||
if (process_received_data(buffer, total_read) == 0) {
|
||
return total_read;
|
||
}
|
||
} else if (bytes < 0) {
|
||
if (errno == EAGAIN || errno == EWOULDBLOCK) {
|
||
break;
|
||
} else {
|
||
perror("S_COMM read error");
|
||
return -1;
|
||
}
|
||
} else {
|
||
break;
|
||
}
|
||
}
|
||
|
||
return total_read;
|
||
}
|
||
} else if (ret == 0) {
|
||
return -1; // 超时
|
||
} else {
|
||
perror("select failed");
|
||
return -1;
|
||
}
|
||
|
||
return total_read;
|
||
}
|
||
|
||
/*
|
||
* 发送指令并等待响应
|
||
*
|
||
* @cmd: 命令码
|
||
* @len: 数据长度
|
||
* @nums: 数据指针
|
||
* @timeout_ms: 等待响应超时时间
|
||
* @return: 响应数据长度，-1表示失败
|
||
*/
|
||
int S_UART_SEND_AND_WAIT(uint8_t cmd, uint16_t len, uint8_t *nums, int timeout_ms)
|
||
{
|
||
if (S_COMM_ON_OFF != 1) {
|
||
cerr << "S_COMM is not available" << endl;
|
||
return -1;
|
||
}
|
||
|
||
if (fd_telemetry < 0 || fd_telecontrol < 0) {
|
||
cerr << "Serial ports not initialized" << endl;
|
||
return -1;
|
||
}
|
||
|
||
// 1. 先清空输入缓冲区
|
||
tcflush(fd_telecontrol, TCIFLUSH);
|
||
|
||
// 2. 发送指令
|
||
S_UART_TELEMETRY_SEND(cmd, len, nums);
|
||
|
||
// 3. 等待响应
|
||
return scomm_read_with_select(timeout_ms);
|
||
}
|
||
|
||
/*
|
||
* 向S测控发送控制指令
|
||
*
|
||
* 将遥控指令封装，然后发送到通信机串口
|
||
*
|
||
* @cmd: 指令数据
|
||
* @len: 指令长度
|
||
*/
|
||
void send_S_COMM_Cmder(uint8_t *cmd, uint16_t len)
|
||
{
|
||
if (S_COMM_ON_OFF != 1) {
|
||
cerr << "S_COMM is not available" << endl;
|
||
return;
|
||
}
|
||
|
||
sendCmderCnt++;
|
||
|
||
// 构造命令数据
|
||
uint8_t cmd_buff[64] = {0};
|
||
uint8_t kc = 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;
|
||
cmd_buff[5] = (uint8_t)len + 1;
|
||
|
||
for (kc = 0; kc < len + 2; kc++) {
|
||
cmd_buff[kc + 6] = cmd[kc];
|
||
}
|
||
|
||
// 发送并等待响应
|
||
int result = S_UART_SEND_AND_WAIT(TELECONTROL_CMD, len + 8, cmd_buff, 100);
|
||
|
||
if (result > 0) {
|
||
sendCmderSuccessCnt++;
|
||
} else {
|
||
sendCmderErrCnt++;
|
||
cerr << "Command timeout (100ms)" << endl;
|
||
}
|
||
}
|
||
|
||
/*
|
||
* 获取S测控常规遥测（同步版本，使用select超时）
|
||
*
|
||
* 向通信机串口写入常规遥测查询的指令，进行遥测的查询
|
||
*
|
||
*/
|
||
void Get_S_COMM_Telemetry_Data(void)
|
||
{
|
||
if (S_COMM_ON_OFF != 1) return;
|
||
|
||
int result = S_UART_SEND_AND_WAIT(CON_TELEMETRY, 1, cmderPadding, 100);
|
||
|
||
if (result > 0) {
|
||
sendGetSelfTelemCmderCnt = 0;
|
||
} else {
|
||
sendGetSelfTelemCmderCnt++;
|
||
if (sendGetSelfTelemCmderCnt >= 3) {
|
||
cerr << "Multiple telemetry queries failed" << endl;
|
||
}
|
||
}
|
||
}
|
||
|
||
/*
|
||
* 获取上行遥控指令（同步版本，使用select超时）
|
||
*
|
||
* 向通信机串口写入获取上行遥控指令，进行上行遥控查询
|
||
*
|
||
*/
|
||
void Get_S_COMM_UP_CMD(void)
|
||
{
|
||
if (S_COMM_ON_OFF != 1) return;
|
||
|
||
int result = S_UART_SEND_AND_WAIT(QUERY_TELECMD, 1, cmderPadding, 100);
|
||
|
||
if (result < 0) {
|
||
cerr << "Uplink command query timeout" << endl;
|
||
}
|
||
}
|
||
|
||
/*
|
||
* 通过S测控发送下行遥测（可选等待确认）
|
||
*
|
||
* @tele: 遥测数据
|
||
* @len: 数据长度
|
||
*/
|
||
void Send_Telemetry_From_S(uint8_t *tele, uint16_t len)
|
||
{
|
||
if (S_COMM_ON_OFF != 1) return;
|
||
|
||
sendTelemCnt++;
|
||
|
||
S_UART_TELEMETRY_SEND(SEND_TELEMETRY, len, tele);
|
||
scomm_read_with_select(100);
|
||
}
|
||
|
||
/*
|
||
* 接收数据处理函数
|
||
*
|
||
* @data: 接收到的数据
|
||
* @size: 数据大小
|
||
*
|
||
* 返回值: 0-成功处理, -1-处理失败, -2-需要更多数据
|
||
*/
|
||
static int process_received_data(uint8_t *data, uint16_t size)
|
||
{
|
||
uint16_t i = 0;
|
||
uint8_t cmd = 0;
|
||
uint16_t frame_len = 0;
|
||
uint8_t checksum0 = 0, checksum1 = 0;
|
||
|
||
if (size < 2) {
|
||
return -2;
|
||
}
|
||
|
||
// 查找有效的帧头
|
||
int header_pos = -1;
|
||
for (i = 0; i < size - 1; i++) {
|
||
if (data[i] == 0xEB && data[i+1] == 0x90) {
|
||
header_pos = i;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (header_pos < 0) {
|
||
if (size > 10) {
|
||
scommHeaderErrCnt++;
|
||
cerr << "Frame header not found" << endl;
|
||
return -1;
|
||
}
|
||
return -2;
|
||
}
|
||
|
||
if (header_pos > 0) {
|
||
memmove(S_Telec1_Data, data + header_pos, size - header_pos);
|
||
size -= header_pos;
|
||
} else {
|
||
memcpy(S_Telec1_Data, data, size);
|
||
}
|
||
|
||
if (size < 6) {
|
||
return -2;
|
||
}
|
||
|
||
cmd = S_Telec1_Data[2];
|
||
frame_len = (uint16_t)(S_Telec1_Data[3] << 8) | (S_Telec1_Data[4]);
|
||
|
||
if (size < (frame_len + 6)) {
|
||
return -2;
|
||
}
|
||
|
||
/* 检查帧长度是否合理 */
|
||
if (frame_len > 500) {
|
||
cerr << "Frame length too large: " << frame_len << endl;
|
||
return -1;
|
||
}
|
||
|
||
checksum0 = S_Telec1_Data[frame_len + 5];
|
||
|
||
/* 计算校验和 */
|
||
checksum1 = 0;
|
||
for (i = 2; i < (frame_len + 5); i++) {
|
||
checksum1 += S_Telec1_Data[i];
|
||
}
|
||
|
||
if (checksum1 != checksum0) {
|
||
scommCheckErrCnt++;
|
||
cerr << "Checksum error: calc=0x" << hex << (int)checksum1
|
||
<< " recv=0x" << hex << (int)checksum0 << endl;
|
||
return -1;
|
||
}
|
||
|
||
/* 根据命令类型处理 */
|
||
switch (cmd) {
|
||
case 0xF0: /* 常规遥测数据 */
|
||
if (size >= (13 + frame_len - 10)) {
|
||
memcpy((uint8_t *)&S_TELE, (uint8_t *)&S_Telec1_Data[13],
|
||
frame_len - 10);
|
||
if (sendGetSelfTelemCmderCnt > 0) {
|
||
sendGetSelfTelemCmderCnt--;
|
||
}
|
||
|
||
if (frame_len - 10 > 0) {
|
||
ComHS_telemetry_Pub((uint8_t *)&S_TELE, fastdds_dest,
|
||
sizeof(S_Comm_telemetry_data_t));
|
||
}
|
||
}
|
||
break;
|
||
|
||
case 0xE1: /* 遥控指令应答 */
|
||
if ((S_Telec1_Data[5] == 0xAA) && (S_Telec1_Data[6] == 0x8C)) {
|
||
sendCmderSuccessCnt++;
|
||
} else {
|
||
sendCmderErrCnt++;
|
||
cerr << "Command execution failed" << endl;
|
||
}
|
||
break;
|
||
|
||
case 0xB4: /* 上行遥控指令 */
|
||
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);
|
||
}
|
||
}
|
||
break;
|
||
|
||
case 0xA5: /* 遥测确认 */
|
||
if ((S_Telec1_Data[5] == 0xAA) && (S_Telec1_Data[6] == 0x50)) {
|
||
sendTelemSuccessCnt = 1;
|
||
}
|
||
break;
|
||
|
||
default:
|
||
scommResIDErrCnt++;
|
||
cerr << "Unknown command ID: 0x" << hex << (int)cmd << endl;
|
||
return -1;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
/*
|
||
* 启动SCOMM服务
|
||
*
|
||
* @dev_telec: 遥控串口设备
|
||
* @dev_telem: 遥测串口设备
|
||
* @baudrate: 波特率
|
||
*/
|
||
void start_scomm_service(const char *dev_telec, const char *dev_telem, int baudrate)
|
||
{
|
||
if (scomm_uart_init(dev_telec, dev_telem, baudrate) < 0) {
|
||
S_COMM_ON_OFF = 0;
|
||
cerr << "S_COMM UART init failed" << endl;
|
||
return;
|
||
}
|
||
|
||
S_COMM_ON_OFF = 1;
|
||
ComHS_init(0, "S_Comm_Hardware_Service");
|
||
}
|
||
|
||
/*
|
||
* 停止SCOMM服务
|
||
*/
|
||
void stop_scomm_service(void)
|
||
{
|
||
g_running = 0;
|
||
|
||
if (fd_telecontrol >= 0) {
|
||
close(fd_telecontrol);
|
||
fd_telecontrol = -1;
|
||
}
|
||
if (fd_telemetry >= 0) {
|
||
close(fd_telemetry);
|
||
fd_telemetry = -1;
|
||
}
|
||
|
||
if (ComHS_part) {
|
||
delete ComHS_part;
|
||
ComHS_part = nullptr;
|
||
}
|
||
} |