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-04 23:24:36 +08:00
parent 5e0901a4d9
commit 7c2396a282
11 changed files with 132 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
App/nav/nav_script.c
View File

@@ -228,7 +228,8 @@ void NavScript_Update(const CorridorObs_t *obs,
/* 检查是否到端 */
bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
if (s_internal.end_armed && front_ok && obs->d_front <= s_cfg.d_entry_exit_front) {
/* 前向距离足够近 -> 到达垄沟末端，准备转向 */
/* 前向距离足够近 -> 到达垄沟末端，递增趟数并准备转向 */
s_internal.pass_count++;
s_internal.turn_start_e_th = state->e_th;
s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg;
s_internal.turn_started = false;
@@ -344,12 +345,21 @@ void NavScript_Update(const CorridorObs_t *obs,
}
if (s_internal.end_armed && front_ok && obs->d_front <= s_cfg.d_entry_exit_front) {
/* 前向距离足够近 -> 到达另一端，继续 180° 转向循环 */
s_internal.turn_start_e_th = state->e_th;
s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg;
s_internal.turn_started = false;
s_internal.post_turn_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
s_stage = SCRIPT_STAGE_TURN_AT_END;
/* [改进H] 到达另一端: 递增趟数并判断是否退出
* pass_count=1(入沟首趟), =2(返回), =3(第三趟) → EXIT */
s_internal.pass_count++;
if (s_internal.pass_count >= 3) {
/* 走了 3 趟 (去-回-去)，进入退出阶段 */
s_stage = SCRIPT_STAGE_EXIT;
s_internal.exit_start_s = 0.0f;
} else {
/* 继续往返: 转向后进入 FORWARD */
s_internal.turn_start_e_th = state->e_th;
s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg;
s_internal.turn_started = false;
s_internal.post_turn_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
s_stage = SCRIPT_STAGE_TURN_AT_END;
}
out->request_corridor = false;
}
break;