在跑校准了，还是有些问题,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)

@@ -36,6 +39,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;
@@ -123,17 +127,23 @@

        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;
                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)
        {
            g_motion_state.pose_valid = HIDO_FALSE;
            g_last_pose_valid = HIDO_FALSE;
@@ -155,7 +165,87 @@

        /* --- 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)
        {
            /* 使用整数表示法打印浮点数，避免栈损坏 */
            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 - fwd_int) * 100);
            int turn_int = (int)output.turn_rate;
            int turn_frac = (int)(fabsf(output.turn_rate - turn_int) * 100);
            
            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",
                        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_int, turn_frac,
                        g_motion_state.nearest_index);
        }

        /* --- 3) 根据控制量更新 PWM，并把 forward/turn 回传给 Python --- */
        HIDO_UINT16 applied_steering = MC_CFG_PWM_CENTER_US;
@@ -166,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;
@@ -205,13 +299,18 @@
        }

        const HIDO_CHAR *stage_label = MotionControl_StageLabel(output.stage);
        const HIDO_FLOAT *target_ptr = (output.target_valid == HIDO_TRUE) ? output.target_xy : HIDO_NULL;
        PythonLink_ReportControl(output.forward_mps,
                                 output.turn_rate,
                                 g_control_freq_hz,
                                 g_last_steering_pwm,
                                 g_last_throttle_pwm,
                                 stage_label,
                                 g_last_sensor_timestamp_ms);
                                 g_last_sensor_timestamp_ms,
                                 output.pos_enu,
                                 output.heading_deg,
                                 output.target_heading_deg,
                                 target_ptr);
        if ((now - g_last_state_report_ms) >= 1000U)
        {
            g_last_state_report_ms = now;
@@ -248,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)
    {
@@ -292,6 +424,7 @@
    }
}


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