#pragma once #ifdef __cplusplus extern "C" { #endif #include "bsp/struct_typedef.h" #include "device/motor_dm.h" #include "component/pid.h" #include "device/motor_rm.h" #include "component/ahrs.h" #define GIMBAL_OK 0 #define GIMBAL_ERR -1 #define GIMBAL_ERR_NULL -2 #define GIMBAL_ERR_MODE (-3) #define GIMBAL_ERR_TYPE (-4) #define AI_GIMBAL_SEARCH (1u << 1) // bit1: 云台搜索模式 /* 软件限位 */ typedef struct { float max; float min; } Gimbal_Limit_t; /* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/ typedef struct { MOTOR_RM_Param_t yaw_6020_motor; /* yaw轴6020电机参数 */ MOTOR_DM_Param_t yaw_4310_motor; /* yaw轴4310电机参数 */ MOTOR_DM_Param_t pitch_4310_motor; /* pitch轴6020电机参数 */ struct { KPID_Params_t yaw_6020_motor_omega; /* yaw轴6020角速度环PID参数 */ KPID_Params_t yaw_6020_motor_angle; /* yaw轴6020角位置环PID参数 */ KPID_Params_t yaw_4310_motor_omega; /* yaw轴4310角速度环PID参数 */ KPID_Params_t yaw_4310_motor_angle; /* yaw轴4310角位置环PID参数 */ KPID_Params_t pitch_4310_motor_omega; /* pitch轴角速度环PID参数 */ KPID_Params_t pitch_4310_motor_angle; /* pitch轴角位置环PID参数 */ KPID_Params_t daohang_6020_motor_angle; /* 导航模式小yaw角位置环PID参数 */ KPID_Params_t daohang_4310_motor_angle; /* 导航模式大yaw角位置环PID参数 */ } pid; /* 低通滤波器截止频率 */ struct { float out; /* 电机输出 */ float gyro; /* 陀螺仪数据 */ } low_pass_cutoff_freq; struct { float yaw_6020; /* 小yaw轴6020机械限位 */ float yaw_4310; /* 大yaw轴4310机械限位 */ float pitch_4310; /* pitch轴4310机械限位 */ } mech_zero; struct { float yaw_6020; /* 小yaw轴6020机械限位行程 -1表示无限位 */ float yaw_4310; /* 大yaw轴4310机械限位行程 -1表示无限位*/ float pitch_4310; /* pitch轴4310机械限位行程 -1表示无限位*/ } travel; struct { float K_vel; /*前馈速度*/ float K_accl; /*前馈加速度*/ }K_forward_pid; } Gimbal_Param_t; typedef struct { AHRS_Gyro_t gyro; AHRS_Eulr_t eulr; AHRS_Quaternion_t quat; } Gimbal_IMU_t; /* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */ typedef struct { Gimbal_IMU_t imu; struct { MOTOR_Feedback_t yaw_4310_motor_feedback; MOTOR_Feedback_t pitch_4310_motor_feedback; MOTOR_Feedback_t yaw_6020_motor_feedback; } motor; } Gimbal_feedback_t; /* 云台输出数据的结构体*/ typedef struct { float yaw_6020; /* yaw轴6020电机输出 */ float yaw_4310; /* yaw轴4310电机输出 */ float pitch_4310; /* pitch轴4310电机输出 */ } Gimbal_Output_t; /* 云台模式 */ typedef enum { GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */ GIMBAL_MODE_DAOHANG, /* 导航模式，云台根据导航数据进行移动 */ } Gimbal_Mode_t; typedef enum { GIMBAL_MODE_REMOTE, GIMBAL_MODE_AI, GIMBAL_MODE_SCAN, }GIMBAL_Ctrl_mode_t; /* UI 导出结构（供 referee 系统绘制） */ typedef struct { Gimbal_Mode_t mode; } Gimbal_RefereeUI_t; typedef struct { Gimbal_Mode_t mode; float delta_yaw_4310; float delta_pitch_4310; float delta_yaw_6020; GIMBAL_Ctrl_mode_t ctrl_mode; uint8_t scan_enable; /* 1=AI无目标时扫描, 0=AI无目标时保持当前姿态 */ float set_yaw; /*自瞄YAW目标值*/ float set_pitch;/*自瞄PITCH目标值*/ float yaw_vel; /*自瞄YAW角速度*/ float yaw_accl; /*自瞄YAW加速度*/ float pit_vel; /*自瞄PITCH角速度*/ float pit_accl; /*自瞄PITCH加速度*/ } Gimbal_CMD_t; typedef struct { uint64_t last_wakeup; float dt; Gimbal_Param_t *param; /* 云台的参数，用Gimbal_Init设定 */ /* 模块通用 */ Gimbal_Mode_t mode; /* 云台模式 */ /* PID计算的目标值 */ struct { AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */ float yaw_4310; /* 大yaw电机目标角度 */ float yaw_vel; /*yaw自瞄角速度*/ float yaw_accl; /*yaw自瞄加速度*/ float pit_vel; /*pitch自瞄角速度*/ float pit_accl; /*pitch自瞄加速度*/ float NUC_Pitch; /* 自瞄用pitch目标角度 */ float NUC_Yaw; /* 自瞄用yaw目标角度 */ } setpoint; struct { KPID_t yaw_6020_angle; /* yaw轴6020角位置环PID */ KPID_t yaw_6020_omega; /* yaw轴6020角速度环PID */ KPID_t yaw_4310_angle; /* yaw轴4310角位置环PID */ KPID_t yaw_4310_omega; /* yaw轴4310角速度环PID */ KPID_t pitch_4310_angle; /* pitch轴4310角位置环PID */ KPID_t pitch_4310_omega; /* pitch轴4310角速度环PID */ KPID_t daohang_6020_angle; /* 导航模式小yaw角位置环PID */ KPID_t daohang_4310_angle; /* 导航模式大yaw角位置环PID */ } pid; struct { Gimbal_Limit_t yaw_6020; Gimbal_Limit_t yaw_4310; Gimbal_Limit_t pitch_4310; } limit; struct { LowPassFilter2p_t yaw_6020; LowPassFilter2p_t yaw_4310; LowPassFilter2p_t pitch_4310; LowPassFilter2p_t gyro_yaw; LowPassFilter2p_t gyro_pitch; LowPassFilter2p_t gyro_rol; } filter_out; Gimbal_Output_t out; /* 云台输出 */ Gimbal_feedback_t feedback; /* 反馈 */ } Gimbal_t; typedef struct { Gimbal_Mode_t mode; } Gimbal_RefereeUI_t; /* Exported functions prototypes -------------------------------------------- */ /** * \brief 初始化云台 * * \param g 包含云台数据的结构体 * \param param 包含云台参数的结构体指针 * \param target_freq 任务预期的运行频率 * * \return 函数运行结果 */ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param, float target_freq); /** * \brief 通过CAN设备更新云台反馈信息 * * \param gimbal 云台 * \param can CAN设备 * * \return 函数运行结果 */ int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal); int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu); /** * \brief 运行云台控制逻辑 * * \param g 包含云台数据的结构体 * \param fb 云台反馈信息 * \param g_cmd 云台控制指令 * \param dt_sec 两次调用的时间间隔 * * \return 函数运行结果 */ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd); /** * \brief 云台输出 * * \param s 包含云台数据的结构体 * \param out CAN设备云台输出结构体 */ void Gimbal_Output(Gimbal_t *g); /** * \brief 云台输出 * * \param s 包含云台数据的结构体 * \param out CAN设备云台输出结构体 */ void Gimbal_Output(Gimbal_t *g); #ifdef __cplusplus } #endif