OTA升级功能调通，准备增加boot的代码

yincheng.zhong

2 天以前 567085ead3f6adaabd884f16ab4b17c62e8f0403

 STM32H743/APL/motion_control_task.c

@@ -11,6 +11,9 @@

#include "stm32h7xx_hal.h"
#include <string.h>
#include <math.h>

#define ENABLE_MC_CTRL_LOG 1

#include "DBG.h"
#include "GPS.h"
@@ -20,6 +23,9 @@
#include "motion_control.h"
#include "motion_path_data.h"
#include "pwm_ctrol.h"
#include "motion_mode.h"
#include "SBUS.h"
#include "AppConfig.h"

#define RAD2DEG                      (57.29577951308232f)

@@ -36,6 +42,7 @@
static ST_GeoOrigin g_motion_origin;
static HIDO_UINT32 g_last_gps_ms = 0U;
static HIDO_UINT32 g_last_imu_ms = 0U;
static HIDO_UINT32 g_last_gprmi_tow = 0U;
static HIDO_UINT32 g_last_control_report_ms = 0U;
static HIDO_UINT32 g_last_pose_report_ms = 0U;
static HIDO_UINT32 g_last_state_report_ms = 0U;
@@ -78,6 +85,11 @@
            &g_motion_config,
            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,
@@ -123,18 +135,36 @@

        if (gps_valid == HIDO_TRUE)
        {
            Geo_GprmiToENU(&gprmi, &g_motion_origin, enu);
            MC_UpdateGps(&g_motion_state, enu, &gprmi);
            memcpy(g_last_enu, enu, sizeof(enu));
            g_last_heading_deg = gprmi.m_fHeadingAngle;
            g_last_pitch_deg = gprmi.m_fPitchAngle;
            g_last_roll_deg = gprmi.m_fRollAngle;
            g_last_pose_valid = HIDO_TRUE;
            g_last_gps_ms = now;
            g_last_sensor_timestamp_ms = gprmi.m_u32TimeOfWeek;
            HIDO_UINT32 gps_timestamp = (gprmi.m_u32UTCTime != 0U) ? gprmi.m_u32UTCTime : (HIDO_UINT32)now;
            if ((gprmi.m_u32UTCTime == 0U) || (gps_timestamp != g_last_gprmi_tow))
            {
                /* 新的GPS数据到来（时间戳变化），才更新航向 */
                Geo_GprmiToENU(&gprmi, &g_motion_origin, enu);
                MC_UpdateGps(&g_motion_state, enu, &gprmi);
                memcpy(g_last_enu, enu, sizeof(enu));
                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;
                g_last_sensor_timestamp_ms = gps_timestamp;
            }
        }
        else if ((now - g_last_gps_ms) > 200U)
        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;
        }
@@ -155,7 +185,293 @@

        /* --- 2) 调用运动控制器（固定 75 Hz dt） --- */
        MC_Output output;
        MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output);
        HIDO_BOOL sbus_valid = (SBUS_IsSignalValid(MOTION_SBUS_TIMEOUT_MS) == HIDO_TRUE);
        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;
        if (gps_valid == HIDO_TRUE)
        {
            HIDO_UINT32 status = gprmi.m_u32StatusFlags;
            HIDO_BOOL init_ok = ((status & IM23A_STATUS_READY) != 0U);
            HIDO_BOOL gnss_connected = ((status & IM23A_STATUS_GNSS_CONNECT) != 0U);
            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）*/
        static HIDO_BOOL s_last_auto_condition = HIDO_FALSE;
        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)
        {
            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
        {
            /* 手动模式或条件不满足：重置输出并让出控制权 */
            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)
            {
#if ENABLE_MC_CTRL_LOG
                if (sbus_valid == HIDO_FALSE)
                {
                    HIDO_Debug2("[MC_CTRL] Waiting: SBUS signal lost\r\n");
                }
                else if (ch8 <= MOTION_SBUS_AUTO_THRESHOLD_US)
                {
                    HIDO_Debug2("[MC_CTRL] Waiting: CH8=%u (manual mode)\r\n", ch8);
                }
                else if (gps_ready == HIDO_FALSE)
                {
                    HIDO_UINT32 status = gps_valid ? gprmi.m_u32StatusFlags : 0U;
                    HIDO_Debug2("[MC_CTRL] Waiting: GPS not ready (status=0x%08X)\r\n", status);
                }
#endif
            }
        }

        static HIDO_UINT32 s_ctrl_log_idx = 0U;
        if ((s_ctrl_log_idx++ % 10U) == 0U)
        {
#if ENABLE_MC_CTRL_LOG
            /* 使用整数表示法打印浮点数，避免栈损坏 */
            int pos_x_int = (int)g_motion_state.pos[0];
            int pos_x_frac = (int)(fabsf(g_motion_state.pos[0] - pos_x_int) * 100);
            int pos_y_int = (int)g_motion_state.pos[1];
            int pos_y_frac = (int)(fabsf(g_motion_state.pos[1] - pos_y_int) * 100);
            int pos_z_int = (int)g_motion_state.pos[2];
            int pos_z_frac = (int)(fabsf(g_motion_state.pos[2] - pos_z_int) * 100);
            int hdg_int = (int)g_motion_state.heading_deg;
            int hdg_frac = (int)(fabsf(g_motion_state.heading_deg - hdg_int) * 100);
            int tgt_x_int = (int)g_motion_state.current_target_xy[0];
            int tgt_x_frac = (int)(fabsf(g_motion_state.current_target_xy[0] - tgt_x_int) * 100);
            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 - (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=%c%d.%02d path_idx=%u\n",
                        g_motion_state.stage,
                        pos_x_int, pos_x_frac,
                        pos_y_int, pos_y_frac,
                        pos_z_int, pos_z_frac,
                        hdg_int, hdg_frac,
                        tgt_x_int, tgt_x_frac,
                        tgt_y_int, tgt_y_frac,
                        fwd_int, fwd_frac,
                        turn_sign, turn_int, turn_frac,
                        g_motion_state.nearest_index);
#endif
        }

        /* --- 3) 根据控制量更新 PWM，并把 forward/turn 回传给 Python --- */
        HIDO_UINT16 applied_steering = MC_CFG_PWM_CENTER_US;
@@ -166,9 +482,55 @@
        }
        else
        {
            MotionControl_StopOutputs();
            applied_steering = MC_CFG_PWM_CENTER_US;
            applied_throttle = MC_CFG_PWM_CENTER_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;
            }
            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;
            }
            else if (output.stage == MC_STAGE_FINISHED)
            {
                 /* 已到达终点：停止输出 */
                 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;
@@ -178,11 +540,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++;
@@ -253,39 +654,74 @@
    }
}

/* 将控制输出映射为 PWM，并发回 Python 端 */
/* 将控制输出映射为 PWM（使用校准模型反解）*/
static void MotionControl_ApplyOutput(const MC_Output *output,
                                      HIDO_UINT16 *_pu16Steering,
                                      HIDO_UINT16 *_pu16Throttle)
{
    float forward_ratio = output->forward_mps / g_motion_config.max_forward_mps;
    float turn_ratio = output->turn_rate / g_motion_config.max_turn_rate;
    forward_ratio = MC_CLAMP(forward_ratio, -1.0f, 1.0f);
    turn_ratio = MC_CLAMP(turn_ratio, -1.0f, 1.0f);

    int32_t throttle = (int32_t)MC_CFG_PWM_CENTER_US + (int32_t)(forward_ratio * (float)MC_CFG_PWM_SPAN_US);
    int32_t steering = (int32_t)MC_CFG_PWM_CENTER_US + (int32_t)(turn_ratio * (float)MC_CFG_PWM_SPAN_US);

    if (throttle < 1000)
    /* ========== 前向速度 → 油门PWM（使用校准模型反解）========== */
    /* 校准模型：v = MC_CFG_FORWARD_K × (1500 - pwm) + MC_CFG_FORWARD_BIAS
     * 反解：pwm = 1500 - (v - MC_CFG_FORWARD_BIAS) / MC_CFG_FORWARD_K
     * 说明：1000=最大前进，1500=停止，2000=最大后退
     */
    float target_velocity = output->forward_mps;
    float throttle_f = 1500.0f - (target_velocity - MC_CFG_FORWARD_BIAS) / MC_CFG_FORWARD_K;
    
    /* 限制PWM范围 */
    if (throttle_f < 1000.0f) 
    {
        throttle = 1000;
        throttle_f = 1000.0f;
    }
    else if (throttle > 2000)
    else if (throttle_f > 2000.0f) 
    {
        throttle = 2000;
        throttle_f = 2000.0f;
    }

    if (steering < 1000)
    
    /* 死区处理：接近1500时直接设为1500 */
    if (fabsf(throttle_f - 1500.0f) < (float)MC_CFG_FORWARD_DEADZONE_PWM)
    {
        steering = 1000;
        throttle_f = 1500.0f;
    }
    else if (steering > 2000)
    
    /* ========== 转向角速度 → 转向PWM（使用校准模型反解）========== */
    /* 校准模型：ω_left  = MC_CFG_STEERING_K_LEFT  × (1500 - pwm)  当 pwm < 1500 (左转)
     *          ω_right = -MC_CFG_STEERING_K_RIGHT × (pwm - 1500)  当 pwm > 1500 (右转)
     * 反解：pwm_left  = 1500 - ω / MC_CFG_STEERING_K_LEFT
     *      pwm_right = 1500 - ω / MC_CFG_STEERING_K_RIGHT
     * 说明：1000=最大左转，1500=直行，2000=最大右转
     */
    float target_yaw_rate = output->turn_rate;  // rad/s，正值=左转，负值=右转
    float steering_f = 1500.0f;
    
    /* 调试LOG：打印转向映射过程（低频） */
    
    if (target_yaw_rate > 0.001f)  // 左转（正角速度）
    {
        steering = 2000;
        steering_f = 1500.0f - target_yaw_rate / MC_CFG_STEERING_K_LEFT;
        if (steering_f < 1000.0f) 
        {
            steering_f = 1000.0f;
        }
    }

    Set_Motor_Pulse((uint32_t)throttle);
    Set_Steering_Pulse((uint32_t)steering);
    else if (target_yaw_rate < -0.001f)  // 右转（负角速度）
    {
        steering_f = 1500.0f - target_yaw_rate / MC_CFG_STEERING_K_RIGHT;
        if (steering_f > 2000.0f) 
        {
            steering_f = 2000.0f;
        }
    }
    else  // 直行：应用转向补偿修正机械偏差（死区 ±0.001 rad/s）
    {
        steering_f = 1500.0f + (float)MC_CFG_STEERING_TRIM;
    }
    
    /* 转换为整数并输出 */
    uint32_t throttle = (uint32_t)(throttle_f + 0.5f);
    uint32_t steering = (uint32_t)(steering_f + 0.5f);
    
    Set_Motor_Pulse(throttle);
    Set_Steering_Pulse(steering);

    if (_pu16Steering != NULL)
    {
@@ -297,6 +733,7 @@
    }
}


/* 失效保护：回中 PWM 并上报零控制量 */
static void MotionControl_StopOutputs(void)
{