128 lines
4.6 KiB
C
128 lines
4.6 KiB
C
|
#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;
|
|||
|
|
}
|
|||
|
|
}
|