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);
}
}

4
App/robot_params.h
View File

@@ -530,9 +530,9 @@ extern "C" {
/* --- 连接段 --- */
/* 在纵向端部通道里北行，从一条垄沟入口到下一条入口，距离=垄沟宽40cm+垄背宽30cm=70cm */
#define PARAM_GNAV_LINK_V 0.10f /* m/s — 连接段速度 (纵向通道内，IMU航向保持) */
#define PARAM_GNAV_LINK_DISTANCE 0.90f /* m — 连接段标称距离 (沟间距) */
#define PARAM_GNAV_LINK_DISTANCE 0.70f /* m — 连接段标称距离 (沟间距中心到中心) */
#define PARAM_GNAV_LINK_TIMEOUT 8000U /* ms — 连接段超时 */
#define PARAM_GNAV_LINK_GAP_RUNOUT 0.06f /* m — 看到下一个沟口后继续前冲距离 */
#define PARAM_GNAV_LINK_GAP_RUNOUT 0.08f /* m — 看到下一个沟口后继续前冲距离 */
#define PARAM_GNAV_LINK_WALL_TARGET 0.10f /* m — 连接段最小离墙距离（小于10cm才触发保底修正） */
#define PARAM_GNAV_LINK_WALL_KP 3.0f /* — 连接段离墙保底增益 */
#define PARAM_GNAV_LINK_WALL_BLEND 0.5f /* — 预留给其他通道状态，连接段本身不再使用融合 */