ASER/App/nav/global_nav.c

/**
 * @file   global_nav.c
 * @brief  赛道级总控 — 混合导航状态机实现
 *
 *         场地结构: 垄沟沿X轴(横向)分布，左右两端各有纵向端部通道
 *         启动区在左下角，入口对齐左端通道
 *
 *         S 型遍历:
 *           入场(北行)→右转入C1(→东)→右端到端→左转→北行→左转入C2(←西)
 *           →左端到端→右转→北行→右转入C3(→东)→...→C6(←西)
 *           →左端到端→左转(朝南)→南行出场→回停启动区
 *
 *         端部通道特点:
 *           - 一侧贴围栏(VL53能测到), 另一侧交替出现垄背端面和垄沟开口
 *           - 不能依赖双侧VL53做EKF, 必须用IMU航向保持+前后激光到端检测
 */

#include "global_nav.h"
#include "est/corridor_filter.h"
#include "preproc/corridor_preproc.h"
#include "robot_params.h"
#include <math.h>
#include <string.h>

/* =========================================================
 * 内部状态
 * ========================================================= */
static struct {
    GlobalNavStage_t stage;
    bool             running;
    bool             initialized;

    /* 赛道级 */
    uint8_t  current_corridor_id;
    uint8_t  corridors_completed;

    /* 转向 */
    float    turn_start_yaw_deg;        /* IMU yaw_continuous at turn start */
    float    turn_target_delta_deg;     /* 90° */
    int8_t   turn_sign;                 /* +1 (CCW) or -1 (CW) */

    /* 里程 (用里程计积分距离) */
    float    stage_entry_odom_vx_accum; /* 进入阶段时的里程计累计距离 */
    float    odom_distance_accum;       /* 运行中持续积分的里程 */

    /* 超时 */
    uint32_t stage_start_ms;

    /* 重捕获 */
    uint8_t  reacquire_ok_count;

    /* 出场 */
    bool     exit_vl53_lost;
    float    exit_lost_distance;

    /* 航向保持 */
    float    heading_ref_deg;

    /* 连接段: 多传感器辅助 */
    float    link_d_front_start;    /* 进入连接段时前激光读数 (m) */
    bool     link_d_front_valid;    /* 进入时前激光是否有效 */
    uint8_t  link_gap_count;        /* 非围栏侧 VL53 连续丢失计数 (沟口确认) */

    /* EKF 进度保存 */
    float    corridor_entry_s;

    /* 配置 */
    GlobalNavConfig_t cfg;
} s_nav;

/* 上一个周期的里程计速度，用于积分 */
static float s_last_odom_vx = 0.0f;
static uint32_t s_last_update_ms = 0;

/* =========================================================
 * 辅助函数
 * ========================================================= */
static inline float gnav_clampf(float val, float lo, float hi)
{
    if (val < lo) return lo;
    if (val > hi) return hi;
    return val;
}

static inline float gnav_fabsf(float x)
{
    return x < 0.0f ? -x : x;
}

/** 简单 P 控制航向保持，输入偏差 (deg)，输出角速度 (rad/s) */
static float heading_hold_pd(float current_yaw_deg, float ref_yaw_deg, float kp)
{
    float err_deg = ref_yaw_deg - current_yaw_deg;
    float w = kp * err_deg;  /* kp 把 ° 映射到 rad/s */
    return gnav_clampf(w, -1.0f, 1.0f);
}

/** 检查侧向 VL53 是否探到壁 (至少有一侧的前后都有效) — 仅用于重捕获 */
static bool side_walls_detected(const CorridorObs_t* obs)
{
    bool left_ok  = (obs->valid_mask & CORRIDOR_OBS_MASK_LF) &&
                    (obs->valid_mask & CORRIDOR_OBS_MASK_LR);
    bool right_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) &&
                    (obs->valid_mask & CORRIDOR_OBS_MASK_RR);
    return left_ok || right_ok;
}

/**
 * @brief 检测非围栏侧 VL53 是否发现垄沟开口 (连接段沟口辅助判定)
 *
 * 在纵向端部通道面朝北行驶时：
 *   - 刚走完向东(EAST)的沟 → 当前在右端通道 → 右侧贴围栏 → 检查左侧VL53
 *   - 刚走完向西(WEST)的沟 → 当前在左端通道 → 左侧贴围栏 → 检查右侧VL53
 *
 * 经过垄背端面时: VL53 测到约 (通道宽/2 - 车宽/2 - VL53内缩) ≈ 10cm → 有效
 * 经过垄沟开口时: VL53 射入沟内 220cm+ → 超出有效距离 → 无效或读数 > 1.2m
 *
 * 因为 VL53 前后两颗间距 12cm，车身 20cm 宽，沟口 40cm 宽，
 * 当车身中心对准沟口时，前后 VL53 都已进入开口区域，两颗均应丢失。
 * 但过渡区有边缘效应，所以只要求 "前后至少一颗丢失" 即视为探到沟口。
 *
 * @param obs     当前观测
 * @param prev_travel_dir  刚走完的那条沟的行驶方向 (EAST/WEST)
 * @return true = 非围栏侧检测到开口
 */
static bool gap_detected_on_open_side(const CorridorObs_t* obs,
                                       TravelDirection_t prev_travel_dir)
{
    if (prev_travel_dir == TRAVEL_DIR_EAST) {
        /* 在右端通道，右侧贴围栏 → 检查左侧 VL53 */
        bool lf_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LF)
                       || (obs->d_lf > 0.5f);  /* >50cm 视为沟口 (正常贴壁约10cm) */
        bool lr_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LR)
                       || (obs->d_lr > 0.5f);
        return lf_lost || lr_lost;
    } else {
        /* 在左端通道，左侧贴围栏 → 检查右侧 VL53 */
        bool rf_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_RF)
                       || (obs->d_rf > 0.5f);
        bool rr_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_RR)
                       || (obs->d_rr > 0.5f);
        return rf_lost || rr_lost;
    }
}

/** 检查侧向 VL53 是否全丢 */
static bool all_side_lost(const CorridorObs_t* obs)
{
    uint8_t side_mask = CORRIDOR_OBS_MASK_LF | CORRIDOR_OBS_MASK_LR |
                        CORRIDOR_OBS_MASK_RF | CORRIDOR_OBS_MASK_RR;
    return (obs->valid_mask & side_mask) == 0U;
}

/** 检查重捕获条件 */
static bool check_reacquire(const CorridorObs_t* obs, const CorridorState_t* state)
{
    /* 条件 1: 至少 3 个侧向传感器有效 */
    uint8_t side_mask = CORRIDOR_OBS_MASK_LF | CORRIDOR_OBS_MASK_LR |
                        CORRIDOR_OBS_MASK_RF | CORRIDOR_OBS_MASK_RR;
    uint8_t active = obs->valid_mask & side_mask;
    int count = 0;
    for (int i = 0; i < 4; i++) {
        if (active & (1U << i)) count++;
    }
    if (count < 3) return false;

    /* 条件 2: 左右距离和 ≈ 走廊宽度 */
    bool left_ok  = (obs->valid_mask & CORRIDOR_OBS_MASK_LF) &&
                    (obs->valid_mask & CORRIDOR_OBS_MASK_LR);
    bool right_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) &&
                    (obs->valid_mask & CORRIDOR_OBS_MASK_RR);

    if (left_ok && right_ok) {
        float d_left  = (obs->d_lf + obs->d_lr) * 0.5f;
        float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
        float total_width = d_left + d_right + PARAM_ROBOT_WIDTH;
        float err = gnav_fabsf(total_width - s_nav.cfg.corridor_width);
        if (err > s_nav.cfg.reacquire_width_tol) return false;
    }

    /* 条件 3: EKF 置信度 */
    if (state->conf < s_nav.cfg.reacquire_conf_thresh) return false;

    return true;
}

/** 获取当前阶段的已行驶里程 */
static float odom_since_entry(void)
{
    return s_nav.odom_distance_accum - s_nav.stage_entry_odom_vx_accum;
}

/** 阶段转移 — 通用初始化 */
static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board);

/* =========================================================
 * 阶段名称表
 * ========================================================= */
static const char* const s_stage_names[] = {
    "IDLE",
    "ENTRY_STRAIGHT",
    "TURN_INTO_CORRIDOR",
    "REACQUIRE",
    "CORRIDOR_TRACK",
    "TURN_OUT",
    "LINK_STRAIGHT",
    "TURN_INTO_NEXT",
    "EXIT_STRAIGHT",
    "DOCK",
    "FINISHED",
    "ERROR"
};

/* =========================================================
 * 转向执行 (统一的 90° 转向逻辑)
 * ========================================================= */
static void execute_turn(const CorridorObs_t* obs,
                         const CorridorState_t* state,
                         const RobotBlackboard_t* board,
                         uint32_t now_ms,
                         GlobalNavOutput_t* out)
{
    float imu_yaw = board->imu_yaw_continuous.is_valid
                    ? board->imu_yaw_continuous.value : 0.0f;

    /* 已转过的角度 (取绝对值) */
    float delta = (imu_yaw - s_nav.turn_start_yaw_deg) * s_nav.turn_sign;
    float target = s_nav.turn_target_delta_deg;
    float remaining_deg = target - delta;

    float omega = s_nav.cfg.turn_omega;

    /* 接近目标时减速 */
    float decel_zone_deg = PARAM_RAD2DEG(s_nav.cfg.turn_decel_zone_rad);
    if (remaining_deg < decel_zone_deg && decel_zone_deg > 0.01f) {
        float ratio = remaining_deg / decel_zone_deg;
        if (ratio < 0.0f) ratio = 0.0f;
        omega = s_nav.cfg.turn_min_omega +
                ratio * (s_nav.cfg.turn_omega - s_nav.cfg.turn_min_omega);
    }

    out->override_v = 0.0f;
    out->override_w = (float)s_nav.turn_sign * omega;
    out->use_override = true;
    out->request_corridor = false;
    out->safety_mode = SAFETY_MODE_TURN;

    /* 转向完成判定 */
    float tolerance_deg = PARAM_RAD2DEG(s_nav.cfg.turn_tolerance_rad);
    if (delta >= target - tolerance_deg) {
        switch (s_nav.stage) {
        case GNAV_TURN_INTO_CORRIDOR:
        case GNAV_TURN_INTO_NEXT:
            transition_to(GNAV_REACQUIRE, board);
            break;
        case GNAV_TURN_OUT_OF_CORRIDOR:
            if (TrackMap_IsLastCorridor(s_nav.current_corridor_id)) {
                transition_to(GNAV_EXIT_STRAIGHT, board);
            } else {
                transition_to(GNAV_LINK_STRAIGHT, board);
            }
            break;
        default:
            break;
        }
        return;
    }

    /* 超时保护 */
    if (now_ms - s_nav.stage_start_ms > s_nav.cfg.turn_timeout_ms) {
        transition_to(GNAV_ERROR, board);
    }
}

/* =========================================================
 * 阶段转移
 * ========================================================= */
static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board)
{
    float imu_yaw = (board != NULL && board->imu_yaw_continuous.is_valid)
                    ? board->imu_yaw_continuous.value : 0.0f;

    /* 通用: 记录进入时间和里程 */
    s_nav.stage_start_ms = s_last_update_ms;
    s_nav.stage_entry_odom_vx_accum = s_nav.odom_distance_accum;
    s_nav.reacquire_ok_count = 0;

    switch (next) {
    case GNAV_ENTRY_STRAIGHT:
        s_nav.heading_ref_deg = imu_yaw;
        s_nav.current_corridor_id = TrackMap_Get()->entry_corridor_id;
        break;

    case GNAV_TURN_INTO_CORRIDOR: {
        const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
        s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
        s_nav.turn_start_yaw_deg = imu_yaw;
        s_nav.turn_target_delta_deg = 90.0f;
        break;
    }
    case GNAV_TURN_OUT_OF_CORRIDOR: {
        const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
        s_nav.turn_sign = (int8_t)cd->exit_turn_dir;
        s_nav.turn_start_yaw_deg = imu_yaw;
        s_nav.turn_target_delta_deg = 90.0f;
        break;
    }
    case GNAV_TURN_INTO_NEXT: {
        uint8_t next_id = TrackMap_GetNextCorridorId(s_nav.current_corridor_id);
        const CorridorDescriptor_t* cd = TrackMap_GetCorridor(next_id);
        s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
        s_nav.turn_start_yaw_deg = imu_yaw;
        s_nav.turn_target_delta_deg = 90.0f;
        s_nav.current_corridor_id = next_id;
        break;
    }
    case GNAV_REACQUIRE:
        /* EKF 重置: 新沟的 e_y 参考不同，必须重建 */
        CorridorFilter_Reset();
        s_nav.heading_ref_deg = imu_yaw;
        break;

    case GNAV_CORRIDOR_TRACK:
        s_nav.corridor_entry_s = 0.0f;  /* EKF 已 reset, s 从 0 开始 */
        break;

    case GNAV_LINK_STRAIGHT:
        s_nav.heading_ref_deg = imu_yaw;
        s_nav.link_d_front_start = 0.0f;
        s_nav.link_d_front_valid = false;  /* 首拍再记录 */
        s_nav.link_gap_count = 0;
        break;

    case GNAV_EXIT_STRAIGHT:
        s_nav.heading_ref_deg = imu_yaw;
        s_nav.exit_vl53_lost = false;
        s_nav.exit_lost_distance = 0.0f;
        break;

    case GNAV_DOCK:
        break;

    case GNAV_FINISHED:
        s_nav.running = false;
        break;

    case GNAV_ERROR:
        break;

    default:
        break;
    }

    s_nav.stage = next;
}

/* =========================================================
 * 公开 API
 * ========================================================= */

void GlobalNav_Init(const GlobalNavConfig_t* cfg)
{
    memset(&s_nav, 0, sizeof(s_nav));
    s_nav.cfg = *cfg;
    s_nav.stage = GNAV_IDLE;
    s_nav.running = false;
    s_nav.initialized = true;
    s_last_odom_vx = 0.0f;
    s_last_update_ms = 0;
}

void GlobalNav_Start(void)
{
    if (!s_nav.initialized) return;
    s_nav.running = true;
    s_nav.corridors_completed = 0;
    s_nav.odom_distance_accum = 0.0f;
    s_nav.stage_entry_odom_vx_accum = 0.0f;
    s_nav.current_corridor_id = TrackMap_Get()->entry_corridor_id;
    /* 不在这里 transition_to，因为还没有 board 数据。
       下一个 Update 周期里发现 running && IDLE 时再 transition。 */
    s_nav.stage = GNAV_IDLE;
}

void GlobalNav_Stop(void)
{
    s_nav.running = false;
    s_nav.stage = GNAV_FINISHED;
}

void GlobalNav_Reset(void)
{
    s_nav.stage = GNAV_IDLE;
    s_nav.running = false;
    s_nav.corridors_completed = 0;
    s_nav.odom_distance_accum = 0.0f;
}

GlobalNavStage_t GlobalNav_GetStage(void)
{
    return s_nav.stage;
}

const char* GlobalNav_GetStageName(GlobalNavStage_t stage)
{
    if (stage <= GNAV_ERROR) {
        return s_stage_names[stage];
    }
    return "UNKNOWN";
}

/* =========================================================
 * 核心 Update
 * ========================================================= */
void GlobalNav_Update(const CorridorObs_t*      obs,
                      const CorridorState_t*     state,
                      const RobotBlackboard_t*   board,
                      GlobalNavOutput_t*         out)
{
    if (!s_nav.initialized) {
        memset(out, 0, sizeof(*out));
        return;
    }

    uint32_t now_ms = s_last_update_ms;
    /* 用 HAL_GetTick 代理 — 实际上 app_tasks 会传入 board 里的时间 */
    {
        /* 里程计积分: Δd = vx * dt */
        float odom_vx = board->odom_vx;
        if (s_last_update_ms > 0) {
            /* 使用 board 中的时间戳估计当前时间 */
            uint32_t cur_ms = board->imu_wz.timestamp_ms;
            if (cur_ms == 0) cur_ms = s_last_update_ms + 20U;
            float dt = (float)(cur_ms - s_last_update_ms) * 0.001f;
            if (dt > 0.0f && dt < 0.5f) {
                s_nav.odom_distance_accum += gnav_fabsf(odom_vx) * dt;
            }
            now_ms = cur_ms;
        } else {
            now_ms = board->imu_wz.timestamp_ms;
            if (now_ms == 0) now_ms = 1;
        }
        s_last_update_ms = now_ms;
        s_last_odom_vx = odom_vx;
    }

    float imu_yaw_deg = board->imu_yaw_continuous.is_valid
                        ? board->imu_yaw_continuous.value : 0.0f;

    /* 默认输出 */
    out->use_override     = true;
    out->request_corridor = false;
    out->override_v       = 0.0f;
    out->override_w       = 0.0f;
    out->safety_mode      = SAFETY_MODE_IDLE;
    out->stage            = s_nav.stage;
    out->corridor_id      = s_nav.current_corridor_id;
    out->corridors_done   = s_nav.corridors_completed;
    out->active           = s_nav.running;
    out->stage_name       = GlobalNav_GetStageName(s_nav.stage);

    if (!s_nav.running) return;

    /* IDLE → 自动进入 ENTRY_STRAIGHT */
    if (s_nav.stage == GNAV_IDLE) {
        transition_to(GNAV_ENTRY_STRAIGHT, board);
        out->stage      = s_nav.stage;
        out->stage_name = GlobalNav_GetStageName(s_nav.stage);
    }

    uint32_t elapsed_ms = now_ms - s_nav.stage_start_ms;

    switch (s_nav.stage) {

    /* ============================================================
     * 入场直线 (从启动区沿左端纵向通道北行)
     *
     * 传感器情况：
     *   - 左侧 VL53 贴围栏，始终有效（不能用来判"到了C1入口"）
     *   - 右侧 VL53 一出启动区就对着C1开口(260cm)，测不到
     *   - 入场距离极短（启动区入口到C1入口仅约 10~30cm）
     *   - 主要靠里程计推进足够距离后即可转向
     * ============================================================ */
    case GNAV_ENTRY_STRAIGHT:
        out->override_v  = s_nav.cfg.entry_v;
        out->override_w  = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
                                            s_nav.cfg.heading_kp);
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        /* 里程计 + 超时判定 (不用 side_walls_detected，因为左侧围栏始终有效会误触发) */
        if (odom_since_entry() >= s_nav.cfg.entry_distance ||
            elapsed_ms > s_nav.cfg.entry_timeout_ms)
        {
            transition_to(GNAV_TURN_INTO_CORRIDOR, board);
        }
        break;

    /* ============================================================
     * 三种转向状态统一处理
     * ============================================================ */
    case GNAV_TURN_INTO_CORRIDOR:
    case GNAV_TURN_OUT_OF_CORRIDOR:
    case GNAV_TURN_INTO_NEXT:
        execute_turn(obs, state, board, now_ms, out);
        break;

    /* ============================================================
     * 重捕获走廊
     * ============================================================ */
    case GNAV_REACQUIRE:
        out->override_v  = s_nav.cfg.reacquire_v;
        out->override_w  = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
                                            s_nav.cfg.heading_kp);
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        if (check_reacquire(obs, state)) {
            s_nav.reacquire_ok_count++;
        } else {
            s_nav.reacquire_ok_count = 0;
        }

        if (s_nav.reacquire_ok_count >= s_nav.cfg.reacquire_confirm_ticks) {
            transition_to(GNAV_CORRIDOR_TRACK, board);
        }
        if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms) {
            transition_to(GNAV_ERROR, board);
        }
        break;

    /* ============================================================
     * 沟内闭环跟踪 (交给 corridor_ctrl)
     * ============================================================ */
    case GNAV_CORRIDOR_TRACK:
        out->use_override     = false;
        out->request_corridor = true;
        out->safety_mode      = SAFETY_MODE_CORRIDOR;

        /* 到端检测 */
        {
            bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
            if (front_valid && obs->d_front <= s_nav.cfg.corridor_end_detect_dist) {
                /* 里程下限保护: 至少走了 1.0m 才允许认定到端，避免假阳性 */
                float corridor_odom = odom_since_entry();
                if (corridor_odom > 1.0f) {
                    s_nav.corridors_completed++;
                    transition_to(GNAV_TURN_OUT_OF_CORRIDOR, board);
                }
            }
        }
        /* 里程超长保护 */
        if (odom_since_entry() > s_nav.cfg.corridor_length_max) {
            s_nav.corridors_completed++;
            transition_to(GNAV_TURN_OUT_OF_CORRIDOR, board);
        }
        break;

    /* ============================================================
     * 连接段直行 (纵向端部通道北行, 方案2: 三信号联合判定)
     *
     * 传感器情况（转向完成后面朝北）：
     *   - 前激光（朝北）→ 上围栏，d_front 随北行递减      [精度高]
     *   - 后激光（朝南）→ 下围栏，d_back 随北行递增       [精度高]
     *   - 围栏侧 VL53 → 始终有效 (~10cm)                [不用于判定]
     *   - 非围栏侧 VL53 → 贴垄背端面时有效，到垄沟开口时丢失  [沟口标志]
     *
     * 信号定义：
     *   A: 里程计     odom >= link_distance * 0.85  (打滑衰减, 权重低)
     *   B: 前激光变化  d_front缩小 >= link_distance * 0.85 (权重高)
     *   C: 非围栏侧VL53 丢失/跳到>50cm，连续2拍确认  (直接探测沟口, 权重中)
     *
     * 触发逻辑: B || (A && C)
     *   - 前激光变化量足够 → 直接触发（最可靠的单一信号）
     *   - 里程计到位 + VL53探到沟口 → 联合触发（互相校验）
     * ============================================================ */
    case GNAV_LINK_STRAIGHT:
        out->override_v  = s_nav.cfg.link_v;
        out->override_w  = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
                                            s_nav.cfg.heading_kp);
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        {
            bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;

            /* 首拍记录前激光基准值 */
            if (!s_nav.link_d_front_valid && front_valid) {
                s_nav.link_d_front_start = obs->d_front;
                s_nav.link_d_front_valid = true;
            }

            /* ---- 信号 A: 里程计 (考虑打滑, 用 85% 容差) ---- */
            bool odom_ok = odom_since_entry() >= s_nav.cfg.link_distance * 0.85f;

            /* ---- 信号 B: 前激光变化量 (高精度) ---- */
            bool laser_ok = false;
            if (s_nav.link_d_front_valid && front_valid) {
                float d_front_delta = s_nav.link_d_front_start - obs->d_front;
                /* 车身中心到前激光有偏置(FRONT_LASER_OFFSET)，但这里用的是差值，偏置抵消 */
                laser_ok = (d_front_delta >= s_nav.cfg.link_distance * 0.85f);
            }

            /* ---- 信号 C: 非围栏侧 VL53 沟口检测 (需连续2拍确认) ----
             *
             * 判定阈值 0.5m 的来源:
             *   正常贴垄背端面时 VL53 读数 ≈ (通道宽/2 - 车宽/2 - VL53内缩)
             *                               = (0.40/2 - 0.20/2 - 0.0)
             *                               = 0.10m
             *   到垄沟开口时 VL53 读数 > 1.2m (超出有效距离) 或无效
             *   阈值 0.5m 在两者之间，足够区分
             */
            const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
            /* 在 LINK_STRAIGHT 阶段，current_corridor_id 仍是刚走完的沟 (尚未更新)
               所以 cd->travel_dir 就是刚走完那条沟的方向，直接用来判断当前在哪个端部通道 */
            bool gap_now = gap_detected_on_open_side(obs, cd->travel_dir);

            if (gap_now) {
                if (s_nav.link_gap_count < 255) s_nav.link_gap_count++;
            } else {
                s_nav.link_gap_count = 0;
            }
            bool gap_confirmed = (s_nav.link_gap_count >= 2);  /* 连续2拍 (40ms @ 20ms周期) */

            /* ---- 联合判定: B || (A && C) ---- */
            if (laser_ok || (odom_ok && gap_confirmed))
            {
                transition_to(GNAV_TURN_INTO_NEXT, board);
            }
        }
        if (elapsed_ms > s_nav.cfg.link_timeout_ms) {
            transition_to(GNAV_ERROR, board);
        }
        break;

    /* ============================================================
     * 出场直行
     * ============================================================ */
    case GNAV_EXIT_STRAIGHT:
        out->override_v  = s_nav.cfg.exit_v;
        out->override_w  = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
                                            s_nav.cfg.heading_kp);
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        /* 检测侧向全丢 */
        if (!s_nav.exit_vl53_lost && all_side_lost(obs)) {
            s_nav.exit_vl53_lost = true;
            s_nav.exit_lost_distance = s_nav.odom_distance_accum;
        }
        if (s_nav.exit_vl53_lost) {
            float since_lost = s_nav.odom_distance_accum - s_nav.exit_lost_distance;
            if (since_lost >= s_nav.cfg.exit_runout) {
                transition_to(GNAV_DOCK, board);
            }
        }
        /* 里程上限保护 */
        if (odom_since_entry() >= s_nav.cfg.exit_max_dist) {
            transition_to(GNAV_DOCK, board);
        }
        if (elapsed_ms > s_nav.cfg.exit_timeout_ms) {
            transition_to(GNAV_DOCK, board);
        }
        break;

    /* ============================================================
     * 回停启动区
     * ============================================================ */
    case GNAV_DOCK:
        out->override_v  = s_nav.cfg.dock_v;
        out->override_w  = 0.0f;
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        if (odom_since_entry() >= s_nav.cfg.dock_distance ||
            elapsed_ms > 5000U)
        {
            transition_to(GNAV_FINISHED, board);
        }
        break;

    /* ============================================================
     * 终态
     * ============================================================ */
    case GNAV_FINISHED:
        out->override_v = 0.0f;
        out->override_w = 0.0f;
        out->safety_mode = SAFETY_MODE_IDLE;
        out->active = false;
        break;

    /* ============================================================
     * 异常态
     * ============================================================ */
    case GNAV_ERROR:
        out->override_v = 0.0f;
        out->override_w = 0.0f;
        out->safety_mode = SAFETY_MODE_IDLE;

        if (elapsed_ms > 2000U) {
            transition_to(GNAV_FINISHED, board);
        }
        break;

    default:
        out->override_v = 0.0f;
        out->override_w = 0.0f;
        break;
    }

    /* 更新输出阶段 (可能在 switch 内已经 transition) */
    out->stage       = s_nav.stage;
    out->stage_name  = GlobalNav_GetStageName(s_nav.stage);
    out->corridor_id = s_nav.current_corridor_id;
    out->corridors_done = s_nav.corridors_completed;
    out->active      = s_nav.running;
}