From 557a41e8b6ba61164050b606ede5510d1abc0a1e Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Sun, 15 Mar 2026 16:20:58 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 User/module/balance_chassis.c | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c
index 7a2efce..ea7b3ef 100644
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@@ -770,7 +770,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     /* 小陀螺平移：将世界坐标系期望速度投影到机体坐标系 */
     float yaw_angle = c->feedback.imu.euler.yaw;
     float world_vx = c_cmd->move_vec.vx * c->param->rotor_ctrl.max_trans_speed;
-    c->chassis_state.target_velocity_x = world_vx * cosf(yaw_angle);
+    float world_vy = c_cmd->move_vec.vy * c->param->rotor_ctrl.max_trans_speed;
+    c->chassis_state.target_velocity_x = world_vx * cosf(yaw_angle) + world_vy * sinf(yaw_angle);
     c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
     c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt;
   } else {
@@ -819,8 +820,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
 
   /* ==================== Yaw轴控制 ==================== */
   if (c_cmd->mode == CHASSIS_MODE_BALANCE_ROTOR || c->mode == CHASSIS_MODE_BALANCE_ROTOR) {
-    /* 小陀螺模式：PID跟踪目标角速度，带速度斜坡 */
-    float target_spin = c_cmd->move_vec.vy * c->param->rotor_ctrl.max_spin_speed;
+    /* 小陀螺模式：自动以max_spin_speed恒速旋转 */
+    float target_spin = c->param->rotor_ctrl.max_spin_speed;
 
     if (c->rotor_state.exiting) {
       /* 退出过渡：目标角速度为0，用减速斜坡 */
@@ -860,7 +861,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
       c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
                                           c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
     } else {
-      float current_yaw_omega = c->feedback.imu.gyro.z;
+      float current_yaw_omega = -c->feedback.imu.gyro.z;  /* 取反匹配yaw_force→轮子旋转方向 */
       c->yaw_control.yaw_force = PID_Calc(&c->pid.rotor, c->rotor_state.current_spin_speed,
                                           current_yaw_omega, 0.0f, c->dt);
     }