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-03-31 23:30:33 +08:00
commit 760043c8e7
1615 changed files with 1406836 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
App/nav/corridor_ctrl.c Normal file
View File

@@ -0,0 +1,83 @@
#include "corridor_ctrl.h"
#include <math.h>
#include <stdbool.h>
/* ====================== 内部状态 ====================== */
static CorridorCtrlConfig_t s_cfg;
static bool s_initialized = false;
/* 辅助：浮点数限幅 */
static inline float clampf(float val, float lo, float hi)
{
if (val < lo) return lo;
if (val > hi) return hi;
return val;
}
void CorridorCtrl_Init(const CorridorCtrlConfig_t *config)
{
s_cfg = *config;
s_initialized = true;
}
void CorridorCtrl_Compute(const CorridorState_t *state,
const CorridorObs_t *obs,
float imu_wz,
RawCmd_t *out_cmd)
{
if (!s_initialized) {
out_cmd->v = 0.0f;
out_cmd->w = 0.0f;
out_cmd->flags = 0U;
return;
}
/* ========================================================
* 核心控制律:
* w_cmd = kp_theta * e_th + kd_theta * (-imu_wz) + kp_y * e_y
*
* - kp_theta * e_th : 比例项，车头偏了就回转
* - kd_theta * (-imu_wz) : 微分阻尼，等价于"阻止车头继续转"
* 用 IMU 直接读数做微分项，比差分 e_th 更丝滑无噪声
* - kp_y * e_y : 横向纠偏，车身偏了就产生角速度拉回来
* ======================================================== */
float w_cmd = s_cfg.kp_theta * state->e_th
+ s_cfg.kd_theta * (-imu_wz)
+ s_cfg.kp_y * state->e_y;
/* 角速度限幅：防止 PD 溢出导致原地打转 */
w_cmd = clampf(w_cmd, -s_cfg.w_max, s_cfg.w_max);
/* ========================================================
* 线速度策略:
* 基础巡航速度 v_cruise，
* 当角速度偏大时适当降速（弯道减速），保持运动学协调
* 公式: v = v_cruise * (1 - k * |w/w_max|)
* k 取 0.3~0.5 较保守
* ======================================================== */
float speed_reduction = s_cfg.speed_reduction_k * fabsf(w_cmd) / s_cfg.w_max;
float v_cmd = s_cfg.v_cruise * (1.0f - speed_reduction);
/* 线速度限幅：不允许倒车，不允许超速 */
v_cmd = clampf(v_cmd, 0.0f, s_cfg.v_max);
/* ========================================================
* 置信度降级保护:
* 当滤波器健康度 conf 过低（两边雷达全瞎），
* 说明走廊参照完全丢失，降低线速度防止盲飞
* ======================================================== */
if (state->conf < 0.3f) {
/* 健康度极低：速度打三折，保持航向惯性滑行 */
v_cmd *= 0.3f;
} else if (state->conf < 0.6f) {
/* 健康度较低（单侧退化）：速度打七折 */
v_cmd *= 0.7f;
}
/* 输出赋值 */
out_cmd->t_ms = state->t_ms;
out_cmd->v = v_cmd;
out_cmd->w = w_cmd;
out_cmd->flags = 0U;
}

47
App/nav/corridor_ctrl.h Normal file
View File

@@ -0,0 +1,47 @@
#ifndef CORRIDOR_CTRL_H
#define CORRIDOR_CTRL_H
#include "preproc/corridor_msgs.h"
/**
* @brief 走廊控制器调参配置
* @note 所有增益参数都应在调试阶段"从小往大调"，防止震荡
*/
typedef struct {
float kp_theta; // 航向偏差比例增益 (rad/s per rad)
float kd_theta; // 航向偏差微分增益 (rad/s per rad/s) [阻尼项]
float kp_y; // 横向偏差比例增益 (rad/s per m)
float v_cruise; // 走廊内巡航线速度 (m/s)
float w_max; // 角速度输出硬限幅 (rad/s)，超过此值一律削峰
float v_max; // 线速度输出硬限幅 (m/s)
float speed_reduction_k; // 弯道减速系数 (0~1)，公式: v = v_cruise*(1-k*|w/w_max|)
} CorridorCtrlConfig_t;
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief 初始化走廊控制器
* @param config 控制器增益与限幅配置
*/
void CorridorCtrl_Init(const CorridorCtrlConfig_t *config);
/**
* @brief 核心函数：根据滤波器输出的走廊状态计算控制量
* @param state 滤波器输出的走廊状态 (e_y, e_th, conf)
* @param obs 预处理层的观测快照 (用于获取 d_front 等辅助信息)
* @param imu_wz 当前的 IMU Z轴角速度 (rad/s)，用于微分阻尼
* @param out_cmd 输出的控制指令 (v, w)
*/
void CorridorCtrl_Compute(const CorridorState_t *state,
const CorridorObs_t *obs,
float imu_wz,
RawCmd_t *out_cmd);
#ifdef __cplusplus
}
#endif
#endif // CORRIDOR_CTRL_H

322
App/nav/nav_script.c Normal file
View File

@@ -0,0 +1,322 @@
/**
* @file nav_script.c
* @brief 段脚本执行器实现：编排完整比赛流程
*/
#include "nav_script.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; // 转向是否已开始
float corridor_s_entry; // 进入垄沟时的 s 里程
int pass_count; // 已走过的垄沟数
float exit_start_s; // 离开垄沟瞬间的 s 里程 (0=未触发)
} s_internal;
/* =========================================================
* 内部辅助函数
* ========================================================= */
static inline float clampf(float val, float lo, float hi)
{
if (val < lo) return lo;
if (val > hi) return hi;
return val;
}
/* =========================================================
* 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.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.pass_count = 1;
s_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
}
}
break;
}
/* ==============================================
* STAGE 2: 向前走垄沟
* ============================================== */
case SCRIPT_STAGE_CORRIDOR_FORWARD: {
/* 使用走廊控制器 */
out->request_corridor = true;
/* 检查是否到端 */
bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
if (front_ok && obs->d_front <= s_cfg.d_entry_exit_front) {
/* 前向距离足够近 -> 到达垄沟末端，准备转向 */
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_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;
}
/* 原地 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 (remaining <= 0.1f) {
/* 转向完成 -> 决定下一步 */
if (s_internal.pass_count < 2) {
/* 只走了一遍，往回走 */
s_internal.pass_count++;
s_stage = SCRIPT_STAGE_CORRIDOR_BACKWARD;
} else {
/* 走了两遍，退出场地 */
s_stage = SCRIPT_STAGE_EXIT;
}
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 (front_ok && obs->d_front <= s_cfg.d_entry_exit_front) {
/* 前向距离足够近 -> 到达垄沟起始端，转向或退出 */
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_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;
}
}
}

133
App/nav/nav_script.h Normal file
View File

@@ -0,0 +1,133 @@
/**
* @file nav_script.h
* @brief 段脚本执行器：编排完整比赛流程
*
* 比赛流程模型：
* ┌─────────────┐
* │ ENTRY_ALIGN │ 入口对准：启动区 -> 第一垄沟入口
* └──────┬──────┘
* │
* v
* ┌─────────────┐
* │ CORRIDOR_A │ 第一垄沟跟踪
* └──────┬──────┘
* │ (到端)
* v
* ┌─────────────┐
* │ TURN_180 │ 原地 180 度转向
* └──────┬──────┘
* │
* v
* ┌─────────────┐
* │ CORRIDOR_B │ 返回垄沟跟踪
* └──────┬──────┘
* │ (到端)
* v
* ┌─────────────┐
* │ EXIT │ 退出比赛场地
* └──────┬──────┘
* │
* v
* ┌─────────────┐
* │ FINISH │ 停在启动区
* └─────────────┘
*/
#ifndef NAV_SCRIPT_H
#define NAV_SCRIPT_H
#include <stdint.h>
#include <stdbool.h>
#include "preproc/corridor_msgs.h"
#include "preproc/corridor_preproc.h"
#ifdef __cplusplus
extern "C" {
#endif
/* =========================================================
* 段脚本阶段枚举
* ========================================================= */
typedef enum {
SCRIPT_STAGE_IDLE = 0, // 待命阶段，不输出任何指令
SCRIPT_STAGE_ENTRY_ALIGN, // 入口对准：从启动区到第一垄沟
SCRIPT_STAGE_CORRIDOR_FORWARD, // 向前走垄沟
SCRIPT_STAGE_TURN_AT_END, // 到端原地转向
SCRIPT_STAGE_CORRIDOR_BACKWARD,// 向后走垄沟
SCRIPT_STAGE_EXIT, // 退出场地
SCRIPT_STAGE_FINISHED, // 比赛完成，安全停车
} NavScriptStage_t;
/* =========================================================
* 段脚本配置参数
* ========================================================= */
typedef struct {
float turn_target_angle; // 原地转向目标角度 (rad)，默认 PI (180度)
float turn_omega; // 原地转向角速度 (rad/s)
float corridor_length; // 垄沟长度估计 (m)，用于触发到端（备用）
float entry_align_timeout; // 入口对准超时 (ms)
float d_entry_exit_front; // 出入口前向距离阈值 (m)
float entry_align_v; // 入口对准慢速前进速度 (m/s)
float exit_runout_m; // 离开走廊后继续冲出的距离 (m)
float exit_v; // 退出场地直线冲出速度 (m/s)
} NavScriptConfig_t;
/* =========================================================
* 段脚本输出
* ========================================================= */
typedef struct {
NavScriptStage_t stage; // 当前执行阶段
const char *stage_name; // 阶段名称字符串
bool active; // 脚本是否正在运行
bool request_corridor; // 是否请求走廊控制器
float override_v; // 覆盖输出的 v (m/s)
float override_w; // 覆盖输出的 w (rad/s)
bool use_override; // 是否使用覆盖值
} NavScriptOutput_t;
/* =========================================================
* API 接口
* ========================================================= */
/**
* @brief 初始化段脚本执行器
*/
void NavScript_Init(const NavScriptConfig_t *config);
/**
* @brief 开始运行比赛脚本
*/
void NavScript_Start(void);
/**
* @brief 核心执行函数：每个导航周期调用一次
* @param obs 预处理层的观测快照
* @param state 滤波器的走廊状态
* @param imu_yaw_continuous_deg IMU unwrap 后的连续偏航角 (度)，用于转弯判定
* @param out 段脚本输出
*/
void NavScript_Update(const CorridorObs_t *obs,
const CorridorState_t *state,
float imu_yaw_continuous_deg,
NavScriptOutput_t *out);
/**
* @brief 获取当前执行阶段
*/
NavScriptStage_t NavScript_GetStage(void);
/**
* @brief 获取阶段名称
*/
const char* NavScript_GetStageName(NavScriptStage_t stage);
/**
* @brief 强制重置脚本到 IDLE
*/
void NavScript_Reset(void);
#ifdef __cplusplus
}
#endif
#endif // NAV_SCRIPT_H

156
App/nav/segment_fsm.c Normal file
View File

@@ -0,0 +1,156 @@
#include "segment_fsm.h"
#include <string.h>
/* ====================== 内部状态 ====================== */
static SegFsmConfig_t s_cfg;
static SegFsmState_t s_state = SEG_STATE_IDLE;
static bool s_initialized = false;
/* 辅助：浮点数限幅 */
static inline float seg_clampf(float val, float lo, float hi)
{
if (val < lo) return lo;
if (val > hi) return hi;
return val;
}
void SegFsm_Init(const SegFsmConfig_t *config)
{
s_cfg = *config;
s_state = SEG_STATE_IDLE;
s_initialized = true;
}
void SegFsm_Start(void)
{
if (s_initialized) {
s_state = SEG_STATE_CORRIDOR;
}
}
SegFsmState_t SegFsm_GetState(void)
{
return s_state;
}
const char* SegFsm_GetStateName(SegFsmState_t s)
{
switch (s) {
case SEG_STATE_IDLE: return "IDLE";
case SEG_STATE_CORRIDOR: return "CORRIDOR";
case SEG_STATE_APPROACH: return "APPROACH";
case SEG_STATE_STOP: return "STOP";
case SEG_STATE_ESTOP: return "E-STOP";
default: return "UNKNOWN";
}
}
void SegFsm_Update(const RawCmd_t *raw_cmd,
const CorridorObs_t *obs,
const CorridorState_t *state,
SegFsmOutput_t *out)
{
if (!s_initialized) {
out->state = SEG_STATE_IDLE;
out->safe_v = 0.0f;
out->safe_w = 0.0f;
return;
}
/* ========================================================
* 1. 全局安全断言：无论在什么状态，满足条件直接超驰
* ======================================================== */
/* 1a. 置信度极低 → 紧急停车 */
if (state->conf < s_cfg.conf_estop_thresh) {
s_state = SEG_STATE_ESTOP;
}
/* ========================================================
* 2. 待命状态：什么都不做
* ======================================================== */
if (s_state == SEG_STATE_IDLE) {
out->state = SEG_STATE_IDLE;
out->safe_v = 0.0f;
out->safe_w = 0.0f;
return;
}
/* ========================================================
* 3. 紧急停车状态：等待置信度恢复
* ======================================================== */
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;
}
return;
}
/* ========================================================
* 4. 前向距离判定：决定 CORRIDOR / APPROACH / STOP
* ======================================================== */
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. 根据当前状态输出最终安全速度
* ======================================================== */
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) {
ratio = (d_front - s_cfg.d_front_stop) / range;
ratio = seg_clampf(ratio, 0.0f, 1.0f);
}
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; // 角速度不限，仍然允许纠偏
break;
}
case SEG_STATE_STOP:
default: {
/* 到端停车 / 紧急停车：零速 */
out->safe_v = 0.0f;
out->safe_w = 0.0f;
break;
}
}
out->state = s_state;
}

78
App/nav/segment_fsm.h Normal file
View File

@@ -0,0 +1,78 @@
#ifndef SEGMENT_FSM_H
#define SEGMENT_FSM_H
#include "preproc/corridor_msgs.h"
#include "preproc/corridor_preproc.h"
/**
* @brief 段状态机的运行阶段枚举
*/
typedef enum {
SEG_STATE_IDLE = 0, // 待命：不输出任何指令
SEG_STATE_CORRIDOR, // 走廊跟踪中：放行控制器输出
SEG_STATE_APPROACH, // 接近端墙：降速保护
SEG_STATE_STOP, // 到端停车：强制零速
SEG_STATE_ESTOP, // 紧急停车：传感器全部失效或异常
} SegFsmState_t;
/**
* @brief 段状态机配置参数
*/
typedef struct {
float d_front_stop; // 前向停车距离阈值 (m)，低于此值直接停车
float d_front_approach; // 前向减速预警距离 (m)，低于此值开始线性减速
float approach_min_v; // 减速区内最低速度 (m/s)，防止爬行太慢永远到不了
float conf_estop_thresh; // 置信度紧急停车阈值，低于此值触发 E-Stop
} SegFsmConfig_t;
/**
* @brief 段状态机对外暴露的完整快照 (供日志或上位机读取)
*/
typedef struct {
SegFsmState_t state; // 当前状态
float safe_v; // 经安全仲裁后的最终线速度 (m/s)
float safe_w; // 经安全仲裁后的最终角速度 (rad/s)
} SegFsmOutput_t;
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief 初始化段状态机
* @param config 距离阈值与降级参数
*/
void SegFsm_Init(const SegFsmConfig_t *config);
/**
* @brief 启动走廊跟踪（从 IDLE 切换到 CORRIDOR）
*/
void SegFsm_Start(void);
/**
* @brief 核心函数：输入控制器的期望指令，输出安全仲裁后的最终指令
* @param raw_cmd 走廊控制器的原始输出 (v, w)
* @param obs 预处理层的观测快照 (提供 d_front 和 valid_mask)
* @param state 滤波器的状态输出 (提供 conf 置信度)
* @param out 安全仲裁后的最终输出
*/
void SegFsm_Update(const RawCmd_t *raw_cmd,
const CorridorObs_t *obs,
const CorridorState_t *state,
SegFsmOutput_t *out);
/**
* @brief 获取当前状态机状态 (用于日志打印)
*/
SegFsmState_t SegFsm_GetState(void);
/**
* @brief 获取状态名称字符串 (用于日志打印)
*/
const char* SegFsm_GetStateName(SegFsmState_t s);
#ifdef __cplusplus
}
#endif
#endif // SEGMENT_FSM_H