425 lines
17 KiB
C
425 lines
17 KiB
C
/**
|
||
* @file nav_script.c
|
||
* @brief 段脚本执行器实现：编排完整比赛流程
|
||
*/
|
||
|
||
#include "nav_script.h"
|
||
#include "est/corridor_filter.h"
|
||
#include "robot_params.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; // 转向是否已开始
|
||
bool turn_coarse_done; // 180° IMU 粗转是否已完成
|
||
uint32_t turn_settle_start_ms; // 进入侧墙精调阶段的时刻
|
||
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
|
||
#define SCRIPT_TURN_SETTLE_KP 1.4f
|
||
#define SCRIPT_TURN_SETTLE_W_MAX 0.6f
|
||
#define SCRIPT_TURN_SETTLE_TOL_RAD 0.05f
|
||
#define SCRIPT_TURN_SETTLE_TIMEOUT_MS 1200u
|
||
|
||
/* =========================================================
|
||
* 内部辅助函数
|
||
* ========================================================= */
|
||
static inline float clampf(float val, float lo, float hi)
|
||
{
|
||
if (val < lo) return lo;
|
||
if (val > hi) return hi;
|
||
return val;
|
||
}
|
||
|
||
static bool compute_wall_heading_error(const CorridorObs_t *obs, float *out_heading_rad)
|
||
{
|
||
float heading_sum = 0.0f;
|
||
int valid_count = 0;
|
||
const float sensor_base = PARAM_SENSOR_BASE_LENGTH;
|
||
|
||
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) {
|
||
heading_sum += atan2f(obs->d_lr - obs->d_lf, sensor_base);
|
||
valid_count++;
|
||
}
|
||
|
||
if (right_ok) {
|
||
heading_sum += atan2f(obs->d_rf - obs->d_rr, sensor_base);
|
||
valid_count++;
|
||
}
|
||
|
||
if (valid_count == 0) {
|
||
return false;
|
||
}
|
||
|
||
*out_heading_rad = heading_sum / (float)valid_count;
|
||
return true;
|
||
}
|
||
|
||
/* =========================================================
|
||
* 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.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;
|
||
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;
|
||
s_internal.turn_coarse_done = false;
|
||
s_internal.turn_settle_start_ms = 0U;
|
||
}
|
||
|
||
/* 原地 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 (!s_internal.turn_coarse_done && remaining <= 0.1f) {
|
||
s_internal.turn_coarse_done = true;
|
||
s_internal.turn_settle_start_ms = obs->t_ms;
|
||
}
|
||
|
||
if (s_internal.turn_coarse_done) {
|
||
float wall_heading = 0.0f;
|
||
bool wall_heading_ok = compute_wall_heading_error(obs, &wall_heading);
|
||
bool settle_timeout = (obs->t_ms - s_internal.turn_settle_start_ms) >= SCRIPT_TURN_SETTLE_TIMEOUT_MS;
|
||
|
||
if (wall_heading_ok) {
|
||
if (fabsf(wall_heading) <= SCRIPT_TURN_SETTLE_TOL_RAD) {
|
||
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 {
|
||
out->override_v = 0.0f;
|
||
out->override_w = clampf(-SCRIPT_TURN_SETTLE_KP * wall_heading,
|
||
-SCRIPT_TURN_SETTLE_W_MAX,
|
||
SCRIPT_TURN_SETTLE_W_MAX);
|
||
out->use_override = true;
|
||
}
|
||
} else if (settle_timeout) {
|
||
/* 极端情况下侧墙暂时不可用，超时后接受 IMU 粗转结果，避免卡死。 */
|
||
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 {
|
||
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) {
|
||
/* [改进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;
|
||
}
|
||
|
||
/* ==============================================
|
||
* 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;
|
||
}
|
||
}
|
||
}
|