1.0

App/app_tasks.c

@@ -142,29 +142,8 @@ void AppTasks_RunMonitorTask(void *argument)
                                   can_ctx->status.diag_bits,
                                   can_ctx->status.online);
 
-    // 4. 里程计更新：从 CAN 0x200 累加器原子取走增量，积分并推送黑板
+    // 4. 里程计更新已迁移到 navTask(20ms)，此处不再消费
-    //    使用 SNC_CAN_ConsumeOdomDelta() 解决漏积分/重复积分问题：
+    //    原因：odom 实时性对 EKF 预测至关重要，需要与导航闭环同频
-    //    - ISR 侧每帧累加，不覆盖
-    //    - 此处原子取走并清零，每份增量恰好消费一次
-    {
-      int16_t  odom_fl, odom_rl, odom_fr, odom_rr;
-      uint32_t odom_span_ms;
-      uint8_t  odom_frames = SNC_CAN_ConsumeOdomDelta(
-                               &odom_fl, &odom_rl,
-                               &odom_fr, &odom_rr,
-                               &odom_span_ms);
-
-      if (odom_frames > 0U) {
-        /* 有新的增量帧需要积分 */
-        Odom_Update(now_ms,
-                    odom_fl, odom_rl,
-                    odom_fr, odom_rr,
-                    odom_span_ms);
-      } else {
-        /* odom 断流保护：超过协议允许时间后主动清零黑板里的速度 */
-        Odom_HandleTimeout(now_ms, SNC_ODOM_TIMEOUT_MS);
-      }
-    }
 
     /* 指令下发已由 navTask 接管，此处不再发送测试指令 */
     // 调用纯净版播报员 (内部自带分频逻辑)
@@ -316,6 +295,27 @@ void AppTasks_RunNavTask_Impl(void *argument)
     }
     last_ms = now_ms;
 
+    /* --- Step 0: 里程计更新（从 monitorTask 迁移至此，20ms 实时消费）--- */
+    {
+      int16_t  odom_fl, odom_rl, odom_fr, odom_rr;
+      uint32_t odom_span_ms;
+      uint8_t  odom_frames = SNC_CAN_ConsumeOdomDelta(
+                               &odom_fl, &odom_rl,
+                               &odom_fr, &odom_rr,
+                               &odom_span_ms);
+
+      if (odom_frames > 0U) {
+        /* 有新的增量帧需要积分 */
+        Odom_Update(now_ms,
+                    odom_fl, odom_rl,
+                    odom_fr, odom_rr,
+                    odom_span_ms);
+      } else {
+        /* odom 断流保护：超过协议允许时间后主动清零黑板里的速度 */
+        Odom_HandleTimeout(now_ms, SNC_ODOM_TIMEOUT_MS);
+      }
+    }
+
     /* --- Step 1: 拍摄黑板快照 --- */
     RobotBlackboard_t board;
     Blackboard_GetSnapshot(&board);