diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index f1af954..3d13cf6 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -208,16 +208,16 @@ static void CMD_NUC_BuildGimbalCmd(CMD_t *ctx) { /* 使用AI提供的云台控制数据 */ - if (ctx->input.nuc.mode!=0) { + // if (ctx->input.nuc.mode!=0) { ctx->output.gimbal.cmd.mode = GIMBAL_MODE_AI_CONTROL; - ctx->output.gimbal.cmd.ai_mode = ctx->input.nuc.mode; + ctx->output.gimbal.cmd.mode_ai = (uint8_t)ctx->input.nuc.mode; /* 直接使用AI模式细分字段 */ ctx->output.gimbal.cmd.setpoint_yaw = ctx->input.nuc.gimbal.setpoint.yaw; ctx->output.gimbal.cmd.setpoint_pit = ctx->input.nuc.gimbal.setpoint.pit; ctx->output.gimbal.cmd.ff_vel_yaw = ctx->input.nuc.gimbal.vel.yaw; ctx->output.gimbal.cmd.ff_vel_pit = ctx->input.nuc.gimbal.vel.pit; ctx->output.gimbal.cmd.ff_accl_yaw = ctx->input.nuc.gimbal.accl.yaw; ctx->output.gimbal.cmd.ff_accl_pit = ctx->input.nuc.gimbal.accl.pit; - } + // } } #endif /* CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_GIMBAL */ diff --git a/User/module/gimbal.c b/User/module/gimbal.c index f81688f..e2ddddf 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -193,6 +193,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f; g->lask_wakeup = BSP_TIME_Get_us(); + /* AI自瞄保护:mode_ai==0 丢失目标,保持AI模式但锁定当前姿态 */ Gimbal_SetMode(g, g_cmd->mode); /* 处理yaw控制命令,软件限位 */ @@ -215,18 +216,10 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { } CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI); - if (g_cmd->mode == GIMBAL_MODE_AI_CONTROL) { - if (g_cmd->setpoint_yaw == 0.0f && g_cmd->setpoint_pit == 0.0f) { - /* 自瞄丢失:切回ABSOLUTE,setpoint锁定当前IMU角度,停在原位 */ - g_cmd->mode = GIMBAL_MODE_ABSOLUTE; - g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; - g->setpoint.eulr.pit = g->feedback.imu.eulr.rol; - Gimbal_SetMode(g, GIMBAL_MODE_ABSOLUTE); - } else { - /* AI 模式:直接从指令获取角度设定值(每周期刷新) */ - g->setpoint.eulr.yaw = g_cmd->setpoint_yaw; - g->setpoint.eulr.pit = g_cmd->setpoint_pit; - } + if (g_cmd->mode == GIMBAL_MODE_AI_CONTROL && g_cmd->mode_ai != 0) { + /* AI 模式且有目标(mode_ai==1/2):跟踪,用AI下发的setpoint */ + g->setpoint.eulr.yaw = g_cmd->setpoint_yaw; + g->setpoint.eulr.pit = g_cmd->setpoint_pit; } else if (g_cmd->mode == GIMBAL_MODE_RECOVER) { /* 双零点选近:mech_zero 和 mech_zero+π,选离当前编码器角度最近的 */ float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle; @@ -260,22 +253,36 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { break; case GIMBAL_MODE_AI_CONTROL: - /* --- YAW --- */ - yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw, - g->feedback.imu.eulr.yaw, 0.0f, g->dt); - yaw_omega_set_point += g_cmd->ff_vel_yaw; /* 速度前馈叠加到角速度设定值 */ - g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point, - g->feedback.imu.gyro.z, 0.f, g->dt); - g->out.yaw += g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA; /* 加速度前馈叠加到力矩输出 */ + if (g_cmd->mode_ai == 0) { + /* 丢失目标:setpoint保持不变,用普通PID锁住当前姿态,无前馈 */ + 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); - /* --- PIT --- */ - pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), -g->setpoint.eulr.pit, - g->feedback.imu.eulr.rol, 0.0f, g->dt); - pit_omega_set_point -= g_cmd->ff_vel_pit; /* 速度前馈 */ - g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point, - g->feedback.imu.gyro.y, 0.f, g->dt); - g->out.pit -= g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA; /* 加速度前馈 */ - break; + 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); + } else { + /* mode_ai==1/2:跟踪目标,带前馈 */ + /* --- YAW --- */ + yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw, + g->feedback.imu.eulr.yaw, 0.0f, g->dt); + yaw_omega_set_point += g_cmd->ff_vel_yaw; + g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point, + g->feedback.imu.gyro.z, 0.f, g->dt); + g->out.yaw += g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA; + + /* --- PIT --- */ + pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), -g->setpoint.eulr.pit, + g->feedback.imu.eulr.rol, 0.0f, g->dt); + pit_omega_set_point -= g_cmd->ff_vel_pit; + g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point, + g->feedback.imu.gyro.y, 0.f, g->dt); + g->out.pit -= g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA; + } + break; case GIMBAL_MODE_RECOVER: g->out.yaw = PID_Calc(&(g->pid.recover.yaw_angle), g->setpoint.eulr.yaw, g->feedback.motor.yaw.rotor_abs_angle, 0.0f, g->dt); diff --git a/User/module/gimbal.h b/User/module/gimbal.h index abf7a61..b111c7f 100644 --- a/User/module/gimbal.h +++ b/User/module/gimbal.h @@ -41,7 +41,7 @@ typedef struct { typedef struct { Gimbal_Mode_t mode; - Gimbal_Mode_t ai_mode; /* AI控制模式下的子模式,用于区分不同的AI控制策略 */ + uint8_t mode_ai; /* AI模式细分,0表示不控制,1表示控制云台但不开火,2表示控制云台且开火 */ float delta_yaw; float delta_pit; /* GIMBAL_MODE_AI_CONTROL 速度/加速度前馈(来自 NUC) */