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 14:30:19 +08:00
parent 521b70c29b
commit 315495bab3
2 changed files with 14 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
App/nav/global_nav.c
View File

@@ -857,12 +857,17 @@ void GlobalNav_Update(const CorridorObs_t* obs,
s_nav.link_d_front_valid = true;
}
bool odom_ok = odom_since_entry() >= s_nav.cfg.link_distance * 0.85f;
/* 连接段入口判定原则：
* 用“看到开口”的那一刻作为唯一锚点，然后固定前冲一小段再转向。
* 这样每次到沟入口的位置更一致，不再混用多套主触发条件。
*/
bool odom_guard_ok = odom_since_entry() >= s_nav.cfg.link_distance * 0.55f;
/* 前激光仅保留为兜底，不再参与主触发。 */
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;
laser_ok = (d_front_delta >= s_nav.cfg.link_distance * 0.85f);
laser_ok = (d_front_delta >= s_nav.cfg.link_distance * 1.05f);
}
bool gap_confirmed = false;
@@ -875,7 +880,7 @@ void GlobalNav_Update(const CorridorObs_t* obs,
}
gap_confirmed = (s_nav.link_gap_count >= 5U);
if (gap_confirmed && !s_nav.link_gap_seen) {
if (odom_guard_ok && gap_confirmed && !s_nav.link_gap_seen) {
s_nav.link_gap_seen = true;
s_nav.link_gap_seen_odom = s_nav.odom_distance_accum;
}
@@ -887,7 +892,10 @@ void GlobalNav_Update(const CorridorObs_t* obs,
>= s_nav.cfg.link_gap_runout;
}
if (odom_ok && (laser_ok || (s_nav.link_gap_seen && gap_runout_ok))) {
if (s_nav.link_gap_seen && gap_runout_ok) {
transition_to(GNAV_TURN_INTO_NEXT, board);
} else if (!s_nav.link_gap_seen && laser_ok) {
/* 极端兜底：如果开口检测失效，才允许前激光变化量接管。 */
transition_to(GNAV_TURN_INTO_NEXT, board);
}
}