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

65
App/nav/segment_fsm.c
View File

@@ -48,6 +48,7 @@ const char* SegFsm_GetStateName(SegFsmState_t s)
void SegFsm_Update(const RawCmd_t *raw_cmd,
const CorridorObs_t *obs,
const CorridorState_t *state,
SafetyMode_t mode,
SegFsmOutput_t *out)
{
if (!s_initialized) {
@@ -58,18 +59,9 @@ void SegFsm_Update(const RawCmd_t *raw_cmd,
}
/* ========================================================
* 1. 全局安全断言：无论在什么状态，满足条件直接超驰
* 0. IDLE 模式：零速，不做任何裁剪
* ======================================================== */
/* 1a. 置信度极低 → 紧急停车 */
if (state->conf < s_cfg.conf_estop_thresh) {
s_state = SEG_STATE_ESTOP;
}
/* ========================================================
* 2. 待命状态：什么都不做
* ======================================================== */
if (s_state == SEG_STATE_IDLE) {
if (mode == SAFETY_MODE_IDLE) {
out->state = SEG_STATE_IDLE;
out->safe_v = 0.0f;
out->safe_w = 0.0f;
@@ -77,14 +69,43 @@ void SegFsm_Update(const RawCmd_t *raw_cmd,
}
/* ========================================================
* 3. 紧急停车状态：等待置信度恢复
* 1. TURN 模式：原地转向，直接放行，不检查前距和置信度
* 解决 RISK-1: 转向阶段不会被安全层卡死
* ======================================================== */
if (mode == SAFETY_MODE_TURN) {
s_state = SEG_STATE_CORRIDOR;
out->state = SEG_STATE_CORRIDOR;
out->safe_v = raw_cmd->v;
out->safe_w = raw_cmd->w;
return;
}
/* ========================================================
* 2. CORRIDOR 模式：完整安全检查 (前向 + 置信度)
* STRAIGHT 模式：前向检查，但不检查置信度
* ======================================================== */
/* 2a. 置信度检查：仅 CORRIDOR 模式下生效 */
if (mode == SAFETY_MODE_CORRIDOR) {
if (state->conf < s_cfg.conf_estop_thresh) {
s_state = SEG_STATE_ESTOP;
}
}
/* 待命状态 */
if (s_state == SEG_STATE_IDLE) {
out->state = SEG_STATE_IDLE;
out->safe_v = 0.0f;
out->safe_w = 0.0f;
return;
}
/* 紧急停车状态：等待置信度恢复 (仅 CORRIDOR 模式可能进入) */
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;
}
@@ -92,45 +113,34 @@ void SegFsm_Update(const RawCmd_t *raw_cmd,
}
/* ========================================================
* 4. 前向距离判定：决定 CORRIDOR / APPROACH / STOP
* 3. 前向距离判定：CORRIDOR 和 STRAIGHT 都做
* ======================================================== */
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. 根据当前状态输出最终安全速度
* 4. 根据当前状态输出最终安全速度
* ======================================================== */
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) {
@@ -139,13 +149,12 @@ void SegFsm_Update(const RawCmd_t *raw_cmd,
}
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; // 角速度不限，仍然允许纠偏
out->safe_w = raw_cmd->w;
break;
}
case SEG_STATE_STOP:
default: {
/* 到端停车 / 紧急停车：零速 */
out->safe_v = 0.0f;
out->safe_w = 0.0f;
break;