189 lines
6.4 KiB
C++
189 lines
6.4 KiB
C++
#ifndef FLYWHEEL_HS_H
|
||
#define FLYWHEEL_HS_H
|
||
|
||
#include <cstdint>
|
||
#include <string>
|
||
#include <vector>
|
||
#include <cstddef>
|
||
|
||
/* ================== 错误码定义 ================== */
|
||
#define SAT_ERR_OK 0 // 成功
|
||
#define SAT_ERR_SERIAL -1 // 串口未打开或无效
|
||
#define SAT_ERR_SEND_CMD -2 // 发送命令失败
|
||
#define SAT_ERR_NO_RESPONSE -3 // 无响应
|
||
#define SAT_ERR_SELECT -4 // select() 系统调用失败
|
||
#define SAT_ERR_HEADER -5 // 帧头错误
|
||
#define SAT_ERR_DATA -6 // 数据解析失败
|
||
#define SAT_ERR_UNKNOWN -7 // 未知错误
|
||
#define SAT_ERR_LEN -8 // 帧长度错误
|
||
#define SAT_ERR_CHECKSUM -9 // 校验和错误
|
||
#define SAT_ERR_TIMEOUT -10 // 接收数据超时
|
||
#define E_wheel_ID -20 // 飞轮ID错误
|
||
|
||
/* ================== 常量定义 ================== */
|
||
#define MAX_FLYWHEELS 8 // 最大飞轮数量
|
||
#define WHEEL_FRAME_LENGTH 19 // 飞轮数据帧长度
|
||
#define WHEEL_EXP 1000 // 数据转换系数
|
||
#define WHEEL_LIMIT4 4.294967296e9 // 32位有符号数最大值
|
||
#define HARDWARE_ID_BASE 0x11 // 硬件ID基数
|
||
|
||
/* ================== 类型定义 ================== */
|
||
|
||
// 飞轮配置结构体
|
||
#pragma pack(push, 1)
|
||
typedef struct {
|
||
uint8_t id; // 飞轮逻辑ID
|
||
std::string device; // 串口设备
|
||
int baudrate; // 波特率
|
||
uint8_t hardware_id; // 硬件ID
|
||
} FlywheelConfig;
|
||
#pragma pack(pop)
|
||
|
||
// 飞轮控制状态（仅用于内部解析）
|
||
#pragma pack(push, 1)
|
||
typedef struct {
|
||
uint8_t soft_err;
|
||
uint8_t soft_ver;
|
||
uint16_t cmd_cnt;
|
||
} Flywheel_Control;
|
||
#pragma pack(pop)
|
||
|
||
// 飞轮信息（仅用于内部解析）
|
||
#pragma pack(push, 1)
|
||
typedef struct {
|
||
float rotate_speed; // 转速 (rpm)
|
||
float current; // 电流
|
||
Flywheel_Control control_state; // 控制状态
|
||
float acc_torque; // 加速度矩
|
||
uint8_t hardware_id; // 硬件ID
|
||
} Flywheel_Info;
|
||
#pragma pack(pop)
|
||
|
||
// 飞轮控制指令
|
||
#pragma pack(push, 1)
|
||
typedef struct {
|
||
uint8_t wheel_id; // 飞轮逻辑ID: 1,2,3...
|
||
uint8_t mode_flag; // 0=速度模式, 1=力矩模式
|
||
float speed; // 速度指令 (rpm)
|
||
float torque; // 力矩指令 (mNm)
|
||
} Flywheel_Cmder;
|
||
#pragma pack(pop)
|
||
|
||
// 飞轮数据帧（用于FastDDS传输）- 精简版
|
||
#pragma pack(push, 1)
|
||
typedef struct {
|
||
// 设备标识
|
||
uint8_t wheel_id; // 逻辑ID (1,2,3...)
|
||
|
||
// 状态数据
|
||
float rotate_speed; // 转速 (rpm)
|
||
float current; // 电流
|
||
float acc_torque; // 加速度矩指令
|
||
|
||
// 错误统计（用于故障诊断）
|
||
uint16_t header_err_cnt; // 帧头错误累计次数
|
||
uint16_t check_err_cnt; // 校验错误累计次数
|
||
uint16_t length_err_cnt; // 长度错误累计次数
|
||
uint16_t uart_reset_cnt; // 串口复位累计次数
|
||
uint16_t fault_cnt; // 总故障计数
|
||
} Flywheel_Frame;
|
||
#pragma pack(pop)
|
||
|
||
// 控制指令缓存
|
||
#pragma pack(push, 1)
|
||
typedef struct {
|
||
uint8_t mode; // 控制模式
|
||
float speed_cmd; // 速度指令
|
||
float torque_cmd; // 扭矩指令
|
||
uint8_t has_cmd; // 是否有有效指令
|
||
} Flywheel_Cmd_Cache;
|
||
#pragma pack(pop)
|
||
|
||
// 设备管理结构体
|
||
#pragma pack(push, 1)
|
||
typedef struct {
|
||
int fd; // 串口文件描述符
|
||
const char *dev; // 设备节点
|
||
int baudrate; // 波特率
|
||
uint8_t hardware_id; // 硬件ID
|
||
uint8_t id; // 逻辑ID (0-based)
|
||
|
||
// 错误统计
|
||
uint16_t header_err_cnt; // 帧头错误计数
|
||
uint16_t check_err_cnt; // 校验错误计数
|
||
uint16_t length_err_cnt; // 帧长错误计数
|
||
uint16_t uart_reset_cnt; // 串口复位计数
|
||
uint16_t fault_cnt; // 故障计数
|
||
uint8_t send_cmd_cnt; // 命令发送计数
|
||
|
||
// 数据缓存
|
||
Flywheel_Frame last_frame; // 最后接收的帧
|
||
uint8_t is_data_valid; // 数据是否有效
|
||
} Flywheel_Device;
|
||
#pragma pack(pop)
|
||
|
||
// 全局变量声明
|
||
extern Flywheel_Device flywheels[];
|
||
extern uint8_t flywheel_count;
|
||
extern pthread_mutex_t cache_mutex;
|
||
extern Flywheel_Cmd_Cache cmd_cache[];
|
||
|
||
/* ================== FastDDS接口声明 ================== */
|
||
void Fastdds_init(uint8_t domainid, std::string appname);
|
||
void Flywheel_telemetry_Pub(uint8_t *data, const std::string& dest, uint16_t len);
|
||
void command_callback(std::string src, std::string dest, std::string type,
|
||
std::string reserve1, std::string reserve2,
|
||
std::vector<uint8_t>& data);
|
||
void telemetry_callback(std::string src, std::string dest, std::string type,
|
||
std::string reserve1, std::string reserve2,
|
||
std::vector<uint8_t>& data);
|
||
|
||
/* ================== 配置文件解析 ================== */
|
||
int parse_config_file(const char* filename, FlywheelConfig flywheel_configs[], int max_flywheels);
|
||
|
||
/* ================== 硬件操作函数声明 ================== */
|
||
int flywheel_uart_init(uint8_t id, const char *dev, int baudrate, uint8_t hardware_id);
|
||
int read_flywheel_data(uint8_t wheel_id, Flywheel_Frame *frame);
|
||
void FlywheelHS_cleanup(void);
|
||
|
||
/* ================== 串口接收函数 ================== */
|
||
/**
|
||
* @brief 检查并接收串口数据
|
||
* @return 接收到的数据帧数量
|
||
*/
|
||
int check_and_receive_serial_data(void);
|
||
|
||
/**
|
||
* @brief 发送控制指令到指定飞轮
|
||
* @param wheel_id 飞轮ID
|
||
* @return 错误码
|
||
*/
|
||
int send_flywheel_command(uint8_t wheel_id);
|
||
|
||
/**
|
||
* @brief 解析飞轮数据帧
|
||
* @param wheel_id 飞轮ID
|
||
* @param buffer 原始数据缓冲区
|
||
* @param frame 输出数据帧
|
||
* @return 错误码
|
||
*/
|
||
int parse_flywheel_frame(uint8_t wheel_id, const uint8_t* buffer, Flywheel_Frame* frame);
|
||
|
||
/**
|
||
* @brief 从设备缓存获取遥测数据
|
||
* @param wheel_id 飞轮ID
|
||
* @param frame 输出数据帧
|
||
* @return 错误码
|
||
*/
|
||
int get_telemetry_from_cache(uint8_t wheel_id, Flywheel_Frame* frame);
|
||
|
||
/* ================== 工具函数声明 ================== */
|
||
const char *flywheel_strerror(int err);
|
||
uint32_t bytes_to_uint32(const uint8_t *a);
|
||
void cal_cmder_para(double k, double exp, uint8_t *a);
|
||
double cal_message_para(const uint8_t *data, int exp);
|
||
int checksum_verify(const uint8_t *data, int len);
|
||
void update_fault_count(Flywheel_Device *wheel);
|
||
uint8_t hardware_id_to_logical_id(uint8_t hardware_id, uint8_t default_id);
|
||
|
||
#endif /* FLYWHEEL_HS_H */ |