暂存2轴云台
This commit is contained in:
parent
d3aabce4f5
commit
697104d1ce
259
assets/User_code/module/2_axis_gimbal.c
Normal file
259
assets/User_code/module/2_axis_gimbal.c
Normal file
@ -0,0 +1,259 @@
|
|||||||
|
/*
|
||||||
|
* 云台模组
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* Includes ----------------------------------------------------------------- */
|
||||||
|
#include "bsp/can.h"
|
||||||
|
#include "bsp/time.h"
|
||||||
|
#include "component/filter.h"
|
||||||
|
#include "component/pid.h"
|
||||||
|
#include "device/motor_dm.h"
|
||||||
|
#include "device/motor_rm.h"
|
||||||
|
#include "gimbal.h"
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
/* Private typedef ---------------------------------------------------------- */
|
||||||
|
/* Private define ----------------------------------------------------------- */
|
||||||
|
/* Private macro ------------------------------------------------------------ */
|
||||||
|
/* Private variables -------------------------------------------------------- */
|
||||||
|
/* Private function -------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 设置云台模式
|
||||||
|
*
|
||||||
|
* \param c 包含云台数据的结构体
|
||||||
|
* \param mode 要设置的模式
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
|
||||||
|
if (g == NULL)
|
||||||
|
return -1;
|
||||||
|
if (mode == g->mode)
|
||||||
|
return GIMBAL_OK;
|
||||||
|
|
||||||
|
PID_Reset(&g->pid.yaw_angle);
|
||||||
|
PID_Reset(&g->pid.yaw_omega);
|
||||||
|
PID_Reset(&g->pid.pit_angle);
|
||||||
|
PID_Reset(&g->pid.pit_omega);
|
||||||
|
LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
|
||||||
|
LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
|
||||||
|
|
||||||
|
MOTOR_DM_Enable(&(g->param->yaw_motor));
|
||||||
|
|
||||||
|
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
|
||||||
|
// if (g->mode == GIMBAL_MODE_RELAX) {
|
||||||
|
// if (mode == GIMBAL_MODE_ABSOLUTE) {
|
||||||
|
// g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
|
||||||
|
// } else if (mode == GIMBAL_MODE_RELATIVE) {
|
||||||
|
// g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
|
||||||
|
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
|
||||||
|
|
||||||
|
g->mode = mode;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Exported functions ------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 初始化云台
|
||||||
|
*
|
||||||
|
* \param g 包含云台数据的结构体
|
||||||
|
* \param param 包含云台参数的结构体指针
|
||||||
|
* \param target_freq 任务预期的运行频率
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
|
||||||
|
float target_freq) {
|
||||||
|
if (g == NULL)
|
||||||
|
return -1;
|
||||||
|
|
||||||
|
g->param = param;
|
||||||
|
g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */
|
||||||
|
|
||||||
|
/* 初始化云台电机控制PID和LPF */
|
||||||
|
PID_Init(&(g->pid.yaw_angle), KPID_MODE_NO_D, target_freq,
|
||||||
|
&(g->param->pid.yaw_angle));
|
||||||
|
PID_Init(&(g->pid.yaw_omega), KPID_MODE_CALC_D, target_freq,
|
||||||
|
&(g->param->pid.yaw_omega));
|
||||||
|
PID_Init(&(g->pid.pit_angle), KPID_MODE_NO_D, target_freq,
|
||||||
|
&(g->param->pid.pit_angle));
|
||||||
|
PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
|
||||||
|
&(g->param->pid.pit_omega));
|
||||||
|
|
||||||
|
LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
|
||||||
|
g->param->low_pass_cutoff_freq.out);
|
||||||
|
LowPassFilter2p_Init(&g->filter_out.pit, target_freq,
|
||||||
|
g->param->low_pass_cutoff_freq.out);
|
||||||
|
g->limit.yaw.max = g->param->mech_zero.yaw + g->param->travel.yaw;
|
||||||
|
g->limit.yaw.min = g->param->mech_zero.yaw;
|
||||||
|
g->limit.pit.max = g->param->mech_zero.pit + g->param->travel.pit;
|
||||||
|
g->limit.pit.min = g->param->mech_zero.pit;
|
||||||
|
BSP_CAN_Init();
|
||||||
|
|
||||||
|
MOTOR_RM_Register(&(g->param->pit_motor));
|
||||||
|
MOTOR_DM_Register(&(g->param->yaw_motor));
|
||||||
|
|
||||||
|
MOTOR_DM_Enable(&(g->param->yaw_motor));
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 通过CAN设备更新云台反馈信息
|
||||||
|
*
|
||||||
|
* \param gimbal 云台
|
||||||
|
* \param can CAN设备
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
|
||||||
|
if (gimbal == NULL)
|
||||||
|
return -1;
|
||||||
|
|
||||||
|
/* 更新RM电机反馈数据（pitch轴） */
|
||||||
|
MOTOR_RM_Update(&(gimbal->param->pit_motor));
|
||||||
|
MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(gimbal->param->pit_motor));
|
||||||
|
if (rm_motor != NULL) {
|
||||||
|
gimbal->feedback.motor.pit = rm_motor->feedback;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* 更新DM电机反馈数据（yaw轴） */
|
||||||
|
MOTOR_DM_Update(&(gimbal->param->yaw_motor));
|
||||||
|
MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->yaw_motor));
|
||||||
|
if (dm_motor != NULL) {
|
||||||
|
gimbal->feedback.motor.yaw = dm_motor->motor.feedback;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu) {
|
||||||
|
|
||||||
|
if (gimbal == NULL) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
gimbal->feedback.imu.gyro = imu->gyro;
|
||||||
|
gimbal->feedback.imu.eulr = imu->eulr;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 运行云台控制逻辑
|
||||||
|
*
|
||||||
|
* \param g 包含云台数据的结构体
|
||||||
|
* \param g_cmd 云台控制指令
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
|
||||||
|
if (g == NULL || g_cmd == NULL) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f;
|
||||||
|
g->lask_wakeup = BSP_TIME_Get_us();
|
||||||
|
|
||||||
|
Gimbal_SetMode(g, g_cmd->mode);
|
||||||
|
|
||||||
|
/* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */
|
||||||
|
float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
|
||||||
|
if (g->param->travel.yaw > 0) {
|
||||||
|
/* 计算当前电机角度与IMU角度的偏差 */
|
||||||
|
float motor_imu_offset =
|
||||||
|
g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
|
||||||
|
/* 处理跨越±π的情况 */
|
||||||
|
if (motor_imu_offset > M_PI)
|
||||||
|
motor_imu_offset -= M_2PI;
|
||||||
|
if (motor_imu_offset < -M_PI)
|
||||||
|
motor_imu_offset += M_2PI;
|
||||||
|
|
||||||
|
/* 计算到限位边界的距离 */
|
||||||
|
const float delta_max = CircleError(
|
||||||
|
g->limit.yaw.max, (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw),
|
||||||
|
M_2PI);
|
||||||
|
const float delta_min = CircleError(
|
||||||
|
g->limit.yaw.min, (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw),
|
||||||
|
M_2PI);
|
||||||
|
|
||||||
|
/* 限制控制命令 */
|
||||||
|
if (delta_yaw > delta_max)
|
||||||
|
delta_yaw = delta_max;
|
||||||
|
if (delta_yaw < delta_min)
|
||||||
|
delta_yaw = delta_min;
|
||||||
|
}
|
||||||
|
CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI);
|
||||||
|
|
||||||
|
/* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
|
||||||
|
float delta_pit = g_cmd->delta_pit * g->dt;
|
||||||
|
if (g->param->travel.pit > 0) {
|
||||||
|
/* 计算当前电机角度与IMU角度的偏差 */
|
||||||
|
float motor_imu_offset =
|
||||||
|
g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol;
|
||||||
|
/* 处理跨越±π的情况 */
|
||||||
|
if (motor_imu_offset > M_PI)
|
||||||
|
motor_imu_offset -= M_2PI;
|
||||||
|
if (motor_imu_offset < -M_PI)
|
||||||
|
motor_imu_offset += M_2PI;
|
||||||
|
|
||||||
|
/* 计算到限位边界的距离 */
|
||||||
|
const float delta_max = CircleError(
|
||||||
|
g->limit.pit.max, (g->setpoint.eulr.pit + motor_imu_offset + delta_pit),
|
||||||
|
M_2PI);
|
||||||
|
const float delta_min = CircleError(
|
||||||
|
g->limit.pit.min, (g->setpoint.eulr.pit + motor_imu_offset + delta_pit),
|
||||||
|
M_2PI);
|
||||||
|
|
||||||
|
/* 限制控制命令 */
|
||||||
|
if (delta_pit > delta_max)
|
||||||
|
delta_pit = delta_max;
|
||||||
|
if (delta_pit < delta_min)
|
||||||
|
delta_pit = delta_min;
|
||||||
|
}
|
||||||
|
CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
|
||||||
|
|
||||||
|
/* 控制相关逻辑 */
|
||||||
|
float yaw_omega_set_point, pit_omega_set_point;
|
||||||
|
switch (g->mode) {
|
||||||
|
case GIMBAL_MODE_RELAX:
|
||||||
|
g->out.yaw = 0.0f;
|
||||||
|
g->out.pit = 0.0f;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case GIMBAL_MODE_ABSOLUTE:
|
||||||
|
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
|
||||||
|
g->feedback.imu.eulr.yaw, 0.0f, g->dt);
|
||||||
|
g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point,
|
||||||
|
g->feedback.imu.gyro.z, 0.f, g->dt);
|
||||||
|
|
||||||
|
pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
|
||||||
|
g->feedback.imu.eulr.rol, 0.0f, g->dt);
|
||||||
|
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
|
||||||
|
g->feedback.imu.gyro.y, 0.f, g->dt);
|
||||||
|
|
||||||
|
break;
|
||||||
|
|
||||||
|
/* 输出滤波 */
|
||||||
|
g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
|
||||||
|
g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 云台输出
|
||||||
|
*
|
||||||
|
* \param s 包含云台数据的结构体
|
||||||
|
* \param out CAN设备云台输出结构体
|
||||||
|
*/
|
||||||
|
void Gimbal_Output(Gimbal_t *g) {
|
||||||
|
MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit);
|
||||||
|
MOTOR_MIT_Output_t output = {0};
|
||||||
|
output.torque = g->out.yaw * 5.0f; // 乘以减速比
|
||||||
|
output.kd = 0.3f;
|
||||||
|
MOTOR_RM_Ctrl(&g->param->pit_motor);
|
||||||
|
MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output);
|
||||||
|
}
|
||||||
179
assets/User_code/module/2_axis_gimbal.h
Normal file
179
assets/User_code/module/2_axis_gimbal.h
Normal file
@ -0,0 +1,179 @@
|
|||||||
|
/*
|
||||||
|
* 云台模组
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Includes ----------------------------------------------------------------- */
|
||||||
|
#include "component/ahrs.h"
|
||||||
|
#include "component/filter.h"
|
||||||
|
#include "component/pid.h"
|
||||||
|
#include "device/motor.h"
|
||||||
|
#include "device/motor_dm.h"
|
||||||
|
#include "device/motor_rm.h"
|
||||||
|
|
||||||
|
/* Exported constants ------------------------------------------------------- */
|
||||||
|
#define GIMBAL_OK (0) /* 运行正常 */
|
||||||
|
#define GIMBAL_ERR (-1) /* 运行时发现了其他错误 */
|
||||||
|
#define GIMBAL_ERR_NULL (-2) /* 运行时发现NULL指针 */
|
||||||
|
#define GIMBAL_ERR_MODE (-3) /* 运行时配置了错误的CMD_GimbalMode_t */
|
||||||
|
|
||||||
|
/* Exported macro ----------------------------------------------------------- */
|
||||||
|
/* Exported types ----------------------------------------------------------- */
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
|
||||||
|
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
|
||||||
|
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
|
||||||
|
} Gimbal_Mode_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
Gimbal_Mode_t mode;
|
||||||
|
float delta_yaw;
|
||||||
|
float delta_pit;
|
||||||
|
} Gimbal_CMD_t;
|
||||||
|
|
||||||
|
/* 软件限位 */
|
||||||
|
typedef struct {
|
||||||
|
float max;
|
||||||
|
float min;
|
||||||
|
} Gimbal_Limit_t;
|
||||||
|
|
||||||
|
/* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
|
||||||
|
typedef struct {
|
||||||
|
MOTOR_RM_Param_t pit_motor; /* pitch轴电机参数 */
|
||||||
|
MOTOR_DM_Param_t yaw_motor; /* yaw轴电机参数 */
|
||||||
|
struct {
|
||||||
|
KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
|
||||||
|
KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */
|
||||||
|
KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
|
||||||
|
KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
|
||||||
|
} pid;
|
||||||
|
|
||||||
|
/* 低通滤波器截止频率 */
|
||||||
|
struct {
|
||||||
|
float out; /* 电机输出 */
|
||||||
|
float gyro; /* 陀螺仪数据 */
|
||||||
|
} low_pass_cutoff_freq;
|
||||||
|
|
||||||
|
struct {
|
||||||
|
float yaw; /* yaw轴机械限位 */
|
||||||
|
float pit; /* pitch轴机械限位 */
|
||||||
|
} mech_zero;
|
||||||
|
|
||||||
|
struct {
|
||||||
|
float yaw; /* yaw轴机械限位行程 -1表示无限位 */
|
||||||
|
float pit; /* pitch轴机械限位行程 -1表示无限位*/
|
||||||
|
} travel;
|
||||||
|
|
||||||
|
} Gimbal_Params_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
AHRS_Gyro_t gyro;
|
||||||
|
AHRS_Eulr_t eulr;
|
||||||
|
} Gimbal_IMU_t;
|
||||||
|
/* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */
|
||||||
|
typedef struct {
|
||||||
|
Gimbal_IMU_t imu;
|
||||||
|
struct {
|
||||||
|
MOTOR_Feedback_t yaw; /* yaw轴电机反馈 */
|
||||||
|
MOTOR_Feedback_t pit; /* pitch轴电机反馈 */
|
||||||
|
} motor;
|
||||||
|
} Gimbal_Feedback_t;
|
||||||
|
|
||||||
|
/* 云台输出数据的结构体*/
|
||||||
|
typedef struct {
|
||||||
|
float yaw; /* yaw轴电机输出 */
|
||||||
|
float pit; /* pitch轴电机输出 */
|
||||||
|
} Gimbal_Output_t;
|
||||||
|
/*
|
||||||
|
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体。
|
||||||
|
* 包含了初始化参数，中间变量，输出变量。
|
||||||
|
*/
|
||||||
|
typedef struct {
|
||||||
|
uint64_t lask_wakeup;
|
||||||
|
float dt;
|
||||||
|
|
||||||
|
Gimbal_Params_t *param; /* 云台的参数，用Gimbal_Init设定 */
|
||||||
|
/* 模块通用 */
|
||||||
|
Gimbal_Mode_t mode; /* 云台模式 */
|
||||||
|
|
||||||
|
/* PID计算的目标值 */
|
||||||
|
struct {
|
||||||
|
AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */
|
||||||
|
} setpoint;
|
||||||
|
|
||||||
|
struct {
|
||||||
|
KPID_t yaw_angle; /* yaw轴角位置环PID */
|
||||||
|
KPID_t yaw_omega; /* yaw轴角速度环PID */
|
||||||
|
KPID_t pit_angle; /* pitch轴角位置环PID */
|
||||||
|
KPID_t pit_omega; /* pitch轴角速度环PID */
|
||||||
|
} pid;
|
||||||
|
|
||||||
|
struct {
|
||||||
|
Gimbal_Limit_t yaw;
|
||||||
|
Gimbal_Limit_t pit;
|
||||||
|
} limit;
|
||||||
|
|
||||||
|
struct {
|
||||||
|
LowPassFilter2p_t yaw;
|
||||||
|
LowPassFilter2p_t pit;
|
||||||
|
} 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, const Gimbal_Params_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);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
Loading…
Reference in New Issue
Block a user