c1a0c821c3
modified: MDK-ARM/DevC.uvprojx modified: MDK-ARM/DevC/DevC.axf modified: MDK-ARM/DevC/DevC.hex modified: User/bsp/uart.c modified: User/bsp/uart.h modified: User/module/config.c modified: User/module/gimbal.c modified: User/module/gimbal.h modified: User/module/shoot.c modified: User/task/ai.c modified: User/task/atti_esti.c modified: User/task/ctrl_gimbal.c modified: User/task/rc.c
427 lines
15 KiB
C
427 lines
15 KiB
C
/*
|
||
* 云台模组
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "gimbal.h"
|
||
#include "bsp/can.h"
|
||
#include "bsp/time.h"
|
||
#include <math.h>
|
||
#include "component/filter.h"
|
||
#include "component/pid.h"
|
||
#include "device/motor_dm.h"
|
||
#include "device/motor_rm.h"
|
||
#include "module/config.h"
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private variables -------------------------------------------------------- */
|
||
/* Private function -------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
#define YAW_MAX_OUTPUT 0.8f /* Yaw轴(大疆电机)最大输出比例限制，范围：0.0 到 1.0 */
|
||
#define PIT_MAX_TORQUE 2.0f /* Pitch轴(达妙电机)最大扭矩限制，单位：N·m (请根据实际情况调整) */
|
||
static LowPassFilter2p_t vision_yaw_lpf;
|
||
static LowPassFilter2p_t vision_pit_lpf;
|
||
static uint8_t is_vision_lpf_init = 0;
|
||
/**
|
||
* \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->pit_motor));
|
||
|
||
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
|
||
|
||
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->yaw_motor));
|
||
MOTOR_DM_Register(&(g->param->pit_motor));
|
||
|
||
MOTOR_DM_Enable(&(g->param->pit_motor));
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* \brief 通过CAN设备更新云台反馈信息
|
||
*
|
||
* \param gimbal 云台
|
||
* \param can CAN设备
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
|
||
if (gimbal == NULL)
|
||
return -1;
|
||
|
||
/* 更新DM电机反馈数据（pitch轴） */
|
||
MOTOR_DM_Update(&(gimbal->param->pit_motor));
|
||
MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->pit_motor));
|
||
if (dm_motor != NULL) {
|
||
gimbal->feedback.motor.pit = dm_motor->motor.feedback;
|
||
}
|
||
|
||
/* 更新RM电机反馈数据（yaw轴） */
|
||
MOTOR_RM_Update(&(gimbal->param->yaw_motor));
|
||
MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(gimbal->param->yaw_motor));
|
||
if (rm_motor != NULL) {
|
||
gimbal->feedback.motor.yaw = rm_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;
|
||
return 0;
|
||
}
|
||
int fan = 0;
|
||
/**
|
||
* \brief 运行云台控制逻辑（集成 AI 速度前馈）
|
||
*
|
||
* \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);
|
||
|
||
/* --- 1. 计算目标设定值增量 --- */
|
||
float delta_yaw = 0.0f;
|
||
float delta_pit = 0.0f;
|
||
|
||
if (g->mode == GIMBAL_MODE_AUTO_AIM && g_cmd->vision.target_found) {
|
||
/* 火控模式且目标已锁定：计算视觉目标角度与当前设定值的偏差 */
|
||
delta_yaw = CircleError((g_cmd->vision.yaw), g->setpoint.eulr.yaw, M_2PI);
|
||
if (fan) delta_pit = CircleError((g_cmd->vision.pit), g->setpoint.eulr.pit, M_2PI);
|
||
else delta_pit = CircleError((g_cmd->vision.pit), g->setpoint.eulr.pit, M_2PI);
|
||
|
||
|
||
// delta_pit = CircleError(g_cmd->vision.pit, g->setpoint.eulr.pit, M_2PI);
|
||
} else {
|
||
/* 常规模式或火控模式下目标丢失：使用操作手遥控器增量 */
|
||
delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
|
||
delta_pit = g_cmd->delta_pit * g->dt;
|
||
}
|
||
|
||
/* --- 2. 软件限位处理 (Yaw) --- */
|
||
if (g->param->travel.yaw > 0) {
|
||
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);
|
||
|
||
/* --- 3. 软件限位处理 (Pitch) --- */
|
||
if (g->param->travel.pit > 0) {
|
||
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);
|
||
|
||
/* --- 4. 闭环控制计算 (串级 PID + 前馈) --- */
|
||
float yaw_omega_set_point, pit_omega_set_point;
|
||
float yaw_v_ff = 0.0f, pit_v_ff = 0.0f;
|
||
|
||
switch (g->mode) {
|
||
case GIMBAL_MODE_RELAX:
|
||
g->out.yaw = 0.0f;
|
||
g->out.pit = 0.0f;
|
||
break;
|
||
|
||
case GIMBAL_MODE_AUTO_AIM:
|
||
/* 如果视觉在线，提取前馈项 */
|
||
if (g_cmd->vision.target_found) {
|
||
g->setpoint.eulr.pit=g_cmd->vision.pit;
|
||
/* 前馈系数建议放在参数结构体中，此处使用 1.0f 作为默认增益 */
|
||
yaw_v_ff = g_cmd->vision.yaw_v_ff * 0.2f;
|
||
pit_v_ff = g_cmd->vision.pit_v_ff * 0.2f;
|
||
}
|
||
/* 顺延执行 PID 计算 */
|
||
case GIMBAL_MODE_ABSOLUTE:
|
||
/* Yaw 轴：角度环 -> 速度环 + 前馈 */
|
||
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.yaw_omega), yaw_omega_set_point,
|
||
g->feedback.imu.gyro.z, 0.f, g->dt) + yaw_v_ff;
|
||
|
||
/* Pitch 轴：角度环 -> 速度环 + 前馈 */
|
||
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) + pit_v_ff;
|
||
if (g->mode == GIMBAL_MODE_AUTO_AIM && g_cmd->vision.target_found) {
|
||
g->out.pit=-g->out.pit;
|
||
}
|
||
break;
|
||
|
||
case GIMBAL_MODE_RELATIVE:
|
||
g->out.yaw = 0.0f;
|
||
g->out.pit = 0.0f;
|
||
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 运行云台控制逻辑（集成 AI 速度前馈与目标滤波）
|
||
*
|
||
* \param g 包含云台数据的结构体
|
||
* \param g_cmd 云台控制指令（包含遥控器增量及视觉数据）
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Gimbal_Control1(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);
|
||
|
||
/* --- 0. 视觉滤波器初始化与复位逻辑 --- */
|
||
if (!is_vision_lpf_init) {
|
||
/* 假设控制频率约为1000Hz(取决于实际dt)，截止频率设为15Hz。截止频率越低越平滑，但延迟越大 */
|
||
LowPassFilter2p_Init(&vision_yaw_lpf, 1.0f / g->dt, 15.0f);
|
||
LowPassFilter2p_Init(&vision_pit_lpf, 1.0f / g->dt, 15.0f);
|
||
is_vision_lpf_init = 1;
|
||
}
|
||
|
||
/* --- 1. 计算目标设定值增量 --- */
|
||
float delta_yaw = 0.0f;
|
||
float delta_pit = 0.0f;
|
||
|
||
if (g->mode == GIMBAL_MODE_AUTO_AIM && g_cmd->vision.target_found) {
|
||
/* 对视觉传来的绝对角度进行二阶低通滤波 */
|
||
float filtered_vision_yaw = LowPassFilter2p_Apply(&vision_yaw_lpf, g_cmd->vision.yaw);
|
||
float filtered_vision_pit = LowPassFilter2p_Apply(&vision_pit_lpf, g_cmd->vision.pit);
|
||
|
||
/* Yaw轴：圆周差值计算 */
|
||
delta_yaw = CircleError(filtered_vision_yaw, g->setpoint.eulr.yaw, M_2PI);
|
||
|
||
/* Pitch轴：纯线性差值计算。根据前置测试，如果极性反了，在此处添加负号 */
|
||
delta_pit = filtered_vision_pit - g->setpoint.eulr.pit; // 如果反向，改为 (-filtered_vision_pit) - g->setpoint.eulr.pit;
|
||
|
||
} else {
|
||
/* 丢失目标或非自瞄模式：复位滤波器，防止下次切入自瞄时产生剧烈阶跃 */
|
||
LowPassFilter2p_Reset(&vision_yaw_lpf, g->setpoint.eulr.yaw);
|
||
LowPassFilter2p_Reset(&vision_pit_lpf, g->setpoint.eulr.pit);
|
||
|
||
/* 使用操作手遥控器增量 */
|
||
delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
|
||
delta_pit = g_cmd->delta_pit * g->dt;
|
||
}
|
||
|
||
/* --- 2. 软件限位处理 (Yaw) --- */
|
||
if (g->param->travel.yaw > 0) {
|
||
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);
|
||
|
||
/* --- 3. 软件限位处理 (Pitch) --- */
|
||
/* Pitch 轴必须使用纯线性累加和限幅，严禁使用 CircleAdd */
|
||
g->setpoint.eulr.pit += delta_pit;
|
||
|
||
if (g->param->travel.pit > 0) {
|
||
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;
|
||
|
||
float target_abs_pit = g->setpoint.eulr.pit + motor_imu_offset;
|
||
|
||
if (target_abs_pit > g->limit.pit.max) {
|
||
g->setpoint.eulr.pit = g->limit.pit.max - motor_imu_offset;
|
||
} else if (target_abs_pit < g->limit.pit.min) {
|
||
g->setpoint.eulr.pit = g->limit.pit.min - motor_imu_offset;
|
||
}
|
||
}
|
||
|
||
/* --- 4. 闭环控制计算 (串级 PID + 前馈) --- */
|
||
float yaw_omega_set_point, pit_omega_set_point;
|
||
float yaw_v_ff = 0.0f, pit_v_ff = 0.0f;
|
||
|
||
switch (g->mode) {
|
||
case GIMBAL_MODE_RELAX:
|
||
g->out.yaw = 0.0f;
|
||
g->out.pit = 0.0f;
|
||
break;
|
||
|
||
case GIMBAL_MODE_AUTO_AIM:
|
||
if (g_cmd->vision.target_found) {
|
||
/* 注意：已删除原有的强行赋值 g->setpoint.eulr.pit=g_cmd->vision.pit; */
|
||
yaw_v_ff = g_cmd->vision.yaw_v_ff * 0.2f;
|
||
pit_v_ff = g_cmd->vision.pit_v_ff * 0.2f;
|
||
}
|
||
/* 顺延执行 PID 计算 */
|
||
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.yaw_omega), yaw_omega_set_point,
|
||
g->feedback.imu.gyro.z, 0.f, g->dt) + yaw_v_ff;
|
||
|
||
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) + pit_v_ff;
|
||
g->out.pit=-g->out.pit; g->out.pit=-g->out.pit;
|
||
/* 注意：已删除此处的 g->out.pit = -g->out.pit; 修正输出极性应在外部 Gimbal_Output 或 PID 计算环节统一处理 */
|
||
break;
|
||
|
||
case GIMBAL_MODE_RELATIVE:
|
||
g->out.yaw = 0.0f;
|
||
g->out.pit = 0.0f;
|
||
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 g 包含云台数据的结构体
|
||
*/
|
||
void Gimbal_Output(Gimbal_t *g){
|
||
if (1) {
|
||
/* 标志位为1：正常工作时的输出逻辑 */
|
||
|
||
/* 1. 对 Yaw 轴输出(大疆 RM 电机)进行限幅 */
|
||
float target_yaw_out = g->out.yaw;
|
||
if (target_yaw_out > YAW_MAX_OUTPUT) {
|
||
target_yaw_out = YAW_MAX_OUTPUT;
|
||
} else if (target_yaw_out < -YAW_MAX_OUTPUT) {
|
||
target_yaw_out = -YAW_MAX_OUTPUT;
|
||
}
|
||
MOTOR_RM_SetOutput(&g->param->yaw_motor, target_yaw_out);
|
||
|
||
/* 2. 计算 Pitch 轴扭矩(达妙 DM 电机)并进行限幅 */
|
||
float target_pit_torque = g->out.pit * 3.0f; // 乘以减速比
|
||
if (target_pit_torque > PIT_MAX_TORQUE) {
|
||
target_pit_torque = PIT_MAX_TORQUE;
|
||
} else if (target_pit_torque < -PIT_MAX_TORQUE) {
|
||
target_pit_torque = -PIT_MAX_TORQUE;
|
||
}
|
||
|
||
MOTOR_MIT_Output_t output = {0};
|
||
output.torque = target_pit_torque;
|
||
output.kd = 0.3f;
|
||
|
||
/* 下发控制指令 */
|
||
MOTOR_RM_Ctrl(&g->param->yaw_motor);
|
||
MOTOR_DM_MITCtrl(&g->param->pit_motor, &output);
|
||
} else {
|
||
/* 标志位为0：强制禁用逻辑，避免断线报错 */
|
||
MOTOR_RM_SetOutput(&g->param->yaw_motor, 0.0f); // 大疆电机强制0电流
|
||
|
||
MOTOR_MIT_Output_t output = {0}; // 达妙MIT模式全零初始化 (位置/速度/扭矩/kp/kd 均为0)
|
||
|
||
MOTOR_RM_Ctrl(&g->param->yaw_motor);
|
||
MOTOR_DM_MITCtrl(&g->param->pit_motor, &output);
|
||
}
|
||
} |