156 lines
6.3 KiB
C
156 lines
6.3 KiB
C
/**
|
|
* @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_ALIGN, /* 捕获成功后停车对齐航线 */
|
|
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 */
|
|
float reacquire_min_odom; /* 重捕获最小入沟里程 m */
|
|
uint8_t reacquire_confirm_ticks; /* 连续确认拍数 */
|
|
uint32_t reacquire_timeout_ms;
|
|
|
|
/* 对齐 */
|
|
float align_kp_th; /* 对齐航向P增益 (rad/s per rad) */
|
|
float align_kp_y; /* 对齐横向P增益 (rad/s per m) */
|
|
float align_th_tol_rad; /* 对齐航向容差 (rad) */
|
|
float align_y_tol_m; /* 对齐横向容差 (m) */
|
|
uint8_t align_confirm_ticks; /* 对齐确认拍数 */
|
|
uint32_t align_timeout_ms; /* 对齐超时 ms */
|
|
float reacquire_min_back_dist; /* 重捕获最小后激光距离 (m)，用于判断是否真正进沟 */
|
|
|
|
/* 沟内 */
|
|
float corridor_end_detect_dist; /* 到端检测距离 m */
|
|
float corridor_length_max; /* 沟内里程保护上限 m */
|
|
|
|
/* 连接段 */
|
|
float link_v;
|
|
float link_distance;
|
|
uint32_t link_timeout_ms;
|
|
float link_gap_runout; /* 检测到沟口后继续前冲距离 (m) */
|
|
float link_wall_target; /* 墙壁跟随目标距离 (m) */
|
|
float link_wall_kp; /* 墙壁跟随P增益 */
|
|
float link_wall_blend; /* 墙壁跟随权重 (0~1) */
|
|
|
|
/* 出场 */
|
|
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 now_ms 当前系统时间 (HAL_GetTick)，由调用方传入，避免内部依赖 IMU 时间戳
|
|
* @param out 导航输出
|
|
*/
|
|
void GlobalNav_Update(const CorridorObs_t* obs,
|
|
const CorridorState_t* state,
|
|
const RobotBlackboard_t* board,
|
|
uint32_t now_ms,
|
|
GlobalNavOutput_t* out);
|
|
|
|
GlobalNavStage_t GlobalNav_GetStage(void);
|
|
const char* GlobalNav_GetStageName(GlobalNavStage_t stage);
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|
|
|
|
#endif /* GLOBAL_NAV_H */
|