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 17:09:19 +08:00
parent ee63bee6e5
commit 1ecdf6c187
9 changed files with 201 additions and 320 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
App/nav/nav_script.c
View File

@@ -25,10 +25,15 @@ static struct {
float turn_start_imu_yaw_deg; // 转向开始时的 IMU 连续偏航角 (deg)
bool turn_started; // 转向是否已开始
float corridor_s_entry; // 进入垄沟时的 s 里程
float end_rearm_s; // 掉头后到端检测重新使能的起始里程
bool end_armed; // 到端检测是否已重新使能
NavScriptStage_t post_turn_stage; // 本次转向完成后要进入的走廊阶段
int pass_count; // 已走过的垄沟数
float exit_start_s; // 离开垄沟瞬间的 s 里程 (0=未触发)
} s_internal;
#define SCRIPT_END_REARM_DIST_M 0.12f
/* =========================================================
* 内部辅助函数
* ========================================================= */
@@ -142,6 +147,8 @@ void NavScript_Update(const CorridorObs_t *obs,
if (left_ok && right_ok && state->conf >= 0.8f) {
/* 两侧雷达都有数据，且置信度高 -> 进入垄沟，开始跟踪 */
s_internal.corridor_s_entry = state->s;
s_internal.end_rearm_s = state->s;
s_internal.end_armed = true;
s_internal.pass_count = 1;
s_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
out->request_corridor = true;
@@ -160,6 +167,8 @@ void NavScript_Update(const CorridorObs_t *obs,
* 否则 pass_count 停留在 0，导致后续 TURN_AT_END 判定时
* 多跑一趟走廊（三趟而非文档描述的两趟）。 */
s_internal.corridor_s_entry = state->s;
s_internal.end_rearm_s = state->s;
s_internal.end_armed = true;
s_internal.pass_count = 1;
s_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
}
@@ -174,13 +183,20 @@ void NavScript_Update(const CorridorObs_t *obs,
/* 使用走廊控制器 */
out->request_corridor = true;
if (!s_internal.end_armed) {
if ((state->s - s_internal.end_rearm_s) >= SCRIPT_END_REARM_DIST_M) {
s_internal.end_armed = true;
}
}
/* 检查是否到端 */
bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
if (front_ok && obs->d_front <= s_cfg.d_entry_exit_front) {
if (s_internal.end_armed && front_ok && obs->d_front <= s_cfg.d_entry_exit_front) {
/* 前向距离足够近 -> 到达垄沟末端，准备转向 */
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_BACKWARD;
s_stage = SCRIPT_STAGE_TURN_AT_END;
out->request_corridor = false;
}
@@ -214,18 +230,10 @@ void NavScript_Update(const CorridorObs_t *obs,
if (remaining <= 0.1f) {
/* 转向完成 -> 决定下一步 */
if (s_internal.pass_count < 2) {
/* 只走了一遍，往回走 */
/* 180° 掉头后，走廊方向基准已经翻转。
* 必须清空上一趟的 EKF/IMU 航向参考，避免返程首拍把新朝向
* 误判成大航向误差，导致一恢复闭环就猛打方向。 */
CorridorFilter_Reset();
s_internal.pass_count++;
s_stage = SCRIPT_STAGE_CORRIDOR_BACKWARD;
} else {
/* 走了两遍，退出场地 */
s_stage = SCRIPT_STAGE_EXIT;
}
CorridorFilter_RebaseAfterTurnaround(imu_yaw_continuous_deg * 0.01745329252f);
s_internal.end_rearm_s = state->s;
s_internal.end_armed = false;
s_stage = s_internal.post_turn_stage;
out->override_v = 0.0f;
out->override_w = 0.0f;
out->use_override = true;
@@ -257,11 +265,21 @@ void NavScript_Update(const CorridorObs_t *obs,
/* P1 修复: 原地转 180° 后车头已调转，返回方向即"向前"，
* 因此到端检测应使用前向雷达 d_front，而非后向雷达 d_back */
bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
if (front_ok && obs->d_front <= s_cfg.d_entry_exit_front) {
/* 前向距离足够近 -> 到达垄沟起始端，转向或退出 */
/* 掉头回来时，前向雷达可能还残留近端读数。
* 必须先离开端墙一小段距离，再允许重新触发到端检测。 */
if (!s_internal.end_armed) {
if ((state->s - s_internal.end_rearm_s) >= SCRIPT_END_REARM_DIST_M) {
s_internal.end_armed = true;
}
}
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;
out->request_corridor = false;
}