修复上电云台正方向问题，以及初步调好了底盘跟随PID，以及小陀螺模式下的运动偏移问题。

底盘/modules/control/chassis.c

@@ -14,6 +14,7 @@
  * 功率:  chassis_control() 的 power_scale 直接缩放四轮输出电流
  * ========================================================================== */
 #include "chassis.h"
+#include <math.h>
 #include "pid.h"
 #include "filter.h"
 #include "attitude.h"
@@ -153,32 +154,30 @@ float chassis_follow_sign(void)
     return dbg_follow_sign * (float)ctoc_encoder_ccw;
 }
 
-/* 跟随一步:反馈为 Yaw GM6020 编码器相对角(零漂移),输出 w */
+/* 跟随一步:反馈为 Yaw GM6020 编码器对齐设定机械零位(4050),输出 w */
 float chassis_follow_w(void)
 {
-    /* 尚未收到 0x205 反馈时不锁定基准(避免首帧假偏差),输出 0 */
+    /* 尚未收到 0x205 反馈时输出 0 */
     if (motor_yaw_follow.fdb.last_online_tick == 0)
-    {
-        follow_latched = 0;
         return 0;
-    }
 
-    if (follow_reset_request || !follow_latched)
+    if (follow_reset_request)
     {
         follow_reset_request = 0;
-        follow_target_tick = (float)motor_yaw_follow.fdb.total_tick;
-        follow_latched = 1;
         PID_PositionClean(&follow_pid);
     }
 
+    /* 计算当前机械编码值距离设定零位(4050)的最短差值(-4096 ~ +4096) */
+    int16_t tick_diff = (int16_t)motor_yaw_follow.fdb.raw_angle - (int16_t)YAW_GM6020_ENCODER_ZERO;
+    if (tick_diff > 4096)
+        tick_diff -= 8192;
+    else if (tick_diff < -4096)
+        tick_diff += 8192;
+
     /* 相对角误差(°):tick 增向按云台自检结果折算为 CCW 正 */
-    float err_deg = ((float)motor_yaw_follow.fdb.total_tick - follow_target_tick) * 360.0f / 8192.0f;
+    float err_deg = (float)tick_diff * (360.0f / 8192.0f);
     if (ctoc_encoder_ccw < 0)
         err_deg = -err_deg;
-    if (err_deg > 180.0f)
-        err_deg -= 360.0f;
-    else if (err_deg < -180.0f)
-        err_deg += 360.0f;
 
     if (err_deg > -CHASSIS_FOLLOW_DEADBAND && err_deg < CHASSIS_FOLLOW_DEADBAND)
         err_deg = 0;
@@ -195,3 +194,42 @@ void chassis_output_zero(void)
 {
     dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
 }
+
+#ifndef PI
+#define PI 3.14159265358979323846f
+#endif
+
+void chassis_vector_to_body(float vx_g, float vy_g, float w, float * vx_c, float * vy_c)
+{
+    /* 电机掉线兜底:若无云台编码器反馈,直接直通输出 */
+    if (motor_yaw_follow.fdb.last_online_tick == 0)
+    {
+        *vx_c = vx_g;
+        *vy_c = vy_g;
+        return;
+    }
+
+    /* 1. 计算当前机械编码值距离设定零位的最短差值(-4096 ~ +4096) */
+    int16_t tick_diff = (int16_t)motor_yaw_follow.fdb.raw_angle - (int16_t)YAW_GM6020_ENCODER_ZERO;
+    if (tick_diff > 4096)
+        tick_diff -= 8192;
+    else if (tick_diff < -4096)
+        tick_diff += 8192;
+
+    /* 2. 相对机械角误差(rad):tick 增向按云台自检结果折算为 CCW 正 */
+    float alpha_rad = (float)tick_diff * (2.0f * PI / 8192.0f);
+    int8_t enc_sign = (ctoc_encoder_ccw != 0) ? ctoc_encoder_ccw : 1;
+    if (enc_sign < 0)
+        alpha_rad = -alpha_rad;
+
+    /* 3. 叠加角速度前馈补偿(-kw 消除通信及响应时延滞后) */
+    float alpha_comp = alpha_rad - (CHASSIS_SPIN_FEEDFORWARD_K * w);
+
+    /* 4. 二维旋转矩阵投影(修正正负号: 适配 vx<0前进, vy>0向左 的坐标系定义) */
+    float cos_a = cosf(alpha_comp);
+    float sin_a = sinf(alpha_comp);
+
+    *vx_c =  vx_g * cos_a + vy_g * sin_a;
+    *vy_c = -vx_g * sin_a + vy_g * cos_a;
+}
+

底盘/modules/control/chassis.h

@@ -46,4 +46,8 @@ extern volatile float dbg_follow_err_deg;
 extern volatile float dbg_follow_out;
 extern volatile float dbg_follow_sign;
 
-#endif
+/* 将以云台为基准的平移速度(vx_g, vy_g)旋转投影到底盘自身坐标系(vx_c, vy_c)
+ * 无论底盘是否处于小陀螺自旋状态，使 vx_g 始终代表沿云台枪口方向前进 */
+extern void chassis_vector_to_body(float vx_g, float vy_g, float w, float * vx_c, float * vy_c);
+
+#endif