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

17
App/nav/global_nav.c
View File

@@ -220,8 +220,18 @@ static void execute_turn(const CorridorObs_t* obs,
uint32_t now_ms,
GlobalNavOutput_t* out)
{
float imu_yaw = board->imu_yaw_continuous.is_valid
? board->imu_yaw_continuous.value : 0.0f;
/* [改进G] IMU 失效安全保护: 没有 IMU 数据时立即停车，不盲转。
* 超时后会被外部超时保护捕获，进入 GNAV_ERROR。 */
if (!board->imu_yaw_continuous.is_valid) {
out->override_v = 0.0f;
out->override_w = 0.0f;
out->use_override = true;
out->request_corridor = false;
out->safety_mode = SAFETY_MODE_IDLE;
return;
}
float imu_yaw = board->imu_yaw_continuous.value;
/* 已转过的角度 (取绝对值) */
float delta = (imu_yaw - s_nav.turn_start_yaw_deg) * s_nav.turn_sign;
@@ -438,8 +448,9 @@ void GlobalNav_Update(const CorridorObs_t* obs,
s_last_odom_vx = odom_vx;
}
/* [改进G] IMU yaw 提取: 失效时使用参考值，保持航向不变而不是跳到 0 */
float imu_yaw_deg = board->imu_yaw_continuous.is_valid
? board->imu_yaw_continuous.value : 0.0f;
? board->imu_yaw_continuous.value : s_nav.heading_ref_deg;
/* 默认输出 */
out->use_override = true;

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;