Falling_jasmine/ASER
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
1ecdf6c1875063223e926b79ca529d52bfb5b4f0
ASER/App/nav/nav_script.c

346 lines
14 KiB
C
Raw Normal View History

1.0
2026-03-31 23:30:33 +08:00
/**
* @file nav_script.c
* @brief 段脚本执行器实现：编排完整比赛流程
*/
#include "nav_script.h"
1.0
2026-04-04 15:59:11 +08:00
#include "est/corridor_filter.h"
1.0
2026-03-31 23:30:33 +08:00
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>
/* =========================================================
* 内部状态
* ========================================================= */
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 里程
1.0
2026-04-04 17:09:19 +08:00
float end_rearm_s; // 掉头后到端检测重新使能的起始里程
bool end_armed; // 到端检测是否已重新使能
NavScriptStage_t post_turn_stage; // 本次转向完成后要进入的走廊阶段
1.0
2026-03-31 23:30:33 +08:00
int pass_count; // 已走过的垄沟数
float exit_start_s; // 离开垄沟瞬间的 s 里程 (0=未触发)
} s_internal;
1.0
2026-04-04 17:09:19 +08:00
#define SCRIPT_END_REARM_DIST_M 0.12f
1.0
2026-03-31 23:30:33 +08:00
/* =========================================================
* 内部辅助函数
* ========================================================= */
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;
1.0
2026-04-04 17:09:19 +08:00
s_internal.end_rearm_s = state->s;
s_internal.end_armed = true;
1.0
2026-03-31 23:30:33 +08:00
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;
1.0
2026-04-04 17:09:19 +08:00
s_internal.end_rearm_s = state->s;
s_internal.end_armed = true;
1.0
2026-03-31 23:30:33 +08:00
s_internal.pass_count = 1;
s_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
}
}
break;
}
/* ==============================================
* STAGE 2: 向前走垄沟
* ============================================== */
case SCRIPT_STAGE_CORRIDOR_FORWARD: {
/* 使用走廊控制器 */
out->request_corridor = true;
1.0
2026-04-04 17:09:19 +08:00
if (!s_internal.end_armed) {
if ((state->s - s_internal.end_rearm_s) >= SCRIPT_END_REARM_DIST_M) {
s_internal.end_armed = true;
}
}
1.0
2026-03-31 23:30:33 +08:00
/* 检查是否到端 */
bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
1.0
2026-04-04 17:09:19 +08:00
if (s_internal.end_armed && front_ok && obs->d_front <= s_cfg.d_entry_exit_front) {
1.0
2026-03-31 23:30:33 +08:00
/* 前向距离足够近 -> 到达垄沟末端，准备转向 */
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;
1.0
2026-04-04 17:09:19 +08:00
s_internal.post_turn_stage = SCRIPT_STAGE_CORRIDOR_BACKWARD;
1.0
2026-03-31 23:30:33 +08:00
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) {
/* 转向完成 -> 决定下一步 */
1.0
2026-04-04 17:09:19 +08:00
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;
1.0
2026-03-31 23:30:33 +08:00
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;
1.0
2026-04-04 17:09:19 +08:00
/* 掉头回来时，前向雷达可能还残留近端读数。
* 必须先离开端墙一小段距离，再允许重新触发到端检测。 */
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° 转向循环 */
1.0
2026-03-31 23:30:33 +08:00
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;
1.0
2026-04-04 17:09:19 +08:00
s_internal.post_turn_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
1.0
2026-03-31 23:30:33 +08:00
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;
}
}
}
Reference in New Issue Copy Permalink