From 4d0c531344e08d76a24fec9703c433b309067197 Mon Sep 17 00:00:00 2001
From: nitiantuhao <2062405236@qq.com>
Date: Sat, 4 Apr 2026 14:49:33 +0800
Subject: [PATCH] 1.0

---
 App/app_tasks.c      |  2 +-
 App/nav/global_nav.c | 16 +++++-----------
 App/nav/global_nav.h | 10 ++++++----
 3 files changed, 12 insertions(+), 16 deletions(-)

diff --git a/App/app_tasks.c b/App/app_tasks.c
index 3e78836..d1a56bc 100644
--- a/App/app_tasks.c
+++ b/App/app_tasks.c
@@ -344,7 +344,7 @@ void AppTasks_RunNavTask_Impl(void *argument)
 
     /* --- Step 4: 赛道级导航 --- */
     GlobalNavOutput_t nav_out;
-    GlobalNav_Update(&obs, &corridor_state, &board, &nav_out);
+    GlobalNav_Update(&obs, &corridor_state, &board, now_ms, &nav_out);
 
     /* --- Step 5: 控制律 --- */
     RawCmd_t raw_cmd;
diff --git a/App/nav/global_nav.c b/App/nav/global_nav.c
index 475f09b..cfd567c 100644
--- a/App/nav/global_nav.c
+++ b/App/nav/global_nav.c
@@ -415,6 +415,7 @@ const char* GlobalNav_GetStageName(GlobalNavStage_t stage)
 void GlobalNav_Update(const CorridorObs_t*      obs,
                       const CorridorState_t*     state,
                       const RobotBlackboard_t*   board,
+                      uint32_t                   now_ms,
                       GlobalNavOutput_t*         out)
 {
     if (!s_nav.initialized) {
@@ -422,23 +423,16 @@ void GlobalNav_Update(const CorridorObs_t*      obs,
         return;
     }
 
-    uint32_t now_ms = s_last_update_ms;
-    /* 用 HAL_GetTick 代理 — 实际上 app_tasks 会传入 board 里的时间 */
+    /* 里程计积分: Δd = vx * dt
+     * now_ms 由调用方传入 (HAL_GetTick)，与任何传感器时间戳无关，
+     * 避免 IMU 时间戳停更时里程和超时双双冻结 (原 TODO-1 修复) */
     {
-        /* 里程计积分: Δd = vx * dt */
         float odom_vx = board->odom_vx;
         if (s_last_update_ms > 0) {
-            /* 使用 board 中的时间戳估计当前时间 */
-            uint32_t cur_ms = board->imu_wz.timestamp_ms;
-            if (cur_ms == 0) cur_ms = s_last_update_ms + 20U;
-            float dt = (float)(cur_ms - s_last_update_ms) * 0.001f;
+            float dt = (float)(now_ms - s_last_update_ms) * 0.001f;
             if (dt > 0.0f && dt < 0.5f) {
                 s_nav.odom_distance_accum += gnav_fabsf(odom_vx) * dt;
             }
-            now_ms = cur_ms;
-        } else {
-            now_ms = board->imu_wz.timestamp_ms;
-            if (now_ms == 0) now_ms = 1;
         }
         s_last_update_ms = now_ms;
         s_last_odom_vx = odom_vx;
diff --git a/App/nav/global_nav.h b/App/nav/global_nav.h
index 21c8d9a..4779c2b 100644
--- a/App/nav/global_nav.h
+++ b/App/nav/global_nav.h
@@ -118,14 +118,16 @@ void             GlobalNav_Reset(void);
 
 /**
  * @brief 核心函数：每个导航周期调用一次
- * @param obs    预处理层的观测快照
- * @param state  EKF 走廊状态
- * @param board  黑板快照 (读 IMU yaw_continuous, odom)
- * @param out    导航输出
+ * @param obs     预处理层的观测快照
+ * @param state   EKF 走廊状态
+ * @param board   黑板快照 (读 IMU yaw_continuous, odom)
+ * @param now_ms  当前系统时间 (HAL_GetTick)，由调用方传入，避免内部依赖 IMU 时间戳
+ * @param out     导航输出
  */
 void GlobalNav_Update(const CorridorObs_t*      obs,
                       const CorridorState_t*     state,
                       const RobotBlackboard_t*   board,
+                      uint32_t                   now_ms,
                       GlobalNavOutput_t*         out);
 
 GlobalNavStage_t GlobalNav_GetStage(void);