test/FlywheelHS.h

#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 */