1.0

App/app_tasks.c

@@ -19,6 +19,8 @@
 #include "nav/corridor_ctrl.h"
 #include "nav/segment_fsm.h"
 #include "nav/nav_script.h"
+#include "nav/global_nav.h"
+#include "nav/track_map.h"
 extern osMutexId_t logMutexHandle;
 
 /* 如果你的项目中没有引入 i2c.h，可以通过 extern 声明 I2C 句柄 */
@@ -297,9 +299,14 @@ void AppTasks_RunNavTask_Impl(void *argument)
   const uint32_t period_ticks = AppTasks_MsToTicks(20U);
   uint32_t last_ms = HAL_GetTick();
 
-  /* 等传感器全部就绪再启动脚本 (避免刚上电全是脏数据) */
+  /* 等传感器全部就绪再启动 (避免刚上电全是脏数据) */
   osDelay(500);
-  NavScript_Start();  // 开始执行比赛脚本
+
+#if USE_GLOBAL_NAV
+  GlobalNav_Start();   /* 开始赛道级导航 */
+#else
+  NavScript_Start();   /* 单沟测试模式 */
+#endif
 
   for (;;) {
     uint32_t now_ms = HAL_GetTick();
@@ -317,11 +324,11 @@ void AppTasks_RunNavTask_Impl(void *argument)
     CorridorObs_t obs;
     CorridorPreproc_ExtractObs(&board, now_ms, &obs);
 
-    /* --- Step 3: 互补滤波 → 走廊状态估计 --- */
+    /* --- Step 3: EKF → 走廊状态估计 --- */
     /* 注意: HWT101 输出 wz 单位是 °/s，EKF 需要 rad/s，必须转换！ */
     float imu_wz_raw = board.imu_wz.is_valid ? board.imu_wz.value : 0.0f;
     float imu_wz  = PARAM_DEG2RAD(imu_wz_raw);
-    float odom_vx = board.odom_vx;  /* 里程计已接入，由 monitorTask 调用 Odom_Update() 更新 */
+    float odom_vx = board.odom_vx;
 
     /* IMU 连续 yaw → rad，作为 EKF 额外航向观测 */
     float imu_yaw_cont_rad = board.imu_yaw_continuous.is_valid
@@ -332,34 +339,62 @@ void AppTasks_RunNavTask_Impl(void *argument)
     CorridorFilter_Update(&obs, imu_wz, odom_vx, dt_s,
                           imu_yaw_cont_rad, imu_yaw_ok, &corridor_state);
 
-    /* --- Step 4: 段脚本执行器 → 决定当前阶段和目标 --- */
+#if USE_GLOBAL_NAV
+    /* ========== 赛道级导航模式 (6沟 S 型遍历) ========== */
+
+    /* --- Step 4: 赛道级导航 --- */
+    GlobalNavOutput_t nav_out;
+    GlobalNav_Update(&obs, &corridor_state, &board, &nav_out);
+
+    /* --- Step 5: 控制律 --- */
+    RawCmd_t raw_cmd;
+    memset(&raw_cmd, 0, sizeof(raw_cmd));
+    raw_cmd.t_ms = now_ms;
+
+    if (nav_out.request_corridor) {
+      /* 沟内闭环：使用走廊控制器 */
+      CorridorCtrl_Compute(&corridor_state, &obs, imu_wz, &raw_cmd);
+    } else if (nav_out.use_override) {
+      /* 赛道级覆盖：直接用导航输出 */
+      raw_cmd.v = nav_out.override_v;
+      raw_cmd.w = nav_out.override_w;
+    }
+    /* else: raw_cmd 已是零速 */
+
+    /* --- Step 6: 安全仲裁 (带动作模式感知) --- */
+    SegFsmOutput_t fsm_out;
+    SegFsm_Update(&raw_cmd, &obs, &corridor_state, nav_out.safety_mode, &fsm_out);
+
+#else
+    /* ========== 单沟测试模式 (原 nav_script) ========== */
+
+    /* --- Step 4: 段脚本执行器 --- */
     NavScriptOutput_t script_out;
     float imu_yaw_cont_deg = board.imu_yaw_continuous.is_valid
                              ? board.imu_yaw_continuous.value : 0.0f;
     NavScript_Update(&obs, &corridor_state, imu_yaw_cont_deg, &script_out);
 
-    /* --- Step 5: 控制律 → 计算期望 v, w --- */
+    /* --- Step 5: 控制律 --- */
     RawCmd_t raw_cmd;
     if (script_out.use_override) {
-      /* 脚本覆盖模式：直接用脚本的输出（入口对准/原地转向/退出等） */
       raw_cmd.t_ms = now_ms;
       raw_cmd.v = script_out.override_v;
       raw_cmd.w = script_out.override_w;
       raw_cmd.flags = 0U;
     } else if (script_out.request_corridor) {
-      /* 走廊跟踪模式：使用走廊控制器 */
       CorridorCtrl_Compute(&corridor_state, &obs, imu_wz, &raw_cmd);
     } else {
-      /* 默认停车 */
       raw_cmd.t_ms = now_ms;
       raw_cmd.v = 0.0f;
       raw_cmd.w = 0.0f;
       raw_cmd.flags = 0U;
     }
 
-    /* --- Step 6: 段状态机 → 安全仲裁 --- */
+    /* --- Step 6: 安全仲裁 (CORRIDOR 模式，兼容旧行为) --- */
     SegFsmOutput_t fsm_out;
-    SegFsm_Update(&raw_cmd, &obs, &corridor_state, &fsm_out);
+    SegFsm_Update(&raw_cmd, &obs, &corridor_state, SAFETY_MODE_CORRIDOR, &fsm_out);
+
+#endif
 
     /* --- Step 7: 将安全后的指令喂给 CAN 发送层 --- */
     CmdSlot_Push(fsm_out.safe_v, fsm_out.safe_w, 0U);
@@ -408,7 +443,43 @@ void AppTasks_Init(void)
   SegFsm_Init(&fsm_cfg);
   SegFsm_Start();  /* P0 修复: 必须显式启动安全状态机，否则 IDLE 状态直接输出零速 */
 
-  /* --- 初始化段脚本执行器 --- */
+#if USE_GLOBAL_NAV
+  /* --- 初始化赛道地图 --- */
+  TrackMap_Init();
+
+  /* --- 初始化赛道级导航状态机 --- */
+  GlobalNavConfig_t gnav_cfg = {
+    .entry_v                = PARAM_GNAV_ENTRY_V,
+    .entry_distance         = PARAM_GNAV_ENTRY_DISTANCE,
+    .entry_timeout_ms       = PARAM_GNAV_ENTRY_TIMEOUT,
+    .turn_omega             = PARAM_GNAV_TURN_OMEGA,
+    .turn_tolerance_rad     = PARAM_GNAV_TURN_TOLERANCE,
+    .turn_decel_zone_rad    = PARAM_GNAV_TURN_DECEL_ZONE,
+    .turn_min_omega         = PARAM_GNAV_TURN_MIN_OMEGA,
+    .turn_timeout_ms        = PARAM_GNAV_TURN_TIMEOUT,
+    .reacquire_v            = PARAM_GNAV_REACQUIRE_V,
+    .reacquire_conf_thresh  = PARAM_GNAV_REACQUIRE_CONF,
+    .reacquire_width_tol    = PARAM_GNAV_REACQUIRE_WIDTH_TOL,
+    .reacquire_confirm_ticks = PARAM_GNAV_REACQUIRE_TICKS,
+    .reacquire_timeout_ms   = PARAM_GNAV_REACQUIRE_TIMEOUT,
+    .corridor_end_detect_dist = PARAM_GNAV_CORRIDOR_END_DIST,
+    .corridor_length_max    = PARAM_GNAV_CORRIDOR_MAX_LEN,
+    .link_v                 = PARAM_GNAV_LINK_V,
+    .link_distance          = PARAM_GNAV_LINK_DISTANCE,
+    .link_timeout_ms        = PARAM_GNAV_LINK_TIMEOUT,
+    .exit_v                 = PARAM_GNAV_EXIT_V,
+    .exit_runout            = PARAM_GNAV_EXIT_RUNOUT,
+    .exit_max_dist          = PARAM_GNAV_EXIT_MAX_DIST,
+    .exit_timeout_ms        = PARAM_GNAV_EXIT_TIMEOUT,
+    .dock_v                 = PARAM_GNAV_DOCK_V,
+    .dock_distance          = PARAM_GNAV_DOCK_DISTANCE,
+    .heading_kp             = PARAM_GNAV_HEADING_KP,
+    .corridor_width         = PARAM_CORRIDOR_WIDTH,
+  };
+  GlobalNav_Init(&gnav_cfg);
+
+#else
+  /* --- 初始化段脚本执行器 (单沟测试模式) --- */
   NavScriptConfig_t script_cfg = {
     .turn_target_angle    = 3.14159265f,                 /* 固定：180度转向 */
     .turn_omega           = PARAM_SCRIPT_TURN_OMEGA,     /* 调优：转向角速度 */
@@ -420,4 +491,5 @@ void AppTasks_Init(void)
     .exit_v               = PARAM_SCRIPT_EXIT_V,         /* P1 修复：退出直线速度独立参数 */
   };
   NavScript_Init(&script_cfg);
+#endif
 }