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-03 08:56:26 +08:00
parent 35e7f70e0f
commit 1bd0a73a73
17 changed files with 3147 additions and 1883 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
App/app_tasks.c
View File

@@ -19,6 +19,8 @@
#include "nav/corridor_ctrl.h"
#include "nav/segment_fsm.h"
#include "nav/nav_script.h"
#include "nav/global_nav.h"
#include "nav/track_map.h"
extern osMutexId_t logMutexHandle;
/* 如果你的项目中没有引入 i2c.h，可以通过 extern 声明 I2C 句柄 */
@@ -297,9 +299,14 @@ void AppTasks_RunNavTask_Impl(void *argument)
const uint32_t period_ticks = AppTasks_MsToTicks(20U);
uint32_t last_ms = HAL_GetTick();
/* 等传感器全部就绪再启动脚本 (避免刚上电全是脏数据) */
/* 等传感器全部就绪再启动 (避免刚上电全是脏数据) */
osDelay(500);
NavScript_Start(); // 开始执行比赛脚本
#if USE_GLOBAL_NAV
GlobalNav_Start(); /* 开始赛道级导航 */
#else
NavScript_Start(); /* 单沟测试模式 */
#endif
for (;;) {
uint32_t now_ms = HAL_GetTick();
@@ -317,11 +324,11 @@ void AppTasks_RunNavTask_Impl(void *argument)
CorridorObs_t obs;
CorridorPreproc_ExtractObs(&board, now_ms, &obs);
/* --- Step 3: 互补滤波 → 走廊状态估计 --- */
/* --- Step 3: EKF → 走廊状态估计 --- */
/* 注意: HWT101 输出 wz 单位是 °/s，EKF 需要 rad/s，必须转换！ */
float imu_wz_raw = board.imu_wz.is_valid ? board.imu_wz.value : 0.0f;
float imu_wz = PARAM_DEG2RAD(imu_wz_raw);
float odom_vx = board.odom_vx; /* 里程计已接入，由 monitorTask 调用 Odom_Update() 更新 */
float odom_vx = board.odom_vx;
/* IMU 连续 yaw → rad，作为 EKF 额外航向观测 */
float imu_yaw_cont_rad = board.imu_yaw_continuous.is_valid
@@ -332,34 +339,62 @@ void AppTasks_RunNavTask_Impl(void *argument)
CorridorFilter_Update(&obs, imu_wz, odom_vx, dt_s,
imu_yaw_cont_rad, imu_yaw_ok, &corridor_state);
/* --- Step 4: 段脚本执行器 → 决定当前阶段和目标 --- */
#if USE_GLOBAL_NAV
/* ========== 赛道级导航模式 (6沟 S 型遍历) ========== */
/* --- Step 4: 赛道级导航 --- */
GlobalNavOutput_t nav_out;
GlobalNav_Update(&obs, &corridor_state, &board, &nav_out);
/* --- Step 5: 控制律 --- */
RawCmd_t raw_cmd;
memset(&raw_cmd, 0, sizeof(raw_cmd));
raw_cmd.t_ms = now_ms;
if (nav_out.request_corridor) {
/* 沟内闭环：使用走廊控制器 */
CorridorCtrl_Compute(&corridor_state, &obs, imu_wz, &raw_cmd);
} else if (nav_out.use_override) {
/* 赛道级覆盖：直接用导航输出 */
raw_cmd.v = nav_out.override_v;
raw_cmd.w = nav_out.override_w;
}
/* else: raw_cmd 已是零速 */
/* --- Step 6: 安全仲裁 (带动作模式感知) --- */
SegFsmOutput_t fsm_out;
SegFsm_Update(&raw_cmd, &obs, &corridor_state, nav_out.safety_mode, &fsm_out);
#else
/* ========== 单沟测试模式 (原 nav_script) ========== */
/* --- Step 4: 段脚本执行器 --- */
NavScriptOutput_t script_out;
float imu_yaw_cont_deg = board.imu_yaw_continuous.is_valid
? board.imu_yaw_continuous.value : 0.0f;
NavScript_Update(&obs, &corridor_state, imu_yaw_cont_deg, &script_out);
/* --- Step 5: 控制律 → 计算期望 v, w --- */
/* --- Step 5: 控制律 --- */
RawCmd_t raw_cmd;
if (script_out.use_override) {
/* 脚本覆盖模式：直接用脚本的输出（入口对准/原地转向/退出等） */
raw_cmd.t_ms = now_ms;
raw_cmd.v = script_out.override_v;
raw_cmd.w = script_out.override_w;
raw_cmd.flags = 0U;
} else if (script_out.request_corridor) {
/* 走廊跟踪模式：使用走廊控制器 */
CorridorCtrl_Compute(&corridor_state, &obs, imu_wz, &raw_cmd);
} else {
/* 默认停车 */
raw_cmd.t_ms = now_ms;
raw_cmd.v = 0.0f;
raw_cmd.w = 0.0f;
raw_cmd.flags = 0U;
}
/* --- Step 6: 段状态机 → 安全仲裁 --- */
/* --- Step 6: 安全仲裁 (CORRIDOR 模式，兼容旧行为) --- */
SegFsmOutput_t fsm_out;
SegFsm_Update(&raw_cmd, &obs, &corridor_state, &fsm_out);
SegFsm_Update(&raw_cmd, &obs, &corridor_state, SAFETY_MODE_CORRIDOR, &fsm_out);
#endif
/* --- Step 7: 将安全后的指令喂给 CAN 发送层 --- */
CmdSlot_Push(fsm_out.safe_v, fsm_out.safe_w, 0U);
@@ -408,7 +443,43 @@ void AppTasks_Init(void)
SegFsm_Init(&fsm_cfg);
SegFsm_Start(); /* P0 修复: 必须显式启动安全状态机，否则 IDLE 状态直接输出零速 */
/* --- 初始化段脚本执行器 --- */
#if USE_GLOBAL_NAV
/* --- 初始化赛道地图 --- */
TrackMap_Init();
/* --- 初始化赛道级导航状态机 --- */
GlobalNavConfig_t gnav_cfg = {
.entry_v = PARAM_GNAV_ENTRY_V,
.entry_distance = PARAM_GNAV_ENTRY_DISTANCE,
.entry_timeout_ms = PARAM_GNAV_ENTRY_TIMEOUT,
.turn_omega = PARAM_GNAV_TURN_OMEGA,
.turn_tolerance_rad = PARAM_GNAV_TURN_TOLERANCE,
.turn_decel_zone_rad = PARAM_GNAV_TURN_DECEL_ZONE,
.turn_min_omega = PARAM_GNAV_TURN_MIN_OMEGA,
.turn_timeout_ms = PARAM_GNAV_TURN_TIMEOUT,
.reacquire_v = PARAM_GNAV_REACQUIRE_V,
.reacquire_conf_thresh = PARAM_GNAV_REACQUIRE_CONF,
.reacquire_width_tol = PARAM_GNAV_REACQUIRE_WIDTH_TOL,
.reacquire_confirm_ticks = PARAM_GNAV_REACQUIRE_TICKS,
.reacquire_timeout_ms = PARAM_GNAV_REACQUIRE_TIMEOUT,
.corridor_end_detect_dist = PARAM_GNAV_CORRIDOR_END_DIST,
.corridor_length_max = PARAM_GNAV_CORRIDOR_MAX_LEN,
.link_v = PARAM_GNAV_LINK_V,
.link_distance = PARAM_GNAV_LINK_DISTANCE,
.link_timeout_ms = PARAM_GNAV_LINK_TIMEOUT,
.exit_v = PARAM_GNAV_EXIT_V,
.exit_runout = PARAM_GNAV_EXIT_RUNOUT,
.exit_max_dist = PARAM_GNAV_EXIT_MAX_DIST,
.exit_timeout_ms = PARAM_GNAV_EXIT_TIMEOUT,
.dock_v = PARAM_GNAV_DOCK_V,
.dock_distance = PARAM_GNAV_DOCK_DISTANCE,
.heading_kp = PARAM_GNAV_HEADING_KP,
.corridor_width = PARAM_CORRIDOR_WIDTH,
};
GlobalNav_Init(&gnav_cfg);
#else
/* --- 初始化段脚本执行器 (单沟测试模式) --- */
NavScriptConfig_t script_cfg = {
.turn_target_angle = 3.14159265f, /* 固定：180度转向 */
.turn_omega = PARAM_SCRIPT_TURN_OMEGA, /* 调优：转向角速度 */
@@ -420,4 +491,5 @@ void AppTasks_Init(void)
.exit_v = PARAM_SCRIPT_EXIT_V, /* P1 修复：退出直线速度独立参数 */
};
NavScript_Init(&script_cfg);
#endif
}

15
App/est/corridor_filter.c
View File

@@ -106,3 +106,18 @@ void CorridorFilter_Update(const CorridorObs_t *obs, float imu_wz, float odom_vx
}
}
}
/* =========================================================
* 重置 (进入新垄沟时调用)
* ========================================================= */
void CorridorFilter_Reset(void)
{
if (!s_initialized) return;
/* 重置 EKF 内核: 状态归零, 协方差恢复初始值 */
CorridorEKF_Reset();
/* 解锁 IMU yaw 参考值, 等待在新沟中重新锁定 */
s_imu_yaw_ref_rad = 0.0f;
s_imu_yaw_ref_set = false;
}

10
App/est/corridor_filter.h
View File

@@ -39,6 +39,16 @@ extern "C" {
float dt_s, float imu_yaw_continuous_rad, bool imu_yaw_valid,
CorridorState_t *out_state);
/**
* @brief 重置滤波器状态 (进入新垄沟时必须调用)
*
* 重置内容：
* - EKF 状态向量清零 (e_y=0, e_th=0, s=0)
* - 协方差恢复到初始值
* - IMU yaw 参考值解锁，等待重新锁定
*/
void CorridorFilter_Reset(void);
#ifdef __cplusplus
}
#endif

596
App/nav/global_nav.c Normal file
View File

@@ -0,0 +1,596 @@
/**
* @file global_nav.c
* @brief 赛道级总控 — 混合导航状态机实现
*
* 实现 6 条垄沟 S 型遍历:
* IDLE → ENTRY → TURN_IN → REACQUIRE → CORRIDOR_TRACK →
* TURN_OUT → LINK → TURN_IN_NEXT → REACQUIRE → ...
* → EXIT → DOCK → FINISHED
*/
#include "global_nav.h"
#include "est/corridor_filter.h"
#include "preproc/corridor_preproc.h"
#include "robot_params.h"
#include <math.h>
#include <string.h>
/* =========================================================
* 内部状态
* ========================================================= */
static struct {
GlobalNavStage_t stage;
bool running;
bool initialized;
/* 赛道级 */
uint8_t current_corridor_id;
uint8_t corridors_completed;
/* 转向 */
float turn_start_yaw_deg; /* IMU yaw_continuous at turn start */
float turn_target_delta_deg; /* 90° */
int8_t turn_sign; /* +1 (CCW) or -1 (CW) */
/* 里程 (用里程计积分距离) */
float stage_entry_odom_vx_accum; /* 进入阶段时的里程计累计距离 */
float odom_distance_accum; /* 运行中持续积分的里程 */
/* 超时 */
uint32_t stage_start_ms;
/* 重捕获 */
uint8_t reacquire_ok_count;
/* 出场 */
bool exit_vl53_lost;
float exit_lost_distance;
/* 航向保持 */
float heading_ref_deg;
/* EKF 进度保存 */
float corridor_entry_s;
/* 配置 */
GlobalNavConfig_t cfg;
} s_nav;
/* 上一个周期的里程计速度，用于积分 */
static float s_last_odom_vx = 0.0f;
static uint32_t s_last_update_ms = 0;
/* =========================================================
* 辅助函数
* ========================================================= */
static inline float gnav_clampf(float val, float lo, float hi)
{
if (val < lo) return lo;
if (val > hi) return hi;
return val;
}
static inline float gnav_fabsf(float x)
{
return x < 0.0f ? -x : x;
}
/** 简单 P 控制航向保持，输入偏差 (deg)，输出角速度 (rad/s) */
static float heading_hold_pd(float current_yaw_deg, float ref_yaw_deg, float kp)
{
float err_deg = ref_yaw_deg - current_yaw_deg;
float w = kp * err_deg; /* kp 把 ° 映射到 rad/s */
return gnav_clampf(w, -1.0f, 1.0f);
}
/** 检查侧向 VL53 是否探到壁 (至少有一侧的前后都有效) */
static bool side_walls_detected(const CorridorObs_t* obs)
{
bool left_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_LF) &&
(obs->valid_mask & CORRIDOR_OBS_MASK_LR);
bool right_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) &&
(obs->valid_mask & CORRIDOR_OBS_MASK_RR);
return left_ok || right_ok;
}
/** 检查侧向 VL53 是否全丢 */
static bool all_side_lost(const CorridorObs_t* obs)
{
uint8_t side_mask = CORRIDOR_OBS_MASK_LF | CORRIDOR_OBS_MASK_LR |
CORRIDOR_OBS_MASK_RF | CORRIDOR_OBS_MASK_RR;
return (obs->valid_mask & side_mask) == 0U;
}
/** 检查重捕获条件 */
static bool check_reacquire(const CorridorObs_t* obs, const CorridorState_t* state)
{
/* 条件 1: 至少 3 个侧向传感器有效 */
uint8_t side_mask = CORRIDOR_OBS_MASK_LF | CORRIDOR_OBS_MASK_LR |
CORRIDOR_OBS_MASK_RF | CORRIDOR_OBS_MASK_RR;
uint8_t active = obs->valid_mask & side_mask;
int count = 0;
for (int i = 0; i < 4; i++) {
if (active & (1U << i)) count++;
}
if (count < 3) return false;
/* 条件 2: 左右距离和 ≈ 走廊宽度 */
bool left_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_LF) &&
(obs->valid_mask & CORRIDOR_OBS_MASK_LR);
bool right_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) &&
(obs->valid_mask & CORRIDOR_OBS_MASK_RR);
if (left_ok && right_ok) {
float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
float total_width = d_left + d_right + PARAM_ROBOT_WIDTH;
float err = gnav_fabsf(total_width - s_nav.cfg.corridor_width);
if (err > s_nav.cfg.reacquire_width_tol) return false;
}
/* 条件 3: EKF 置信度 */
if (state->conf < s_nav.cfg.reacquire_conf_thresh) return false;
return true;
}
/** 获取当前阶段的已行驶里程 */
static float odom_since_entry(void)
{
return s_nav.odom_distance_accum - s_nav.stage_entry_odom_vx_accum;
}
/** 阶段转移 — 通用初始化 */
static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board);
/* =========================================================
* 阶段名称表
* ========================================================= */
static const char* const s_stage_names[] = {
"IDLE",
"ENTRY_STRAIGHT",
"TURN_INTO_CORRIDOR",
"REACQUIRE",
"CORRIDOR_TRACK",
"TURN_OUT",
"LINK_STRAIGHT",
"TURN_INTO_NEXT",
"EXIT_STRAIGHT",
"DOCK",
"FINISHED",
"ERROR"
};
/* =========================================================
* 转向执行 (统一的 90° 转向逻辑)
* ========================================================= */
static void execute_turn(const CorridorObs_t* obs,
const CorridorState_t* state,
const RobotBlackboard_t* board,
uint32_t now_ms,
GlobalNavOutput_t* out)
{
float imu_yaw = board->imu_yaw_continuous.is_valid
? board->imu_yaw_continuous.value : 0.0f;
/* 已转过的角度 (取绝对值) */
float delta = (imu_yaw - s_nav.turn_start_yaw_deg) * s_nav.turn_sign;
float target = s_nav.turn_target_delta_deg;
float remaining_deg = target - delta;
float omega = s_nav.cfg.turn_omega;
/* 接近目标时减速 */
float decel_zone_deg = PARAM_RAD2DEG(s_nav.cfg.turn_decel_zone_rad);
if (remaining_deg < decel_zone_deg && decel_zone_deg > 0.01f) {
float ratio = remaining_deg / decel_zone_deg;
if (ratio < 0.0f) ratio = 0.0f;
omega = s_nav.cfg.turn_min_omega +
ratio * (s_nav.cfg.turn_omega - s_nav.cfg.turn_min_omega);
}
out->override_v = 0.0f;
out->override_w = (float)s_nav.turn_sign * omega;
out->use_override = true;
out->request_corridor = false;
out->safety_mode = SAFETY_MODE_TURN;
/* 转向完成判定 */
float tolerance_deg = PARAM_RAD2DEG(s_nav.cfg.turn_tolerance_rad);
if (delta >= target - tolerance_deg) {
switch (s_nav.stage) {
case GNAV_TURN_INTO_CORRIDOR:
case GNAV_TURN_INTO_NEXT:
transition_to(GNAV_REACQUIRE, board);
break;
case GNAV_TURN_OUT_OF_CORRIDOR:
if (TrackMap_IsLastCorridor(s_nav.current_corridor_id)) {
transition_to(GNAV_EXIT_STRAIGHT, board);
} else {
transition_to(GNAV_LINK_STRAIGHT, board);
}
break;
default:
break;
}
return;
}
/* 超时保护 */
if (now_ms - s_nav.stage_start_ms > s_nav.cfg.turn_timeout_ms) {
transition_to(GNAV_ERROR, board);
}
}
/* =========================================================
* 阶段转移
* ========================================================= */
static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board)
{
float imu_yaw = (board != NULL && board->imu_yaw_continuous.is_valid)
? board->imu_yaw_continuous.value : 0.0f;
/* 通用: 记录进入时间和里程 */
s_nav.stage_start_ms = s_last_update_ms;
s_nav.stage_entry_odom_vx_accum = s_nav.odom_distance_accum;
s_nav.reacquire_ok_count = 0;
switch (next) {
case GNAV_ENTRY_STRAIGHT:
s_nav.heading_ref_deg = imu_yaw;
s_nav.current_corridor_id = TrackMap_Get()->entry_corridor_id;
break;
case GNAV_TURN_INTO_CORRIDOR: {
const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
s_nav.turn_start_yaw_deg = imu_yaw;
s_nav.turn_target_delta_deg = 90.0f;
break;
}
case GNAV_TURN_OUT_OF_CORRIDOR: {
const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
s_nav.turn_sign = (int8_t)cd->exit_turn_dir;
s_nav.turn_start_yaw_deg = imu_yaw;
s_nav.turn_target_delta_deg = 90.0f;
break;
}
case GNAV_TURN_INTO_NEXT: {
uint8_t next_id = TrackMap_GetNextCorridorId(s_nav.current_corridor_id);
const CorridorDescriptor_t* cd = TrackMap_GetCorridor(next_id);
s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
s_nav.turn_start_yaw_deg = imu_yaw;
s_nav.turn_target_delta_deg = 90.0f;
s_nav.current_corridor_id = next_id;
break;
}
case GNAV_REACQUIRE:
/* EKF 重置: 新沟的 e_y 参考不同，必须重建 */
CorridorFilter_Reset();
s_nav.heading_ref_deg = imu_yaw;
break;
case GNAV_CORRIDOR_TRACK:
s_nav.corridor_entry_s = 0.0f; /* EKF 已 reset, s 从 0 开始 */
break;
case GNAV_LINK_STRAIGHT:
s_nav.heading_ref_deg = imu_yaw;
break;
case GNAV_EXIT_STRAIGHT:
s_nav.heading_ref_deg = imu_yaw;
s_nav.exit_vl53_lost = false;
s_nav.exit_lost_distance = 0.0f;
break;
case GNAV_DOCK:
break;
case GNAV_FINISHED:
s_nav.running = false;
break;
case GNAV_ERROR:
break;
default:
break;
}
s_nav.stage = next;
}
/* =========================================================
* 公开 API
* ========================================================= */
void GlobalNav_Init(const GlobalNavConfig_t* cfg)
{
memset(&s_nav, 0, sizeof(s_nav));
s_nav.cfg = *cfg;
s_nav.stage = GNAV_IDLE;
s_nav.running = false;
s_nav.initialized = true;
s_last_odom_vx = 0.0f;
s_last_update_ms = 0;
}
void GlobalNav_Start(void)
{
if (!s_nav.initialized) return;
s_nav.running = true;
s_nav.corridors_completed = 0;
s_nav.odom_distance_accum = 0.0f;
s_nav.stage_entry_odom_vx_accum = 0.0f;
s_nav.current_corridor_id = TrackMap_Get()->entry_corridor_id;
/* 不在这里 transition_to，因为还没有 board 数据。
下一个 Update 周期里发现 running && IDLE 时再 transition。 */
s_nav.stage = GNAV_IDLE;
}
void GlobalNav_Stop(void)
{
s_nav.running = false;
s_nav.stage = GNAV_FINISHED;
}
void GlobalNav_Reset(void)
{
s_nav.stage = GNAV_IDLE;
s_nav.running = false;
s_nav.corridors_completed = 0;
s_nav.odom_distance_accum = 0.0f;
}
GlobalNavStage_t GlobalNav_GetStage(void)
{
return s_nav.stage;
}
const char* GlobalNav_GetStageName(GlobalNavStage_t stage)
{
if (stage <= GNAV_ERROR) {
return s_stage_names[stage];
}
return "UNKNOWN";
}
/* =========================================================
* 核心 Update
* ========================================================= */
void GlobalNav_Update(const CorridorObs_t* obs,
const CorridorState_t* state,
const RobotBlackboard_t* board,
GlobalNavOutput_t* out)
{
if (!s_nav.initialized) {
memset(out, 0, sizeof(*out));
return;
}
uint32_t now_ms = s_last_update_ms;
/* 用 HAL_GetTick 代理 — 实际上 app_tasks 会传入 board 里的时间 */
{
/* 里程计积分: Δd = vx * dt */
float odom_vx = board->odom_vx;
if (s_last_update_ms > 0) {
/* 使用 board 中的时间戳估计当前时间 */
uint32_t cur_ms = board->imu_wz.timestamp_ms;
if (cur_ms == 0) cur_ms = s_last_update_ms + 20U;
float dt = (float)(cur_ms - s_last_update_ms) * 0.001f;
if (dt > 0.0f && dt < 0.5f) {
s_nav.odom_distance_accum += gnav_fabsf(odom_vx) * dt;
}
now_ms = cur_ms;
} else {
now_ms = board->imu_wz.timestamp_ms;
if (now_ms == 0) now_ms = 1;
}
s_last_update_ms = now_ms;
s_last_odom_vx = odom_vx;
}
float imu_yaw_deg = board->imu_yaw_continuous.is_valid
? board->imu_yaw_continuous.value : 0.0f;
/* 默认输出 */
out->use_override = true;
out->request_corridor = false;
out->override_v = 0.0f;
out->override_w = 0.0f;
out->safety_mode = SAFETY_MODE_IDLE;
out->stage = s_nav.stage;
out->corridor_id = s_nav.current_corridor_id;
out->corridors_done = s_nav.corridors_completed;
out->active = s_nav.running;
out->stage_name = GlobalNav_GetStageName(s_nav.stage);
if (!s_nav.running) return;
/* IDLE → 自动进入 ENTRY_STRAIGHT */
if (s_nav.stage == GNAV_IDLE) {
transition_to(GNAV_ENTRY_STRAIGHT, board);
out->stage = s_nav.stage;
out->stage_name = GlobalNav_GetStageName(s_nav.stage);
}
uint32_t elapsed_ms = now_ms - s_nav.stage_start_ms;
switch (s_nav.stage) {
/* ============================================================
* 入场直线
* ============================================================ */
case GNAV_ENTRY_STRAIGHT:
out->override_v = s_nav.cfg.entry_v;
out->override_w = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
s_nav.cfg.heading_kp);
out->safety_mode = SAFETY_MODE_STRAIGHT;
if (side_walls_detected(obs) ||
odom_since_entry() >= s_nav.cfg.entry_distance ||
elapsed_ms > s_nav.cfg.entry_timeout_ms)
{
transition_to(GNAV_TURN_INTO_CORRIDOR, board);
}
break;
/* ============================================================
* 三种转向状态统一处理
* ============================================================ */
case GNAV_TURN_INTO_CORRIDOR:
case GNAV_TURN_OUT_OF_CORRIDOR:
case GNAV_TURN_INTO_NEXT:
execute_turn(obs, state, board, now_ms, out);
break;
/* ============================================================
* 重捕获走廊
* ============================================================ */
case GNAV_REACQUIRE:
out->override_v = s_nav.cfg.reacquire_v;
out->override_w = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
s_nav.cfg.heading_kp);
out->safety_mode = SAFETY_MODE_STRAIGHT;
if (check_reacquire(obs, state)) {
s_nav.reacquire_ok_count++;
} else {
s_nav.reacquire_ok_count = 0;
}
if (s_nav.reacquire_ok_count >= s_nav.cfg.reacquire_confirm_ticks) {
transition_to(GNAV_CORRIDOR_TRACK, board);
}
if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms) {
transition_to(GNAV_ERROR, board);
}
break;
/* ============================================================
* 沟内闭环跟踪 (交给 corridor_ctrl)
* ============================================================ */
case GNAV_CORRIDOR_TRACK:
out->use_override = false;
out->request_corridor = true;
out->safety_mode = SAFETY_MODE_CORRIDOR;
/* 到端检测 */
{
bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
if (front_valid && obs->d_front <= s_nav.cfg.corridor_end_detect_dist) {
/* 里程下限保护: 至少走了 1.0m 才允许认定到端，避免假阳性 */
float corridor_odom = odom_since_entry();
if (corridor_odom > 1.0f) {
s_nav.corridors_completed++;
transition_to(GNAV_TURN_OUT_OF_CORRIDOR, board);
}
}
}
/* 里程超长保护 */
if (odom_since_entry() > s_nav.cfg.corridor_length_max) {
s_nav.corridors_completed++;
transition_to(GNAV_TURN_OUT_OF_CORRIDOR, board);
}
break;
/* ============================================================
* 连接段直行
* ============================================================ */
case GNAV_LINK_STRAIGHT:
out->override_v = s_nav.cfg.link_v;
out->override_w = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
s_nav.cfg.heading_kp);
out->safety_mode = SAFETY_MODE_STRAIGHT;
if (odom_since_entry() >= s_nav.cfg.link_distance ||
side_walls_detected(obs))
{
transition_to(GNAV_TURN_INTO_NEXT, board);
}
if (elapsed_ms > s_nav.cfg.link_timeout_ms) {
transition_to(GNAV_ERROR, board);
}
break;
/* ============================================================
* 出场直行
* ============================================================ */
case GNAV_EXIT_STRAIGHT:
out->override_v = s_nav.cfg.exit_v;
out->override_w = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
s_nav.cfg.heading_kp);
out->safety_mode = SAFETY_MODE_STRAIGHT;
/* 检测侧向全丢 */
if (!s_nav.exit_vl53_lost && all_side_lost(obs)) {
s_nav.exit_vl53_lost = true;
s_nav.exit_lost_distance = s_nav.odom_distance_accum;
}
if (s_nav.exit_vl53_lost) {
float since_lost = s_nav.odom_distance_accum - s_nav.exit_lost_distance;
if (since_lost >= s_nav.cfg.exit_runout) {
transition_to(GNAV_DOCK, board);
}
}
/* 里程上限保护 */
if (odom_since_entry() >= s_nav.cfg.exit_max_dist) {
transition_to(GNAV_DOCK, board);
}
if (elapsed_ms > s_nav.cfg.exit_timeout_ms) {
transition_to(GNAV_DOCK, board);
}
break;
/* ============================================================
* 回停启动区
* ============================================================ */
case GNAV_DOCK:
out->override_v = s_nav.cfg.dock_v;
out->override_w = 0.0f;
out->safety_mode = SAFETY_MODE_STRAIGHT;
if (odom_since_entry() >= s_nav.cfg.dock_distance ||
elapsed_ms > 5000U)
{
transition_to(GNAV_FINISHED, board);
}
break;
/* ============================================================
* 终态
* ============================================================ */
case GNAV_FINISHED:
out->override_v = 0.0f;
out->override_w = 0.0f;
out->safety_mode = SAFETY_MODE_IDLE;
out->active = false;
break;
/* ============================================================
* 异常态
* ============================================================ */
case GNAV_ERROR:
out->override_v = 0.0f;
out->override_w = 0.0f;
out->safety_mode = SAFETY_MODE_IDLE;
if (elapsed_ms > 2000U) {
transition_to(GNAV_FINISHED, board);
}
break;
default:
out->override_v = 0.0f;
out->override_w = 0.0f;
break;
}
/* 更新输出阶段 (可能在 switch 内已经 transition) */
out->stage = s_nav.stage;
out->stage_name = GlobalNav_GetStageName(s_nav.stage);
out->corridor_id = s_nav.current_corridor_id;
out->corridors_done = s_nav.corridors_completed;
out->active = s_nav.running;
}

138
App/nav/global_nav.h Normal file
View File

@@ -0,0 +1,138 @@
/**
* @file global_nav.h
* @brief 赛道级总控 — 混合导航状态机
*
* 三层架构中的最上层：
* 上层 (本模块): 知道"当前第几沟、下一沟是谁、该往哪转"
* 中层 (内嵌): 转向/连接段/重捕获动作执行
* 下层 (已有): 沟内 corridor_ctrl 闭环
*
* 替代原 nav_script.c 的赛道编排职能。
*/
#ifndef GLOBAL_NAV_H
#define GLOBAL_NAV_H
#include <stdint.h>
#include <stdbool.h>
#include "preproc/corridor_msgs.h"
#include "Contract/robot_blackboard.h"
#include "nav/track_map.h"
#ifdef __cplusplus
extern "C" {
#endif
/* =========================================================
* 赛道级阶段枚举
* ========================================================= */
typedef enum {
GNAV_IDLE = 0, /* 启动区等待 */
GNAV_ENTRY_STRAIGHT, /* 入场直线 */
GNAV_TURN_INTO_CORRIDOR, /* 转向进入垄沟 (原地转 90°) */
GNAV_REACQUIRE, /* 重捕获走廊 */
GNAV_CORRIDOR_TRACK, /* 沟内闭环跟踪 */
GNAV_TURN_OUT_OF_CORRIDOR, /* 到端后出沟转向 (原地转 90°) */
GNAV_LINK_STRAIGHT, /* 连接段直行 */
GNAV_TURN_INTO_NEXT, /* 转向进入下一条沟 (原地转 90°) */
GNAV_EXIT_STRAIGHT, /* 出场直行 */
GNAV_DOCK, /* 回停启动区 */
GNAV_FINISHED, /* 终态 */
GNAV_ERROR /* 异常态 (超时兜底) */
} GlobalNavStage_t;
/* =========================================================
* 配置参数
* ========================================================= */
typedef struct {
/* 入场段 */
float entry_v; /* 入场速度 m/s */
float entry_distance; /* 入场里程上限 m */
uint32_t entry_timeout_ms; /* 入场超时 ms */
/* 转向 */
float turn_omega; /* 转向角速度 rad/s */
float turn_tolerance_rad; /* 转向完成容差 rad */
float turn_decel_zone_rad; /* 接近目标角时的减速区 rad */
float turn_min_omega; /* 减速区最低角速度 rad/s */
uint32_t turn_timeout_ms; /* 单次转向超时 ms */
/* 重捕获 */
float reacquire_v; /* 重捕获速度 m/s */
float reacquire_conf_thresh; /* 重捕获置信度阈值 */
float reacquire_width_tol; /* 走廊宽度容差 m */
uint8_t reacquire_confirm_ticks; /* 连续确认拍数 */
uint32_t reacquire_timeout_ms;
/* 沟内 */
float corridor_end_detect_dist; /* 到端检测距离 m */
float corridor_length_max; /* 沟内里程保护上限 m */
/* 连接段 */
float link_v; /* 连接段速度 m/s */
float link_distance; /* 连接段标称距离 m */
uint32_t link_timeout_ms;
/* 出场 */
float exit_v; /* 出场速度 m/s */
float exit_runout; /* 出场冲刺距离 m */
float exit_max_dist; /* 出场里程保护 m */
uint32_t exit_timeout_ms;
/* 回停 */
float dock_v; /* 回停速度 m/s */
float dock_distance; /* 回停距离 m */
/* 航向保持 */
float heading_kp; /* 航向保持P增益 */
/* 走廊宽度 (用于重捕获判定) */
float corridor_width; /* 走廊宽度 m */
} GlobalNavConfig_t;
/* =========================================================
* 输出结构
* ========================================================= */
typedef struct {
GlobalNavStage_t stage; /* 当前阶段 */
const char* stage_name; /* 阶段名字符串 (调试) */
uint8_t corridor_id; /* 当前/目标垄沟 ID */
uint8_t corridors_done; /* 已完成垄沟数 */
bool request_corridor; /* 是否请求沟内闭环 */
bool use_override; /* 是否用 override 指令 */
float override_v;
float override_w;
SafetyMode_t safety_mode; /* 当前安全模式 */
bool active; /* 导航是否在运行 */
} GlobalNavOutput_t;
/* =========================================================
* API
* ========================================================= */
void GlobalNav_Init(const GlobalNavConfig_t* cfg);
void GlobalNav_Start(void);
void GlobalNav_Stop(void);
void GlobalNav_Reset(void);
/**
* @brief 核心函数：每个导航周期调用一次
* @param obs 预处理层的观测快照
* @param state EKF 走廊状态
* @param board 黑板快照 (读 IMU yaw_continuous, odom)
* @param out 导航输出
*/
void GlobalNav_Update(const CorridorObs_t* obs,
const CorridorState_t* state,
const RobotBlackboard_t* board,
GlobalNavOutput_t* out);
GlobalNavStage_t GlobalNav_GetStage(void);
const char* GlobalNav_GetStageName(GlobalNavStage_t stage);
#ifdef __cplusplus
}
#endif
#endif /* GLOBAL_NAV_H */

65
App/nav/segment_fsm.c
View File

@@ -48,6 +48,7 @@ const char* SegFsm_GetStateName(SegFsmState_t s)
void SegFsm_Update(const RawCmd_t *raw_cmd,
const CorridorObs_t *obs,
const CorridorState_t *state,
SafetyMode_t mode,
SegFsmOutput_t *out)
{
if (!s_initialized) {
@@ -58,18 +59,9 @@ void SegFsm_Update(const RawCmd_t *raw_cmd,
}
/* ========================================================
* 1. 全局安全断言：无论在什么状态，满足条件直接超驰
* 0. IDLE 模式：零速，不做任何裁剪
* ======================================================== */
/* 1a. 置信度极低 → 紧急停车 */
if (state->conf < s_cfg.conf_estop_thresh) {
s_state = SEG_STATE_ESTOP;
}
/* ========================================================
* 2. 待命状态：什么都不做
* ======================================================== */
if (s_state == SEG_STATE_IDLE) {
if (mode == SAFETY_MODE_IDLE) {
out->state = SEG_STATE_IDLE;
out->safe_v = 0.0f;
out->safe_w = 0.0f;
@@ -77,14 +69,43 @@ void SegFsm_Update(const RawCmd_t *raw_cmd,
}
/* ========================================================
* 3. 紧急停车状态：等待置信度恢复
* 1. TURN 模式：原地转向，直接放行，不检查前距和置信度
* 解决 RISK-1: 转向阶段不会被安全层卡死
* ======================================================== */
if (mode == SAFETY_MODE_TURN) {
s_state = SEG_STATE_CORRIDOR;
out->state = SEG_STATE_CORRIDOR;
out->safe_v = raw_cmd->v;
out->safe_w = raw_cmd->w;
return;
}
/* ========================================================
* 2. CORRIDOR 模式：完整安全检查 (前向 + 置信度)
* STRAIGHT 模式：前向检查，但不检查置信度
* ======================================================== */
/* 2a. 置信度检查：仅 CORRIDOR 模式下生效 */
if (mode == SAFETY_MODE_CORRIDOR) {
if (state->conf < s_cfg.conf_estop_thresh) {
s_state = SEG_STATE_ESTOP;
}
}
/* 待命状态 */
if (s_state == SEG_STATE_IDLE) {
out->state = SEG_STATE_IDLE;
out->safe_v = 0.0f;
out->safe_w = 0.0f;
return;
}
/* 紧急停车状态：等待置信度恢复 (仅 CORRIDOR 模式可能进入) */
if (s_state == SEG_STATE_ESTOP) {
out->state = SEG_STATE_ESTOP;
out->safe_v = 0.0f;
out->safe_w = 0.0f;
/* 自动恢复开关：当置信度重新回到安全区且前方有路，恢复巡航 */
if (state->conf >= 0.5f) {
s_state = SEG_STATE_CORRIDOR;
}
@@ -92,45 +113,34 @@ void SegFsm_Update(const RawCmd_t *raw_cmd,
}
/* ========================================================
* 4. 前向距离判定：决定 CORRIDOR / APPROACH / STOP
* 3. 前向距离判定：CORRIDOR 和 STRAIGHT 都做
* ======================================================== */
bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
float d_front = obs->d_front;
if (front_valid) {
if (d_front <= s_cfg.d_front_stop) {
/* 距离过近：到端停车 */
s_state = SEG_STATE_STOP;
} else if (d_front <= s_cfg.d_front_approach) {
/* 进入减速预警区 */
s_state = SEG_STATE_APPROACH;
} else {
/* 前方安全：正常走廊跟踪 */
if (s_state == SEG_STATE_STOP || s_state == SEG_STATE_APPROACH) {
s_state = SEG_STATE_CORRIDOR;
}
}
} else {
/* 前向雷达失效：保守策略，保持当前状态但降速
* (不往 STOP 跳，避免前雷达偶发掉线就急停) */
}
/* ========================================================
* 5. 根据当前状态输出最终安全速度
* 4. 根据当前状态输出最终安全速度
* ======================================================== */
switch (s_state) {
case SEG_STATE_CORRIDOR: {
/* 正常放行控制器输出 */
out->safe_v = raw_cmd->v;
out->safe_w = raw_cmd->w;
break;
}
case SEG_STATE_APPROACH: {
/* 线性插值减速:
* 当 d_front 从 approach 距离滑向 stop 距离时，
* 速度从 raw_cmd->v 线性衰减至 approach_min_v
*/
float range = s_cfg.d_front_approach - s_cfg.d_front_stop;
float ratio = 1.0f;
if (range > 0.001f) {
@@ -139,13 +149,12 @@ void SegFsm_Update(const RawCmd_t *raw_cmd,
}
float v_scaled = s_cfg.approach_min_v + ratio * (raw_cmd->v - s_cfg.approach_min_v);
out->safe_v = seg_clampf(v_scaled, 0.0f, raw_cmd->v);
out->safe_w = raw_cmd->w; // 角速度不限，仍然允许纠偏
out->safe_w = raw_cmd->w;
break;
}
case SEG_STATE_STOP:
default: {
/* 到端停车 / 紧急停车：零速 */
out->safe_v = 0.0f;
out->safe_w = 0.0f;
break;

2
App/nav/segment_fsm.h
View File

@@ -54,11 +54,13 @@ void SegFsm_Start(void);
* @param raw_cmd 走廊控制器的原始输出 (v, w)
* @param obs 预处理层的观测快照 (提供 d_front 和 valid_mask)
* @param state 滤波器的状态输出 (提供 conf 置信度)
* @param mode 安全模式 (CORRIDOR/TURN/STRAIGHT/IDLE)，由赛道级导航指定
* @param out 安全仲裁后的最终输出
*/
void SegFsm_Update(const RawCmd_t *raw_cmd,
const CorridorObs_t *obs,
const CorridorState_t *state,
SafetyMode_t mode,
SegFsmOutput_t *out);
/**

78
App/nav/track_map.c Normal file
View File

@@ -0,0 +1,78 @@
/**
* @file track_map.c
* @brief 赛道地图 — S 型遍历拓扑表
*
* S 型路径:
* C1(→) → 左转 → 连接 → 左转 → C2(←) → 右转 → 连接 → 右转 →
* C3(→) → 左转 → 连接 → 左转 → C4(←) → 右转 → 连接 → 右转 →
* C5(→) → 左转 → 连接 → 左转 → C6(←) → 左转 → 出场
*/
#include "track_map.h"
/* =========================================================
* 硬编码 S 型遍历表
* ========================================================= */
static const TrackMap_t s_map = {
.corridors = {
/* id travel_dir exit_turn entry_turn is_last
* -- ---------- --------- ---------- ------- */
{ 0, TRAVEL_DIR_POSITIVE, TURN_DIR_LEFT, TURN_DIR_LEFT, false }, /* C1: → 到右端后左转 */
{ 1, TRAVEL_DIR_NEGATIVE, TURN_DIR_RIGHT, TURN_DIR_RIGHT, false }, /* C2: ← 到左端后右转 */
{ 2, TRAVEL_DIR_POSITIVE, TURN_DIR_LEFT, TURN_DIR_LEFT, false }, /* C3: → */
{ 3, TRAVEL_DIR_NEGATIVE, TURN_DIR_RIGHT, TURN_DIR_RIGHT, false }, /* C4: ← */
{ 4, TRAVEL_DIR_POSITIVE, TURN_DIR_LEFT, TURN_DIR_LEFT, false }, /* C5: → */
{ 5, TRAVEL_DIR_NEGATIVE, TURN_DIR_LEFT, TURN_DIR_LEFT, true }, /* C6: ← 最后一条, 左转出场 */
},
.entry_corridor_id = 0,
.link_distance_m = TRACK_MAP_LINK_DISTANCE_M,
.corridor_length_m = TRACK_MAP_CORRIDOR_LENGTH_M,
};
/* =========================================================
* API 实现
* ========================================================= */
void TrackMap_Init(void)
{
/* 静态表，无需运行时初始化 */
}
const TrackMap_t* TrackMap_Get(void)
{
return &s_map;
}
const CorridorDescriptor_t* TrackMap_GetCorridor(uint8_t id)
{
if (id >= TRACK_MAP_CORRIDOR_COUNT) {
return &s_map.corridors[0];
}
return &s_map.corridors[id];
}
uint8_t TrackMap_GetNextCorridorId(uint8_t current_id)
{
if (current_id + 1 >= TRACK_MAP_CORRIDOR_COUNT) {
return current_id; /* 已是最后一条 */
}
return current_id + 1;
}
bool TrackMap_IsLastCorridor(uint8_t id)
{
if (id >= TRACK_MAP_CORRIDOR_COUNT) return true;
return s_map.corridors[id].is_last;
}
TurnDirection_t TrackMap_GetExitTurnDir(uint8_t id)
{
if (id >= TRACK_MAP_CORRIDOR_COUNT) return TURN_DIR_LEFT;
return s_map.corridors[id].exit_turn_dir;
}
TurnDirection_t TrackMap_GetEntryTurnDir(uint8_t id)
{
if (id >= TRACK_MAP_CORRIDOR_COUNT) return TURN_DIR_LEFT;
return s_map.corridors[id].entry_turn_dir;
}

80
App/nav/track_map.h Normal file
View File

@@ -0,0 +1,80 @@
/**
* @file track_map.h
* @brief 赛道地图 — 固化 S 型遍历拓扑
*
* 地图不做全局坐标定位，只回答三个问题：
* 1. 从第 N 条沟完成后，下一条是第几条？
* 2. 这次该往哪转？(左/右)
* 3. 当前是不是最后一条沟？
*/
#ifndef TRACK_MAP_H
#define TRACK_MAP_H
#include <stdint.h>
#include <stdbool.h>
#ifdef __cplusplus
extern "C" {
#endif
/* =========================================================
* 常量
* ========================================================= */
#define TRACK_MAP_CORRIDOR_COUNT 6 /* 总共 6 条垄沟 */
#define TRACK_MAP_LINK_DISTANCE_M 0.70f /* 沟间距 (中心到中心) */
#define TRACK_MAP_CORRIDOR_LENGTH_M 2.20f /* 垄沟标称长度 */
/* =========================================================
* 枚举
* ========================================================= */
/** 沟内行驶方向 */
typedef enum {
TRAVEL_DIR_POSITIVE = 0, /* 从左端到右端 (→) */
TRAVEL_DIR_NEGATIVE = 1 /* 从右端到左端 (←) */
} TravelDirection_t;
/** 转向方向 */
typedef enum {
TURN_DIR_LEFT = +1, /* 逆时针 (CCW), w > 0 */
TURN_DIR_RIGHT = -1 /* 顺时针 (CW), w < 0 */
} TurnDirection_t;
/* =========================================================
* 数据结构
* ========================================================= */
/** 单条垄沟描述 */
typedef struct {
uint8_t id; /* 0-5 */
TravelDirection_t travel_dir; /* 本沟行驶方向 */
TurnDirection_t exit_turn_dir; /* 出沟时的转向方向 */
TurnDirection_t entry_turn_dir; /* 入沟时的转向方向 */
bool is_last; /* 是否为最后一条沟 */
} CorridorDescriptor_t;
/** 完整赛道地图 */
typedef struct {
CorridorDescriptor_t corridors[TRACK_MAP_CORRIDOR_COUNT];
uint8_t entry_corridor_id; /* 入场后第一条沟 = 0 */
float link_distance_m; /* 连接段标称距离 */
float corridor_length_m; /* 垄沟标称长度 */
} TrackMap_t;
/* =========================================================
* API
* ========================================================= */
void TrackMap_Init(void);
const TrackMap_t* TrackMap_Get(void);
const CorridorDescriptor_t* TrackMap_GetCorridor(uint8_t id);
uint8_t TrackMap_GetNextCorridorId(uint8_t current_id);
bool TrackMap_IsLastCorridor(uint8_t id);
TurnDirection_t TrackMap_GetExitTurnDir(uint8_t id);
TurnDirection_t TrackMap_GetEntryTurnDir(uint8_t id);
#ifdef __cplusplus
}
#endif
#endif /* TRACK_MAP_H */

10
App/preproc/corridor_msgs.h
View File

@@ -55,4 +55,14 @@ typedef struct {
uint8_t flags; // 控制特殊标志位
} RawCmd_t;
/* =========================================================
* 安全模式枚举 (赛道级导航 → 安全状态机 的模式指示)
* ========================================================= */
typedef enum {
SAFETY_MODE_IDLE = 0, // 零速，不做任何裁剪
SAFETY_MODE_CORRIDOR, // 沟内: 前向减速/停车/E-STOP 全开
SAFETY_MODE_TURN, // 转向: 允许 v=0+w!=0, 前向不全停, 不检查 conf
SAFETY_MODE_STRAIGHT // 直行段: 前后激光防撞, 不检查 conf
} SafetyMode_t;
#endif // CORRIDOR_MSGS_H

48
App/robot_params.h
View File

@@ -371,6 +371,54 @@ extern "C" {
*/
#define PARAM_IMU_YAW_OFFSET 0.0f
/* =========================================================
* 【P6】赛道级导航参数 (混合导航方案)
* ========================================================= */
/** @brief 编译开关：1=赛道模式(6沟S型遍历) 0=单沟测试模式(原nav_script) */
#define USE_GLOBAL_NAV 1
/* --- 入场段 --- */
#define PARAM_GNAV_ENTRY_V 0.08f /* m/s — 入场速度 */
#define PARAM_GNAV_ENTRY_DISTANCE 0.65f /* m — 入场里程上限 */
#define PARAM_GNAV_ENTRY_TIMEOUT 10000U /* ms — 入场超时 */
/* --- 转向 --- */
#define PARAM_GNAV_TURN_OMEGA 1.0f /* rad/s — 转向角速度 */
#define PARAM_GNAV_TURN_TOLERANCE 0.087f /* rad — 转向完成容差 (~5°) */
#define PARAM_GNAV_TURN_DECEL_ZONE 0.5f /* rad — 接近目标时减速区 (~28°) */
#define PARAM_GNAV_TURN_MIN_OMEGA 0.3f /* rad/s — 减速区最低角速度 */
#define PARAM_GNAV_TURN_TIMEOUT 8000U /* ms — 单次转向超时 */
/* --- 重捕获 --- */
#define PARAM_GNAV_REACQUIRE_V 0.05f /* m/s — 重捕获入沟速度 */
#define PARAM_GNAV_REACQUIRE_CONF 0.6f /* — 重捕获置信度阈值 */
#define PARAM_GNAV_REACQUIRE_WIDTH_TOL 0.05f /* m — 走廊宽度容差 */
#define PARAM_GNAV_REACQUIRE_TICKS 5 /* 拍 — 连续确认次数 (5×20ms=100ms) */
#define PARAM_GNAV_REACQUIRE_TIMEOUT 5000U /* ms — 重捕获超时 */
/* --- 沟内 --- */
#define PARAM_GNAV_CORRIDOR_MAX_LEN 2.50f /* m — 沟内里程保护上限 */
#define PARAM_GNAV_CORRIDOR_END_DIST 0.10f /* m — 到端检测距离 */
/* --- 连接段 --- */
#define PARAM_GNAV_LINK_V 0.10f /* m/s — 连接段速度 */
#define PARAM_GNAV_LINK_DISTANCE 0.70f /* m — 连接段标称距离 (沟间距) */
#define PARAM_GNAV_LINK_TIMEOUT 8000U /* ms — 连接段超时 */
/* --- 出场 --- */
#define PARAM_GNAV_EXIT_V 0.15f /* m/s — 出场速度 */
#define PARAM_GNAV_EXIT_RUNOUT 1.50f /* m — 出场侧向丢失后冲刺距离 */
#define PARAM_GNAV_EXIT_MAX_DIST 4.00f /* m — 出场里程保护 */
#define PARAM_GNAV_EXIT_TIMEOUT 20000U /* ms — 出场超时 */
/* --- 回停 --- */
#define PARAM_GNAV_DOCK_V 0.05f /* m/s — 回停速度 */
#define PARAM_GNAV_DOCK_DISTANCE 0.50f /* m — 回停推进距离 */
/* --- 航向保持 --- */
#define PARAM_GNAV_HEADING_KP 0.03f /* — — 航向保持P增益 (输入°，输出rad/s) */
/* =========================================================
* 宏工具函数
* ========================================================= */