323 lines
13 KiB
C
323 lines
13 KiB
C
/**
|
||
* @file nav_script.c
|
||
* @brief 段脚本执行器实现：编排完整比赛流程
|
||
*/
|
||
|
||
#include "nav_script.h"
|
||
#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 里程
|
||
int pass_count; // 已走过的垄沟数
|
||
float exit_start_s; // 离开垄沟瞬间的 s 里程 (0=未触发)
|
||
} s_internal;
|
||
|
||
/* =========================================================
|
||
* 内部辅助函数
|
||
* ========================================================= */
|
||
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.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.pass_count = 1;
|
||
s_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
|
||
/* ==============================================
|
||
* STAGE 2: 向前走垄沟
|
||
* ============================================== */
|
||
case SCRIPT_STAGE_CORRIDOR_FORWARD: {
|
||
/* 使用走廊控制器 */
|
||
out->request_corridor = true;
|
||
|
||
/* 检查是否到端 */
|
||
bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
|
||
if (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_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) {
|
||
/* 转向完成 -> 决定下一步 */
|
||
if (s_internal.pass_count < 2) {
|
||
/* 只走了一遍，往回走 */
|
||
s_internal.pass_count++;
|
||
s_stage = SCRIPT_STAGE_CORRIDOR_BACKWARD;
|
||
} else {
|
||
/* 走了两遍，退出场地 */
|
||
s_stage = SCRIPT_STAGE_EXIT;
|
||
}
|
||
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 (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_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;
|
||
}
|
||
}
|
||
}
|