/** * @file nav_script.c * @brief 段脚本执行器实现：编排完整比赛流程 */ #include "nav_script.h" #include "est/corridor_filter.h" #include #include #include #include /* ========================================================= * 内部状态 * ========================================================= */ static NavScriptConfig_t s_cfg; static NavScriptStage_t s_stage = SCRIPT_STAGE_IDLE; static bool s_initialized = false; static bool s_running = false; /* 阶段内部状态变量 */ static struct { uint32_t entry_align_start_ms; // 入口对准开始时间 float turn_start_e_th; // 转向开始时的 EKF 航向 (保留作后备) 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 /* ========================================================= * 内部辅助函数 * ========================================================= */ static inline float clampf(float val, float lo, float hi) { if (val < lo) return lo; if (val > hi) return hi; return val; } /* ========================================================= * API 实现 * ========================================================= */ void NavScript_Init(const NavScriptConfig_t *config) { s_cfg = *config; s_stage = SCRIPT_STAGE_IDLE; s_running = false; s_initialized = true; /* 初始化内部状态 */ memset(&s_internal, 0, sizeof(s_internal)); } void NavScript_Start(void) { if (s_initialized) { s_stage = SCRIPT_STAGE_ENTRY_ALIGN; s_running = true; s_internal.entry_align_start_ms = 0; s_internal.pass_count = 0; } } NavScriptStage_t NavScript_GetStage(void) { return s_stage; } const char* NavScript_GetStageName(NavScriptStage_t stage) { switch (stage) { case SCRIPT_STAGE_IDLE: return "IDLE"; case SCRIPT_STAGE_ENTRY_ALIGN: return "ENTRY_ALIGN"; case SCRIPT_STAGE_CORRIDOR_FORWARD: return "CORRIDOR_FWD"; case SCRIPT_STAGE_TURN_AT_END: return "TURN_AT_END"; case SCRIPT_STAGE_CORRIDOR_BACKWARD: return "CORRIDOR_BWD"; case SCRIPT_STAGE_EXIT: return "EXIT"; case SCRIPT_STAGE_FINISHED: return "FINISHED"; default: return "UNKNOWN"; } } void NavScript_Reset(void) { s_stage = SCRIPT_STAGE_IDLE; s_running = false; memset(&s_internal, 0, sizeof(s_internal)); } void NavScript_Update(const CorridorObs_t *obs, const CorridorState_t *state, float imu_yaw_continuous_deg, NavScriptOutput_t *out) { if (!s_initialized) { out->stage = SCRIPT_STAGE_IDLE; out->stage_name = "IDLE"; out->active = false; out->request_corridor = false; out->override_v = 0.0f; out->override_w = 0.0f; out->use_override = true; return; } /* 默认输出 */ out->stage = s_stage; out->stage_name = NavScript_GetStageName(s_stage); out->active = s_running; out->request_corridor = true; // 默认请求走廊控制器 out->use_override = false; // 默认不覆盖 if (!s_running) { out->request_corridor = false; out->override_v = 0.0f; out->override_w = 0.0f; out->use_override = true; return; } /* ======================================================== * 核心状态机：按比赛流程执行 * ======================================================== */ switch (s_stage) { /* ============================================== * STAGE 1: 入口对准 * ============================================== */ case SCRIPT_STAGE_ENTRY_ALIGN: { if (s_internal.entry_align_start_ms == 0) { s_internal.entry_align_start_ms = state->t_ms; } /* 检查是否已进入垄沟：两侧都有距离数据 */ bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) && ((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U); bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) && ((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U); 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; } else { /* 入口对准策略：慢速前进，让侧向雷达找墙 */ out->override_v = s_cfg.entry_align_v; // 慢速前进 (由 PARAM_SCRIPT_ENTRY_V 配置) out->override_w = 0.0f; out->use_override = true; out->request_corridor = false; /* P1 修复: 超时保护使用配置参数 s_cfg.entry_align_timeout， * 而非硬编码 30000 ms */ uint32_t elapsed = state->t_ms - s_internal.entry_align_start_ms; if (elapsed > (uint32_t)s_cfg.entry_align_timeout) { /* 超时强制进入走廊跟踪：必须补齐与正常入沟一致的内部状态， * 否则 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; } } break; } /* ============================================== * STAGE 2: 向前走垄沟 * ============================================== */ case SCRIPT_STAGE_CORRIDOR_FORWARD: { /* 使用走廊控制器 */ 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 (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; } break; } /* ============================================== * STAGE 3: 到端原地转向 * ============================================== */ case SCRIPT_STAGE_TURN_AT_END: { out->request_corridor = false; if (!s_internal.turn_started) { s_internal.turn_start_e_th = state->e_th; s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg; s_internal.turn_started = true; } /* 原地 180 度转向策略 */ float target_delta = s_cfg.turn_target_angle; // 默认 PI (180度) /* ---- 使用 IMU 连续 yaw 判定转角 (deg → rad) ---- * * 优点：IMU 内部维护的 yaw 经过模块校准，比外部积分 wz 更稳定， * 且 unwrap 后不存在 ±180° 跳变问题。 * 后备：如果 IMU 离线，退化回 EKF e_th 差值判定。 */ float delta_turned; float imu_delta_deg = imu_yaw_continuous_deg - s_internal.turn_start_imu_yaw_deg; delta_turned = imu_delta_deg * 0.01745329252f; // deg → rad float remaining = target_delta - fabsf(delta_turned); if (remaining <= 0.1f) { /* 转向完成 -> 决定下一步 */ 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; } else { /* 继续转向：根据目标方向选择角速度 */ float turn_dir = (target_delta > 0.0f) ? 1.0f : -1.0f; /* 接近目标时减速 */ float turn_speed = s_cfg.turn_omega; if (remaining < 0.5f) { turn_speed *= (remaining / 0.5f); // 线性减速 turn_speed = clampf(turn_speed, 0.3f, s_cfg.turn_omega); } out->override_v = 0.0f; // 线速度为 0，原地转 out->override_w = turn_dir * turn_speed; out->use_override = true; } break; } /* ============================================== * STAGE 4: 返程走垄沟 (原地转 180° 后继续正向行驶) * ============================================== */ case SCRIPT_STAGE_CORRIDOR_BACKWARD: { /* 使用走廊控制器，正向行驶 */ out->request_corridor = true; /* P1 修复: 原地转 180° 后车头已调转，返回方向即"向前"， * 因此到端检测应使用前向雷达 d_front，而非后向雷达 d_back */ bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U; /* 掉头回来时，前向雷达可能还残留近端读数。 * 必须先离开端墙一小段距离，再允许重新触发到端检测。 */ 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; } break; } /* ============================================== * STAGE 5: 退出场地 * ============================================== */ case SCRIPT_STAGE_EXIT: { /* P1 修复: 退出速度使用独立的 exit_v 参数， * 而非误用角速度参数 turn_omega * 0.5f */ out->request_corridor = false; out->override_v = s_cfg.exit_v; // 直线冲出速度 (由 PARAM_SCRIPT_EXIT_V 配置) out->override_w = 0.0f; out->use_override = true; /* 检测是否已离开：两侧雷达都丢失 */ bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) && ((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U); bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) && ((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U); if (!left_ok && !right_ok) { /* 两侧都丢了 -> 已离开垄沟，再往前冲 exit_runout_m 后停车 */ if (s_internal.exit_start_s == 0.0f) { s_internal.exit_start_s = state->s; } if (state->s - s_internal.exit_start_s >= s_cfg.exit_runout_m) { /* 冲够了，停车 */ s_stage = SCRIPT_STAGE_FINISHED; s_internal.exit_start_s = 0.0f; } } break; } /* ============================================== * STAGE 6: 比赛完成，安全停车 * ============================================== */ case SCRIPT_STAGE_FINISHED: { out->request_corridor = false; out->override_v = 0.0f; out->override_w = 0.0f; out->use_override = true; s_running = false; break; } /* ============================================== * DEFAULT/IDLE: 安全停车 * ============================================== */ case SCRIPT_STAGE_IDLE: default: { out->request_corridor = false; out->override_v = 0.0f; out->override_w = 0.0f; out->use_override = true; break; } } }