还原到路径第一次调通的版本，

yincheng.zhong

2025-12-10 8d0d8dec18cc9170f3fb82a4dba18160dd8e5233

 STM32H743/APL/motion_control_task.c

@@ -84,6 +84,11 @@
            g_motion_path_xy,
            g_motion_path_point_count);

    DBG_Printf("[MC_INIT] Path loaded: count=%u, first_point=(%.2f,%.2f)\r\n",
               g_motion_path_point_count,
               g_motion_path_xy[0],
               g_motion_path_xy[1]);

    BaseType_t ret = xTaskCreate(
        MotionControl_TaskEntry,
        "MotionCtrl",
@@ -138,6 +143,12 @@
                g_last_heading_deg = gprmi.m_fHeadingAngle;
                g_last_pitch_deg = gprmi.m_fPitchAngle;
                g_last_roll_deg = gprmi.m_fRollAngle;
                
                /* 检测pose_valid状态变化 */
                if (g_last_pose_valid == HIDO_FALSE)
                {
                    HIDO_Debug2("[MC_POSE]pose_valid: FALSE->TRUE (GPS recovered)\r\n");
                }
                g_last_pose_valid = HIDO_TRUE;
                g_last_gps_ms = now;
                g_last_gprmi_tow = gps_timestamp;
@@ -146,6 +157,12 @@
        }
        else if (gps_valid == HIDO_FALSE && (now - g_last_gps_ms) > 200U)
        {
            /* 检测pose_valid状态变化 */
            if (g_last_pose_valid == HIDO_TRUE)
            {
                HIDO_Debug2("[MC_POSE]pose_valid: TRUE->FALSE (GPS timeout, age=%ums)\r\n", 
                            now - g_last_gps_ms);
            }
            g_motion_state.pose_valid = HIDO_FALSE;
            g_last_pose_valid = HIDO_FALSE;
        }
@@ -167,7 +184,40 @@
        /* --- 2) 调用运动控制器（固定 75 Hz dt） --- */
        MC_Output output;
        HIDO_BOOL sbus_valid = (SBUS_IsSignalValid(MOTION_SBUS_TIMEOUT_MS) == HIDO_TRUE);
        HIDO_UINT16 ch8 = SBUS_GetChannel(MOTION_SBUS_AUTO_CHANNEL);
        HIDO_UINT16 ch8_raw = SBUS_GetChannel(MOTION_SBUS_AUTO_CHANNEL);
        
        /* 过滤SBUS异常值：
         * 1024 = SBUS_CENTER_VALUE（信号丢失时的默认返回值）
         * <172 或 >1811 = SBUS有效范围之外（对应PWM 1000-2000）
         * 当检测到异常值时，保持之前的有效值不变 */
        static HIDO_UINT16 ch8 = 1000;  /* 默认手动模式 */
        static HIDO_UINT32 s_ch8_failsafe_count = 0;
        static HIDO_UINT16 s_ch8_last_valid = 1000;
        
        /* 判断是否为有效SBUS值：172-1811范围内，且不是精确的1024 */
        HIDO_BOOL is_valid = (ch8_raw >= 172 && ch8_raw <= 1811 && ch8_raw != 1024);
        
        if (is_valid)
        {
            ch8 = ch8_raw;  /* 只更新有效值 */
            s_ch8_last_valid = ch8_raw;
            if (s_ch8_failsafe_count > 0)
            {
                DBG_Printf("[MC_CTRL] CH8 recovered from failsafe, count=%u, new value=%u\r\n", 
                           s_ch8_failsafe_count, ch8);
                s_ch8_failsafe_count = 0;
            }
        }
        else
        {
            /* 异常值，保持之前的值，记录failsafe事件 */
            s_ch8_failsafe_count++;
            if (s_ch8_failsafe_count == 1)
            {
                DBG_Printf("[MC_CTRL] CH8 failsafe detected (%u), keeping previous value=%u\r\n", 
                           ch8_raw, ch8);
            }
        }
        
        /* 检查 GPS 状态：必须初始化完成且连接到 GNSS */
        HIDO_BOOL gps_ready = HIDO_FALSE;
@@ -208,29 +258,148 @@
#endif
        
        /* 只有当满足所有条件时才执行自动控制：
        
        /* 只有当满足所有条件时才执行自动控制：
         * 1. SBUS 信号有效
         * 2. CH8 > 阈值（自动模式开关）
         * 2. CH8 > 阈值（自动模式开关，带迟滞）
         * 3. GPS 初始化完成（FINIT_OK）
         * 4. GPS 连接到 GNSS（GNSS_CONNECT）*/
        static HIDO_BOOL s_last_auto_condition = HIDO_FALSE;
        HIDO_BOOL current_auto_condition = (sbus_valid == HIDO_TRUE && ch8 > MOTION_SBUS_AUTO_THRESHOLD_US && gps_ready == HIDO_TRUE);
        static HIDO_BOOL s_last_sbus_valid = HIDO_FALSE;
        static HIDO_BOOL s_last_gps_ready = HIDO_FALSE;
        static HIDO_UINT16 s_last_ch8 = 0;
        static HIDO_BOOL s_ch8_auto_state = HIDO_FALSE;  /* CH8迟滞状态 */
        static HIDO_BOOL s_last_ch8_auto_state = HIDO_FALSE;  /* 上一次的CH8迟滞状态 */

        /* 检测到从手动切换到自动模式的上升沿：重置状态机从头开始 */
        /* CH8迟滞：防止在阈值附近抖动时反复触发
         * 进入自动模式：CH8 > 1600 (高阈值)
         * 退出自动模式：CH8 < 1400 (低阈值)
         * 死区：1400-1600，在此区间保持之前的状态
         * 
         * 连续确认机制：需要连续3次检测到相同状态才切换
         * 这是最稳定的防抖方案，防止瞬态干扰 */
        #define CH8_THRESHOLD_ENTER  1600
        #define CH8_THRESHOLD_EXIT   1400
        #define CH8_CONFIRM_COUNT    3
        
        static HIDO_UINT8 s_ch8_enter_count = 0;  /* 连续检测到进入自动模式的次数 */
        static HIDO_UINT8 s_ch8_exit_count = 0;   /* 连续检测到退出自动模式的次数 */
        
        if (s_ch8_auto_state == HIDO_FALSE)
        {
            /* 当前在手动模式，检查是否应该进入自动模式 */
            if (ch8 > CH8_THRESHOLD_ENTER)
            {
                s_ch8_enter_count++;
                s_ch8_exit_count = 0;  /* 重置退出计数 */
                
                if (s_ch8_enter_count >= CH8_CONFIRM_COUNT)
                {
                    /* 连续3次确认，切换到自动模式 */
                    s_ch8_auto_state = HIDO_TRUE;
                    s_ch8_enter_count = 0;
                    DBG_Printf("[MC_CTRL] CH8 state confirmed: MANUAL -> AUTO (ch8=%u)\r\n", ch8);
                }
            }
            else
            {
                s_ch8_enter_count = 0;  /* 未满足条件，重置计数 */
            }
        }
        else
        {
            /* 当前在自动模式，检查是否应该退出自动模式 */
            if (ch8 < CH8_THRESHOLD_EXIT)
            {
                s_ch8_exit_count++;
                s_ch8_enter_count = 0;  /* 重置进入计数 */
                
                if (s_ch8_exit_count >= CH8_CONFIRM_COUNT)
                {
                    /* 连续3次确认，切换到手动模式 */
                    s_ch8_auto_state = HIDO_FALSE;
                    s_ch8_exit_count = 0;
                    DBG_Printf("[MC_CTRL] CH8 state confirmed: AUTO -> MANUAL (ch8=%u)\r\n", ch8);
                }
            }
            else
            {
                s_ch8_exit_count = 0;  /* 未满足条件，重置计数 */
            }
        }
        
        HIDO_BOOL current_auto_condition = (sbus_valid == HIDO_TRUE && s_ch8_auto_state == HIDO_TRUE && gps_ready == HIDO_TRUE);

        /* 检测到从手动切换到自动模式的上升沿：重置状态机从头开始
         * 增加稳定性要求：只有在IDLE或FINISHED状态时才允许重新初始化
         * 如果正在执行任务（GOTO_START或FOLLOW_PATH），忽略模式切换抖动 */
        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");
            E_MCStage current_stage = g_motion_state.stage;
            
            if (current_stage == MC_STAGE_IDLE || current_stage == MC_STAGE_FINISHED)
            {
                DBG_Printf("[MC_CTRL] *** Auto mode triggered! Resetting to GOTO_START ***\r\n");
                DBG_Printf("[MC_CTRL]   SBUS: %d->%d, CH8: %u->%u (state=%d->%d), GPS_READY: %d->%d\r\n",
                           s_last_sbus_valid, sbus_valid,
                           s_last_ch8, ch8,
                           s_last_ch8_auto_state, s_ch8_auto_state,
                           s_last_gps_ready, gps_ready);
            MC_Init(&g_motion_state,
                    &g_motion_config,
                    g_motion_path_xy,
                    g_motion_path_point_count);
        }
            else
            {
                /* 正在执行任务，忽略此次模式切换（可能是信号抖动） */
                DBG_Printf("[MC_CTRL] WARNING: Auto mode re-triggered during %d, ignoring (SBUS interference)\r\n", 
                           current_stage);
            }
        }
        
        /* 检测条件变化并记录 */
        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);
        }
        
        /* 记录状态变化（已在上面连续确认时记录，这里只更新last状态） */
        s_last_ch8_auto_state = s_ch8_auto_state;
        
        s_last_auto_condition = current_auto_condition;
        s_last_sbus_valid = sbus_valid;
        s_last_gps_ready = gps_ready;
        s_last_ch8 = ch8;

        if (current_auto_condition == HIDO_TRUE)
        {
            /* 记录状态切换 */
            static E_MCStage s_last_stage = MC_STAGE_IDLE;
            E_MCStage prev_stage = g_motion_state.stage;
            
            MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output);
            
            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");
                    DBG_Printf("[MC_CTRL]   nearest_idx=%u, path_count=%u, pos=(%.2f,%.2f)\r\n",
                               g_motion_state.nearest_index,
                               g_motion_state.path_count,
                               g_motion_state.pos[0],
                               g_motion_state.pos[1]);
                }
            }
            s_last_stage = output.stage;
        }
        else
        {
@@ -278,11 +447,14 @@
            int tgt_y_int = (int)g_motion_state.current_target_xy[1];
            int tgt_y_frac = (int)(fabsf(g_motion_state.current_target_xy[1] - tgt_y_int) * 100);
            int fwd_int = (int)output.forward_mps;
            int fwd_frac = (int)(fabsf(output.forward_mps - fwd_int) * 100);
            int turn_int = (int)output.turn_rate;
            int turn_frac = (int)(fabsf(output.turn_rate - turn_int) * 100);
            int fwd_frac = (int)(fabsf(output.forward_mps - (float)fwd_int) * 100.0f);
            /* 修复：保留turn的符号，使用带符号格式输出 */
            float turn_abs = fabsf(output.turn_rate);
            int turn_int = (int)turn_abs;
            int turn_frac = (int)((turn_abs - (float)turn_int) * 100.0f);
            char turn_sign = (output.turn_rate < 0.0f) ? '-' : '+';
            
            HIDO_Debug2("[MC_CTRL] stage=%d pos=(%d.%02d,%d.%02d,%d.%02d) hdg=%d.%02d tgt=(%d.%02d,%d.%02d) fwd=%d.%02d turn=%d.%02d path_idx=%u\n",
            HIDO_Debug2("[MC_CTRL] stage=%d pos=(%d.%02d,%d.%02d,%d.%02d) hdg=%d.%02d tgt=(%d.%02d,%d.%02d) fwd=%d.%02d turn=%c%d.%02d path_idx=%u\n",
                        g_motion_state.stage,
                        pos_x_int, pos_x_frac,
                        pos_y_int, pos_y_frac,
@@ -291,7 +463,7 @@
                        tgt_x_int, tgt_x_frac,
                        tgt_y_int, tgt_y_frac,
                        fwd_int, fwd_frac,
                        turn_int, turn_frac,
                        turn_sign, turn_int, turn_frac,
                        g_motion_state.nearest_index);
        }

@@ -355,11 +527,50 @@
        g_last_pitch_deg = output.pitch_deg;
        g_last_roll_deg = output.roll_deg;
        g_last_pose_valid = g_motion_state.pose_valid;
        g_last_target_valid = output.target_valid;
        
        /* 检测目标点跳变到(0,0)的情况 */
        static HIDO_BOOL s_warn_zero_target = HIDO_FALSE;
        if (output.target_valid == HIDO_TRUE)
        {
            /* 检测目标点突然跳到接近(0,0)的情况 */
            HIDO_BOOL is_near_zero = (fabsf(output.target_xy[0]) < 0.1f && fabsf(output.target_xy[1]) < 0.1f);
            HIDO_BOOL was_far_from_zero = (fabsf(g_last_target_xy[0]) > 1.0f || fabsf(g_last_target_xy[1]) > 1.0f);
            
            if (is_near_zero && was_far_from_zero && g_last_target_valid)
            {
                int old_x_int = (int)g_last_target_xy[0];
                int old_x_frac = (int)(fabsf(g_last_target_xy[0] - old_x_int) * 100);
                int old_y_int = (int)g_last_target_xy[1];
                int old_y_frac = (int)(fabsf(g_last_target_xy[1] - old_y_int) * 100);
                
                HIDO_Debug2("[MC_TGT]WARNING: Target jumped to (0,0)! Previous target=(%d.%02d,%d.%02d) stage=%d\r\n",
                            old_x_int, old_x_frac, old_y_int, old_y_frac, output.stage);
                s_warn_zero_target = HIDO_TRUE;
            }
            else if (!is_near_zero && s_warn_zero_target)
            {
                int new_x_int = (int)output.target_xy[0];
                int new_x_frac = (int)(fabsf(output.target_xy[0] - new_x_int) * 100);
                int new_y_int = (int)output.target_xy[1];
                int new_y_frac = (int)(fabsf(output.target_xy[1] - new_y_int) * 100);
                
                HIDO_Debug2("[MC_TGT]Target recovered from (0,0) to (%d.%02d,%d.%02d)\r\n",
                            new_x_int, new_x_frac, new_y_int, new_y_frac);
                s_warn_zero_target = HIDO_FALSE;
            }
            
            g_last_target_xy[0] = output.target_xy[0];
            g_last_target_xy[1] = output.target_xy[1];
            g_last_target_valid = HIDO_TRUE;
        }
        else
        {
            /* target_valid变为FALSE */
            if (g_last_target_valid)
            {
                HIDO_Debug2("[MC_TGT]target_valid: TRUE->FALSE in task (will report 0,0)\r\n");
            }
            g_last_target_valid = HIDO_FALSE;
        }

        g_freq_sample_count++;