xb/----
0
0
Fork 0
Code Issues Activity

Auto commit from DCSP - 2026/1/5 20:36:13

Browse Source
This commit is contained in:
xb
2026-01-05 20:36:13 +08:00
commit 292c313b15
49 changed files with 6994 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

838
星敏硬件服务化/Star_sensorHS.cpp Normal file
View File

@@ -0,0 +1,838 @@
#include <iostream>
#include <cstring>
#include <cstdlib>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <pthread.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <sys/select.h>
#include <errno.h>
#include <signal.h>
#include <chrono>
#include <arpa/inet.h>
#include <string>
#include <vector>
#include "Star_sensorHS.h"
/* ------------------ 全局变量 ------------------ */
// 全局设备管理
Star_Device nano_stars[MAX_STAR_NUM_PER_TYPE] = {0};
Star_Device pico_stars[MAX_STAR_NUM_PER_TYPE] = {0};
uint8_t nano_star_count = 0;
uint8_t pico_star_count = 0;
// 全局UTC时间变量
static uint8_t UTC_TIME_SET[5] = {0};
// 电星模数据结构体
static CMD_ESD_S CMD_ESD_S_DATA = {0};
// 互斥锁
static pthread_mutex_t cache_mutex = PTHREAD_MUTEX_INITIALIZER;
// 发布配置
static int star_pub_interval_ms = 500;
// FastDDS回调函数
static CommandCallbackFunc command_callback_func = nullptr;
static TelemetryCallbackFunc telemetry_callback_func = nullptr;
/* ------------------ 工具函数 ------------------ */
static int checksum_verify(const uint8_t *data, int len) {
if (len <= 1 || data == NULL) return SAT_ERR_DATA;
uint8_t checksum = 0;
for (int i = 0; i < len - 1; ++i) checksum += data[i];
if (checksum == data[len - 1]) return SAT_ERR_OK;
fprintf(stderr, "[ERROR] Checksum error: calculated=0x%02X, received=0x%02X\n", checksum, data[len - 1]);
return SAT_ERR_CHECKSUM;
}
/* ------------------ 串口初始化 ------------------ */
int star_uart_init(star_type_t type, uint8_t num, const char *dev, int baudrate) {
if (type >= STAR_TYPE_MAX || num == 0 || num > MAX_STAR_NUM_PER_TYPE) {
fprintf(stderr, "[ERROR] Invalid star type=%d or num=%d\n", type, num);
return SAT_ERR_DATA;
}
uint8_t idx = num - 1;
Star_Device *star = (type == STAR_TYPE_NANO) ? &nano_stars[idx] : &pico_stars[idx];
// 如果已经打开，先关闭
if (star->fd > 0) {
close(star->fd);
star->fd = -1;
}
// 打开串口
star->fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (star->fd < 0) {
perror("[ERROR] Open star serial failed");
return SAT_ERR_SERIAL;
}
// 配置串口参数
struct termios options;
if (tcgetattr(star->fd, &options) < 0) {
perror("[ERROR] Get serial attributes failed");
close(star->fd);
star->fd = -1;
return SAT_ERR_SERIAL;
}
// 设置波特率
speed_t baud = B115200;
switch (baudrate) {
case 9600: baud = B9600; break;
case 19200: baud = B19200; break;
case 38400: baud = B38400; break;
case 57600: baud = B57600; break;
case 115200: baud = B115200; break;
case 230400: baud = B230400; break;
case 460800: baud = B460800; break;
case 921600: baud = B921600; break;
default: baud = B115200;
}
cfsetispeed(&options, baud);
cfsetospeed(&options, baud);
// 8位数据位，无奇偶校验，1位停止位
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
// 启用接收
options.c_cflag |= CREAD | CLOCAL;
// 禁用软件流控
options.c_iflag &= ~(IXON | IXOFF | IXANY);
// 原始模式
options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
options.c_oflag &= ~OPOST;
// 设置超时
options.c_cc[VMIN] = 0;
options.c_cc[VTIME] = 10; // 1秒超时
if (tcsetattr(star->fd, TCSANOW, &options) < 0) {
perror("[ERROR] Set serial attributes failed");
close(star->fd);
star->fd = -1;
return SAT_ERR_SERIAL;
}
// 刷新缓冲区
tcflush(star->fd, TCIOFLUSH);
// 初始化设备信息
star->dev = dev;
star->baudrate = baudrate;
star->on_off_status = 1;
printf("[OK] %s STAR%d UART init: dev=%s, baud=%d, fd=%d\n",
type == STAR_TYPE_NANO ? "NANO" : "PICO", num, dev, baudrate, star->fd);
return SAT_ERR_OK;
}
/* ------------------ 发送星敏命令 ------------------ */
int send_star_cmd(star_type_t type, uint8_t num, enum SENSOR_Cmder cmd, uint8_t param) {
if (type >= STAR_TYPE_MAX || num == 0 || num > MAX_STAR_NUM_PER_TYPE) return SAT_ERR_DATA;
uint8_t idx = num - 1;
Star_Device *star = (type == STAR_TYPE_NANO) ? &nano_stars[idx] : &pico_stars[idx];
if (star->fd < 0) {
fprintf(stderr, "[ERROR] Serial not open for %s STAR%d\n",
type == STAR_TYPE_NANO ? "NANO" : "PICO", num);
return SAT_ERR_SERIAL;
}
uint8_t send_cmd[260] = {0};
int cmd_len = 5;
uint8_t i = 0;
switch (cmd) {
case CMD_MCT_OST:
send_cmd[0] = 0X74; send_cmd[1] = 0XA0; send_cmd[2] = 0X00; send_cmd[3] = 0X00; send_cmd[4] = 0X14;
break;
case CMD_MCT_ESI:
send_cmd[0] = 0X74; send_cmd[1] = 0XA0; send_cmd[2] = 0X00; send_cmd[3] = 0X03; send_cmd[4] = 0X17;
break;
case CMD_SAT:
send_cmd[0] = 0X74; send_cmd[1] = 0XA3;
memcpy(send_cmd + 2, UTC_TIME_SET, 5);
send_cmd[7] = 0;
for (i = 0; i < 7; i++) send_cmd[7] += send_cmd[i];
cmd_len = 8;
break;
case CMD_WM_NORM:
send_cmd[0] = 0X74; send_cmd[1] = 0XA4; send_cmd[2] = 0X01; send_cmd[3] = 0X01; send_cmd[4] = 0X1A;
break;
case CMD_WM_ALSKY:
send_cmd[0] = 0X74; send_cmd[1] = 0XA4; send_cmd[2] = 0X02; send_cmd[3] = 0X02; send_cmd[4] = 0X1C;
break;
case CMD_WM_TST:
send_cmd[0] = 0X74; send_cmd[1] = 0XA4; send_cmd[2] = 0X04; send_cmd[3] = 0X04; send_cmd[4] = 0X20;
break;
case CMD_WM_DLD:
send_cmd[0] = 0X74; send_cmd[1] = 0XA4; send_cmd[2] = 0X08; send_cmd[3] = 0X08; send_cmd[4] = 0X28;
break;
case CMD_WM_IDLE:
send_cmd[0] = 0X74; send_cmd[1] = 0XA4; send_cmd[2] = 0XF0; send_cmd[3] = 0XF0; send_cmd[4] = 0XF8;
break;
case CMD_WM_HK:
send_cmd[0] = 0X74; send_cmd[1] = 0XA4; send_cmd[2] = 0X0A; send_cmd[3] = 0X0A; send_cmd[4] = 0X2C;
break;
case CMD_WM_RST:
send_cmd[0] = 0X74; send_cmd[1] = 0XA4; send_cmd[2] = 0XFF; send_cmd[3] = 0XFF; send_cmd[4] = 0X16;
break;
case CMD_SEN_EXPO:
send_cmd[0] = 0X74; send_cmd[1] = 0XA9; send_cmd[2] = 0X60; send_cmd[3] = param;
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_SEN_AGAIN:
send_cmd[0] = 0X74; send_cmd[1] = 0XA9; send_cmd[2] = 0X30; send_cmd[3] = param;
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_SEN_DGAIN:
send_cmd[0] = 0X74; send_cmd[1] = 0XA9; send_cmd[2] = 0X90; send_cmd[3] = param;
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_ADE:
send_cmd[0] = 0X74; send_cmd[1] = 0XAC; send_cmd[2] = 0X03; send_cmd[3] = param;
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_SSE:
send_cmd[0] = 0X74; send_cmd[1] = 0XAC; send_cmd[2] = 0X05; send_cmd[3] = param;
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_TKR:
send_cmd[0] = 0X74; send_cmd[1] = 0XAC; send_cmd[2] = 0X06; send_cmd[3] = param;
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_ADN:
send_cmd[0] = 0X74; send_cmd[1] = 0XAC; send_cmd[2] = 0X09; send_cmd[3] = param;
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_OFST:
send_cmd[0] = 0X74; send_cmd[1] = 0XAC; send_cmd[2] = 0X0A; send_cmd[3] = param;
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_OFST_IMG_0:
send_cmd[0] = 0X74; send_cmd[1] = 0XAC; send_cmd[2] = 0X0F; send_cmd[3] = param;
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_OFST_IMG_1:
send_cmd[0] = 0X74; send_cmd[1] = 0XAC; send_cmd[2] = 0X1F; send_cmd[3] = param;
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_OFST_IMG_2:
send_cmd[0] = 0X74; send_cmd[1] = 0XAC; send_cmd[2] = 0X2F; send_cmd[3] = param;
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_OFST_IMG_3:
send_cmd[0] = 0X74; send_cmd[1] = 0XAC; send_cmd[2] = 0X3F; send_cmd[3] = param;
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_OFST_IMG_4:
send_cmd[0] = 0X74; send_cmd[1] = 0XAC; send_cmd[2] = 0X4F; send_cmd[3] = param;
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_WIN:
send_cmd[0] = 0X74; send_cmd[1] = 0XAC; send_cmd[2] = 0X0E; send_cmd[3] = param;
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_RST:
send_cmd[0] = 0X74; send_cmd[1] = 0XAC; send_cmd[2] = 0X0B; send_cmd[3] = param;
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_ESD:
{
CMD_ESD_S_DATA.FRAME_HEAD = htons(0x7456);
CMD_ESD_S_DATA.FRAME_ID = htons(0x0001);
memcpy(CMD_ESD_S_DATA.UTC, UTC_TIME_SET, 5);
CMD_ESD_S_DATA.STAR_NUM_S[0] = 1;
memset(&CMD_ESD_S_DATA.SS_SCP_X, 0, sizeof(CMD_ESD_S) - 8);
uint8_t crc = 0;
uint8_t *data_ptr = (uint8_t *)&CMD_ESD_S_DATA;
for (uint32_t i = 0; i < sizeof(CMD_ESD_S) - 1; i++) {
crc += data_ptr[i];
}
CMD_ESD_S_DATA.SUM_CRC = crc;
if (write(star->fd, &CMD_ESD_S_DATA, sizeof(CMD_ESD_S)) != sizeof(CMD_ESD_S)) {
perror("[ERROR] Send ESD cmd failed");
star->send_cmd_cnt++;
return SAT_ERR_SEND_CMD;
}
star->send_cmd_cnt = 0;
return SAT_ERR_OK;
}
case CMD_MCT_FLT_ON:
send_cmd[0] = 0X74; send_cmd[1] = 0XA0; send_cmd[2] = 0X00; send_cmd[3] = 0x05; send_cmd[4] = 0x19;
break;
case CMD_MCT_FLT_OFF:
send_cmd[0] = 0X74; send_cmd[1] = 0XA0; send_cmd[2] = 0X00; send_cmd[3] = 0x07; send_cmd[4] = 0x1B;
break;
case CMD_MCT_PPS_ON:
send_cmd[0] = 0X74; send_cmd[1] = 0XA0; send_cmd[2] = 0X00; send_cmd[3] = 0x0A; send_cmd[4] = 0x1E;
break;
case CMD_MCT_PPS_OFF:
send_cmd[0] = 0X74; send_cmd[1] = 0XA0; send_cmd[2] = 0X00; send_cmd[3] = 0x0C; send_cmd[4] = 0x20;
break;
case CMD_MCT_PPS_CNT_RST:
send_cmd[0] = 0X74; send_cmd[1] = 0XA0; send_cmd[2] = 0X00; send_cmd[3] = 0x0E; send_cmd[4] = 0x22;
break;
case CMD_TLM_1:
if (type != STAR_TYPE_NANO) return SAT_ERR_DATA;
send_cmd[0] = 0X74; send_cmd[1] = 0XA1; send_cmd[2] = 0X00; send_cmd[3] = 0x00; send_cmd[4] = 0x15;
break;
case CMD_TLM_2:
if (type != STAR_TYPE_PICO) return SAT_ERR_DATA;
send_cmd[0] = 0X74; send_cmd[1] = 0XA1; send_cmd[2] = 0X00; send_cmd[3] = 0x05; send_cmd[4] = 0x1A;
break;
default:
fprintf(stderr, "[ERROR] Unsupported cmd=%d for %s STAR%d\n", cmd,
type == STAR_TYPE_NANO ? "NANO" : "PICO", num);
return SAT_ERR_DATA;
}
if (write(star->fd, send_cmd, cmd_len) != cmd_len) {
perror("[ERROR] Send star cmd failed");
star->send_cmd_cnt++;
return SAT_ERR_SEND_CMD;
}
star->send_cmd_cnt = 0;
return SAT_ERR_OK;
}
/* ------------------ 数据解析纳型星敏 ------------------ */
static int parse_nano_quaternion(const uint8_t *buf, STAR_INFO *info) {
if (!buf || !info) return SAT_ERR_DATA;
int32_t raw_q0 = (int32_t)((buf[4] << 24) | (buf[5] << 16) | (buf[6] << 8) | buf[7]);
int32_t raw_q1 = (int32_t)((buf[8] << 24) | (buf[9] << 16) | (buf[10] << 8) | buf[11]);
int32_t raw_q2 = (int32_t)((buf[12] << 24) | (buf[13] << 16) | (buf[14] << 8) | buf[15]);
int32_t raw_q3 = (int32_t)((buf[16] << 24) | (buf[17] << 16) | (buf[18] << 8) | buf[19]);
info->q0 = (float)raw_q0 / 2147483647.5f;
info->q1 = (float)raw_q1 / 2147483647.5f;
info->q2 = (float)raw_q2 / 2147483647.5f;
info->q3 = (float)raw_q3 / 2147483647.5f;
int16_t raw_omega_x = (int16_t)((buf[45] << 8) | buf[46]);
int16_t raw_omega_y = (int16_t)((buf[47] << 8) | buf[48]);
int16_t raw_omega_z = (int16_t)((buf[49] << 8) | buf[50]);
info->wx = (float)raw_omega_x * 2e-13f;
info->wy = (float)raw_omega_y * 2e-13f;
info->wz = (float)raw_omega_z * 2e-13f;
info->bitpara_state.ext = buf[20];
info->star_num_d = buf[35];
return SAT_ERR_OK;
}
/* ------------------ 数据解析皮型星敏 ------------------ */
static int parse_pico_quaternion(const uint8_t *buf, STAR_INFO *info) {
if (!buf || !info) return SAT_ERR_DATA;
int32_t raw_q1 = (int32_t)((buf[7] << 24) | (buf[6] << 16) | (buf[5] << 8) | buf[4]);
int32_t raw_q2 = (int32_t)((buf[11] << 24) | (buf[10] << 16) | (buf[9] << 8) | buf[8]);
int32_t raw_q3 = (int32_t)((buf[15] << 24) | (buf[14] << 16) | (buf[13] << 8) | buf[12]);
int32_t raw_q0 = (int32_t)((buf[19] << 24) | (buf[18] << 16) | (buf[17] << 8) | buf[16]);
info->q0 = (float)raw_q0 / 2147483647.0f;
info->q1 = (float)raw_q1 / 2147483647.0f;
info->q2 = (float)raw_q2 / 2147483647.0f;
info->q3 = (float)raw_q3 / 2147483647.0f;
info->star_num_d = (buf[34] >> 1) & 0x7F;
info->bitpara_state.str_state1_data1 = buf[31] & 0x0F;
info->bitpara_state.str_state1_data2 = (buf[31] >> 4) & 0x03;
info->bitpara_state.str_state1_data3 = (buf[32] >> 6) & 0x03;
info->wx = 0.0f;
info->wy = 0.0f;
info->wz = 0.0f;
return SAT_ERR_OK;
}
/* ------------------ 读取单个星敏数据 ------------------ */
int read_single_star_data(star_type_t type, uint8_t num, enum SENSOR_Cmder cmd, STAR_INFO *info) {
if (type >= STAR_TYPE_MAX || num == 0 || num > MAX_STAR_NUM_PER_TYPE || !info) {
return SAT_ERR_DATA;
}
uint8_t idx = num - 1;
Star_Device *star = (type == STAR_TYPE_NANO) ? &nano_stars[idx] : &pico_stars[idx];
if (star->fd < 0) {
fprintf(stderr, "[ERROR] Serial not open for %s STAR%d\n",
type == STAR_TYPE_NANO ? "NANO" : "PICO", num);
return SAT_ERR_SERIAL;
}
int expected_bin_len = 0;
if (cmd == CMD_TLM_1 && type == STAR_TYPE_NANO) {
expected_bin_len = 64;
} else if (cmd == CMD_TLM_2 && type == STAR_TYPE_PICO) {
expected_bin_len = 256;
} else {
fprintf(stderr, "[ERROR] Unknown frame length for cmd=%d, type=%d\n", cmd, type);
return SAT_ERR_DATA;
}
const int MAX_RETRY = 10;
uint8_t buf[MAX_BUF_SIZE] = {0};
for (int retry = 0; retry < MAX_RETRY; ++retry) {
// 清空串口缓冲区
tcflush(star->fd, TCIOFLUSH);
memset(buf, 0, sizeof(buf));
int total_read = 0;
// 发送命令
int send_ret = send_star_cmd(type, num, cmd, 0);
if (send_ret != SAT_ERR_OK) {
fprintf(stderr, "[ERROR] Retry %d: send cmd failed\n", retry);
star_rpc_update_error(type, num, SAT_ERR_SEND_CMD);
continue;
}
// 设置超时
struct timeval start_time, current_time;
gettimeofday(&start_time, NULL);
const long TOTAL_TIMEOUT_MS = 1000;
// 读取数据
while (total_read < expected_bin_len && total_read < MAX_BUF_SIZE) {
gettimeofday(&current_time, NULL);
long elapsed_ms = (current_time.tv_sec - start_time.tv_sec) * 1000 +
(current_time.tv_usec - start_time.tv_usec) / 1000;
if (elapsed_ms > TOTAL_TIMEOUT_MS) {
fprintf(stderr, "[ERROR] Timeout: read %d/%d bytes\n", total_read, expected_bin_len);
star->send_cmd_cnt++;
star_rpc_update_error(type, num, SAT_ERR_TIMEOUT);
break;
}
// 使用select等待数据
struct timeval timeout = {0, 100000};
fd_set readfds;
FD_ZERO(&readfds);
FD_SET(star->fd, &readfds);
int ret = select(star->fd + 1, &readfds, NULL, NULL, &timeout);
if (ret < 0) {
perror("[ERROR] Select failed");
return SAT_ERR_SELECT;
} else if (ret == 0) {
continue; // 超时，继续等待
}
// 读取数据
if (FD_ISSET(star->fd, &readfds)) {
int bytes = read(star->fd, buf + total_read, expected_bin_len - total_read);
if (bytes > 0) {
total_read += bytes;
} else if (bytes == 0) {
usleep(10000); // 没有数据，等待10ms
} else {
perror("[ERROR] Read star data failed");
return SAT_ERR_SERIAL;
}
}
}
// 检查是否超时过多
if (star->send_cmd_cnt >= 10) {
fprintf(stderr, "[ERROR] Too many timeouts, resetting %s STAR%d UART\n",
type == STAR_TYPE_NANO ? "NANO" : "PICO", num);
star_uart_init(type, num, star->dev, star->baudrate);
star->uart_reset_cnt++;
star->send_cmd_cnt = 0;
continue;
}
// 检查数据长度
if (total_read < expected_bin_len) {
fprintf(stderr, "[ERROR] Frame too short: %d/%d bytes\n", total_read, expected_bin_len);
star->length_err_cnt++;
star_rpc_update_error(type, num, SAT_ERR_LEN);
continue;
}
// 检查帧头
int header_valid = 0;
if (type == STAR_TYPE_NANO && cmd == CMD_TLM_1) {
header_valid = (buf[0] == 0x8A && buf[1] == 0xA1);
} else if (type == STAR_TYPE_PICO && cmd == CMD_TLM_2) {
header_valid = (buf[0] == 0xEB && buf[1] == 0x50 && buf[2] == 0x22);
}
if (!header_valid) {
fprintf(stderr, "[ERROR] Invalid frame header for %s STAR%d\n",
type == STAR_TYPE_NANO ? "NANO" : "PICO", num);
star->header_err_cnt++;
star_rpc_update_error(type, num, SAT_ERR_HEADER);
continue;
}
// 检查校验和
if (checksum_verify(buf, expected_bin_len) != SAT_ERR_OK) {
star->check_err_cnt++;
star_rpc_update_error(type, num, SAT_ERR_CHECKSUM);
continue;
}
// 解析数据
int parse_ret = SAT_ERR_DATA;
if (type == STAR_TYPE_NANO) {
parse_ret = parse_nano_quaternion(buf, info);
} else if (type == STAR_TYPE_PICO) {
parse_ret = parse_pico_quaternion(buf, info);
}
if (parse_ret == SAT_ERR_OK) {
return SAT_ERR_OK;
} else {
fprintf(stderr, "[ERROR] Parse data failed for %s STAR%d\n",
type == STAR_TYPE_NANO ? "NANO" : "PICO", num);
star_rpc_update_error(type, num, SAT_ERR_DATA);
continue;
}
}
fprintf(stderr, "[ERROR] Read failed after %d retries for %s STAR%d\n",
MAX_RETRY, type == STAR_TYPE_NANO ? "NANO" : "PICO", num);
star_uart_init(type, num, star->dev, star->baudrate);
star->uart_reset_cnt++;
star_rpc_update_error(type, num, SAT_ERR_UNKNOWN);
return SAT_ERR_UNKNOWN;
}
/* ------------------ 构建数据帧 ------------------ */
static void build_star_data_frame(star_type_t type, uint8_t num, const STAR_INFO *info, StarDataFrame *frame) {
if (!frame || !info) return;
memset(frame, 0, sizeof(StarDataFrame));
// 填充帧头
frame->header.frame_type = 0; // 数据帧
frame->header.star_type = type;
frame->header.star_num = num;
frame->header.cmd = CMD_TLM_1; // 默认遥测包1
frame->header.param = 0;
frame->header.data_len = sizeof(StarDataFrame) - sizeof(StarFrameHeader);
struct timeval tv;
gettimeofday(&tv, NULL);
frame->header.timestamp = tv.tv_sec;
frame->header.timestamp_us = tv.tv_usec;
// 填充数据
frame->q0 = info->q0;
frame->q1 = info->q1;
frame->q2 = info->q2;
frame->q3 = info->q3;
frame->wx = info->wx;
frame->wy = info->wy;
frame->wz = info->wz;
frame->star_num_d = info->star_num_d;
frame->on_off_status = (type == STAR_TYPE_NANO) ? nano_stars[num-1].on_off_status : pico_stars[num-1].on_off_status;
frame->error_flags = 0;
}
/* ------------------ 构建错误帧 ------------------ */
static void build_star_error_frame(star_type_t type, uint8_t num, int err_code, StarErrorFrame *frame) {
if (!frame) return;
memset(frame, 0, sizeof(StarErrorFrame));
// 填充帧头
frame->header.frame_type = 2; // 错误帧
frame->header.star_type = type;
frame->header.star_num = num;
frame->header.cmd = 0;
frame->header.param = 0;
frame->header.data_len = sizeof(StarErrorFrame) - sizeof(StarFrameHeader);
struct timeval tv;
gettimeofday(&tv, NULL);
frame->header.timestamp = tv.tv_sec;
frame->header.timestamp_us = tv.tv_usec;
// 填充错误信息
Star_Device *star = (type == STAR_TYPE_NANO) ? &nano_stars[num-1] : &pico_stars[num-1];
frame->header_err_cnt = star->header_err_cnt;
frame->check_err_cnt = star->check_err_cnt;
frame->length_err_cnt = star->length_err_cnt;
frame->uart_reset_cnt = star->uart_reset_cnt;
frame->error_code = err_code;
}
/* ------------------ FastDDS硬件服务化初始化 ------------------ */
int Star_sensorHS_init(int argc, const char* name) {
// 设置默认回调函数
if (command_callback_func == nullptr) {
command_callback_func = command_callback;
std::cout << "[INFO] Using internal command callback" << std::endl;
}
if (telemetry_callback_func == nullptr) {
telemetry_callback_func = telemetry_callback;
std::cout << "[INFO] Using internal telemetry callback" << std::endl;
}
return 0;
}
/* ------------------ FastDDS硬件服务化清理 ------------------ */
void Star_sensorHS_cleanup(void) {
std::cout << "[INFO] Star_sensorHS cleaning up..." << std::endl;
// 重置回调函数
command_callback_func = nullptr;
telemetry_callback_func = nullptr;
std::cout << "[OK] Star_sensorHS cleaned up" << std::endl;
}
/* ------------------ 发布遥测数据 ------------------ */
void Star_sensorHS_telemetry_Pub(uint8_t *data, const std::string& dest, uint16_t len) {
if (data == nullptr || len == 0) {
std::cerr << "[ERROR] Invalid telemetry data" << std::endl;
return;
}
// 打印调试信息（实际项目中应通过FastDDS发布）
std::cout << "[DEBUG] Telemetry publish to " << dest << ", length=" << len << " bytes" << std::endl;
// TODO: 实现FastDDS数据发布逻辑
// 实际实现：
// 1. 创建数据样本
// 2. 填充数据
// 3. 发布数据
}
/* ------------------ 设置命令回调 ------------------ */
void Star_sensorHS_set_command_callback(CommandCallbackFunc callback) {
command_callback_func = callback;
std::cout << "[INFO] Command callback set" << std::endl;
}
/* ------------------ 设置遥测回调 ------------------ */
void Star_sensorHS_set_telemetry_callback(TelemetryCallbackFunc callback) {
telemetry_callback_func = callback;
std::cout << "[INFO] Telemetry callback set" << std::endl;
}
/* ------------------ 命令帧处理回调 ------------------ */
void command_callback(std::string src, std::string dest, std::string type,
std::string reserve1, std::string reserve2,
std::vector<uint8_t>& data) {
std::cout << "[INFO] Command received from " << src << " to " << dest << std::endl;
if (data.size() < sizeof(StarCmdFrame)) {
std::cerr << "[ERROR] Invalid command frame size: " << data.size() << std::endl;
return;
}
StarCmdFrame *cmd_frame = (StarCmdFrame *)data.data();
// 解析命令
star_type_t star_type = (star_type_t)cmd_frame->header.star_type;
uint8_t star_num = cmd_frame->header.star_num;
enum SENSOR_Cmder cmd = (enum SENSOR_Cmder)cmd_frame->header.cmd;
uint8_t param = cmd_frame->header.param;
std::cout << "[INFO] Cmd: star_type=" << (star_type == STAR_TYPE_NANO ? "NANO" : "PICO")
<< ", num=" << (int)star_num << ", cmd=" << cmd << ", param=" << (int)param << std::endl;
// 执行命令
int result = send_star_cmd(star_type, star_num, cmd, param);
if (result != SAT_ERR_OK) {
std::cerr << "[ERROR] Send command failed: " << star_strerror(result) << std::endl;
}
}
/* ------------------ 遥测帧处理回调 ------------------ */
void telemetry_callback(std::string src, std::string dest, std::string type,
std::string reserve1, std::string reserve2,
std::vector<uint8_t>& data) {
std::cout << "[INFO] Telemetry request received from " << src << " to " << dest << std::endl;
if (data.size() < sizeof(StarCmdFrame)) {
std::cerr << "[ERROR] Invalid telemetry frame size: " << data.size() << std::endl;
return;
}
StarCmdFrame *cmd_frame = (StarCmdFrame *)data.data();
// 解析请求
star_type_t star_type = (star_type_t)cmd_frame->header.star_type;
uint8_t star_num = cmd_frame->header.star_num;
enum SENSOR_Cmder cmd = (enum SENSOR_Cmder)cmd_frame->header.cmd;
// 读取数据
STAR_INFO info = {0};
int result = read_single_star_data(star_type, star_num, cmd, &info);
// 构建响应帧
uint8_t response_buf[sizeof(StarDataFrame) + sizeof(StarErrorFrame)];
uint16_t response_len = 0;
if (result == SAT_ERR_OK) {
StarDataFrame data_frame;
build_star_data_frame(star_type, star_num, &info, &data_frame);
memcpy(response_buf, &data_frame, sizeof(StarDataFrame));
response_len = sizeof(StarDataFrame);
} else {
StarErrorFrame error_frame;
build_star_error_frame(star_type, star_num, result, &error_frame);
memcpy(response_buf, &error_frame, sizeof(StarErrorFrame));
response_len = sizeof(StarErrorFrame);
}
// 通过FastDDS发布响应
Star_sensorHS_telemetry_Pub(response_buf, dest, response_len);
}
/* ------------------ 错误信息更新 ------------------ */
void star_rpc_update_error(star_type_t type, uint8_t num, int err_code) {
if (type >= STAR_TYPE_MAX || num == 0 || num > MAX_STAR_NUM_PER_TYPE) return;
uint8_t idx = num - 1;
Star_Device *star = (type == STAR_TYPE_NANO) ? &nano_stars[idx] : &pico_stars[idx];
// 构建错误帧并发布
StarErrorFrame error_frame;
build_star_error_frame(type, num, err_code, &error_frame);
// 通过FastDDS发布错误帧
Star_sensorHS_telemetry_Pub((uint8_t *)&error_frame, "GNC_Service", sizeof(StarErrorFrame));
}
/* ------------------ 发布线程 ------------------ */
void *star_publish_thread(void *arg) {
(void)arg;
STAR_INFO info = {0};
printf("[INFO] Star publish thread started, interval=%d ms\n", star_pub_interval_ms);
while (1) {
pthread_mutex_lock(&cache_mutex);
// 遍历所有纳型星敏
for (uint8_t num = 1; num <= nano_star_count; num++) {
uint8_t idx = num - 1;
Star_Device *star = &nano_stars[idx];
if (star->fd < 0) continue;
int rc = read_single_star_data(STAR_TYPE_NANO, num, CMD_TLM_1, &info);
if (rc == SAT_ERR_OK) {
StarDataFrame data_frame;
build_star_data_frame(STAR_TYPE_NANO, num, &info, &data_frame);
Star_sensorHS_telemetry_Pub((uint8_t *)&data_frame, "Star_sensor_Hardware_Service", sizeof(StarDataFrame));
} else {
star_rpc_update_error(STAR_TYPE_NANO, num, rc);
}
}
// 遍历所有皮型星敏
for (uint8_t num = 1; num <= pico_star_count; num++) {
uint8_t idx = num - 1;
Star_Device *star = &pico_stars[idx];
if (star->fd < 0) continue;
int rc = read_single_star_data(STAR_TYPE_PICO, num, CMD_TLM_2, &info);
if (rc == SAT_ERR_OK) {
StarDataFrame data_frame;
build_star_data_frame(STAR_TYPE_PICO, num, &info, &data_frame);
Star_sensorHS_telemetry_Pub((uint8_t *)&data_frame, "Star_sensor_Hardware_Service", sizeof(StarDataFrame));
} else {
star_rpc_update_error(STAR_TYPE_PICO, num, rc);
}
}
pthread_mutex_unlock(&cache_mutex);
// 按配置间隔休眠
usleep(star_pub_interval_ms * 1000);
}
return NULL;
}
/* ------------------ 星敏设备初始化 ------------------ */
int star_devices_init(void) {
// 初始化纳型星敏
char *nano_devs = getenv("STAR_NANO_DEVS");
if (nano_devs) {
char *token = strtok(nano_devs, ",");
while (token && nano_star_count < MAX_STAR_NUM_PER_TYPE) {
char dev[32];
int baud;
if (sscanf(token, "%[^:]:%d", dev, &baud) == 2) {
if (star_uart_init(STAR_TYPE_NANO, nano_star_count + 1, dev, baud) == SAT_ERR_OK) {
nano_star_count++;
}
}
token = strtok(NULL, ",");
}
} else {
// 默认配置
if (star_uart_init(STAR_TYPE_NANO, 1, "/dev/ttyS0", 115200) == SAT_ERR_OK) {
nano_star_count = 1;
}
}
// 初始化皮型星敏
char *pico_devs = getenv("STAR_PICO_DEVS");
if (pico_devs) {
char *token = strtok(pico_devs, ",");
while (token && pico_star_count < MAX_STAR_NUM_PER_TYPE) {
char dev[32];
int baud;
if (sscanf(token, "%[^:]:%d", dev, &baud) == 2) {
if (star_uart_init(STAR_TYPE_PICO, pico_star_count + 1, dev, baud) == SAT_ERR_OK) {
pico_star_count++;
}
}
token = strtok(NULL, ",");
}
} else {
// 默认配置
if (star_uart_init(STAR_TYPE_PICO, 1, "/dev/ttyS1", 115200) == SAT_ERR_OK) {
pico_star_count = 1;
}
}
printf("[INFO] Initialized %d nano stars, %d pico stars\n", nano_star_count, pico_star_count);
return (nano_star_count + pico_star_count) > 0 ? 0 : -1;
}