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

960 lines
32 KiB
C++
Raw Permalink 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 <fstream>
#include <jsoncpp/json/json.h>
#include "SimMsg.h"
#include "Star_sensorHS.h"
using namespace std;
/* ------------------ 全局变量 ------------------ */
// FastDDS硬件服务化相关
SimMsg* StarHS_part = nullptr;
string servername = "Star_sensor_Hardware_Service";
string topic_name_cmd = "Command";
string topic_name_tlm = "Telemetry";
// 星敏设备相关
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;
/* ------------------ 解析json配置文件 ------------------ */
int parse_config_file(const char* filename, StarConfig star_configs[], int max_stars) {
ifstream config_file(filename);
if (!config_file.is_open()) {
cerr << "[ERROR] Cannot open config file: " << filename << endl;
return -1;
}
// 解析JSON
Json::CharReaderBuilder reader_builder;
Json::Value root;
string errs;
if (!Json::parseFromStream(reader_builder, config_file, &root, &errs)) {
cerr << "[ERROR] Failed to parse JSON: " << errs << endl;
return -1;
}
// 读取星敏配置
int star_count = 0;
if (root.isMember("star_sensors") && root["star_sensors"].isArray()) {
const Json::Value& stars = root["star_sensors"];
for (unsigned int i = 0; i < stars.size() && star_count < max_stars; i++) {
const Json::Value& star = stars[i];
// 检查是否启用
if (star.isMember("enabled") && !star["enabled"].asBool()) {
continue; // 跳过禁用的星敏
}
// 读取配置参数
StarConfig config;
// 星敏类型
string type_str = star.isMember("type") ? star["type"].asString() : "nano";
if (type_str == "nano") {
config.type = STAR_TYPE_NANO;
} else if (type_str == "pico") {
config.type = STAR_TYPE_PICO;
} else {
cerr << "[WARNING] Unknown star type: " << type_str << ", defaulting to nano" << endl;
config.type = STAR_TYPE_NANO;
}
// 串口设备
config.device = star.isMember("device") ? star["device"].asString() : "";
if (config.device.empty()) {
cerr << "[WARNING] Star sensor " << i << " has no device specified, skipping" << endl;
continue;
}
// 波特率
config.baudrate = star.isMember("baudrate") ? star["baudrate"].asInt() : 115200;
// 星敏编号
config.star_num = star.isMember("star_num") ? star["star_num"].asInt() : (star_count + 1);
config.enabled = true;
star_configs[star_count] = config;
star_count++;
}
} else {
cerr << "[ERROR] No 'star_sensors' array found in config file" << endl;
return -1;
}
return star_count;
}
/* ------------------ 日志函数 ------------------ */
void StarHSWriteLog(const std::string &msg)
{
std::cout << "[StarHS] " << msg << std::endl;
}
/* ------------------ FastDDS初始化 ------------------ */
void Fastdds_init(uint8_t domainid, string appname)
{
vector<string> parameters;
string expression = "dest = '"+ servername + "'";
if (nullptr == StarHS_part)
{
StarHS_part = new SimMsg(domainid, 3000, appname, StarHSWriteLog);
// 创建发布者和订阅者
StarHS_part->create_pub(topic_name_tlm);
// 注册回调函数
StarHS_part->create_sub(topic_name_cmd, command_callback, expression, parameters);
StarHS_part->create_sub(topic_name_tlm, telemetry_callback, expression, parameters);
}
}
/* ------------------ 命令回调函数 ------------------ */
void command_callback(string src, string dest, string type,
string reserve1, string reserve2,
vector<uint8_t>& data) {
cout << "[INFO] Command received from " << src << " to " << dest << endl;
// 解析命令数据
if (data.size() >= 4) {
uint8_t star_type = data[0];
uint8_t star_num = data[1];
uint8_t cmd = data[2];
uint8_t param = data[3];
cout << "[INFO] Executing: star_type=" << (star_type == 0 ? "NANO" : "PICO")
<< ", num=" << (int)star_num << ", cmd=" << (int)cmd << ", param=" << (int)param << endl;
// 加锁保护串口访问
pthread_mutex_lock(&cache_mutex);
// 执行命令
int result = send_star_cmd((star_type_t)star_type, star_num, (enum SENSOR_Cmder)cmd, param);
pthread_mutex_unlock(&cache_mutex);
if (result != SAT_ERR_OK) {
cerr << "[ERROR] Send command failed: " << result << endl;
} else {
cout << "[OK] Command executed successfully" << endl;
}
} else {
cerr << "[ERROR] Invalid command data size: " << data.size() << endl;
}
}
/* ------------------ 遥测回调函数 ------------------ */
void telemetry_callback(string src, string dest, string type,
string reserve1, string reserve2,
vector<uint8_t>& data) {
// 解析请求：前2字节为star_type和star_num
if (data.size() >= 2) {
star_type_t star_type = (star_type_t)data[0];
uint8_t star_num = data[1];
enum SENSOR_Cmder cmd = (star_type == STAR_TYPE_NANO) ? CMD_TLM_1 : CMD_TLM_2;
cout << "[INFO] Reading telemetry: star_type="
<< (star_type == STAR_TYPE_NANO ? "NANO" : "PICO")
<< ", num=" << (int)star_num << endl;
// 加锁保护串口访问
pthread_mutex_lock(&cache_mutex);
// 读取数据
Star_sensorHS_Frame response_frame;
int result = read_single_star_data(star_type, star_num, cmd, &response_frame);
pthread_mutex_unlock(&cache_mutex);
if (result == SAT_ERR_OK) {
// 发布遥测响应
Star_sensorHS_telemetry_Pub((uint8_t *)&response_frame, src, sizeof(Star_sensorHS_Frame));
cout << "[OK] Telemetry data sent to " << src << endl;
} else {
cerr << "[ERROR] Failed to read star data: " << result << endl;
// 即使读取失败，也发送错误状态
memset(&response_frame, 0, sizeof(Star_sensorHS_Frame));
response_frame.on_off_status = 0; // 标记为错误状态
Star_sensorHS_telemetry_Pub((uint8_t *)&response_frame, src, sizeof(Star_sensorHS_Frame));
}
} else {
cerr << "[ERROR] Invalid telemetry request data size: " << data.size() << endl;
}
}
/* ------------------ 遥测发布 ------------------ */
void Star_sensorHS_telemetry_Pub(uint8_t* data, const string& dest, uint16_t len) {
StarHS_part->publish(topic_name_tlm, servername, dest, "telemetry", data, len);
}
/* ------------------ 星敏设备初始化 ------------------ */
int star_devices_init(void) {
pthread_mutex_init(&cache_mutex, NULL);
// 初始化纳型星敏
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;
}
/* ------------------ 串口初始化 ------------------ */
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 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 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: // UTC时间（40bit，量纲ms）
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: // 星图子块灰度均值0~5
send_cmd[0] = 0X74;
send_cmd[1] = 0XAC;
send_cmd[2] = 0X0F;
send_cmd[3] = param; // 默认为5
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_OFST_IMG_1: // 星图子块灰度均值6~100
send_cmd[0] = 0X74;
send_cmd[1] = 0XAC;
send_cmd[2] = 0X1F;
send_cmd[3] = param; // 默认为10
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_OFST_IMG_2: // 星图子块灰度均值101~150
send_cmd[0] = 0X74;
send_cmd[1] = 0XAC;
send_cmd[2] = 0X2F;
send_cmd[3] = param; // 默认为12
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_OFST_IMG_3: // 星图子块灰度均值151~200
send_cmd[0] = 0X74;
send_cmd[1] = 0XAC;
send_cmd[2] = 0X3F;
send_cmd[3] = param; // 默认为15
send_cmd[4] = 0;
for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
break;
case CMD_PAR_OFST_IMG_4: // 星图子块灰度均值201~255
send_cmd[0] = 0X74;
send_cmd[1] = 0XAC;
send_cmd[2] = 0X4F;
send_cmd[3] = param; // 默认为20
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++) { // 排除最后1字节SUM_CRC
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: // PPS校时使能开
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: // PPS使能关
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: // PPS计数清零
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: // 读取纳型遥测数据包1（64字节）
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: // 读取皮型遥测数据包2（256字节）
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;
}
/* ------------------ 读取单个星敏数据 ------------------ */
int read_single_star_data(star_type_t type, uint8_t num, enum SENSOR_Cmder cmd, Star_sensorHS_Frame *frame) {
if (type >= STAR_TYPE_MAX || num == 0 || num > MAX_STAR_NUM_PER_TYPE || !frame) {
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->send_cmd_cnt++;
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++;
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++;
update_fault_count(star);
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++;
update_fault_count(star);
continue;
}
// 检查校验和
if (checksum_verify(buf, expected_bin_len) != SAT_ERR_OK) {
star->check_err_cnt++;
update_fault_count(star);
continue;
}
// 解析数据
int parse_ret = SAT_ERR_DATA;
if (type == STAR_TYPE_NANO) {
parse_ret = parse_nano_quaternion(buf, frame);
} else if (type == STAR_TYPE_PICO) {
parse_ret = parse_pico_quaternion(buf, frame);
}
if (parse_ret == SAT_ERR_OK) {
// 填充设备状态
frame->on_off_status = star->on_off_status;
// 填充错误计数信息
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->fault_cnt = star->fault_cnt;
return SAT_ERR_OK;
} else {
fprintf(stderr, "[ERROR] Parse data failed for %s STAR%d\n",
type == STAR_TYPE_NANO ? "NANO" : "PICO", num);
star->check_err_cnt++; // 解析失败也算校验错误
update_fault_count(star);
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++;
update_fault_count(star);
// 返回错误数据
memset(frame, 0, sizeof(Star_sensorHS_Frame));
frame->on_off_status = star->on_off_status;
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->fault_cnt = star->fault_cnt;
return SAT_ERR_UNKNOWN;
}
/* ------------------ 数据解析纳型星敏 ------------------ */
int parse_nano_quaternion(const uint8_t *buf, Star_sensorHS_Frame *frame) {
if (!buf || !frame) 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]);
frame->q0 = (float)raw_q0 / 2147483647.5f;
frame->q1 = (float)raw_q1 / 2147483647.5f;
frame->q2 = (float)raw_q2 / 2147483647.5f;
frame->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]);
frame->wx = (float)raw_omega_x * 2e-13f;
frame->wy = (float)raw_omega_y * 2e-13f;
frame->wz = (float)raw_omega_z * 2e-13f;
frame->bitpara_state.ext = buf[20];
frame->star_num_d = buf[35];
return SAT_ERR_OK;
}
/* ------------------ 数据解析皮型星敏 ------------------ */
int parse_pico_quaternion(const uint8_t *buf, Star_sensorHS_Frame *frame) {
if (!buf || !frame) 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]);
frame->q0 = (float)raw_q0 / 2147483647.0f;
frame->q1 = (float)raw_q1 / 2147483647.0f;
frame->q2 = (float)raw_q2 / 2147483647.0f;
frame->q3 = (float)raw_q3 / 2147483647.0f;
frame->star_num_d = (buf[34] >> 1) & 0x7F;
frame->bitpara_state.str_state1_data1 = buf[31] & 0x0F;
frame->bitpara_state.str_state1_data2 = (buf[31] >> 4) & 0x03;
frame->bitpara_state.str_state1_data3 = (buf[32] >> 6) & 0x03;
frame->wx = 0.0f;
frame->wy = 0.0f;
frame->wz = 0.0f;
return SAT_ERR_OK;
}
/* ------------------ 硬件服务化清理 ------------------ */
void Star_sensorHS_cleanup(void) {
cout << "[INFO] Star_sensorHS cleaning up..." << endl;
// 清理FastDDS组件
if (StarHS_part != nullptr) {
delete StarHS_part;
StarHS_part = nullptr;
}
// 关闭所有串口
for (uint8_t i = 0; i < MAX_STAR_NUM_PER_TYPE; i++) {
if (nano_stars[i].fd > 0) {
close(nano_stars[i].fd);
nano_stars[i].fd = -1;
}
if (pico_stars[i].fd > 0) {
close(pico_stars[i].fd);
pico_stars[i].fd = -1;
}
}
// 销毁互斥锁
pthread_mutex_destroy(&cache_mutex);
}
Reference in New Issue View Git Blame Copy Permalink