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

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;
}