81 lines
2.5 KiB
C
81 lines
2.5 KiB
C
#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 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);
|
||
|
||
/**
|
||
* @brief 获取当前状态机状态 (用于日志打印)
|
||
*/
|
||
SegFsmState_t SegFsm_GetState(void);
|
||
|
||
/**
|
||
* @brief 获取状态名称字符串 (用于日志打印)
|
||
*/
|
||
const char* SegFsm_GetStateName(SegFsmState_t s);
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|
||
|
||
#endif // SEGMENT_FSM_H
|