1.0

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;