robofish/rm_balance
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

上ai

Browse Source
This commit is contained in:
Robofish 2026-03-19 07:54:36 +08:00
parent 5133123dc3
commit c72de9e0c3
3 changed files with 38 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
User/module/cmd/cmd.c
View File

@ -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 */

35
User/module/gimbal.c
View File

@ -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 模式：直接从指令获取角度设定值（每周期刷新） */
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,21 +253,35 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
break;
case GIMBAL_MODE_AI_CONTROL:
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_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; /* 速度前馈叠加到角速度设定值 */
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; /* 加速度前馈叠加到力矩输出 */
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; /* 速度前馈 */
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; /* 加速度前馈 */
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,

2
User/module/gimbal.h
View File

@ -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） */