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

yincheng.zhong

2025-12-10 8d0d8dec18cc9170f3fb82a4dba18160dd8e5233

 STM32H743/FML/GPS.c

@@ -206,9 +206,20 @@
    l_stGPRMI.m_fNorthVelocity = vel_n;
    l_stGPRMI.m_fEastVelocity = vel_e;
    l_stGPRMI.m_fUpVelocity = (HIDO_FLOAT)(-vel_d);
    l_stGPRMI.m_fRollAngle = roll;
    l_stGPRMI.m_fPitchAngle = pitch;
    l_stGPRMI.m_fHeadingAngle = heading;
    /* IM23A协议中 roll/pitch/heading 均为弧度，需转换为角度 */
    l_stGPRMI.m_fRollAngle = roll * 57.29577951308232f;     // RAD2DEG
    l_stGPRMI.m_fPitchAngle = pitch * 57.29577951308232f;   // RAD2DEG
    l_stGPRMI.m_fHeadingAngle = heading * 57.29577951308232f;  // RAD2DEG
    
    /* Heading 转换为 0-360 范围 */
    while (l_stGPRMI.m_fHeadingAngle < 0.0f)
    {
        l_stGPRMI.m_fHeadingAngle += 360.0f;
    }
    while (l_stGPRMI.m_fHeadingAngle >= 360.0f)
    {
        l_stGPRMI.m_fHeadingAngle -= 360.0f;
    }
    l_stGPRMI.m_fHorizontalVelStdDev = pos_accuracy;
    l_stGPRMI.m_fAccelBiasX = accel_bias_x;
    l_stGPRMI.m_fAccelBiasY = accel_bias_y;