1.0

App/nav/segment_fsm.h

@@ -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