校准完成，准备修改一下运动控制算法，转向的时候有直行分量，保存一版

yincheng.zhong

2025-12-05 18f1d1afd16ae159b9f20cef640a594c848ad249

 STM32H743/APL/motion_control_task.c

@@ -23,6 +23,7 @@
#include "pwm_ctrol.h"
#include "motion_mode.h"
#include "SBUS.h"
#include "AppConfig.h"

#define RAD2DEG                      (57.29577951308232f)

@@ -178,12 +179,56 @@
            gps_ready = (init_ok == HIDO_TRUE) && (gnss_connected == HIDO_TRUE);
        }
        
#if HITL_SIMULATION
        /* 硬件在环仿真模式：
         * 1. 强制 SBUS 信号有效
         * 2. 模拟 CH8 通道：上电前 10 秒为 1000 (手动)，10 秒后变为 1800 (自动)
         *    这样可以测试从手动切换到自动的逻辑 (状态机复位)
         */
        sbus_valid = HIDO_TRUE;
        
        /* 仿真模式下，只要收到过一次有效数据，就认为 GPS 就绪，避免因为标志位未置位导致不启动 */
        if (gps_valid == HIDO_TRUE)
        {
            gps_ready = HIDO_TRUE; 
        }

        static HIDO_UINT16 g_hitl_ch8 = 1000; 
        static HIDO_UINT32 s_hitl_start_ms = 0;
        if (s_hitl_start_ms == 0) 
        {
            s_hitl_start_ms = HAL_GetTick();
        }
        
        if (HAL_GetTick() - s_hitl_start_ms > 1000U) 
        {
            g_hitl_ch8 = 1800; // 10秒后自动切入自动模式
        }
        ch8 = g_hitl_ch8;
#endif
        
        /* 只有当满足所有条件时才执行自动控制：
        
        /* 只有当满足所有条件时才执行自动控制：
         * 1. SBUS 信号有效
         * 2. CH8 > 阈值（自动模式开关）
         * 3. GPS 初始化完成（FINIT_OK）
         * 4. GPS 连接到 GNSS（GNSS_CONNECT）*/
        if (sbus_valid == HIDO_TRUE && ch8 > MOTION_SBUS_AUTO_THRESHOLD_US && gps_ready == HIDO_TRUE)
        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);

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

        if (current_auto_condition == HIDO_TRUE)
        {
            MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output);
        }
@@ -193,6 +238,9 @@
            memset(&output, 0, sizeof(MC_Output));
            output.active = HIDO_FALSE;
            
            /* 移除这里的强制回中，改为在下方根据具体原因处理 */
            // MotionControl_StopOutputs();
            
            /* 定期打印状态提示 */
            static HIDO_UINT32 s_status_log = 0U;
            if ((s_status_log++ % 100U) == 0U)
@@ -256,13 +304,48 @@
        }
        else
        {
            if(ch8 > MOTION_SBUS_AUTO_THRESHOLD_US)
             /* 非自动模式下，MotionControl_StopOutputs() 已在上面调用，这里不再重复设置
              * 如果需要手动模式直通，应该在 SBUS 模块处理，或者在这里读取 SBUS 手动通道值并输出
              * 目前逻辑是：非自动模式下，运动控制任务输出回中（安全停止），手动控制由 SBUS 回调或其他机制接管
              * 如果系统设计是 MotionControl 任务在手动模式下也负责透传遥控器信号，则需要修改此处。
              * 假设：手动模式下，SBUS 模块的中断回调会直接控制电机，或者有其他任务处理。
              * 为防止冲突，这里仅保持记录。但上面已经调用 StopOutputs 设置了 PWM 为中值。
              * 如果 SBUS 直接控制电机，这里的 StopOutputs 会与 SBUS 冲突吗？
              * 通常做法：自动模式下本任务控制 PWM；手动模式下本任务不控制 PWM（或者输出无效值）。
              * 但 StopOutputs 显式设置了 1500。如果 SBUS 也是设置 PWM，会有竞争。
              * 
              * 修正：如果 ch8 < 1500 (手动模式)，本任务不应该 Set_Motor_Pulse(1500)，
              * 否则会覆盖 SBUS 的手动控制信号。
              * 
              * 修改逻辑：
              * 1. 自动模式 (current_auto_condition == TRUE): ApplyOutput -> Set_Pulse
              * 2. 异常停止 (sbus_valid=FALSE 或 gps_ready=FALSE): StopOutputs -> Set_Pulse(1500)
              * 3. 手动模式 (sbus_valid=TRUE && ch8 < 1500): 不操作 PWM，让权给手动控制逻辑
              */
             
            if (sbus_valid == HIDO_FALSE)
            {
                 /* 遥控器信号丢失：强制停车 */
            MotionControl_StopOutputs();
            applied_steering = MC_CFG_PWM_CENTER_US;
            applied_throttle = MC_CFG_PWM_CENTER_US;
            }
            /* 显式回退到手动模式（遥控器直通在 SBUS 模块中处理，或者在此处完全不输出） */
            else if (ch8 <= MOTION_SBUS_AUTO_THRESHOLD_US)
            {
                 /* 手动模式：不输出 PWM，仅记录（假设 SBUS 模块或其他逻辑在控制电机）
                  * 为了回显正确，这里赋值为 1500 或读取当前 PWM（如果能读到）
                  */
                 applied_steering = MC_CFG_PWM_CENTER_US; 
                 applied_throttle = MC_CFG_PWM_CENTER_US;
                 /* 注意：不要调用 MotionControl_StopOutputs()，否则会在此处把手动油门强行归零 */
            }
            else if (gps_ready == HIDO_FALSE)
            {
                 /* 自动模式开关打开，但 GPS 未就绪：安全停车 */
                 MotionControl_StopOutputs();
                 applied_steering = MC_CFG_PWM_CENTER_US;
                 applied_throttle = MC_CFG_PWM_CENTER_US;
            }
        }
        g_last_steering_pwm = applied_steering;
        g_last_throttle_pwm = applied_throttle;