Falling_jasmine/ASER
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1.0

Browse Source
This commit is contained in:
nitiantuhao
2026-04-12 11:57:14 +08:00
parent cedcd4738e
commit 9ffb750072
10 changed files with 521 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files

396
App/nav/global_nav.c
View File

@@ -49,6 +49,12 @@ static struct {
/* 重捕获 */
uint8_t reacquire_ok_count;
/* 对齐 */
uint8_t align_ok_count;
uint8_t wall_heading_stable_count;
float wall_heading_prev_rad;
bool wall_heading_prev_valid;
/* 出场 */
bool exit_vl53_lost;
float exit_lost_distance;
@@ -60,6 +66,8 @@ static struct {
float link_d_front_start; /* 进入连接段时前激光读数 (m) */
bool link_d_front_valid; /* 进入时前激光是否有效 */
uint8_t link_gap_count; /* 非围栏侧 VL53 连续丢失计数 (沟口确认) */
bool link_gap_seen; /* 是否已经确认看到下一个沟口 */
float link_gap_seen_odom; /* 看到沟口时的累计里程 */
/* EKF 进度保存 */
float corridor_entry_s;
@@ -127,19 +135,20 @@ static bool gap_detected_on_open_side(const CorridorObs_t* obs,
TravelDirection_t prev_travel_dir)
{
if (prev_travel_dir == TRAVEL_DIR_EAST) {
/* 在右端通道，右侧贴围栏 → 检查左侧 VL53 */
/* 在右端通道，右侧贴围栏 → 检查左侧 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;
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;
return rf_lost && rr_lost; /* 前后都丢失才算沟口 */
}
}
@@ -151,26 +160,95 @@ static bool all_side_lost(const CorridorObs_t* obs)
return (obs->valid_mask & side_mask) == 0U;
}
/** 计算墙壁航向误差（用于转向后微调） */
static bool compute_wall_heading_error(const CorridorObs_t* obs, float* out_heading_rad)
{
const float sensor_base = PARAM_SENSOR_BASE_LENGTH;
bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) &&
((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U);
bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) &&
((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U);
/* 赛道模式这里要求双侧都有效，单侧/退化数据不允许触发航向摆正。 */
if (!left_ok || !right_ok) {
return false;
}
float left_heading = atan2f(obs->d_lr - obs->d_lf, sensor_base);
float right_heading = atan2f(obs->d_rf - obs->d_rr, sensor_base);
/* 双侧估计必须基本一致，避免沟口边缘/单边异常导致突然大打角。 */
if (gnav_fabsf(left_heading - right_heading) > PARAM_DEG2RAD(8.0f)) {
return false;
}
*out_heading_rad = 0.5f * (left_heading + right_heading);
return true;
}
static void update_wall_heading_stability(bool valid, float heading_rad)
{
if (!valid) {
s_nav.wall_heading_stable_count = 0;
s_nav.wall_heading_prev_valid = false;
return;
}
if (s_nav.wall_heading_prev_valid &&
gnav_fabsf(heading_rad - s_nav.wall_heading_prev_rad) <= PARAM_DEG2RAD(4.0f)) {
if (s_nav.wall_heading_stable_count < 255U) {
s_nav.wall_heading_stable_count++;
}
} else {
s_nav.wall_heading_stable_count = 1;
}
s_nav.wall_heading_prev_rad = heading_rad;
s_nav.wall_heading_prev_valid = true;
}
/* 重捕获已确认后，判断是否需要短暂停车摆正。
* 除了航向误差本身，还把“明显贴墙/明显偏中心”作为触发条件。
* 这样即使车头看起来近似平行，但整车已经贴到一侧，也会先停一下再进跟踪。 */
static bool need_align_after_reacquire(const CorridorObs_t* obs, float wall_heading_error)
{
bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) &&
((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U);
bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) &&
((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U);
if (!left_ok || !right_ok) {
return gnav_fabsf(wall_heading_error) > s_nav.cfg.align_th_tol_rad;
}
{
float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
float half_gap = 0.5f * (s_nav.cfg.corridor_width - PARAM_ROBOT_WIDTH);
float min_side = (d_left < d_right) ? d_left : d_right;
bool heading_bad = gnav_fabsf(wall_heading_error) > s_nav.cfg.align_th_tol_rad;
bool near_wall = min_side < (half_gap - s_nav.cfg.align_y_tol_m);
bool off_center = gnav_fabsf(d_left - d_right) > (2.0f * s_nav.cfg.align_y_tol_m);
return heading_bad || near_wall || off_center;
}
}
/** 检查重捕获条件 */
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: 左右距离和 ≈ 走廊宽度 */
/* 条件 1: 四个侧向传感器都有效
* 赛道模式的重捕获要更稳，不能接受 3/4 这种退化几何。 */
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) {
if (!left_ok || !right_ok) return false;
/* 条件 2: 左右距离和 ≈ 走廊宽度，且这里是必检项 */
{
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;
@@ -201,6 +279,7 @@ static const char* const s_stage_names[] = {
"ENTRY_STRAIGHT",
"TURN_INTO_CORRIDOR",
"REACQUIRE",
"ALIGN",
"CORRIDOR_TRACK",
"TURN_OUT",
"LINK_STRAIGHT",
@@ -294,6 +373,10 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board)
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;
s_nav.align_ok_count = 0;
s_nav.wall_heading_stable_count = 0;
s_nav.wall_heading_prev_rad = 0.0f;
s_nav.wall_heading_prev_valid = false;
switch (next) {
case GNAV_ENTRY_STRAIGHT:
@@ -339,6 +422,8 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board)
s_nav.link_d_front_start = 0.0f;
s_nav.link_d_front_valid = false; /* 首拍再记录 */
s_nav.link_gap_count = 0;
s_nav.link_gap_seen = false;
s_nav.link_gap_seen_odom = 0.0f;
break;
case GNAV_EXIT_STRAIGHT:
@@ -501,7 +586,7 @@ void GlobalNav_Update(const CorridorObs_t* obs,
break;
/* ============================================================
* 三种转向状态统一处理
* 转向进入下一条沟 (原地转 90°)
* ============================================================ */
case GNAV_TURN_INTO_CORRIDOR:
case GNAV_TURN_OUT_OF_CORRIDOR:
@@ -511,27 +596,117 @@ void GlobalNav_Update(const CorridorObs_t* obs,
/* ============================================================
* 重捕获走廊
*
* 问题背景：
* 转弯刚完成时，车身可能还在沟口外（端部通道内），
* 但此时两侧VL53同样能测到两侧垄背端面（~10cm），
* 宽度和 = 10+10+20 = 40cm，与真正在沟内完全一致。
* 这导致尚未入沟就满足重捕获条件，切到ALIGN，
* EKF的e_y可能是大偏差，车卡死在入口。
*
* 修复方案：
* 引入后部激光距离检测：只有后激光距离 > 40cm，
* 说明车身已完全进入沟内，才允许开始确认计数。
* 这确保四颗VL53全部脱离沟口边缘进入稳定侧壁区域。
* ============================================================ */
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->override_w = 0.0f; /* 不做航向控制，让车自然进沟，ALIGN阶段再摆正 */
out->safety_mode = SAFETY_MODE_STRAIGHT;
if (check_reacquire(obs, state)) {
s_nav.reacquire_ok_count++;
} else {
s_nav.reacquire_ok_count = 0;
{
/* 进沟深度守卫：后激光 + 最小入沟里程 双保险。
* 尤其对 C1，不能只靠后激光，否则在入口区就可能过早假成功。 */
bool back_ok = false;
bool odom_ok = (odom_since_entry() >= s_nav.cfg.reacquire_min_odom);
if ((obs->valid_mask & CORRIDOR_OBS_MASK_BACK) != 0U) {
if (obs->d_back > s_nav.cfg.reacquire_min_back_dist) {
back_ok = true;
}
}
if (back_ok && odom_ok && 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);
float wall_heading_error = 0.0f;
bool wall_heading_ok = compute_wall_heading_error(obs, &wall_heading_error);
update_wall_heading_stability(wall_heading_ok, wall_heading_error);
/* 重捕获成功后，优先用 side VL53 短暂停车摆正车头。
* 只有 wall heading 已稳定时才允许进入 ALIGN 或直接放行。 */
if (wall_heading_ok && s_nav.wall_heading_stable_count >= 4U) {
if (need_align_after_reacquire(obs, wall_heading_error)) {
transition_to(GNAV_ALIGN, board);
} else {
if (board->imu_yaw_continuous.is_valid) {
CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f);
s_nav.heading_ref_deg = board->imu_yaw_continuous.value;
}
transition_to(GNAV_CORRIDOR_TRACK, board);
}
}
}
if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms) {
transition_to(GNAV_ERROR, board);
/* 取消重捕获失败态：超时后不进 ERROR，
* 直接转入短暂停车摆正阶段，让车停下来继续自恢复。 */
transition_to(GNAV_ALIGN, board);
}
break;
/* ============================================================
* 短暂停车摆正航向 (ALIGN)
*
* 只用 side VL53 的前后差来摆正车头，不做横向居中。
* 目的不是把车居中，而是避免“斜着进沟时前轮先蹭墙，
* 控制器还继续朝墙边打”的情况。
* ============================================================ */
case GNAV_ALIGN: {
float wall_heading_error = 0.0f;
bool wall_heading_ok = compute_wall_heading_error(obs, &wall_heading_error);
update_wall_heading_stability(wall_heading_ok, wall_heading_error);
out->override_v = 0.0f;
out->use_override = true;
out->request_corridor = false;
out->safety_mode = SAFETY_MODE_STRAIGHT;
if (wall_heading_ok && s_nav.wall_heading_stable_count >= 2U) {
out->override_w = gnav_clampf(-s_nav.cfg.align_kp_th * wall_heading_error,
-0.25f, 0.25f);
} else {
out->override_w = 0.0f;
}
if (wall_heading_ok && gnav_fabsf(wall_heading_error) < s_nav.cfg.align_th_tol_rad) {
s_nav.align_ok_count++;
} else {
s_nav.align_ok_count = 0;
}
if (s_nav.align_ok_count >= s_nav.cfg.align_confirm_ticks) {
if (board->imu_yaw_continuous.is_valid) {
CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f);
s_nav.heading_ref_deg = board->imu_yaw_continuous.value;
}
transition_to(GNAV_CORRIDOR_TRACK, board);
}
if (elapsed_ms > s_nav.cfg.align_timeout_ms) {
if (board->imu_yaw_continuous.is_valid) {
CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f);
s_nav.heading_ref_deg = board->imu_yaw_continuous.value;
}
transition_to(GNAV_CORRIDOR_TRACK, board);
}
break;
}
/* ============================================================
* 沟内闭环跟踪 (交给 corridor_ctrl)
* ============================================================ */
@@ -540,11 +715,9 @@ void GlobalNav_Update(const CorridorObs_t* obs,
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++;
@@ -552,7 +725,7 @@ void GlobalNav_Update(const CorridorObs_t* obs,
}
}
}
/* 里程超长保护 */
if (odom_since_entry() > s_nav.cfg.corridor_length_max) {
s_nav.corridors_completed++;
transition_to(GNAV_TURN_OUT_OF_CORRIDOR, board);
@@ -560,91 +733,128 @@ void GlobalNav_Update(const CorridorObs_t* obs,
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探到沟口 → 联合触发（互相校验）
* 控制策略：IMU 决定主航向；单边 VL53 只做离墙保底。
* 也就是说：
* - 默认纯航向保持
* - 只有当贴围栏侧距离小于阈值时，才附加一个远离墙面的修正
* - 不做墙跟随融合，避免单边 VL53 把主方向带偏
* ============================================================ */
case GNAV_LINK_STRAIGHT:
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;
float w_heading = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
s_nav.cfg.heading_kp);
float w_wall_guard = 0.0f;
const CorridorDescriptor_t* corridor = TrackMap_GetCorridor(s_nav.current_corridor_id);
if (corridor != NULL && corridor->travel_dir == TRAVEL_DIR_EAST) {
if ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) &&
(obs->valid_mask & CORRIDOR_OBS_MASK_RR)) {
float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
if (d_right < s_nav.cfg.link_wall_target) {
float error = s_nav.cfg.link_wall_target - d_right;
w_wall_guard = s_nav.cfg.link_wall_kp * error;
}
}
} else if (corridor != NULL) {
if ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) &&
(obs->valid_mask & CORRIDOR_OBS_MASK_LR)) {
float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
if (d_left < s_nav.cfg.link_wall_target) {
float error = s_nav.cfg.link_wall_target - d_left;
w_wall_guard = -(s_nav.cfg.link_wall_kp * error);
}
}
}
out->override_w = gnav_clampf(w_heading + w_wall_guard, -1.0f, 1.0f);
{
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);
bool gap_confirmed = false;
if (corridor != NULL) {
bool gap_now = gap_detected_on_open_side(obs, corridor->travel_dir);
if (gap_now) {
if (s_nav.link_gap_count < 255U) s_nav.link_gap_count++;
} else {
s_nav.link_gap_count = 0;
}
gap_confirmed = (s_nav.link_gap_count >= 5U);
if (gap_now) {
if (s_nav.link_gap_count < 255) s_nav.link_gap_count++;
} else {
s_nav.link_gap_count = 0;
if (gap_confirmed && !s_nav.link_gap_seen) {
s_nav.link_gap_seen = true;
s_nav.link_gap_seen_odom = s_nav.odom_distance_accum;
}
}
bool gap_confirmed = (s_nav.link_gap_count >= 2); /* 连续2拍 (40ms @ 20ms周期) */
/* ---- 联合判定: B || (A && C) ---- */
if (laser_ok || (odom_ok && gap_confirmed))
{
bool gap_runout_ok = false;
if (s_nav.link_gap_seen) {
gap_runout_ok = (s_nav.odom_distance_accum - s_nav.link_gap_seen_odom)
>= s_nav.cfg.link_gap_runout;
}
if (odom_ok && (laser_ok || (s_nav.link_gap_seen && gap_runout_ok))) {
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:
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;
/* 检测侧向全丢 */
float w_heading = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
s_nav.cfg.heading_kp);
float w_wall = 0.0f;
bool rf_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U;
bool rr_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U;
if (rf_ok || rr_ok) {
float d_right;
if (rf_ok && rr_ok) {
d_right = (obs->d_rf + obs->d_rr) * 0.5f;
} else if (rf_ok) {
d_right = obs->d_rf;
} else {
d_right = obs->d_rr;
}
/* 出场阶段在左端通道向南走，右侧仍然是围栏。
* 用右侧 VL53 直接叠加一个更硬的保墙约束，避免一路蹭墙。 */
{
float error = s_nav.cfg.link_wall_target - d_right;
w_wall = 1.5f * s_nav.cfg.link_wall_kp * error;
}
}
out->override_w = gnav_clampf(w_heading + w_wall, -1.0f, 1.0f);
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;
@@ -655,7 +865,6 @@ void GlobalNav_Update(const CorridorObs_t* obs,
transition_to(GNAV_DOCK, board);
}
}
/* 里程上限保护 */
if (odom_since_entry() >= s_nav.cfg.exit_max_dist) {
transition_to(GNAV_DOCK, board);
}
@@ -663,21 +872,48 @@ void GlobalNav_Update(const CorridorObs_t* obs,
transition_to(GNAV_DOCK, board);
}
break;
}
/* ============================================================
* 回停启动区
* ============================================================ */
case GNAV_DOCK:
case GNAV_DOCK: {
out->override_v = s_nav.cfg.dock_v;
out->override_w = 0.0f;
out->safety_mode = SAFETY_MODE_STRAIGHT;
float w_heading = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
s_nav.cfg.heading_kp);
float w_wall = 0.0f;
bool rf_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U;
bool rr_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U;
if (rf_ok || rr_ok) {
float d_right;
if (rf_ok && rr_ok) {
d_right = (obs->d_rf + obs->d_rr) * 0.5f;
} else if (rf_ok) {
d_right = obs->d_rf;
} else {
d_right = obs->d_rr;
}
/* 回停阶段右墙保持更硬：
* IMU 仍然给主方向，但右侧距离约束直接叠加，不再做 50% 混合。 */
{
float error = s_nav.cfg.link_wall_target - d_right;
w_wall = 1.5f * s_nav.cfg.link_wall_kp * error;
}
}
out->override_w = gnav_clampf(w_heading + w_wall, -1.0f, 1.0f);
if (odom_since_entry() >= s_nav.cfg.dock_distance ||
elapsed_ms > 5000U)
{
elapsed_ms > 5000U) {
transition_to(GNAV_FINISHED, board);
}
break;
}
/* ============================================================
* 终态