仿真跑起来了，但是起点转向不太对。

yincheng.zhong

2025-11-24 346cc7d685283df529aadbcf9c156de040ce44f9

 STM32H743/FML/motion_control.c

@@ -176,6 +176,7 @@
    float dist = mc_distance(start_point, state->pos);

    float desired_heading = arm_atan2_f32(vec_y, vec_x);
    out->target_heading_deg = desired_heading * RAD2DEG;
    float heading_err = mc_wrap_angle(desired_heading - state->heading_rad);
    float yaw_rate_cmd = state->config.heading_kp * heading_err;
    yaw_rate_cmd = MC_CLAMP(yaw_rate_cmd, -state->config.max_turn_rate, state->config.max_turn_rate);
@@ -227,6 +228,7 @@
    float vec_x = target[0] - state->pos[0];
    float vec_y = target[1] - state->pos[1];
    float desired_heading = arm_atan2_f32(vec_y, vec_x);
    out->target_heading_deg = desired_heading * RAD2DEG;
    float heading_err = mc_wrap_angle(desired_heading - state->heading_rad);
    float heading_err_rate = (heading_err - state->last_heading_err) / (dt_s > 0.0f ? dt_s : 0.013f);
    state->last_heading_err = heading_err;
@@ -313,8 +315,17 @@
    _state->pos[0] = _enu[0];
    _state->pos[1] = _enu[1];
    _state->pos[2] = _enu[2];
    _state->heading_rad = mc_wrap_angle((_gprmi->m_fHeadingAngle) * DEG2RAD);
    _state->heading_deg = _gprmi->m_fHeadingAngle;
    float gps_heading_rad = mc_wrap_angle((_gprmi->m_fHeadingAngle) * DEG2RAD);
    if (_state->imu_valid == HIDO_TRUE)
    {
        float heading_err = mc_wrap_angle(gps_heading_rad - _state->heading_rad);
        _state->heading_rad = mc_wrap_angle(_state->heading_rad + heading_err * MC_CFG_GPS_HEADING_BLEND);
    }
    else
    {
        _state->heading_rad = gps_heading_rad;
    }
    _state->heading_deg = _state->heading_rad * RAD2DEG;
    _state->pitch_deg = _gprmi->m_fPitchAngle;
    _state->roll_deg = _gprmi->m_fRollAngle;
    _state->vel[0] = _gprmi->m_fEastVelocity;
@@ -364,11 +375,19 @@
        return;
    }

    if (_state->imu_valid == HIDO_TRUE && _dt_s > 0.0f)
    {
        _state->heading_rad = mc_wrap_angle(_state->heading_rad + _state->yaw_rate_rad * _dt_s);
        _state->heading_deg = _state->heading_rad * RAD2DEG;
    }

    if (_state->stage == MC_STAGE_IDLE)
    {
        _state->stage = MC_STAGE_GOTO_START;
    }

    _out->target_heading_deg = _state->heading_deg;

    switch (_state->stage)
    {
    case MC_STAGE_GOTO_START:
@@ -379,6 +398,7 @@
        break;
    case MC_STAGE_FINISHED:
        _out->active = HIDO_FALSE;
        _out->target_heading_deg = _state->heading_deg;
        break;
    default:
        break;