From ee63bee6e551f36af9f37ecf75d8314719c9faac Mon Sep 17 00:00:00 2001
From: nitiantuhao <2062405236@qq.com>
Date: Sat, 4 Apr 2026 15:59:11 +0800
Subject: [PATCH] 1.0

---
 App/nav/corridor_ctrl.c | 6 +++---
 App/nav/nav_script.c    | 5 +++++
 App/robot_params.h      | 6 +++---
 3 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/App/nav/corridor_ctrl.c b/App/nav/corridor_ctrl.c
index 8596d6b..9132796 100644
--- a/App/nav/corridor_ctrl.c
+++ b/App/nav/corridor_ctrl.c
@@ -42,9 +42,9 @@ void CorridorCtrl_Compute(const CorridorState_t *state,
      * - kp_y * e_y : 横向纠偏，车身偏了就产生角速度拉回来
      * ======================================================== */
 
-    float w_cmd = s_cfg.kp_theta * state->e_th
-                + s_cfg.kd_theta * (-imu_wz)
-                + s_cfg.kp_y    * state->e_y;
+    float w_cmd = -(s_cfg.kp_theta * state->e_th
+                  + s_cfg.kd_theta * imu_wz
+                  + s_cfg.kp_y    * state->e_y);
 
     /* 角速度限幅：防止 PD 溢出导致原地打转 */
     w_cmd = clampf(w_cmd, -s_cfg.w_max, s_cfg.w_max);
diff --git a/App/nav/nav_script.c b/App/nav/nav_script.c
index 2b8a871..145c2be 100644
--- a/App/nav/nav_script.c
+++ b/App/nav/nav_script.c
@@ -4,6 +4,7 @@
  */
 
 #include "nav_script.h"
+#include "est/corridor_filter.h"
 #include <stdint.h>
 #include <stdbool.h>
 #include <math.h>
@@ -215,6 +216,10 @@ void NavScript_Update(const CorridorObs_t *obs,
                 /* 转向完成 -> 决定下一步 */
                 if (s_internal.pass_count < 2) {
                     /* 只走了一遍，往回走 */
+                    /* 180° 掉头后，走廊方向基准已经翻转。
+                     * 必须清空上一趟的 EKF/IMU 航向参考，避免返程首拍把新朝向
+                     * 误判成大航向误差，导致一恢复闭环就猛打方向。 */
+                    CorridorFilter_Reset();
                     s_internal.pass_count++;
                     s_stage = SCRIPT_STAGE_CORRIDOR_BACKWARD;
                 } else {
diff --git a/App/robot_params.h b/App/robot_params.h
index e4ea46c..a58916f 100644
--- a/App/robot_params.h
+++ b/App/robot_params.h
@@ -101,7 +101,7 @@ extern "C" {
  *  测量方法：查阅 F407 固件配置，或实车转动一圈数脉冲
  *  典型值：500 (F407 默认固件)
  */
-#define PARAM_ENCODER_CPR           500
+#define PARAM_ENCODER_CPR           3680
 
 /* =========================================================
  * 【P1】里程计参数 (实车标定)
@@ -226,7 +226,7 @@ extern "C" {
  *  过大：车头转动被过度抑制，响应迟钝
  *  过小：阻尼不足，容易超调震荡
  */
-#define PARAM_CTRL_KD_THETA         0.1f
+#define PARAM_CTRL_KD_THETA         0.4f
 
 /** @brief [调优] 横向比例增益 kp_y [1/(m·s)]
  *  含义：横向偏差 1m 时产生的角速度修正
@@ -376,7 +376,7 @@ extern "C" {
  * ========================================================= */
 
 /** @brief 编译开关：1=赛道模式(6沟S型遍历)  0=单沟测试模式(原nav_script) */
-#define USE_GLOBAL_NAV                1
+#define USE_GLOBAL_NAV                0
 
 /* --- 入场段 --- */
 /* 启动区入口(Y=40)距第一条垄沟(Y=36~39)极近，入场距离仅约 10~40cm */