2026-03-31 23:30:33 +08:00
|
|
|
|
#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
|
2026-04-12 13:31:07 +08:00
|
|
|
|
/* 侧向 VL53 靠墙过近时的退化阈值 (m)
|
|
|
|
|
|
* 低于此值虽然可能还有数字，但已接近最小量程区，不再作为可信几何量使用。 */
|
|
|
|
|
|
#define PREPROC_SAT_NEAR_SIDE_RANGE_M PARAM_VL53_SIDE_SAT_NEAR_M
|
2026-03-31 23:30:33 +08:00
|
|
|
|
|
|
|
|
|
|
/* 前后向雷达近战盲区阈值 (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
|
|
|
|
|
|
|
2026-04-12 13:31:07 +08:00
|
|
|
|
#endif // CORRIDOR_PREPROC_H
|
