954 lines
35 KiB
C
954 lines
35 KiB
C
/**
|
||
* @file global_nav.c
|
||
* @brief 赛道级总控 — 混合导航状态机实现
|
||
*
|
||
* 场地结构: 垄沟沿X轴(横向)分布，左右两端各有纵向端部通道
|
||
* 启动区在左下角，入口对齐左端通道
|
||
*
|
||
* S 型遍历:
|
||
* 入场(北行)→右转入C1(→东)→右端到端→左转→北行→左转入C2(←西)
|
||
* →左端到端→右转→北行→右转入C3(→东)→...→C6(←西)
|
||
* →左端到端→左转(朝南)→南行出场→回停启动区
|
||
*
|
||
* 端部通道特点:
|
||
* - 一侧贴围栏(VL53能测到), 另一侧交替出现垄背端面和垄沟开口
|
||
* - 不能依赖双侧VL53做EKF, 必须用IMU航向保持+前后激光到端检测
|
||
*/
|
||
|
||
#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;
|
||
|
||
/* 对齐 */
|
||
uint8_t align_ok_count;
|
||
uint8_t wall_heading_stable_count;
|
||
float wall_heading_prev_rad;
|
||
bool wall_heading_prev_valid;
|
||
|
||
/* 出场 */
|
||
bool exit_vl53_lost;
|
||
float exit_lost_distance;
|
||
|
||
/* 航向保持 */
|
||
float heading_ref_deg;
|
||
|
||
/* 连接段: 多传感器辅助 */
|
||
float link_d_front_start; /* 进入连接段时前激光读数 (m) */
|
||
bool link_d_front_valid; /* 进入时前激光是否有效 */
|
||
uint8_t link_gap_count; /* 非围栏侧 VL53 连续丢失计数 (沟口确认) */
|
||
bool link_gap_seen; /* 是否已经确认看到下一个沟口 */
|
||
float link_gap_seen_odom; /* 看到沟口时的累计里程 */
|
||
|
||
/* 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;
|
||
}
|
||
|
||
/**
|
||
* @brief 检测非围栏侧 VL53 是否发现垄沟开口 (连接段沟口辅助判定)
|
||
*
|
||
* 在纵向端部通道面朝北行驶时：
|
||
* - 刚走完向东(EAST)的沟 → 当前在右端通道 → 右侧贴围栏 → 检查左侧VL53
|
||
* - 刚走完向西(WEST)的沟 → 当前在左端通道 → 左侧贴围栏 → 检查右侧VL53
|
||
*
|
||
* 经过垄背端面时: VL53 测到约 (通道宽/2 - 车宽/2 - VL53内缩) ≈ 10cm → 有效
|
||
* 经过垄沟开口时: VL53 射入沟内 220cm+ → 超出有效距离 → 无效或读数 > 1.2m
|
||
*
|
||
* 因为 VL53 前后两颗间距 12cm，车身 20cm 宽，沟口 40cm 宽，
|
||
* 当车身中心对准沟口时，前后 VL53 都已进入开口区域，两颗均应丢失。
|
||
* 但过渡区有边缘效应，所以只要求 "前后至少一颗丢失" 即视为探到沟口。
|
||
*
|
||
* @param obs 当前观测
|
||
* @param prev_travel_dir 刚走完的那条沟的行驶方向 (EAST/WEST)
|
||
* @return true = 非围栏侧检测到开口
|
||
*/
|
||
static bool gap_detected_on_open_side(const CorridorObs_t* obs,
|
||
TravelDirection_t prev_travel_dir)
|
||
{
|
||
if (prev_travel_dir == TRAVEL_DIR_EAST) {
|
||
/* 在右端通道，右侧贴围栏 → 检查左侧 VL53
|
||
* 修改为"与"逻辑：前后都丢失才算沟口，避免单个传感器失效导致误判 */
|
||
bool lf_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LF)
|
||
|| (obs->d_lf > 0.5f); /* >50cm 视为沟口 (正常贴壁约10cm) */
|
||
bool lr_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LR)
|
||
|| (obs->d_lr > 0.5f);
|
||
return lf_lost && lr_lost; /* 前后都丢失才算沟口 */
|
||
} else {
|
||
/* 在左端通道，左侧贴围栏 → 检查右侧 VL53 */
|
||
bool rf_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_RF)
|
||
|| (obs->d_rf > 0.5f);
|
||
bool rr_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_RR)
|
||
|| (obs->d_rr > 0.5f);
|
||
return rf_lost && rr_lost; /* 前后都丢失才算沟口 */
|
||
}
|
||
}
|
||
|
||
/** 检查侧向 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 compute_wall_heading_error(const CorridorObs_t* obs, float* out_heading_rad)
|
||
{
|
||
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 || !right_ok) {
|
||
return false;
|
||
}
|
||
|
||
float left_heading = atan2f(obs->d_lr - obs->d_lf, sensor_base);
|
||
float right_heading = atan2f(obs->d_rf - obs->d_rr, sensor_base);
|
||
|
||
/* 双侧估计必须基本一致，避免沟口边缘/单边异常导致突然大打角。 */
|
||
if (gnav_fabsf(left_heading - right_heading) > PARAM_DEG2RAD(8.0f)) {
|
||
return false;
|
||
}
|
||
|
||
*out_heading_rad = 0.5f * (left_heading + right_heading);
|
||
return true;
|
||
}
|
||
|
||
static void update_wall_heading_stability(bool valid, float heading_rad)
|
||
{
|
||
if (!valid) {
|
||
s_nav.wall_heading_stable_count = 0;
|
||
s_nav.wall_heading_prev_valid = false;
|
||
return;
|
||
}
|
||
|
||
if (s_nav.wall_heading_prev_valid &&
|
||
gnav_fabsf(heading_rad - s_nav.wall_heading_prev_rad) <= PARAM_DEG2RAD(4.0f)) {
|
||
if (s_nav.wall_heading_stable_count < 255U) {
|
||
s_nav.wall_heading_stable_count++;
|
||
}
|
||
} else {
|
||
s_nav.wall_heading_stable_count = 1;
|
||
}
|
||
|
||
s_nav.wall_heading_prev_rad = heading_rad;
|
||
s_nav.wall_heading_prev_valid = true;
|
||
}
|
||
|
||
/* 重捕获已确认后，判断是否需要短暂停车摆正。
|
||
* 除了航向误差本身，还把“明显贴墙/明显偏中心”作为触发条件。
|
||
* 这样即使车头看起来近似平行，但整车已经贴到一侧，也会先停一下再进跟踪。 */
|
||
static bool need_align_after_reacquire(const CorridorObs_t* obs, float wall_heading_error)
|
||
{
|
||
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) {
|
||
return gnav_fabsf(wall_heading_error) > s_nav.cfg.align_th_tol_rad;
|
||
}
|
||
|
||
{
|
||
float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
|
||
float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
|
||
float half_gap = 0.5f * (s_nav.cfg.corridor_width - PARAM_ROBOT_WIDTH);
|
||
float min_side = (d_left < d_right) ? d_left : d_right;
|
||
bool heading_bad = gnav_fabsf(wall_heading_error) > s_nav.cfg.align_th_tol_rad;
|
||
bool near_wall = min_side < (half_gap - s_nav.cfg.align_y_tol_m);
|
||
bool off_center = gnav_fabsf(d_left - d_right) > (2.0f * s_nav.cfg.align_y_tol_m);
|
||
|
||
return heading_bad || near_wall || off_center;
|
||
}
|
||
}
|
||
|
||
/** 检查重捕获条件 */
|
||
static bool check_reacquire(const CorridorObs_t* obs, const CorridorState_t* state)
|
||
{
|
||
/* 条件 1: 四个侧向传感器都有效
|
||
* 赛道模式的重捕获要更稳，不能接受 3/4 这种退化几何。 */
|
||
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) return false;
|
||
|
||
/* 条件 2: 左右距离和 ≈ 走廊宽度，且这里是必检项 */
|
||
{
|
||
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",
|
||
"ALIGN",
|
||
"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)
|
||
{
|
||
/* [改进G] IMU 失效安全保护: 没有 IMU 数据时立即停车，不盲转。
|
||
* 超时后会被外部超时保护捕获，进入 GNAV_ERROR。 */
|
||
if (!board->imu_yaw_continuous.is_valid) {
|
||
out->override_v = 0.0f;
|
||
out->override_w = 0.0f;
|
||
out->use_override = true;
|
||
out->request_corridor = false;
|
||
out->safety_mode = SAFETY_MODE_IDLE;
|
||
return;
|
||
}
|
||
|
||
float imu_yaw = board->imu_yaw_continuous.value;
|
||
|
||
/* 已转过的角度 (取绝对值) */
|
||
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;
|
||
s_nav.align_ok_count = 0;
|
||
s_nav.wall_heading_stable_count = 0;
|
||
s_nav.wall_heading_prev_rad = 0.0f;
|
||
s_nav.wall_heading_prev_valid = false;
|
||
|
||
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;
|
||
s_nav.link_d_front_start = 0.0f;
|
||
s_nav.link_d_front_valid = false; /* 首拍再记录 */
|
||
s_nav.link_gap_count = 0;
|
||
s_nav.link_gap_seen = false;
|
||
s_nav.link_gap_seen_odom = 0.0f;
|
||
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,
|
||
uint32_t now_ms,
|
||
GlobalNavOutput_t* out)
|
||
{
|
||
if (!s_nav.initialized) {
|
||
memset(out, 0, sizeof(*out));
|
||
return;
|
||
}
|
||
|
||
/* 里程计积分: Δd = vx * dt
|
||
* now_ms 由调用方传入 (HAL_GetTick)，与任何传感器时间戳无关，
|
||
* 避免 IMU 时间戳停更时里程和超时双双冻结 (原 TODO-1 修复) */
|
||
{
|
||
float odom_vx = board->odom_vx;
|
||
if (s_last_update_ms > 0) {
|
||
float dt = (float)(now_ms - s_last_update_ms) * 0.001f;
|
||
if (dt > 0.0f && dt < 0.5f) {
|
||
s_nav.odom_distance_accum += gnav_fabsf(odom_vx) * dt;
|
||
}
|
||
}
|
||
s_last_update_ms = now_ms;
|
||
s_last_odom_vx = odom_vx;
|
||
}
|
||
|
||
/* [改进G] IMU yaw 提取: 失效时使用参考值，保持航向不变而不是跳到 0 */
|
||
float imu_yaw_deg = board->imu_yaw_continuous.is_valid
|
||
? board->imu_yaw_continuous.value : s_nav.heading_ref_deg;
|
||
|
||
/* 默认输出 */
|
||
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) {
|
||
|
||
/* ============================================================
|
||
* 入场直线 (从启动区沿左端纵向通道北行)
|
||
*
|
||
* 传感器情况：
|
||
* - 左侧 VL53 贴围栏，始终有效（不能用来判"到了C1入口"）
|
||
* - 右侧 VL53 一出启动区就对着C1开口(260cm)，测不到
|
||
* - 入场距离极短（启动区入口到C1入口仅约 10~30cm）
|
||
* - 主要靠里程计推进足够距离后即可转向
|
||
* ============================================================ */
|
||
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;
|
||
|
||
/* 里程计 + 超时判定 (不用 side_walls_detected，因为左侧围栏始终有效会误触发) */
|
||
if (odom_since_entry() >= s_nav.cfg.entry_distance ||
|
||
elapsed_ms > s_nav.cfg.entry_timeout_ms)
|
||
{
|
||
transition_to(GNAV_TURN_INTO_CORRIDOR, board);
|
||
}
|
||
break;
|
||
|
||
/* ============================================================
|
||
* 转向进入下一条沟 (原地转 90°)
|
||
* ============================================================ */
|
||
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;
|
||
|
||
/* ============================================================
|
||
* 重捕获走廊
|
||
*
|
||
* 问题背景：
|
||
* 转弯刚完成时，车身可能还在沟口外（端部通道内），
|
||
* 但此时两侧VL53同样能测到两侧垄背端面（~10cm），
|
||
* 宽度和 = 10+10+20 = 40cm，与真正在沟内完全一致。
|
||
* 这导致尚未入沟就满足重捕获条件，切到ALIGN，
|
||
* EKF的e_y可能是大偏差，车卡死在入口。
|
||
*
|
||
* 修复方案：
|
||
* 引入后部激光距离检测：只有后激光距离 > 40cm，
|
||
* 说明车身已完全进入沟内，才允许开始确认计数。
|
||
* 这确保四颗VL53全部脱离沟口边缘进入稳定侧壁区域。
|
||
* ============================================================ */
|
||
case GNAV_REACQUIRE:
|
||
out->override_v = s_nav.cfg.reacquire_v;
|
||
out->override_w = 0.0f; /* 不做航向控制，让车自然进沟，ALIGN阶段再摆正 */
|
||
out->safety_mode = SAFETY_MODE_STRAIGHT;
|
||
|
||
{
|
||
/* 进沟深度守卫：后激光 + 最小入沟里程 双保险。
|
||
* 尤其对 C1，不能只靠后激光，否则在入口区就可能过早假成功。 */
|
||
bool back_ok = false;
|
||
bool odom_ok = (odom_since_entry() >= s_nav.cfg.reacquire_min_odom);
|
||
if ((obs->valid_mask & CORRIDOR_OBS_MASK_BACK) != 0U) {
|
||
if (obs->d_back > s_nav.cfg.reacquire_min_back_dist) {
|
||
back_ok = true;
|
||
}
|
||
}
|
||
|
||
if (back_ok && odom_ok && 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) {
|
||
float wall_heading_error = 0.0f;
|
||
bool wall_heading_ok = compute_wall_heading_error(obs, &wall_heading_error);
|
||
update_wall_heading_stability(wall_heading_ok, wall_heading_error);
|
||
|
||
/* 重捕获成功后，优先用 side VL53 短暂停车摆正车头。
|
||
* 只有 wall heading 已稳定时才允许进入 ALIGN 或直接放行。 */
|
||
if (wall_heading_ok && s_nav.wall_heading_stable_count >= 4U) {
|
||
if (need_align_after_reacquire(obs, wall_heading_error)) {
|
||
transition_to(GNAV_ALIGN, board);
|
||
} else {
|
||
if (board->imu_yaw_continuous.is_valid) {
|
||
CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f);
|
||
s_nav.heading_ref_deg = board->imu_yaw_continuous.value;
|
||
}
|
||
transition_to(GNAV_CORRIDOR_TRACK, board);
|
||
}
|
||
}
|
||
}
|
||
if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms) {
|
||
/* 取消重捕获失败态：超时后不进 ERROR，
|
||
* 直接转入短暂停车摆正阶段，让车停下来继续自恢复。 */
|
||
transition_to(GNAV_ALIGN, board);
|
||
}
|
||
break;
|
||
|
||
/* ============================================================
|
||
* 短暂停车摆正航向 (ALIGN)
|
||
*
|
||
* 只用 side VL53 的前后差来摆正车头，不做横向居中。
|
||
* 目的不是把车居中，而是避免“斜着进沟时前轮先蹭墙，
|
||
* 控制器还继续朝墙边打”的情况。
|
||
* ============================================================ */
|
||
case GNAV_ALIGN: {
|
||
float wall_heading_error = 0.0f;
|
||
bool wall_heading_ok = compute_wall_heading_error(obs, &wall_heading_error);
|
||
update_wall_heading_stability(wall_heading_ok, wall_heading_error);
|
||
|
||
out->override_v = 0.0f;
|
||
out->use_override = true;
|
||
out->request_corridor = false;
|
||
out->safety_mode = SAFETY_MODE_STRAIGHT;
|
||
|
||
if (wall_heading_ok && s_nav.wall_heading_stable_count >= 2U) {
|
||
out->override_w = gnav_clampf(-s_nav.cfg.align_kp_th * wall_heading_error,
|
||
-0.25f, 0.25f);
|
||
} else {
|
||
out->override_w = 0.0f;
|
||
}
|
||
|
||
if (wall_heading_ok && gnav_fabsf(wall_heading_error) < s_nav.cfg.align_th_tol_rad) {
|
||
s_nav.align_ok_count++;
|
||
} else {
|
||
s_nav.align_ok_count = 0;
|
||
}
|
||
|
||
if (s_nav.align_ok_count >= s_nav.cfg.align_confirm_ticks) {
|
||
if (board->imu_yaw_continuous.is_valid) {
|
||
CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f);
|
||
s_nav.heading_ref_deg = board->imu_yaw_continuous.value;
|
||
}
|
||
transition_to(GNAV_CORRIDOR_TRACK, board);
|
||
}
|
||
if (elapsed_ms > s_nav.cfg.align_timeout_ms) {
|
||
if (board->imu_yaw_continuous.is_valid) {
|
||
CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f);
|
||
s_nav.heading_ref_deg = board->imu_yaw_continuous.value;
|
||
}
|
||
transition_to(GNAV_CORRIDOR_TRACK, 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) {
|
||
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;
|
||
|
||
/* ============================================================
|
||
* 连接段直行 (端部通道内，从一条沟到下一条沟)
|
||
*
|
||
* 控制策略：IMU 决定主航向；单边 VL53 只做离墙保底。
|
||
* 也就是说：
|
||
* - 默认纯航向保持
|
||
* - 只有当贴围栏侧距离小于阈值时，才附加一个远离墙面的修正
|
||
* - 不做墙跟随融合，避免单边 VL53 把主方向带偏
|
||
* ============================================================ */
|
||
case GNAV_LINK_STRAIGHT: {
|
||
out->override_v = s_nav.cfg.link_v;
|
||
out->safety_mode = SAFETY_MODE_STRAIGHT;
|
||
|
||
float w_heading = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
|
||
s_nav.cfg.heading_kp);
|
||
float w_wall_guard = 0.0f;
|
||
const CorridorDescriptor_t* corridor = TrackMap_GetCorridor(s_nav.current_corridor_id);
|
||
|
||
if (corridor != NULL && corridor->travel_dir == TRAVEL_DIR_EAST) {
|
||
if ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) &&
|
||
(obs->valid_mask & CORRIDOR_OBS_MASK_RR)) {
|
||
float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
|
||
if (d_right < s_nav.cfg.link_wall_target) {
|
||
float error = s_nav.cfg.link_wall_target - d_right;
|
||
w_wall_guard = s_nav.cfg.link_wall_kp * error;
|
||
}
|
||
}
|
||
} else if (corridor != NULL) {
|
||
if ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) &&
|
||
(obs->valid_mask & CORRIDOR_OBS_MASK_LR)) {
|
||
float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
|
||
if (d_left < s_nav.cfg.link_wall_target) {
|
||
float error = s_nav.cfg.link_wall_target - d_left;
|
||
w_wall_guard = -(s_nav.cfg.link_wall_kp * error);
|
||
}
|
||
}
|
||
}
|
||
|
||
out->override_w = gnav_clampf(w_heading + w_wall_guard, -1.0f, 1.0f);
|
||
|
||
{
|
||
bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
|
||
|
||
if (!s_nav.link_d_front_valid && front_valid) {
|
||
s_nav.link_d_front_start = obs->d_front;
|
||
s_nav.link_d_front_valid = true;
|
||
}
|
||
|
||
bool odom_ok = odom_since_entry() >= s_nav.cfg.link_distance * 0.85f;
|
||
|
||
bool laser_ok = false;
|
||
if (s_nav.link_d_front_valid && front_valid) {
|
||
float d_front_delta = s_nav.link_d_front_start - obs->d_front;
|
||
laser_ok = (d_front_delta >= s_nav.cfg.link_distance * 0.85f);
|
||
}
|
||
|
||
bool gap_confirmed = false;
|
||
if (corridor != NULL) {
|
||
bool gap_now = gap_detected_on_open_side(obs, corridor->travel_dir);
|
||
if (gap_now) {
|
||
if (s_nav.link_gap_count < 255U) s_nav.link_gap_count++;
|
||
} else {
|
||
s_nav.link_gap_count = 0;
|
||
}
|
||
gap_confirmed = (s_nav.link_gap_count >= 5U);
|
||
|
||
if (gap_confirmed && !s_nav.link_gap_seen) {
|
||
s_nav.link_gap_seen = true;
|
||
s_nav.link_gap_seen_odom = s_nav.odom_distance_accum;
|
||
}
|
||
}
|
||
|
||
bool gap_runout_ok = false;
|
||
if (s_nav.link_gap_seen) {
|
||
gap_runout_ok = (s_nav.odom_distance_accum - s_nav.link_gap_seen_odom)
|
||
>= s_nav.cfg.link_gap_runout;
|
||
}
|
||
|
||
if (odom_ok && (laser_ok || (s_nav.link_gap_seen && gap_runout_ok))) {
|
||
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->safety_mode = SAFETY_MODE_STRAIGHT;
|
||
|
||
float w_heading = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
|
||
s_nav.cfg.heading_kp);
|
||
float w_wall = 0.0f;
|
||
bool rf_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U;
|
||
bool rr_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U;
|
||
|
||
if (rf_ok || rr_ok) {
|
||
float d_right;
|
||
|
||
if (rf_ok && rr_ok) {
|
||
d_right = (obs->d_rf + obs->d_rr) * 0.5f;
|
||
} else if (rf_ok) {
|
||
d_right = obs->d_rf;
|
||
} else {
|
||
d_right = obs->d_rr;
|
||
}
|
||
|
||
/* 出场阶段在左端通道向南走，右侧仍然是围栏。
|
||
* 用右侧 VL53 直接叠加一个更硬的保墙约束，避免一路蹭墙。 */
|
||
{
|
||
float error = s_nav.cfg.link_wall_target - d_right;
|
||
w_wall = 1.5f * s_nav.cfg.link_wall_kp * error;
|
||
}
|
||
}
|
||
|
||
out->override_w = gnav_clampf(w_heading + w_wall, -1.0f, 1.0f);
|
||
|
||
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->safety_mode = SAFETY_MODE_STRAIGHT;
|
||
|
||
float w_heading = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
|
||
s_nav.cfg.heading_kp);
|
||
float w_wall = 0.0f;
|
||
bool rf_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U;
|
||
bool rr_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U;
|
||
|
||
if (rf_ok || rr_ok) {
|
||
float d_right;
|
||
|
||
if (rf_ok && rr_ok) {
|
||
d_right = (obs->d_rf + obs->d_rr) * 0.5f;
|
||
} else if (rf_ok) {
|
||
d_right = obs->d_rf;
|
||
} else {
|
||
d_right = obs->d_rr;
|
||
}
|
||
|
||
/* 回停阶段右墙保持更硬：
|
||
* IMU 仍然给主方向，但右侧距离约束直接叠加，不再做 50% 混合。 */
|
||
{
|
||
float error = s_nav.cfg.link_wall_target - d_right;
|
||
w_wall = 1.5f * s_nav.cfg.link_wall_kp * error;
|
||
}
|
||
}
|
||
|
||
out->override_w = gnav_clampf(w_heading + w_wall, -1.0f, 1.0f);
|
||
|
||
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;
|
||
}
|