337 lines
10 KiB
C++
337 lines
10 KiB
C++
#ifndef STAR_SENSORHS_H
|
||
#define STAR_SENSORHS_H
|
||
|
||
#include <cstdint>
|
||
#include <string>
|
||
#include <vector>
|
||
#include <cstddef>
|
||
|
||
/* ================== 错误码定义 ================== */
|
||
|
||
#ifndef SAT_ERR_OK
|
||
#define SAT_ERR_OK 0 // 成功
|
||
#endif
|
||
|
||
#ifndef SAT_ERR_SERIAL
|
||
#define SAT_ERR_SERIAL -1 // 串口未打开或无效
|
||
#endif
|
||
|
||
#ifndef SAT_ERR_SEND_CMD
|
||
#define SAT_ERR_SEND_CMD -2 // 发送命令失败
|
||
#endif
|
||
|
||
#ifndef SAT_ERR_NO_RESPONSE
|
||
#define SAT_ERR_NO_RESPONSE -3 // 无响应
|
||
#endif
|
||
|
||
#ifndef SAT_ERR_SELECT
|
||
#define SAT_ERR_SELECT -4 // select() 系统调用失败
|
||
#endif
|
||
|
||
#ifndef SAT_ERR_HEADER
|
||
#define SAT_ERR_HEADER -5 // 帧头错误
|
||
#endif
|
||
|
||
#ifndef SAT_ERR_DATA
|
||
#define SAT_ERR_DATA -6 // 数据解析失败
|
||
#endif
|
||
|
||
#ifndef SAT_ERR_UNKNOWN
|
||
#define SAT_ERR_UNKNOWN -7 // 未知错误
|
||
#endif
|
||
|
||
#ifndef SAT_ERR_LEN
|
||
#define SAT_ERR_LEN -8 // 帧长度错误
|
||
#endif
|
||
|
||
#ifndef SAT_ERR_CHECKSUM
|
||
#define SAT_ERR_CHECKSUM -9 // 校验和错误
|
||
#endif
|
||
|
||
#ifndef SAT_ERR_TIMEOUT
|
||
#define SAT_ERR_TIMEOUT -10 // 接收数据超时
|
||
#endif
|
||
|
||
/* ================== 常量定义 ================== */
|
||
#define SS_SCP_ARR_LEN 20
|
||
#define DS_SCP_ARR_LEN 20
|
||
#define MAX_STAR_NUM_PER_TYPE 8
|
||
#define MAX_BUF_SIZE 256
|
||
|
||
/* ================== 类型定义 ================== */
|
||
|
||
// 星敏子类型
|
||
typedef enum {
|
||
STAR_TYPE_NANO = 0,
|
||
STAR_TYPE_PICO,
|
||
STAR_TYPE_MAX
|
||
} star_type_t;
|
||
|
||
// 星敏配置结构体
|
||
typedef struct {
|
||
star_type_t type; // 星敏类型
|
||
std::string device; // 串口设备
|
||
int baudrate; // 波特率
|
||
bool enabled; // 是否启用
|
||
int star_num; // 星敏编号
|
||
} StarConfig;
|
||
|
||
// 星敏感器状态字位域
|
||
#pragma pack(push, 1)
|
||
typedef union {
|
||
uint8_t ext;
|
||
struct {
|
||
uint8_t str_state1_data4 : 1;
|
||
uint8_t str_state1_data3 : 1;
|
||
uint8_t str_state1_data2 : 2;
|
||
uint8_t str_state1_data1 : 4;
|
||
};
|
||
} Bitpara_State;
|
||
#pragma pack(pop)
|
||
|
||
// 统一遥测数据帧（包含数据和错误信息）
|
||
#pragma pack(push, 1)
|
||
typedef struct {
|
||
// 四元数数据
|
||
float q0, q1, q2, q3;
|
||
|
||
// 角速度
|
||
float wx, wy, wz;
|
||
|
||
// 状态信息
|
||
Bitpara_State bitpara_state; // 星敏感器状态字
|
||
uint8_t star_num_d; // 定姿星数
|
||
uint8_t on_off_status; // 设备开关状态
|
||
|
||
// 错误计数部分
|
||
uint16_t header_err_cnt; // 帧头错误累计
|
||
uint16_t check_err_cnt; // 校验错误累计
|
||
uint16_t length_err_cnt; // 长度错误累计
|
||
uint16_t uart_reset_cnt; // 串口复位累计
|
||
uint16_t fault_cnt; // 故障计数（所有错误的总和）
|
||
} Star_sensorHS_Frame;
|
||
#pragma pack(pop)
|
||
|
||
// 星敏命令枚举
|
||
enum SENSOR_Cmder {
|
||
CMD_MCT_OST=0, // 光信号激励
|
||
CMD_MCT_ESI, // 电信号激励
|
||
CMD_MCT_PD_ON, // 无
|
||
CMD_MCT_PD_OFF, // 无
|
||
CMD_SAT, // UTC时间(校时)
|
||
CMD_WM_NORM, // 正常工作流程
|
||
CMD_WM_ALSKY, // 仅工作于全天识别模式
|
||
CMD_WM_TST, // 测试模式，用于标定
|
||
CMD_WM_DLD, // 星图下传
|
||
CMD_WM_IDLE, // 待机模式
|
||
CMD_WM_HK, // 至启动模式进行在轨维护
|
||
CMD_WM_RST, // 软件复位重启
|
||
CMD_SEN_EXPO, // 曝光时间设置
|
||
CMD_SEN_AGAIN, // 模拟增益设置
|
||
CMD_SEN_DGAIN, // 数字增益设置
|
||
CMD_PAR_ADE, // 设置星对角距匹配容差
|
||
CMD_PAR_SSE, // 设置单星匹配容差
|
||
CMD_PAR_TKR, // 星跟踪半径/二次迭代容差
|
||
CMD_PAR_ADN, // 设置定姿星数
|
||
CMD_PAR_OFST, // 波门星点提取阈值偏移量
|
||
CMD_PAR_OFST_IMG_0, // 星图子块灰度均值0~5
|
||
CMD_PAR_OFST_IMG_1, // 星图子块灰度均值6~100
|
||
CMD_PAR_OFST_IMG_2, // 星图子块灰度均值101~150
|
||
CMD_PAR_OFST_IMG_3, // 星图子块灰度均值151~200
|
||
CMD_PAR_OFST_IMG_4, // 星图子块灰度均值201~255
|
||
CMD_PAR_WIN, // 波门图像大小
|
||
CMD_PAR_RST, // 软件参数恢复默认值
|
||
CMD_ESD, // 电星模数据注入命令
|
||
CMD_MCT_FLT_ON, // 姿态滤波开
|
||
CMD_MCT_FLT_OFF, // 姿态滤波关
|
||
CMD_MCT_PPS_ON, // PPS校时使能开
|
||
CMD_MCT_PPS_OFF, // PPS使能关
|
||
CMD_MCT_PPS_CNT_RST, // PPS计数清零
|
||
CMD_TLM_1, // 读取遥测数据包1（64字节）
|
||
CMD_TLM_2 // 读取遥测数据包2（256字节）
|
||
};
|
||
|
||
// 电星模数据注入命令结构体
|
||
#pragma pack(push, 1)
|
||
typedef struct {
|
||
uint16_t FRAME_HEAD; // 帧头
|
||
uint16_t FRAME_ID;
|
||
uint8_t UTC[5];
|
||
uint8_t STAR_NUM_S[10];
|
||
float SS_SCP_X;
|
||
float SS_SCP_Y;
|
||
float SS_SCP_G;
|
||
uint8_t RESERVED[45];
|
||
uint8_t SUM_CRC;
|
||
} CMD_ESD_S;
|
||
#pragma pack(pop)
|
||
|
||
// 设备管理结构体
|
||
typedef struct {
|
||
int fd; // 串口文件描述符
|
||
const char *dev; // 设备节点
|
||
int baudrate; // 波特率
|
||
uint8_t header_err_cnt; // 帧头错误计数
|
||
uint8_t check_err_cnt; // 校验错误计数
|
||
uint8_t length_err_cnt; // 帧长错误计数
|
||
uint8_t uart_reset_cnt; // 串口复位计数
|
||
uint16_t fault_cnt; // 故障计数（所有错误的总和）
|
||
uint8_t send_cmd_cnt; // 命令发送计数
|
||
uint8_t on_off_status; // 上电状态（1=正常，0=异常）
|
||
} Star_Device;
|
||
|
||
|
||
// 全局变量声明（在对应的.cpp文件中定义）
|
||
extern Star_Device nano_stars[];
|
||
extern Star_Device pico_stars[];
|
||
|
||
/**
|
||
* @brief 解析JSON配置文件
|
||
* @param filename 配置文件名
|
||
* @param star_configs 星敏配置数组
|
||
* @param max_stars 最大星敏数量
|
||
* @return 成功解析的星敏数量，-1表示失败
|
||
*/
|
||
int parse_config_file(const char* filename, StarConfig star_configs[], int max_stars);
|
||
|
||
/* ================== FastDDS接口声明 ================== */
|
||
|
||
/**
|
||
* @brief 初始化FastDDS硬件服务化
|
||
* @param domainid 域名
|
||
* @param appname 服务名称
|
||
*/
|
||
void Fastdds_init(uint8_t domainid, std::string appname);
|
||
|
||
/**
|
||
* @brief 发布遥测数据
|
||
* @param data 数据指针
|
||
* @param dest 目标地址
|
||
* @param len 数据长度
|
||
*/
|
||
void Star_sensorHS_telemetry_Pub(uint8_t *data, const std::string& dest, uint16_t len);
|
||
|
||
/**
|
||
* @brief 命令回调函数
|
||
*/
|
||
void command_callback(std::string src, std::string dest, std::string type,
|
||
std::string reserve1, std::string reserve2,
|
||
std::vector<uint8_t>& data);
|
||
|
||
/**
|
||
* @brief 遥测回调函数
|
||
*/
|
||
void telemetry_callback(std::string src, std::string dest, std::string type,
|
||
std::string reserve1, std::string reserve2,
|
||
std::vector<uint8_t>& data);
|
||
|
||
/* ================== 工具函数 ================== */
|
||
|
||
/**
|
||
* @brief 验证数据的校验和
|
||
* @param data 指向数据缓冲区的指针
|
||
* @param len 数据长度（包括校验和字节）
|
||
* @return SAT_ERR_OK 如果校验和正确，SAT_ERR_CHECKSUM 如果校验和错误，SAT_ERR_DATA 如果参数无效
|
||
* @note 校验和计算方式：所有字节（除了最后一个校验和字节）的和与最后一个字节相等
|
||
*/
|
||
int checksum_verify(const uint8_t *data, int len);
|
||
|
||
/* ================== 硬件操作函数声明 ================== */
|
||
|
||
/**
|
||
* @brief 初始化星敏串口
|
||
* @param type 星敏类型 (NANO/PICO)
|
||
* @param num 星敏编号 (1~MAX_STAR_NUM_PER_TYPE)
|
||
* @param dev 串口设备路径
|
||
* @param baudrate 波特率
|
||
* @return 错误码，SAT_ERR_OK表示成功
|
||
*/
|
||
int star_uart_init(star_type_t type, uint8_t num, const char *dev, int baudrate);
|
||
|
||
/**
|
||
* @brief 发送星敏命令
|
||
* @param type 星敏类型
|
||
* @param num 星敏编号
|
||
* @param cmd 命令枚举
|
||
* @param param 命令参数
|
||
* @return 错误码
|
||
*/
|
||
int send_star_cmd(star_type_t type, uint8_t num, enum SENSOR_Cmder cmd, uint8_t param);
|
||
|
||
/**
|
||
* @brief 读取单个星敏数据
|
||
* @param type 星敏类型
|
||
* @param num 星敏编号
|
||
* @param cmd 遥测命令 (CMD_TLM_1或CMD_TLM_2)
|
||
* @param frame 输出数据帧
|
||
* @return 错误码
|
||
*/
|
||
int read_single_star_data(star_type_t type, uint8_t num, enum SENSOR_Cmder cmd, Star_sensorHS_Frame *frame);
|
||
|
||
/**
|
||
* @brief 初始化所有星敏设备
|
||
* @return 成功返回0，失败返回-1
|
||
*/
|
||
int star_devices_init(void);
|
||
|
||
/**
|
||
* @brief 清理硬件服务化资源
|
||
*/
|
||
void Star_sensorHS_cleanup(void);
|
||
|
||
/**
|
||
* @brief 解析纳型星敏四元数数据
|
||
* @param buf 原始数据缓冲区
|
||
* @param frame 输出数据帧
|
||
* @return 错误码
|
||
*/
|
||
int parse_nano_quaternion(const uint8_t *buf, Star_sensorHS_Frame *frame);
|
||
|
||
/**
|
||
* @brief 解析皮型星敏四元数数据
|
||
* @param buf 原始数据缓冲区
|
||
* @param frame 输出数据帧
|
||
* @return 错误码
|
||
*/
|
||
int parse_pico_quaternion(const uint8_t *buf, Star_sensorHS_Frame *frame);
|
||
|
||
/**
|
||
* @brief 验证校验和
|
||
* @param data 数据指针
|
||
* @param len 数据长度
|
||
* @return 错误码
|
||
*/
|
||
int checksum_verify(const uint8_t *data, int len);
|
||
|
||
/**
|
||
* @brief 更新故障计数
|
||
* @param star 设备指针
|
||
*/
|
||
static inline void update_fault_count(Star_Device *star) {
|
||
star->fault_cnt = star->header_err_cnt + star->check_err_cnt +
|
||
star->length_err_cnt + star->uart_reset_cnt;
|
||
}
|
||
|
||
/**
|
||
* @brief 获取错误码对应的字符串描述
|
||
* @param err 错误码
|
||
* @return 错误描述字符串
|
||
*/
|
||
static inline const char *star_strerror(int err) {
|
||
switch (err) {
|
||
case SAT_ERR_OK: return "OK";
|
||
case SAT_ERR_SERIAL: return "Serial not open or invalid";
|
||
case SAT_ERR_SEND_CMD: return "Failed to send command";
|
||
case SAT_ERR_NO_RESPONSE: return "No response after sending command";
|
||
case SAT_ERR_SELECT: return "Select() system call failed";
|
||
case SAT_ERR_HEADER: return "Invalid frame header";
|
||
case SAT_ERR_DATA: return "Failed to parse data";
|
||
case SAT_ERR_UNKNOWN: return "Unknown error, check connection or output format";
|
||
case SAT_ERR_LEN: return "Frame length error";
|
||
case SAT_ERR_CHECKSUM: return "Checksum error";
|
||
case SAT_ERR_TIMEOUT: return "Timeout while waiting for response";
|
||
default: return "Unrecognized error";
|
||
}
|
||
}
|
||
|
||
#endif /* STAR_SENSORHS_H */ |