1.0

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

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 {