259 lines
7.4 KiB
C
259 lines
7.4 KiB
C
#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)
|
||
|
||
|
||
|
||
|
||
/* 软件限位 */
|
||
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;
|
||
|
||
|
||
/* 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 |