xb/----
0
0
Fork 0
Code Issues Activity
Files
292c313b158544e73e36ad49086e55d7d7af7827
----/星敏硬件服务化/Star_sensorHS.cpp

838 lines
31 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#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;
}
Reference in New Issue View Git Blame Copy Permalink