1.0

App/preproc/corridor_preproc.c

@@ -0,0 +1,119 @@
+#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;
+    }
+    
+    *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;
+    }
+}