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

App/preproc/corridor_msgs.h

@@ -0,0 +1,75 @@
+#ifndef CORRIDOR_MSGS_H
+#define CORRIDOR_MSGS_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/* =========================================================
+ * EKF 状态维度定义
+ * ========================================================= */
+#define EKF_STATE_DIM   3   // [e_y, e_th, s]
+
+/* [改进I] 观测维度 (与实际实现对齐):
+ *   侧墙观测: 1DOF 标量更新 (z_ey)
+ *   IMU 航向: 独立 1DOF 标量更新 (z_eth_imu)，在 Update 后单独调用
+ *   侧墙航向 (z_eth_L/z_eth_R) 已取消 — VL53 差分噪声过大不适合做航向源
+ *   旧值 3 对应已取消的 [z_ey, z_eth_L, z_eth_R] 三维观测
+ */
+#define EKF_OBS_DIM     1   // 实际: 侧墙 1DOF (z_ey)
+
+/* χ² 检验门限 (95% 置信度) */
+/* 1 DOF: 3.84, 2 DOF: 5.99, 3 DOF: 7.81 */
+#define CHI2_THRESHOLD_1DOF   3.84f
+#define CHI2_THRESHOLD_2DOF   5.99f
+#define CHI2_THRESHOLD_3DOF   7.81f
+
+/**
+ * @brief 走廊观测快照 (由 Blackboard 数据转化清洗而来)
+ * @note  所有距离单位统一为 米 (m)
+ */
+typedef struct {
+  uint32_t t_ms;           // 观测时间戳
+  float d_lf, d_lr;        // 左侧前后距离 (m)
+  float d_rf, d_rr;        // 右侧前后距离 (m)
+  float d_front, d_back;   // 前后防撞/到端距离 (m)
+  uint8_t valid_mask;      // 位域掩码：标记哪些雷达数据是当前存活且合法的
+} CorridorObs_t;
+
+/**
+ * @brief 走廊相对定位状态 (EKF 的输出结果)
+ */
+typedef struct {
+  uint32_t t_ms;           // 状态更新时间戳
+  float e_y;               // 横向偏差 (m)，向左偏为正
+  float e_th;              // 航向偏差 (rad)，车头偏左为正
+  float s;                 // 沿走廊进度里程 (m)
+  float conf;              // 置信度/健康度 (0.0~1.0)，用于触发降级
+  
+  /* EKF 扩展输出 */
+  float P[EKF_STATE_DIM][EKF_STATE_DIM];  // 状态协方差矩阵
+  float innovation[EKF_OBS_DIM];          // 新息向量 (观测残差)
+  float mahalanobis_d2;                    // 马氏距离平方
+  uint8_t obs_reject_mask;                 // 被拒绝的观测位掩码
+} CorridorState_t;
+
+/**
+ * @brief 纯控制指令 (准备交给安全层仲裁)
+ */
+typedef struct {
+  uint32_t t_ms;
+  float v;                 // 期望线速度 (m/s)
+  float w;                 // 期望角速度 (rad/s)
+  uint8_t flags;           // 控制特殊标志位
+} RawCmd_t;
+
+/* =========================================================
+ * 安全模式枚举 (赛道级导航 → 安全状态机 的模式指示)
+ * ========================================================= */
+typedef enum {
+  SAFETY_MODE_IDLE = 0,    // 零速，不做任何裁剪
+  SAFETY_MODE_CORRIDOR,    // 沟内: 前向减速/停车/E-STOP 全开
+  SAFETY_MODE_TURN,        // 转向: 允许 v=0+w!=0, 前向不全停, 不检查 conf
+  SAFETY_MODE_STRAIGHT     // 直行段: 前后激光防撞, 不检查 conf
+} SafetyMode_t;
+
+#endif // CORRIDOR_MSGS_H

App/preproc/corridor_preproc.c

@@ -0,0 +1,127 @@
+#include "corridor_preproc.h"
+
+/**
+ * @brief 内部辅助函数：处理单个侧向 VL53L0X 测距
+ * @return true 表示数据合法且在量程内，false 表示异常或超量程飞点
+ */
+static bool process_side_laser(const SensorItem_t *item, float *out_m)
+{
+    // 驱动层报异常，直接抛弃
+    if (!item->is_valid) {
+        return false;
+    }
+    
+    // 毫米转米
+    float dist_m = item->value / 1000.0f;
+    
+    // 剔除硬件量程外的死区或异常噪点
+    if (dist_m > PREPROC_MAX_SIDE_RANGE_M || dist_m < PREPROC_MIN_SIDE_RANGE_M) {
+        return false;
+    }
+
+    /* 靠墙过近退化：
+     * 侧向 VL53 在最小量程附近（例如实际 3cm、模组最小可信 4cm）时，
+     * 直接判无效会让系统退化成单边观测，反而更容易贴墙。
+     * 这里改成“钳位到最小可信距离”，既保留该侧存在感，又避免把不可信的超近值直接送上去。 */
+    if (dist_m < PREPROC_SAT_NEAR_SIDE_RANGE_M) {
+        dist_m = PREPROC_SAT_NEAR_SIDE_RANGE_M;
+    }
+    
+    *out_m = dist_m;
+    return true;
+}
+
+/**
+ * @brief 内部辅助函数：处理前后向互补雷达 (STP 远距离 + ATK 近盲区)
+ * @note  完美解决 STP-23L 的 7cm 物理盲区问题，防止“幽灵撞墙”
+ */
+static bool process_complementary_laser(const SensorItem_t *stp, const SensorItem_t *atk, float *out_m)
+{
+    bool stp_ok = stp->is_valid && (stp->value > 0.0f);
+    bool atk_ok = atk->is_valid && (atk->value > 0.0f);
+
+    float stp_m = stp->value / 1000.0f;
+    float atk_m = atk->value / 1000.0f;
+
+    /* 1. 近距离补盲优先：如果 ATK 存活，且测距进入 8cm 内 (7cm盲区+1cm裕量) */
+    /* 此时 STP 的数据大概率是垃圾数据（卡死在7cm或报错），无条件信任 ATK */
+    if (atk_ok && atk_m <= PREPROC_BLIND_ZONE_M) {
+        *out_m = atk_m;
+        return true;
+    }
+
+    /* 2. 正常中远距离：两者都存活，取保守值防撞 */
+    if (stp_ok && atk_ok) {
+        // 如果 STP 读数等于 7cm 极限值，它可能已经瞎了，直接用 ATK 的值
+        if (stp_m <= 0.07f) {
+            *out_m = atk_m;
+        } else {
+            *out_m = (stp_m < atk_m) ? stp_m : atk_m; // 正常情况取两者中更近的
+        }
+        return true;
+    }
+
+    /* 3. 硬件单体降级：只剩下一个雷达活着 */
+    if (stp_ok) {
+        // 只有 STP 活着，但读数在盲区内。这数据极其危险（骗人的），宁可报失效触发急停！
+        if (stp_m <= 0.07f) {
+            return false; 
+        }
+        *out_m = stp_m;
+        return true;
+    }
+    
+    if (atk_ok) {
+        *out_m = atk_m;
+        return true;
+    }
+
+    /* 全挂了，通知上层失明 */
+    return false;
+}
+
+void CorridorPreproc_ExtractObs(const RobotBlackboard_t *board, uint32_t now_ms, CorridorObs_t *out_obs)
+{
+    // 初始化输出结构体，清空掩码
+    out_obs->t_ms = now_ms;
+    out_obs->valid_mask = 0U;
+    
+    out_obs->d_lf = 0.0f;
+    out_obs->d_lr = 0.0f;
+    out_obs->d_rf = 0.0f;
+    out_obs->d_rr = 0.0f;
+    out_obs->d_front = 0.0f;
+    out_obs->d_back = 0.0f;
+
+    /* --- 1. 独立清洗侧向 4 个 VL53L0X 数据 --- */
+    if (process_side_laser(&board->dist_left_f, &out_obs->d_lf)) {
+        out_obs->valid_mask |= CORRIDOR_OBS_MASK_LF;
+    }
+    if (process_side_laser(&board->dist_left_r, &out_obs->d_lr)) {
+        out_obs->valid_mask |= CORRIDOR_OBS_MASK_LR;
+    }
+    if (process_side_laser(&board->dist_right_f, &out_obs->d_rf)) {
+        out_obs->valid_mask |= CORRIDOR_OBS_MASK_RF;
+    }
+    if (process_side_laser(&board->dist_right_r, &out_obs->d_rr)) {
+        out_obs->valid_mask |= CORRIDOR_OBS_MASK_RR;
+    }
+
+    /* --- 2. 前向雷达：远近互补融合 --- */
+    if (process_complementary_laser(&board->dist_front_stp, &board->dist_front_atk, &out_obs->d_front)) {
+        /* 补偿传感器内缩：将"传感器到墙"转换为"车体前端到墙"
+         * d_body = d_sensor - front_laser_offset
+         * 当 offset > 0 时表示传感器在车体内部，车体前端比传感器更接近墙 */
+        out_obs->d_front -= PARAM_FRONT_LASER_OFFSET;
+        if (out_obs->d_front < 0.0f) out_obs->d_front = 0.0f;  /* 安全下限 */
+        out_obs->valid_mask |= CORRIDOR_OBS_MASK_FRONT;
+    }
+
+    /* --- 3. 后向雷达：远近互补融合 --- */
+    if (process_complementary_laser(&board->dist_rear_stp, &board->dist_rear_atk, &out_obs->d_back)) {
+        /* 补偿传感器内缩：同前向逻辑 */
+        out_obs->d_back -= PARAM_REAR_LASER_OFFSET;
+        if (out_obs->d_back < 0.0f) out_obs->d_back = 0.0f;
+        out_obs->valid_mask |= CORRIDOR_OBS_MASK_BACK;
+    }
+}

App/preproc/corridor_preproc.h

@@ -0,0 +1,46 @@
+#ifndef CORRIDOR_PREPROC_H
+#define CORRIDOR_PREPROC_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "Contract/robot_blackboard.h"
+#include "corridor_msgs.h"
+
+/* 传感器有效性掩码定义 (对应 CorridorObs_t.valid_mask) */
+#define CORRIDOR_OBS_MASK_LF    (1U << 0)  // 左前有效
+#define CORRIDOR_OBS_MASK_LR    (1U << 1)  // 左后有效
+#define CORRIDOR_OBS_MASK_RF    (1U << 2)  // 右前有效
+#define CORRIDOR_OBS_MASK_RR    (1U << 3)  // 右后有效
+#define CORRIDOR_OBS_MASK_FRONT (1U << 4)  // 前方互补测距有效
+#define CORRIDOR_OBS_MASK_BACK  (1U << 5)  // 后方互补测距有效
+
+/* VL53L0X 侧向雷达的物理有效探测区间 (m) */
+#define PREPROC_MAX_SIDE_RANGE_M  2.0f
+#define PREPROC_MIN_SIDE_RANGE_M  0.02f
+/* 侧向 VL53 靠墙过近时的退化阈值 (m)
+ * 低于此值虽然可能还有数字，但已接近最小量程区，不再作为可信几何量使用。 */
+#define PREPROC_SAT_NEAR_SIDE_RANGE_M PARAM_VL53_SIDE_SAT_NEAR_M
+
+/* 前后向雷达近战盲区阈值 (m) (STP 7cm盲区 + 1cm工程裕量) */
+#define PREPROC_BLIND_ZONE_M      0.08f
+
+/* 前后激光传感器内缩补偿 (从 robot_params.h 引入) */
+#include "robot_params.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+  /**
+   * @brief 从黑板快照中提取、仲裁并清洗走廊观测数据
+   * @param board   输入的黑板数据快照
+   * @param now_ms  当前的系统时间戳
+   * @param out_obs 输出的清洗后观测值 (仅保留合法数据，单位统一为 m)
+   */
+  void CorridorPreproc_ExtractObs(const RobotBlackboard_t *board, uint32_t now_ms, CorridorObs_t *out_obs);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // CORRIDOR_PREPROC_H