重复进入GOTO_START问题诊断

问题描述

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

根本原因

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

/* 检测到从手动切换到自动模式的上升沿：重置状态机从头开始 */
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);
}

自动模式条件

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

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

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

可能的原因

1. SBUS信号不稳定

遥控器信号弱
接收机连接不良
电磁干扰
SBUS failsafe逻辑问题

2. GPS状态不稳定

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

3. CH8通道抖动

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

诊断方法

已添加详细的log来追踪：

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

/* 检测条件变化并记录 */
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. 自动模式触发记录

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. 状态切换记录

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");
    }
}

测试步骤

编译并烧录最新代码
启动实车测试
观察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

根据log确定问题源：

如果看到频繁的"SBUS valid changed" → SBUS信号问题
如果看到频繁的"GPS ready changed" → GPS状态问题
如果看到频繁的"CH8 crossed threshold" → 遥控器通道抖动
如果看到"Auto mode triggered"频繁出现 → 需要添加防抖逻辑

可能的解决方案

方案1：添加防抖逻辑

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

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阈值添加迟滞：

#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过程中重新初始化：

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(...);
    }
}

总结

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