Initial commit from DCSP - 2026/1/9 14:42:31
This commit is contained in:
xb
195
gyroHS.h
Normal file
195
gyroHS.h
Normal file
@@ -0,0 +1,195 @@
|
||||
#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 */
|
||||
Reference in New Issue
Block a user