在跑校准了，还是有些问题,GPS坐标有时候不更新

yincheng.zhong

2025-12-04 f88f3da8f132cd1dd321dfc584a1fe68b6eb2138

 STM32H743/APL/motion_control_task.c

@@ -11,6 +11,7 @@

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

#include "DBG.h"
#include "GPS.h"
@@ -20,6 +21,8 @@
#include "motion_control.h"
#include "motion_path_data.h"
#include "pwm_ctrol.h"
#include "motion_mode.h"
#include "SBUS.h"

#define RAD2DEG                      (57.29577951308232f)

@@ -122,19 +125,23 @@
        HIDO_BOOL imu_valid = (GPS_GetGPIMU(&gpimu) == HIDO_OK);
        HIDO_UINT32 now = HAL_GetTick();

        if (gps_valid == HIDO_TRUE && gprmi.m_u32TimeOfWeek != g_last_gprmi_tow)
        if (gps_valid == HIDO_TRUE)
        {
            /* 新的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;
            g_last_pose_valid = HIDO_TRUE;
            g_last_gps_ms = now;
            g_last_gprmi_tow = gprmi.m_u32TimeOfWeek;
            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;
                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 (gps_valid == HIDO_FALSE && (now - g_last_gps_ms) > 200U)
        {
@@ -158,7 +165,53 @@

        /* --- 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 = SBUS_GetChannel(MOTION_SBUS_AUTO_CHANNEL);
        
        /* 检查 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);
        }
        
        /* 只有当满足所有条件时才执行自动控制：
         * 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)
        {
            MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output);
        }
        else
        {
            /* 手动模式或条件不满足：重置输出并让出控制权 */
            memset(&output, 0, sizeof(MC_Output));
            output.active = HIDO_FALSE;
            
            /* 定期打印状态提示 */
            static HIDO_UINT32 s_status_log = 0U;
            if ((s_status_log++ % 100U) == 0U)
            {
                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);
                }
            }
        }

        static HIDO_UINT32 s_ctrl_log_idx = 0U;
        if ((s_ctrl_log_idx++ % 10U) == 0U)
@@ -203,9 +256,13 @@
        }
        else
        {
            if(ch8 > MOTION_SBUS_AUTO_THRESHOLD_US)
            {
            MotionControl_StopOutputs();
            applied_steering = MC_CFG_PWM_CENTER_US;
            applied_throttle = MC_CFG_PWM_CENTER_US;
            }
            /* 显式回退到手动模式（遥控器直通在 SBUS 模块中处理，或者在此处完全不输出） */
        }
        g_last_steering_pwm = applied_steering;
        g_last_throttle_pwm = applied_throttle;
@@ -290,39 +347,72 @@
    }
}

/* 将控制输出映射为 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;
    
    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  // 直行：应用转向补偿修正机械偏差
    {
        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)
    {
@@ -334,6 +424,7 @@
    }
}


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