From 9a2ff3e1501f570ba23313ffd12be0cd96fd244b Mon Sep 17 00:00:00 2001 From: Xiaocheng <2544262366@qq.com> Date: Wed, 18 Mar 2026 13:53:32 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/device/ai.c | 2 +- User/module/chassis.c | 23 ++++++++++++++++++++++- User/module/config.c | 2 +- User/module/gimbal.c | 21 ++++++++++++++------- User/task/gimbal.c | 1 + 5 files changed, 39 insertions(+), 10 deletions(-) diff --git a/User/device/ai.c b/User/device/ai.c index 56fc4ee..ad97b28 100644 --- a/User/device/ai.c +++ b/User/device/ai.c @@ -76,7 +76,7 @@ int8_t MCU_Send(PackageMCU_t* mcu,Gimbal_feedback_t* motor,AHRS_Quaternion_t* qu return DEVICE_ERR; } - mcu->data.mode=0; + mcu->data.mode=1; mcu->data.q[0]=motor->imu.quat.q0; mcu->data.q[1]=motor->imu.quat.q1; mcu->data.q[2]=motor->imu.quat.q2; diff --git a/User/module/chassis.c b/User/module/chassis.c index b6f6e03..05af61b 100644 --- a/User/module/chassis.c +++ b/User/module/chassis.c @@ -64,6 +64,8 @@ fp32 vofa_send[8]; //vofa输出数据 #define CHASSIS_ROTOR_OMEGA 0.001 #define CHASSIS_ROTOR_WZ_MIN 0.6f /* 小陀螺旋转位移下界 */ #define CHASSIS_ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上界 */ +#define CHASSIS_FOLLOW_LOCK_DEADBAND_RAD 0.02f /* 跟随锁定死区，约1.1度 */ +#define CHASSIS_FOLLOW_VW_DEADBAND 0.03f /* 跟随角速度输出死区 */ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode ,uint32_t now) { @@ -405,7 +407,26 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now) // 跟随云台模式 c->move_vec.Vx =-c_cmd->Vy; c->move_vec.Vy =-c_cmd->Vx; - c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 4.51735544f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt); + { + const float follow_target = c->mech_zero_4310; + const float follow_err = CircleError(follow_target, + c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, + M_2PI); + if (fabsf(follow_err) < CHASSIS_FOLLOW_LOCK_DEADBAND_RAD) { + c->move_vec.Vw = 0.0f; + PID_ResetIntegral(&c->pid.chassis_follow_gimbal_pid); + } else { + float follow_vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, + follow_target, + c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, + 0.0f, + c->dt); + if (fabsf(follow_vw) < CHASSIS_FOLLOW_VW_DEADBAND) { + follow_vw = 0.0f; + } + c->move_vec.Vw = follow_vw; + } + } // c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt); break; diff --git a/User/module/config.c b/User/module/config.c index 9b3937c..edad7ea 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -268,7 +268,7 @@ Config_Param_t config = { .mech_zero = { .yaw_6020 = 1.31f,/*1.31*/ - .yaw_4310 = 2.06f, /*大yaw零点*/ + .yaw_4310 = 4.517f, /*大yaw零点*/ .pitch_4310 = 0.93f, }, .travel = { diff --git a/User/module/gimbal.c b/User/module/gimbal.c index 3ce354f..7934062 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -56,7 +56,7 @@ static float g_scan_pitch_dir = 1.0f; static float g_scan_yaw_dir = 1.0f; static int8_t g_scan_yaw_edge_latch = 0; /* -1: min边界锁存, 1: max边界锁存 */ -#define GIMBAL_SCAN_SMALL_YAW_SPEED (0.20f) +#define GIMBAL_SCAN_SMALL_YAW_SPEED (0.40f) #define GIMBAL_SCAN_BIG_YAW_PERIOD_SEC (5.0f) #define GIMBAL_SCAN_PITCH_SPEED (0.16f) #define GIMBAL_SCAN_PITCH_AMPLITUDE (0.18f) @@ -66,6 +66,7 @@ static int8_t g_scan_yaw_edge_latch = 0; /* -1: min边界锁存, 1: max边界锁 #define GIMBAL_SCAN_PITCH_EDGE_MARGIN (0.12f) #define GIMBAL_SCAN_EDGE_SLOW_BAND (0.26f) #define GIMBAL_REMOTE_BIG_YAW_SPEED_GAIN (1.6f) +#define GIMBAL_REMOTE_SMALL_YAW_GAIN (16.0f) #define GIMBAL_REMOTE_EDGE_ASSIST_BAND (0.20f) #define GIMBAL_YAW6020_INPUT_DEADBAND (0.0005f) #define GIMBAL_CONTROL_DT_MAX (0.02f) @@ -218,7 +219,9 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){ } gimbal->feedback.imu.gyro = imu->gyro; - gimbal->feedback.imu.eulr.yaw = imu->eulr.yaw; + /* 先同步完整欧拉角，保证rol轴可用于90度装配下的pitch控制 */ + gimbal->feedback.imu.eulr = imu->eulr; + /* 保留pit映射，兼容现有使用pit坐标的逻辑 */ gimbal->feedback.imu.eulr.pit = -imu->eulr.rol; // 坐标系转换 gimbal->feedback.imu.quat = imu->quat; @@ -290,7 +293,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){ if (!g_scan_active) { float yaw_mid = 0.5f * (g->limit.yaw_6020.max + g->limit.yaw_6020.min); g_scan_active = 1; - g_scan_pitch_center = g->feedback.imu.eulr.pit; + g_scan_pitch_center = g->feedback.imu.eulr.rol; g_scan_pitch_dir = 1.0f; g_scan_yaw_dir = (yaw_abs >= yaw_mid) ? -1.0f : 1.0f; g_scan_yaw_edge_latch = 0; @@ -350,9 +353,9 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){ } } - if (g->feedback.imu.eulr.pit >= g_scan_pitch_center + GIMBAL_SCAN_PITCH_AMPLITUDE) { + if (g->feedback.imu.eulr.rol >= g_scan_pitch_center + GIMBAL_SCAN_PITCH_AMPLITUDE) { g_scan_pitch_dir = -1.0f; - } else if (g->feedback.imu.eulr.pit <= g_scan_pitch_center - GIMBAL_SCAN_PITCH_AMPLITUDE) { + } else if (g->feedback.imu.eulr.rol <= g_scan_pitch_center - GIMBAL_SCAN_PITCH_AMPLITUDE) { g_scan_pitch_dir = 1.0f; } @@ -376,7 +379,11 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){ } /*处理小yaw轴控制命令*/ -float delta_yaw_6020 = 10.0f * g_cmd->delta_yaw_6020 * g->dt; +float yaw_6020_gain = 10.0f; +if (g_cmd->ctrl_mode == GIMBAL_MODE_REMOTE) { + yaw_6020_gain = GIMBAL_REMOTE_SMALL_YAW_GAIN; +} +float delta_yaw_6020 = yaw_6020_gain * g_cmd->delta_yaw_6020 * g->dt; float yaw_abs_now = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle; float yaw_abs_target = yaw_abs_now; @@ -517,7 +524,7 @@ switch (g->mode) // g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出 /*控制云台4310电机 也是单环，但是加了重力补偿函数，可以根据不一样的情况去拟合函数*/ pitch_omega_set_point =PID_Calc(&g->pid.pitch_4310_angle,g->setpoint.eulr.pit, - g->feedback.imu.eulr.rol,-g->feedback.imu.gyro.y,g->dt); + g->feedback.imu.eulr.rol,-g->feedback.imu.gyro.y,g->dt); g->out.pitch_4310 = pitch_omega_set_point+g->param->K_forward_pid.K_vel*g->setpoint.pit_vel+g->param->K_forward_pid.K_accl*g->setpoint.pit_accl+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle); // g->out.pitch_4310 = PID_Calc(&g->pid.pitch_4310_omega,8*pitch_omega_set_point, // -g->feedback.imu.gyro.y,0.0f,g->dt)+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle); diff --git a/User/task/gimbal.c b/User/task/gimbal.c index e78cdcc..750f78b 100644 --- a/User/task/gimbal.c +++ b/User/task/gimbal.c @@ -83,6 +83,7 @@ if (cmd_gimbal.ctrl_mode == GIMBAL_MODE_AI) { Gimbal_UpdateFeedback(&gimbal); osMessageQueueReset(task_runtime.msgq.gimbal.yaw6020); +/* 底盘跟随统一使用大YAW反馈，避免跟随锁到小YAW */ osMessageQueuePut(task_runtime.msgq.gimbal.yaw6020, &gimbal.feedback.motor.yaw_4310_motor_feedback, 0, 0); osMessageQueueReset(task_runtime.msgq.gimbal.ai.feedback);