导航部分效果最好的版本，主分支

App/Contract/chassis_can_msg.h

@@ -0,0 +1,68 @@
+#ifndef CHASSIS_CAN_MSG_H
+#define CHASSIS_CAN_MSG_H
+
+#include <stdint.h>
+
+/* 强制 1 字节对齐，完美匹配物理总线上的 8 字节数据帧 */
+#pragma pack(push, 1)
+
+/* =========================================================
+ * TX: 上位机 (H743) -> 底盘 (F407)
+ * ========================================================= */
+
+/**
+ * @brief 0x100 速度控制帧 (建议 20ms 发送周期) [cite: 608-612, 624]
+ */
+typedef struct {
+    int16_t vx_x1000;         // Byte 0-1: 线速度 m/s * 1000 (小端) [cite: 612]
+    int16_t wz_x1000;         // Byte 2-3: 角速度 rad/s * 1000 (小端) [cite: 612]
+    uint8_t ctrl_flags;       // Byte 4: 控制标志位 [cite: 612]
+    uint8_t reserved;         // Byte 5: 预留，固定填 0 [cite: 612]
+    uint8_t rolling_counter;  // Byte 6: 滚动计数器 (新counter相对上一帧差值必须在1..3之间) [cite: 612, 622]
+    uint8_t crc8;             // Byte 7: 对 Byte0~6 做的 CRC8-SAE J1850 [cite: 612]
+} CanMsg_CmdVel_0x100_t;
+
+
+/* =========================================================
+ * RX: 底盘 (F407) -> 上位机 (H743)
+ * ========================================================= */
+
+/**
+ * @brief 0x181 底盘状态帧 (20ms 发送周期) [cite: 633-637]
+ */
+typedef struct {
+    uint8_t  system_state;    // Byte 0: 0=BOOTING, 1=OPERATIONAL, 2=SAFE_FAULT [cite: 636, 665]
+    uint8_t  system_health;   // Byte 1: 0=OK, 1=WARNING, 2=FAULT [cite: 636, 667]
+    uint32_t diag_bits;       // Byte 2-5: 诊断位图 (小端) [cite: 637, 669]
+    uint8_t  cmd_age_10ms;    // Byte 6: 距最近一次合法 0x100 过去多少个 10ms [cite: 637]
+    uint8_t  status_counter;  // Byte 7: 状态帧发送计数器 [cite: 637]
+} CanMsg_Status_0x181_t;
+
+/**
+ * @brief 0x200 里程增量帧 (约 60ms 轮询周期) [cite: 655-658]
+ * @note  这是时间窗内的脉冲增量，上位机需自行累加积分以推算里程 [cite: 660-662]
+ */
+typedef struct {
+    int16_t fl_delta_ticks;   // Byte 0-1: 左前轮增量 [cite: 658]
+    int16_t rl_delta_ticks;   // Byte 2-3: 左后轮增量 [cite: 658]
+    int16_t fr_delta_ticks;   // Byte 4-5: 右前轮增量 [cite: 658]
+    int16_t rr_delta_ticks;   // Byte 6-7: 右后轮增量 [cite: 658]
+} CanMsg_OdomDelta_0x200_t;
+
+/**
+ * @brief 0x184 通信诊断帧 (100ms 发送周期) [cite: 651-653]
+ */
+typedef struct {
+    uint8_t valid_cmd_total;        // Byte 0: 合法命令累计 [cite: 653]
+    uint8_t crc_error_total;        // Byte 1: CRC 错误累计 [cite: 653]
+    uint8_t counter_reject_total;   // Byte 2: Counter 拒收累计 [cite: 653]
+    uint8_t can_tx_drop_total;      // Byte 3: CAN 发送丢帧累计 [cite: 653]
+    uint8_t busoff_total;           // Byte 4: Bus-Off 累计 [cite: 653]
+    uint8_t rx_overrun_total;       // Byte 5: FIFO overrun 累计 [cite: 653]
+    uint8_t last_accepted_counter;  // Byte 6: 最近一次接受的 counter [cite: 653]
+    uint8_t err_nibbles;            // Byte 7: 高4位=连续counter错，低4位=连续CRC错 [cite: 653]
+} CanMsg_CommDiag_0x184_t;
+
+#pragma pack(pop)
+
+#endif // CHASSIS_CAN_MSG_H

App/Contract/robot_blackboard.c

@@ -0,0 +1,111 @@
+#include "robot_blackboard.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include <string.h>
+
+/* 全局唯一的黑板实例 */
+static RobotBlackboard_t g_blackboard = {0};
+
+/* =========================================================
+ * 生产者 API 实现
+ * ========================================================= */
+
+void Blackboard_UpdateIMU(const HWT101_Data_t *imu_data) {
+    if (imu_data == NULL) return;
+    
+    taskENTER_CRITICAL();
+    // 更新原始航向角 ([-180, +180) 度)
+    g_blackboard.imu_yaw.value = imu_data->yaw;
+    g_blackboard.imu_yaw.timestamp_ms = imu_data->last_update;
+    g_blackboard.imu_yaw.is_valid = (imu_data->online != 0);
+
+    // 更新 unwrap 后的连续航向角 (无跳变, 度)
+    g_blackboard.imu_yaw_continuous.value = imu_data->yaw_continuous;
+    g_blackboard.imu_yaw_continuous.timestamp_ms = imu_data->last_update;
+    g_blackboard.imu_yaw_continuous.is_valid = (imu_data->online != 0);
+
+    // 更新角速度 (deg/s)
+    g_blackboard.imu_wz.value = imu_data->wz;
+    g_blackboard.imu_wz.timestamp_ms = imu_data->last_update;
+    g_blackboard.imu_wz.is_valid = (imu_data->online != 0);
+    taskEXIT_CRITICAL();
+}
+
+void Blackboard_UpdateVl53(const Vl53BoardSnapshot_t *vl_data) {
+    if (vl_data == NULL) return;
+    
+    taskENTER_CRITICAL();
+    // 约定: [0]左前, [1]左后, [2]右前, [3]右后
+    g_blackboard.dist_left_f.value        = vl_data->range_mm_filtered[0];
+    g_blackboard.dist_left_f.is_valid     = (vl_data->valid_mask & (1 << 0)) != 0;
+    g_blackboard.dist_left_f.timestamp_ms = vl_data->tick_ms;
+
+    g_blackboard.dist_left_r.value        = vl_data->range_mm_filtered[1];
+    g_blackboard.dist_left_r.is_valid     = (vl_data->valid_mask & (1 << 1)) != 0;
+    g_blackboard.dist_left_r.timestamp_ms = vl_data->tick_ms;
+
+    g_blackboard.dist_right_f.value        = vl_data->range_mm_filtered[2];
+    g_blackboard.dist_right_f.is_valid     = (vl_data->valid_mask & (1 << 2)) != 0;
+    g_blackboard.dist_right_f.timestamp_ms = vl_data->tick_ms;
+
+    g_blackboard.dist_right_r.value        = vl_data->range_mm_filtered[3];
+    g_blackboard.dist_right_r.is_valid     = (vl_data->valid_mask & (1 << 3)) != 0;
+    g_blackboard.dist_right_r.timestamp_ms = vl_data->tick_ms;
+    taskEXIT_CRITICAL();
+}
+
+void Blackboard_UpdateLaser(const laser_simple_snapshot_t *ls_data) {
+    if (ls_data == NULL) return;
+
+    taskENTER_CRITICAL();
+    
+    // 1. 前方 STP
+    g_blackboard.dist_front_stp.value = ls_data->ch[LASER_CH_FRONT_STP].distance_mm;
+    g_blackboard.dist_front_stp.is_valid = ls_data->ch[LASER_CH_FRONT_STP].valid;
+    g_blackboard.dist_front_stp.timestamp_ms = ls_data->ch[LASER_CH_FRONT_STP].update_tick_ms;
+
+    // 2. 前方 ATK
+    g_blackboard.dist_front_atk.value = ls_data->ch[LASER_CH_FRONT_ATK].distance_mm;
+    g_blackboard.dist_front_atk.is_valid = ls_data->ch[LASER_CH_FRONT_ATK].valid;
+    g_blackboard.dist_front_atk.timestamp_ms = ls_data->ch[LASER_CH_FRONT_ATK].update_tick_ms;
+
+    // 3. 后方 STP
+    g_blackboard.dist_rear_stp.value = ls_data->ch[LASER_CH_REAR_STP].distance_mm;
+    g_blackboard.dist_rear_stp.is_valid = ls_data->ch[LASER_CH_REAR_STP].valid;
+    g_blackboard.dist_rear_stp.timestamp_ms = ls_data->ch[LASER_CH_REAR_STP].update_tick_ms;
+
+    // 4. 后方 ATK
+    g_blackboard.dist_rear_atk.value = ls_data->ch[LASER_CH_REAR_ATK].distance_mm;
+    g_blackboard.dist_rear_atk.is_valid = ls_data->ch[LASER_CH_REAR_ATK].valid;
+    g_blackboard.dist_rear_atk.timestamp_ms = ls_data->ch[LASER_CH_REAR_ATK].update_tick_ms;
+
+    taskEXIT_CRITICAL();
+}
+
+void Blackboard_UpdateOdom(float vx, float wz) {
+    taskENTER_CRITICAL();
+    g_blackboard.odom_vx = vx;
+    g_blackboard.odom_wz = wz;
+    taskEXIT_CRITICAL();
+}
+
+void Blackboard_UpdateChassisState(uint8_t state, uint32_t diag, bool online) {
+    taskENTER_CRITICAL();
+    g_blackboard.chassis_state = state;
+    g_blackboard.chassis_diag = diag;
+    g_blackboard.chassis_online = online;
+    taskEXIT_CRITICAL();
+}
+
+/* =========================================================
+ * 消费者 API 实现
+ * ========================================================= */
+
+void Blackboard_GetSnapshot(RobotBlackboard_t *out_snapshot) {
+    if (out_snapshot == NULL) return;
+    
+    // 关中断拷贝整个结构体，保证上层算法拿到的数据处于同一个时间切片
+    taskENTER_CRITICAL();
+    *out_snapshot = g_blackboard;
+    taskEXIT_CRITICAL();
+}

App/Contract/robot_blackboard.h

@@ -0,0 +1,74 @@
+#ifndef ROBOT_BLACKBOARD_H
+#define ROBOT_BLACKBOARD_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "IMU/hwt101.h"         // 引用 IMU 接口
+#include "laser/laser_manager.h"  // 引用前后雷达接口
+#include "vl53_board.h"     // 引用侧向雷达接口
+
+/**
+ * @brief 标准原子数据，自带时间戳，专治“传感器偷偷掉线”
+ */
+typedef struct {
+    float    value;         // 数据本体 (距离、角度、速度等)
+    uint32_t timestamp_ms;  // 数据更新时的系统时间 (HAL_GetTick)
+    bool     is_valid;      // 驱动层判定的有效性 (如是否超量程、是否报警)
+} SensorItem_t;
+
+/**
+ * @brief 机器人全局情报中心 (System Context)
+ * @note  对齐抽象后的数据，算法层不需要再关心这是 1 号雷达还是 2 号雷达
+ */
+typedef struct {
+    /* --- 1. 空间姿态 (整合自 HWT101) --- */
+    SensorItem_t imu_yaw;            // 原始航向角 (deg)，范围 [-180, +180)，跨界时跳变
+    SensorItem_t imu_yaw_continuous; // unwrap 后的连续航向角 (deg)，无跳变，可直接做差计算转角
+    SensorItem_t imu_wz;             // Z轴角速度 (deg/s，上层使用前需转 rad/s)
+
+    /* --- 2. 侧向走廊观测 (整合自 Vl53Board) --- */
+    SensorItem_t dist_left_f;  // 左前侧向距离 (mm)
+    SensorItem_t dist_left_r;  // 左后侧向距离 (mm)
+    SensorItem_t dist_right_f; // 右前侧向距离 (mm)
+    SensorItem_t dist_right_r; // 右后侧向距离 (mm)
+
+    /* --- 3. 前后到端/防撞观测 (整合自 Laser Manager) --- */
+    SensorItem_t dist_front_stp; // 前方 STP 激光测距 (mm)
+    SensorItem_t dist_front_atk; // 前方 ATK 激光测距 (mm)
+    SensorItem_t dist_rear_stp;  // 后方 STP 激光测距 (mm)
+    SensorItem_t dist_rear_atk;  // 后方 ATK 激光测距 (mm)
+
+    /* --- 4. 底盘反馈 (整合自 CAN 0x181 & 0x200) --- */
+    float    odom_vx;          // 基于 ticks 积分推算的当前线速度 (m/s)
+    float    odom_wz;          // 基于 ticks 积分推算的当前角速度 (rad/s)
+    uint32_t chassis_diag;     // 底盘硬诊断位图 [cite: 669]
+    uint8_t  chassis_state;    // 0=BOOTING, 1=OPERATIONAL, 2=SAFE_FAULT [cite: 665]
+    bool     chassis_online;   // 底盘 CAN 通讯是否正常
+} RobotBlackboard_t;
+
+
+/* =========================================================
+ * 生产者 API (各种中断/驱动采集任务调用)
+ * ========================================================= */
+void Blackboard_UpdateIMU(const HWT101_Data_t *imu_data);
+void Blackboard_UpdateVl53(const Vl53BoardSnapshot_t *vl_data);
+void Blackboard_UpdateLaser(const laser_simple_snapshot_t *ls_data);
+
+// 由你的里程计解算函数调用 (传入算好的实时速度)
+void Blackboard_UpdateOdom(float vx, float wz);
+
+// 由 CAN 0x181 接收回调调用
+void Blackboard_UpdateChassisState(uint8_t state, uint32_t diag, bool online);
+
+
+/* =========================================================
+ * 消费者 API (算法/控制任务调用)
+ * ========================================================= */
+/**
+ * @brief 拍一张极速快照，获取全部对齐的传感器数据
+ * @note  内部自带临界区保护，绝对防撕裂
+ */
+void Blackboard_GetSnapshot(RobotBlackboard_t *out_snapshot);
+
+#endif // ROBOT_BLACKBOARD_H

App/Contract/robot_cmd_slot.c

@@ -0,0 +1,44 @@
+#include "robot_cmd_slot.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "main.h" // 提供 HAL_GetTick()
+
+/* 全局唯一的指令槽实例 */
+static RobotTargetCmd_t g_cmd_slot = {0};
+
+void CmdSlot_Push(float vx, float wz, uint8_t flags)
+{
+  // 1. 获取当前系统时间作为时间戳
+  uint32_t now_ms = HAL_GetTick();
+
+  // 2. 进入临界区，关中断，防止写入一半时被高优先级任务或中断打断（防数据撕裂）
+  taskENTER_CRITICAL();
+  g_cmd_slot.target_vx = vx;
+  g_cmd_slot.target_wz = wz;
+  g_cmd_slot.ctrl_flags = flags;
+  g_cmd_slot.gen_tick_ms = now_ms;
+  taskEXIT_CRITICAL();
+}
+
+bool CmdSlot_Pop(RobotTargetCmd_t *out_cmd, uint32_t timeout_ms)
+{
+  if (out_cmd == NULL) {
+    return false;
+  }
+
+  // 1. 进入临界区，安全地拷贝走整个结构体
+  taskENTER_CRITICAL();
+  *out_cmd = g_cmd_slot;
+  taskEXIT_CRITICAL();
+
+  // 2. 计算指令老化时间 (利用无符号整型减法自然处理溢出回绕)
+  uint32_t now_ms = HAL_GetTick();
+  uint32_t age_ms = now_ms - out_cmd->gen_tick_ms;
+
+  // 3. 判定是否超时
+  if (age_ms > timeout_ms) {
+    return false; // 🚨 指令太老了，算法层可能死机了！
+  }
+
+  return true; // ✅ 指令新鲜，可以发送
+}

App/Contract/robot_cmd_slot.h

@@ -0,0 +1,37 @@
+#ifndef ROBOT_CMD_SLOT_H
+#define ROBOT_CMD_SLOT_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/**
+ * @brief 算法层生成的抽象目标指令
+ */
+typedef struct {
+  float    target_vx;      // 目标线速度 (m/s)
+  float    target_wz;      // 目标角速度 (rad/s)
+  uint8_t  ctrl_flags;     // 控制模式/标志
+  uint32_t gen_tick_ms;    // 指令生成时的系统时间 (用于底层判断算法是否卡死)
+} RobotTargetCmd_t;
+
+/* =========================================================
+ * API 接口
+ * ========================================================= */
+
+/**
+ * @brief 写入接口：由主导航/控制任务调用
+ * @param vx    期望线速度 (m/s)
+ * @param wz    期望角速度 (rad/s)
+ * @param flags 扩展控制标志
+ */
+void CmdSlot_Push(float vx, float wz, uint8_t flags);
+
+/**
+ * @brief 读取接口：由 20ms 的 CAN Tx 任务专属调用
+ * @param out_cmd    获取到的指令拷贝
+ * @param timeout_ms 容忍算法不更新的最大时间 (推荐 100ms)
+ * @return true: 指令新鲜可用; false: 算法超时太久，建议强制发 0 停车保护
+ */
+bool CmdSlot_Pop(RobotTargetCmd_t *out_cmd, uint32_t timeout_ms);
+
+#endif // ROBOT_CMD_SLOT_H

App/Contract/robot_odom.c

@@ -0,0 +1,287 @@
+/**
+ * @file   robot_odom.c
+ * @brief  里程计积分实现：消费 CAN 0x200 轮增量帧，
+ *         经差分驱动运动学转换为机体速度并推送黑板。
+ *
+ *         调用链（monitorTask 100ms 周期）:
+ *           CAN Rx ISR ──> snc_parse_odom_delta()  [snc_can_app.c]
+ *                             │  (每帧累加到 odom_accum，不覆盖)
+ *                             v
+ *           monitorTask ──> SNC_CAN_ConsumeOdomDelta()  [snc_can_app.c]
+ *                             │  (原子取走累计增量并清零)
+ *                             v
+ *                          Odom_Update()  [robot_odom.c]
+ *                             │  (仅在有新帧时调用，杜绝重复积分)
+ *                             v
+ *           Blackboard_UpdateOdom(vx, wz)  [robot_blackboard.c]
+ *                             │
+ *                             v
+ *           navTask ──> Blackboard_GetSnapshot() ──> CorridorFilter_Update()
+ *
+ *         运动学公式:
+ *           v_left  = (fl_delta + rl_delta) / 2  * TICKS_TO_M  / dt
+ *           v_right = (fr_delta + rr_delta) / 2  * TICKS_TO_M  / dt
+ *           vx      = (v_left + v_right) / 2       [m/s]
+ *           wz      = (v_right - v_left) / track   [rad/s]
+ *
+ *         里程累加（用于沿程 s 估计）:
+ *           s += |vx| * dt   [m]
+ */
+
+#include "robot_odom.h"
+#include "robot_blackboard.h"
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include <math.h>
+
+/* =========================================================
+ * 内部静态状态
+ * ========================================================= */
+
+/** @brief 里程计运行时数据（全部由临界区保护） */
+static struct {
+    float    vx;           /**< 机体线速度 [m/s] */
+    float    wz;           /**< 机体角速度 [rad/s] */
+    float    distance;     /**< 累计行驶距离 [m]（从 init 复位后积分） */
+    bool     online;       /**< 里程计有效标志 */
+    uint32_t last_update_ms; /**< 上次有效更新时的系统时间 [ms] */
+    uint16_t zero_cnt;     /**< 连续收到零增量的帧数 */
+    bool     initialized;  /**< 是否已完成初始化 */
+} s_odom = {
+    .vx            = 0.0f,
+    .wz            = 0.0f,
+    .distance      = 0.0f,
+    .online        = false,
+    .last_update_ms = 0U,
+    .zero_cnt      = 0U,
+    .initialized   = false,
+};
+
+/* =========================================================
+ * 内部辅助函数
+ * ========================================================= */
+
+/**
+ * @brief  内部临界区读写接口（使用 taskENTER/EXIT_CRITICAL）
+ */
+static inline void lock_odom(void)
+{
+    taskENTER_CRITICAL();
+}
+
+static inline void unlock_odom(void)
+{
+    taskEXIT_CRITICAL();
+}
+
+/**
+ * @brief  差分驱动运动学核心
+ *
+ * @param  fl  左前轮增量 ticks
+ * @param  rl  左后轮增量 ticks
+ * @param  fr  右前轮增量 ticks
+ * @param  rr  右后轮增量 ticks
+ * @param  dt  采样周期 [s]
+ * @param  out_vx  输出线速度 [m/s]
+ * @param  out_wz  输出角速度 [rad/s]
+ *
+ * @note   内部使用均值滤波：同侧前后轮求平均后再做差速，
+ *         这样可以抑制单轮打滑对整体估计的影响。
+ */
+static void differential_kinematics(int16_t fl, int16_t rl,
+                                    int16_t fr, int16_t rr,
+                                    float dt,
+                                    float *out_vx, float *out_wz)
+{
+    /* 1. 同侧均值滤波（抵消前后轮差分） */
+    float v_left_ticks  = 0.5f * ((float)fl + (float)rl);  /* [ticks/period] */
+    float v_right_ticks = 0.5f * ((float)fr + (float)rr); /* [ticks/period] */
+
+    /* 2. tick -> m/s */
+    float v_left  = v_left_ticks  * ODOM_TICKS_TO_M / dt;  /* [m/s] */
+    float v_right = v_right_ticks * ODOM_TICKS_TO_M / dt;  /* [m/s] */
+
+    /* 3. 差分驱动运动学逆解 */
+    /*    vx = (vL + vR) / 2 */
+    /*    wz = (vR - vL) / track */
+    *out_vx = 0.5f * (v_left + v_right);
+    *out_wz = (v_right - v_left) * ODOM_WHEEL_TRACK_INV;
+
+    /* 4. 合理性检查：物理约束限幅
+     *    H743 主频 480MHz，dt 最小约 1ms，
+     *    若单帧 tick 绝对值超过 65535/dt，说明增量帧发生异常合并/撕裂。 */
+    const float MAX_VX_PERIOD = 5.0f;  /* 允许的最大瞬时线速度 [m/s]（车辆标称最高 1m/s） */
+    const float MAX_WZ_PERIOD = 20.0f; /* 允许的最大瞬时角速度 [rad/s]（极端原地转向） */
+
+    if (fabsf(*out_vx) > MAX_VX_PERIOD) {
+        *out_vx = (*out_vx > 0.0f) ? MAX_VX_PERIOD : -MAX_VX_PERIOD;
+    }
+    if (fabsf(*out_wz) > MAX_WZ_PERIOD) {
+        *out_wz = (*out_wz > 0.0f) ? MAX_WZ_PERIOD : -MAX_WZ_PERIOD;
+    }
+}
+
+/* =========================================================
+ * 公共 API 实现
+ * ========================================================= */
+
+void Odom_Init(void)
+{
+    lock_odom();
+    s_odom.vx             = 0.0f;
+    s_odom.wz             = 0.0f;
+    s_odom.distance       = 0.0f;
+    s_odom.online         = false;
+    s_odom.last_update_ms = 0U;
+    s_odom.zero_cnt      = 0U;
+    s_odom.initialized    = true;
+    unlock_odom();
+}
+
+void Odom_Update(uint32_t now_ms,
+                 int16_t fl_delta, int16_t rl_delta,
+                 int16_t fr_delta, int16_t rr_delta,
+                 uint32_t odom_span_ms)
+{
+    if (!s_odom.initialized) {
+        Odom_Init();
+    }
+
+    /* 计算采样周期 dt
+     *
+     * 优先使用 odom_span_ms（CAN 0x200 帧的真实时间跨度），
+     * 这是编码器增量实际覆盖的时间窗，不受 monitorTask 调度抖动影响。
+     *
+     * 退化策略：
+     *   - odom_span_ms > 0  → 直接使用（最精确）
+     *   - odom_span_ms == 0 → 仅收到一帧，无法算帧间 dt，
+     *                          退化为 now_ms - last_update_ms（兼容旧行为）
+     *   - 首帧（last_update_ms == 0 且 odom_span_ms == 0）→ 默认 60ms
+     *     （0x200 帧典型间隔）
+     */
+    float dt;
+    if (odom_span_ms > 0U && odom_span_ms <= 1000U) {
+        /* 有真实帧时间跨度，优先使用 */
+        dt = (float)odom_span_ms / 1000.0f;
+    } else if (s_odom.last_update_ms == 0U || !s_odom.online) {
+        /*
+         * 系统首帧，或 odom 刚从断流中恢复但当前只有一帧时，
+         * 无法可靠反推出这份增量覆盖的真实时间窗，退化为典型帧间隔。
+         */
+        dt = 0.06f;
+    } else {
+        uint32_t dt_ms = now_ms - s_odom.last_update_ms;
+        if (dt_ms == 0U || dt_ms > 1000U) {
+            /* 超时（超过 1 秒没有新数据）视为里程计离线 */
+            dt = 0.0f;
+        } else {
+            dt = (float)dt_ms / 1000.0f;
+        }
+    }
+
+    /* 检测零增量：四轮全 0 可能是底盘未动或通信丢失 */
+    bool is_zero_delta = (fl_delta == 0 && rl_delta == 0 &&
+                          fr_delta == 0 && rr_delta == 0);
+
+    if (dt <= 0.0f) {
+        /* 非法 dt，强制离线 */
+        lock_odom();
+        s_odom.online = false;
+        unlock_odom();
+        return;
+    }
+
+    /* 计算差分运动学 */
+    float vx_local, wz_local;
+    differential_kinematics(fl_delta, rl_delta, fr_delta, rr_delta,
+                            dt, &vx_local, &wz_local);
+
+    /* 零速度保护：连续多帧零增量后，强制归零（防止漂移） */
+    lock_odom();
+    if (is_zero_delta) {
+        s_odom.zero_cnt++;
+        if (s_odom.zero_cnt >= 3U) {
+            /* 连续 3 帧零增量（~180ms @ 60ms 帧率），认为机器人静止 */
+            s_odom.vx = 0.0f;
+            s_odom.wz = 0.0f;
+            s_odom.online = true;
+        }
+    } else {
+        s_odom.zero_cnt = 0U;
+        s_odom.vx = vx_local;
+        s_odom.wz = wz_local;
+        s_odom.online = true;
+
+        /* 积分累积里程（取线速度的绝对值，保证后退也累加） */
+        s_odom.distance += fabsf(s_odom.vx) * dt;
+    }
+
+    s_odom.last_update_ms = now_ms;
+    unlock_odom();
+
+    /* 推送黑板（黑板内部自带临界区保护） */
+    Blackboard_UpdateOdom(s_odom.vx, s_odom.wz);
+}
+
+void Odom_HandleTimeout(uint32_t now_ms, uint32_t timeout_ms)
+{
+    bool timed_out = false;
+
+    if (!s_odom.initialized || timeout_ms == 0U) {
+        return;
+    }
+
+    lock_odom();
+    if (s_odom.online &&
+        s_odom.last_update_ms != 0U &&
+        (now_ms - s_odom.last_update_ms) > timeout_ms) {
+        s_odom.vx = 0.0f;
+        s_odom.wz = 0.0f;
+        s_odom.online = false;
+        s_odom.zero_cnt = 0U;
+        timed_out = true;
+    }
+    unlock_odom();
+
+    if (timed_out) {
+        Blackboard_UpdateOdom(0.0f, 0.0f);
+    }
+}
+
+void Odom_GetSpeed(float *out_vx, float *out_wz, OdomStatus_t *out_status)
+{
+    if (out_vx != NULL)    *out_vx = s_odom.vx;
+    if (out_wz != NULL)    *out_wz = s_odom.wz;
+
+    if (out_status != NULL) {
+        lock_odom();
+        out_status->online           = s_odom.online;
+        out_status->zero_delta_count = s_odom.zero_cnt;
+        out_status->last_update_ms   = s_odom.last_update_ms;
+        unlock_odom();
+    }
+}
+
+float Odom_GetDistance(void)
+{
+    float d;
+    lock_odom();
+    d = s_odom.distance;
+    unlock_odom();
+    return d;
+}
+
+void Odom_Reset(void)
+{
+    lock_odom();
+    s_odom.vx             = 0.0f;
+    s_odom.wz             = 0.0f;
+    s_odom.distance       = 0.0f;
+    s_odom.online         = false;
+    s_odom.last_update_ms = 0U;
+    s_odom.zero_cnt       = 0U;
+    unlock_odom();
+
+    Blackboard_UpdateOdom(0.0f, 0.0f);
+}

App/Contract/robot_odom.h

@@ -0,0 +1,115 @@
+/**
+ * @file   robot_odom.h
+ * @brief  里程计积分模块：从 CAN 0x200 轮增量帧累加积分，
+ *         经差分驱动运动学转换为机体线速度 vx 与角速度 wz，
+ *         最终通过 Blackboard_UpdateOdom() 推送给导航流水线。
+ *
+ * @note  本模块不依赖任何传感器，只消费 CAN 协议层已有的 odom_delta 数据。
+ *        所有标定参数统一在 robot_params.h 中管理。
+ *
+ *        运动学模型（差分驱动）:
+ *          v_left  = (fl_delta + rl_delta) / 2
+ *          v_right = (fr_delta + rr_delta) / 2
+ *          vx      = (v_left + v_right) / 2   [m/s]
+ *          wz      = (v_right - v_left) / wheel_track  [rad/s]
+ */
+
+#ifndef ROBOT_ODOM_H
+#define ROBOT_ODOM_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "robot_params.h"  /* 统一参数头文件 */
+
+/* =========================================================
+ * 标定参数（从 robot_params.h 引入，勿在此文件重复定义）
+ * ========================================================= */
+#define ODOM_TICKS_PER_REV    PARAM_ENCODER_CPR
+#define ODOM_WHEEL_DIAMETER   PARAM_WHEEL_DIAMETER
+#define ODOM_WHEEL_TRACK      PARAM_WHEEL_TRACK
+
+/* =========================================================
+ * 预计算常数（自动生成，勿手动修改）
+ * ========================================================= */
+#define ODOM_TICKS_TO_M       (3.14159265358979f * (ODOM_WHEEL_DIAMETER) / (ODOM_TICKS_PER_REV))
+/**< @brief 每 tick 对应的轮缘弧长 [m]，即 PI*D / CPR */
+#define ODOM_WHEEL_TRACK_INV  (1.0f / (ODOM_WHEEL_TRACK))
+/**< @brief 轮距倒数 [1/m]，用于快速计算角速度 */
+
+/* =========================================================
+ * 配置结构
+ * ========================================================= */
+typedef struct {
+    /** @brief 里程计有效标志，false 时 vx/wz 强制为 0 */
+    bool     online;
+    /** @brief 里程计在线但 Tick 增量为 0 的连续帧数（用于检测卡死） */
+    uint16_t zero_delta_count;
+    /** @brief 上次更新时的系统时间 [ms] */
+    uint32_t last_update_ms;
+} OdomStatus_t;
+
+/* =========================================================
+ * API 接口
+ * ========================================================= */
+
+/**
+ * @brief  里程计模块初始化
+ * @note   重置内部累加器，建议在系统启动阶段调用一次
+ */
+void Odom_Init(void);
+
+/**
+ * @brief  里程计更新主函数
+ * @param  now_ms       当前系统时间 [ms]，由 HAL_GetTick() 提供
+ * @param  fl_delta     左前轮增量 ticks（本周期内编码器增量，可为负）
+ * @param  rl_delta     左后轮增量 ticks
+ * @param  fr_delta     右前轮增量 ticks
+ * @param  rr_delta     右后轮增量 ticks
+ * @param  odom_span_ms 本批次增量帧的真实时间跨度 [ms]，由
+ *                      SNC_CAN_ConsumeOdomDelta() 返回。
+ *                      传 0 表示无法计算（如仅一帧），此时退化为
+ *                      now_ms - last_update_ms。
+ *
+ * @note   由调用方（本例中 monitorTask 或 navTask）定期轮询 CAN 上下文
+ *         中的 odom_delta 数据后主动调用，推荐调用周期 20~100ms。
+ *         函数内部完成：均值滤波 -> 差分运动学 -> Blackboard 更新。
+ */
+void Odom_Update(uint32_t now_ms,
+                 int16_t fl_delta, int16_t rl_delta,
+                 int16_t fr_delta, int16_t rr_delta,
+                 uint32_t odom_span_ms);
+
+/**
+ * @brief  odom 断流超时处理
+ * @param  now_ms 当前系统时间 [ms]
+ * @param  timeout_ms 允许的最大无更新时长 [ms]
+ *
+ * @note   超时后会将 vx/wz 清零、标记离线，并同步刷新黑板，
+ *         防止导航层继续使用最后一次有效速度。
+ */
+void Odom_HandleTimeout(uint32_t now_ms, uint32_t timeout_ms);
+
+/**
+ * @brief  读取当前里程计状态快照（供外部任务消费）
+ * @param  out_status  输出状态结构体
+ * @param  out_vx      输出线速度 [m/s]
+ * @param  out_wz      输出角速度 [rad/s]
+ *
+ * @note   内部使用临界区保护，调用方可在任何上下文安全使用。
+ */
+void Odom_GetSpeed(float *out_vx, float *out_wz, OdomStatus_t *out_status);
+
+/**
+ * @brief  获取累积里程（沿程 s）
+ * @return 累计行驶距离 [m]（从 Odom_Init 复位后开始积分）
+ *
+ * @note   由走廊状态机用于段落终止判断。也可用于比赛计时参考。
+ */
+float Odom_GetDistance(void);
+
+/**
+ * @brief  复位里程计（清零累积距离和状态）
+ */
+void Odom_Reset(void);
+
+#endif /* ROBOT_ODOM_H */