195 lines
5.6 KiB
C++
195 lines
5.6 KiB
C++
#ifndef GYRO_H
|
||
#define GYRO_H
|
||
|
||
#include <cstdint>
|
||
#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_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 GYRO_FRAME_LENGTH 27
|
||
#define MAX_BUF_SIZE 256
|
||
|
||
/* ================== 类型定义 ================== */
|
||
// 陀螺仪配置结构体
|
||
typedef struct {
|
||
std::string device; // 串口设备
|
||
int baudrate; // 波特率
|
||
bool enabled; // 是否启用
|
||
} GyroConfig;
|
||
|
||
// 陀螺仪原始数据帧结构体
|
||
#pragma pack(push, 1)
|
||
typedef struct {
|
||
uint8_t header;
|
||
uint8_t x_angle_delta[3]; // X轴角度增量
|
||
uint16_t x_integral_time; // X轴积分时间
|
||
uint8_t y_angle_delta[3]; // Y轴角度增量
|
||
uint16_t y_integral_time; // Y轴积分时间
|
||
uint8_t z_angle_delta[3]; // Z轴角度增量
|
||
uint16_t z_integral_time; // Z轴积分时间
|
||
uint8_t rsvd[10];
|
||
uint8_t checksum;
|
||
} Gyro_Frame_Raw;
|
||
#pragma pack(pop)
|
||
|
||
// 陀螺仪解析后数据结构体
|
||
#pragma pack(push, 1)
|
||
typedef struct {
|
||
float x_angle_delta; // X轴角速度（度/秒）
|
||
float y_angle_delta; // Y轴角速度（度/秒）
|
||
float z_angle_delta; // Z轴角速度（度/秒）
|
||
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; // 故障计数
|
||
} GYRO_Info_Frame;
|
||
#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; // 上电状态
|
||
} Gyro_Device;
|
||
|
||
// 全局陀螺仪设备变量声明
|
||
extern Gyro_Device gyro_device;
|
||
|
||
extern pthread_mutex_t cache_mutex;
|
||
|
||
/* ================== FastDDS接口声明 ================== */
|
||
/**
|
||
* @brief 初始化FastDDS硬件服务化
|
||
* @param domainid 域名
|
||
* @param appname 服务名称
|
||
*/
|
||
void Fastdds_init(uint8_t domainid, std::string appname);
|
||
|
||
/**
|
||
* @brief 发布遥测数据
|
||
* @param data 数据指针
|
||
* @param dest 目标地址
|
||
* @param len 数据长度
|
||
*/
|
||
void telemetry_Pub(uint8_t *data, const std::string& dest, uint16_t len);
|
||
|
||
/**
|
||
* @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 解析JSON配置文件
|
||
* @param filename 配置文件名
|
||
* @param config 陀螺仪配置
|
||
* @return 成功返回0，失败返回-1
|
||
*/
|
||
int parse_gyro_config_file(const char* filename, GyroConfig* config);
|
||
|
||
/* ================== 硬件操作函数 ================== */
|
||
/**
|
||
* @brief 初始化陀螺仪串口
|
||
* @param dev 串口设备路径
|
||
* @param baudrate 波特率
|
||
* @return 错误码
|
||
*/
|
||
int gyro_uart_init(const char *dev, int baudrate);
|
||
|
||
/**
|
||
* @brief 读取陀螺仪角速度数据
|
||
* @param info 输出数据
|
||
* @return 错误码
|
||
*/
|
||
int read_gyro_angle_delta(GYRO_Info_Frame *info);
|
||
|
||
|
||
/**
|
||
* @brief 清理硬件服务化资源
|
||
*/
|
||
void GyroHS_cleanup(void);
|
||
|
||
/* ================== 工具函数 ================== */
|
||
/**
|
||
* @brief 更新故障计数
|
||
* @param gyro 设备指针
|
||
*/
|
||
static inline void update_gyro_fault_count(Gyro_Device *gyro) {
|
||
gyro->fault_cnt = gyro->header_err_cnt + gyro->check_err_cnt +
|
||
gyro->length_err_cnt + gyro->uart_reset_cnt;
|
||
}
|
||
|
||
/**
|
||
* @brief 获取错误码对应的字符串描述
|
||
* @param err 错误码
|
||
* @return 错误描述字符串
|
||
*/
|
||
static inline const char *gyro_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_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";
|
||
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 /* GYRO_H */ |