# 重复进入GOTO_START问题诊断

## 问题描述

车辆先走向原点（第一个路径点），然后走向第二个轨迹点，之后目标点又回退到原点，如此反复。Log显示状态从`follow_path`反复回到`goto_start`。

## 根本原因

在`motion_control_task.c`第221-228行，当检测到从手动模式切换到自动模式的上升沿时，会调用`MC_Init`重新初始化状态机：

```c
/* 检测到从手动切换到自动模式的上升沿：重置状态机从头开始 */
if (s_last_auto_condition == HIDO_FALSE && current_auto_condition == HIDO_TRUE)
{
    DBG_Printf("[MC_CTRL] Auto mode triggered! Resetting to GOTO_START.\r\n");
    MC_Init(&g_motion_state, &g_motion_config, g_motion_path_xy, g_motion_path_point_count);
}
```

### 自动模式条件

```c
current_auto_condition = (sbus_valid == HIDO_TRUE && 
                         ch8 > MOTION_SBUS_AUTO_THRESHOLD_US && 
                         gps_ready == HIDO_TRUE);
```

**如果这三个条件中的任何一个反复变化，就会导致状态机重复重置到`GOTO_START`：**

1. **SBUS信号波动**：`sbus_valid`在TRUE和FALSE之间切换
2. **GPS状态不稳定**：`gps_ready`（GPS初始化完成且连接到GNSS）反复变化
3. **CH8值抖动**：`ch8`在阈值（1500）附近波动

## 可能的原因

### 1. SBUS信号不稳定
- 遥控器信号弱
- 接收机连接不良
- 电磁干扰
- SBUS failsafe逻辑问题

### 2. GPS状态不稳定
- RTK信号质量差，固定解和浮点解之间切换
- GPS模块与主控之间通信不稳定
- `gps_ready`的判断条件过于严格

### 3. CH8通道抖动
- 遥控器拨杆在中间位置，轻微抖动跨越阈值
- 遥控器ADC精度问题
- SBUS解码误差

## 诊断方法

已添加详细的log来追踪：

### 1. 自动模式条件变化记录

```c
/* 检测条件变化并记录 */
if (s_last_sbus_valid != sbus_valid)
{
    DBG_Printf("[MC_CTRL] SBUS valid changed: %d -> %d\r\n", s_last_sbus_valid, sbus_valid);
}
if (s_last_gps_ready != gps_ready)
{
    DBG_Printf("[MC_CTRL] GPS ready changed: %d -> %d\r\n", s_last_gps_ready, gps_ready);
}
if (s_last_ch8 != ch8 && (ch8 < MOTION_SBUS_AUTO_THRESHOLD_US || s_last_ch8 < MOTION_SBUS_AUTO_THRESHOLD_US))
{
    DBG_Printf("[MC_CTRL] CH8 crossed threshold: %u -> %u (threshold=%u)\r\n", 
               s_last_ch8, ch8, MOTION_SBUS_AUTO_THRESHOLD_US);
}
```

### 2. 自动模式触发记录

```c
if (s_last_auto_condition == HIDO_FALSE && current_auto_condition == HIDO_TRUE)
{
    DBG_Printf("[MC_CTRL] *** Auto mode triggered! Resetting to GOTO_START ***\r\n");
    DBG_Printf("[MC_CTRL]   SBUS: %d->%d, CH8: %u->%u, GPS_READY: %d->%d\r\n",
               s_last_sbus_valid, sbus_valid,
               s_last_ch8, ch8,
               s_last_gps_ready, gps_ready);
}
```

### 3. 状态切换记录

```c
if (prev_stage != output.stage)
{
    const HIDO_CHAR *prev_label = MotionControl_StageLabel(prev_stage);
    const HIDO_CHAR *curr_label = MotionControl_StageLabel(output.stage);
    DBG_Printf("[MC_CTRL] *** Stage transition: %s -> %s ***\r\n", prev_label, curr_label);
    
    if (output.stage == MC_STAGE_GOTO_START && prev_stage == MC_STAGE_FOLLOW_PATH)
    {
        /* 异常：从follow_path回到goto_start！ */
        DBG_Printf("[MC_CTRL] WARNING: Unexpected transition from FOLLOW_PATH to GOTO_START!\r\n");
    }
}
```

## 测试步骤

1. **编译并烧录**最新代码
2. **启动实车测试**
3. **观察log**，重点关注：
   - `[MC_CTRL] SBUS valid changed`
   - `[MC_CTRL] GPS ready changed`
   - `[MC_CTRL] CH8 crossed threshold`
   - `[MC_CTRL] *** Auto mode triggered!`
   - `[MC_CTRL] *** Stage transition`

4. **根据log确定问题源**：
   - 如果看到频繁的"SBUS valid changed" → SBUS信号问题
   - 如果看到频繁的"GPS ready changed" → GPS状态问题
   - 如果看到频繁的"CH8 crossed threshold" → 遥控器通道抖动
   - 如果看到"Auto mode triggered"频繁出现 → 需要添加防抖逻辑

## 可能的解决方案

### 方案1：添加防抖逻辑

为自动模式条件添加时间延迟，只有条件稳定保持一定时间（如0.5秒）后才切换：

```c
static HIDO_UINT32 s_stable_start_ms = 0;
const HIDO_UINT32 DEBOUNCE_MS = 500;

if (current_auto_condition == HIDO_TRUE)
{
    if (s_last_auto_condition == HIDO_FALSE)
    {
        /* 刚变为TRUE，开始计时 */
        if (s_stable_start_ms == 0)
        {
            s_stable_start_ms = HAL_GetTick();
        }
        else if ((HAL_GetTick() - s_stable_start_ms) >= DEBOUNCE_MS)
        {
            /* 稳定保持500ms，确认切换 */
            MC_Init(...);
            s_stable_start_ms = 0;
        }
    }
}
else
{
    s_stable_start_ms = 0;  /* 重置计时器 */
}
```

### 方案2：CH8死区

为CH8阈值添加迟滞：

```c
#define CH8_THRESHOLD_ENTER  1600  // 进入自动模式
#define CH8_THRESHOLD_EXIT   1400  // 退出自动模式

static HIDO_BOOL auto_mode_active = HIDO_FALSE;

if (auto_mode_active)
{
    if (ch8 < CH8_THRESHOLD_EXIT)
        auto_mode_active = HIDO_FALSE;
}
else
{
    if (ch8 > CH8_THRESHOLD_ENTER)
        auto_mode_active = HIDO_TRUE;
}
```

### 方案3：仅在IDLE或FINISHED时允许重置

不允许在`FOLLOW_PATH`过程中重新初始化：

```c
if (s_last_auto_condition == HIDO_FALSE && current_auto_condition == HIDO_TRUE)
{
    /* 只有在IDLE或FINISHED状态时才重新初始化 */
    if (g_motion_state.stage == MC_STAGE_IDLE || g_motion_state.stage == MC_STAGE_FINISHED)
    {
        MC_Init(...);
    }
}
```

## 相关文件

- `STM32H743/APL/motion_control_task.c` - 添加诊断log
- `STM32H743/FML/motion_control.c` - 状态切换逻辑

## 总结

问题的根本原因是自动模式条件（SBUS、GPS、CH8）不稳定，导致状态机反复重置到`GOTO_START`。通过添加详细的log，可以精确定位是哪个条件在波动，然后针对性地添加防抖或迟滞逻辑来解决。