347 lines
11 KiB
C++
347 lines
11 KiB
C++
#ifndef STAR_SENSORHS_H
|
||
#define STAR_SENSORHS_H
|
||
|
||
#include <stdint.h>
|
||
#include <string>
|
||
#include <vector>
|
||
|
||
/* ================== 错误码定义 ================== */
|
||
|
||
#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;
|
||
|
||
// 统一遥测数据帧（包含数据和错误信息）
|
||
#pragma pack(push, 1)
|
||
typedef struct {
|
||
// 帧类型和星敏信息
|
||
uint8_t frame_type; // 帧类型: 1=遥测数据帧
|
||
uint8_t star_type; // 星敏类型: 0=纳型, 1=皮型
|
||
uint8_t star_num; // 星敏编号: 1~MAX_STAR_NUM_PER_TYPE
|
||
uint8_t cmd; // 命令编号
|
||
uint8_t param; // 参数
|
||
|
||
// 时间戳
|
||
uint32_t timestamp; // 时间戳(秒)
|
||
uint32_t timestamp_us; // 时间戳(微秒)
|
||
|
||
// 四元数数据
|
||
float q0, q1, q2, q3;
|
||
|
||
// 角速度
|
||
float wx, wy, wz;
|
||
|
||
// 状态信息
|
||
uint8_t star_num_d;
|
||
uint8_t on_off_status;
|
||
|
||
// 错误信息部分
|
||
uint8_t has_error; // 是否有错误（0=无，1=有）
|
||
int32_t current_error; // 当前错误代码
|
||
uint16_t header_err_cnt; // 帧头错误累计
|
||
uint16_t check_err_cnt; // 校验错误累计
|
||
uint16_t length_err_cnt; // 长度错误累计
|
||
uint16_t uart_reset_cnt; // 串口复位累计
|
||
|
||
// 保留字段
|
||
uint8_t reserved[4];
|
||
} 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字节）
|
||
};
|
||
|
||
// 原有的星敏数据结构体
|
||
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;
|
||
|
||
typedef struct {
|
||
float q0; // 四元数q0
|
||
float q1; // 四元数q1
|
||
float q2; // 四元数q2
|
||
float q3; // 四元数q3
|
||
float wx; // 角速度wsi_x
|
||
float wy; // 角速度wsi_y
|
||
float wz; // 角速度wsi_z
|
||
Bitpara_State bitpara_state; // 星敏感器状态字
|
||
uint8_t star_num_d; // 定姿星数
|
||
} STAR_INFO;
|
||
|
||
// 电星模数据注入命令结构体
|
||
typedef struct __attribute__((packed)) {
|
||
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;
|
||
|
||
// 设备管理结构体
|
||
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; // 串口复位计数
|
||
uint8_t send_cmd_cnt; // 命令发送计数
|
||
uint8_t on_off_status; // 上电状态（1=正常，0=异常）
|
||
} Star_Device;
|
||
|
||
/* ================== FastDDS相关类型定义 ================== */
|
||
|
||
// 命令回调函数类型
|
||
typedef void (*CommandCallbackFunc)(std::string src, std::string dest, std::string type,
|
||
std::string reserve1, std::string reserve2,
|
||
std::vector<uint8_t>& data);
|
||
|
||
// 遥测回调函数类型
|
||
typedef void (*TelemetryCallbackFunc)(std::string src, std::string dest, std::string type,
|
||
std::string reserve1, std::string reserve2,
|
||
std::vector<uint8_t>& data);
|
||
|
||
/* ================== 硬件操作函数声明 ================== */
|
||
|
||
/**
|
||
* @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 info 输出数据指针
|
||
* @return 错误码
|
||
*/
|
||
int read_single_star_data(star_type_t type, uint8_t num, enum SENSOR_Cmder cmd, STAR_INFO *info);
|
||
|
||
/**
|
||
* @brief 初始化所有星敏设备
|
||
* @return 成功返回0，失败返回-1
|
||
*/
|
||
int star_devices_init(void);
|
||
|
||
/**
|
||
* @brief 构建星敏统一数据帧
|
||
* @param type 星敏类型
|
||
* @param num 星敏编号
|
||
* @param info 星敏数据
|
||
* @param result 读取结果
|
||
* @param frame 输出数据帧
|
||
*/
|
||
void build_star_sensorHS_frame(star_type_t type, uint8_t num, const STAR_INFO *info,
|
||
int result, Star_sensorHS_Frame *frame);
|
||
|
||
/**
|
||
* @brief 星敏数据发布线程
|
||
* @param arg 线程参数（未使用）
|
||
* @return 线程返回值
|
||
*/
|
||
void *star_publish_thread(void *arg);
|
||
|
||
/**
|
||
* @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";
|
||
}
|
||
}
|
||
|
||
/* ================== FastDDS服务化接口声明 ================== */
|
||
|
||
/**
|
||
* @brief 初始化FastDDS硬件服务化
|
||
* @param argc 参数数量（保留）
|
||
* @param name 服务名称
|
||
* @return 成功返回0，失败返回-1
|
||
*/
|
||
int Star_sensorHS_init(int argc, const char* name);
|
||
|
||
/**
|
||
* @brief 清理FastDDS硬件服务化资源
|
||
*/
|
||
void Star_sensorHS_cleanup(void);
|
||
|
||
/**
|
||
* @brief 发布遥测数据
|
||
* @param data 数据指针
|
||
* @param dest 目标地址
|
||
* @param len 数据长度
|
||
*/
|
||
void Star_sensorHS_telemetry_Pub(uint8_t *data, const std::string& dest, uint16_t len);
|
||
|
||
/**
|
||
* @brief 设置命令回调函数
|
||
* @param callback 回调函数指针
|
||
*/
|
||
void Star_sensorHS_set_command_callback(CommandCallbackFunc callback);
|
||
|
||
/**
|
||
* @brief 设置遥测回调函数
|
||
* @param callback 回调函数指针
|
||
*/
|
||
void Star_sensorHS_set_telemetry_callback(TelemetryCallbackFunc callback);
|
||
|
||
/* ================== 默认回调函数声明 ================== */
|
||
|
||
/**
|
||
* @brief 默认命令回调函数
|
||
* @param src 源地址
|
||
* @param dest 目标地址
|
||
* @param type 消息类型
|
||
* @param reserve1 保留参数1
|
||
* @param reserve2 保留参数2
|
||
* @param data 命令数据
|
||
*/
|
||
void command_callback(std::string src, std::string dest, std::string type,
|
||
std::string reserve1, std::string reserve2,
|
||
std::vector<uint8_t>& data);
|
||
|
||
/**
|
||
* @brief 默认遥测回调函数
|
||
* @param src 源地址
|
||
* @param dest 目标地址
|
||
* @param type 消息类型
|
||
* @param reserve1 保留参数1
|
||
* @param reserve2 保留参数2
|
||
* @param data 遥测数据
|
||
*/
|
||
void telemetry_callback(std::string src, std::string dest, std::string type,
|
||
std::string reserve1, std::string reserve2,
|
||
std::vector<uint8_t>& data);
|
||
|
||
#endif /* STAR_SENSORHS_H */ |