修改

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;	

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;

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 = {

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;
 

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);