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

yincheng.zhong

2025-12-10 8d0d8dec18cc9170f3fb82a4dba18160dd8e5233

 STM32H743/FML/motion_control.c

@@ -67,6 +67,14 @@
/* 读取路径点，索引越界时截到末尾 */
static void mc_get_point(const MC_State *state, HIDO_UINT32 idx, float outPt[2])
{
    /* 防御性检查：如果路径数据未初始化，返回原点 */
    if (state->path_xy == HIDO_NULL || state->path_count == 0U)
    {
        outPt[0] = 0.0f;
        outPt[1] = 0.0f;
        return;
    }
    
    if (idx >= state->path_count)
    {
        idx = state->path_count - 1U;
@@ -231,6 +239,26 @@
{
    float start_point[2];
    mc_get_point(state, 0U, start_point);
    
    /* 检测目标点变化并记录 */
    static HIDO_UINT32 s_goto_log_idx = 0U;
    HIDO_BOOL target_changed = (fabsf(state->current_target_xy[0] - start_point[0]) > 0.01f) ||
                                (fabsf(state->current_target_xy[1] - start_point[1]) > 0.01f);
    
    if (target_changed || (s_goto_log_idx++ % 50U) == 0U)
    {
        int tgt_x_int = (int)start_point[0];
        int tgt_x_frac = (int)(fabsf(start_point[0] - tgt_x_int) * 100);
        int tgt_y_int = (int)start_point[1];
        int tgt_y_frac = (int)(fabsf(start_point[1] - tgt_y_int) * 100);
        
        if (target_changed)
        {
            HIDO_Debug2("[MC_TGT]GOTO_START target changed: (%d.%02d,%d.%02d)\r\n",
                        tgt_x_int, tgt_x_frac, tgt_y_int, tgt_y_frac);
        }
    }
    
    state->current_target_xy[0] = start_point[0];
    state->current_target_xy[1] = start_point[1];
    out->target_valid = HIDO_TRUE;
@@ -260,7 +288,9 @@
    }
    else if (fabsf(heading_err) < state->config.start_heading_tolerance_rad)
    {
        /* 到达起点且航向对准，切换到路径跟踪模式 */
        state->stage = MC_STAGE_FOLLOW_PATH;
        state->nearest_index = 0U;  /* 重置为路径第一个点 */
    }

    out->forward_mps = forward;
@@ -353,6 +383,28 @@

    float target[2];
    mc_get_point(state, lookahead_idx, target);
    
    /* 检测目标点变化并记录 */
    HIDO_BOOL target_changed = (fabsf(state->current_target_xy[0] - target[0]) > 0.01f) ||
                                (fabsf(state->current_target_xy[1] - target[1]) > 0.01f);
    
    if (target_changed)
    {
        int old_x_int = (int)state->current_target_xy[0];
        int old_x_frac = (int)(fabsf(state->current_target_xy[0] - old_x_int) * 100);
        int old_y_int = (int)state->current_target_xy[1];
        int old_y_frac = (int)(fabsf(state->current_target_xy[1] - old_y_int) * 100);
        int new_x_int = (int)target[0];
        int new_x_frac = (int)(fabsf(target[0] - new_x_int) * 100);
        int new_y_int = (int)target[1];
        int new_y_frac = (int)(fabsf(target[1] - new_y_int) * 100);
        
        HIDO_Debug2("[MC_TGT]FOLLOW target changed: (%d.%02d,%d.%02d)->(%d.%02d,%d.%02d) near=%u look=%u\r\n",
                    old_x_int, old_x_frac, old_y_int, old_y_frac,
                    new_x_int, new_x_frac, new_y_int, new_y_frac,
                    nearest_idx, lookahead_idx);
    }
    
    state->current_target_xy[0] = target[0];
    state->current_target_xy[1] = target[1];
    out->target_valid = HIDO_TRUE;
@@ -427,6 +479,8 @@
{
    if (_state == HIDO_NULL || _cfg == HIDO_NULL || _path_xy == HIDO_NULL || _point_count < 2U)
    {
        DBG_Printf("[MC_Init] ERROR: Invalid parameters (state=%p, cfg=%p, path=%p, count=%u)\r\n",
                   (void*)_state, (void*)_cfg, (void*)_path_xy, _point_count);
        return;
    }

@@ -437,6 +491,9 @@
    _state->stage = MC_STAGE_GOTO_START;
    _state->nearest_index = 0U;
    _state->lookahead_index = 0U;
    
    DBG_Printf("[MC_Init] OK: path_count=%u, first_point=(%.2f,%.2f)\r\n",
               _point_count, _path_xy[0], _path_xy[1]);
}

/* 注入最新 ENU 位姿及航向/速度 */
@@ -500,15 +557,32 @@
    _out->target_xy[0] = 0.0f;
    _out->target_xy[1] = 0.0f;

    /* 记录target_valid变为FALSE的原因 */
    static HIDO_BOOL s_last_path_valid = HIDO_TRUE;
    static HIDO_BOOL s_last_pose_valid = HIDO_TRUE;
    
    if (_state->path_xy == HIDO_NULL || _state->path_count < 2U)
    {
        if (s_last_path_valid)
        {
            HIDO_Debug2("[MC_TGT]WARNING: target_valid=FALSE, path invalid (path=%p, count=%u)\r\n",
                        (void*)_state->path_xy, _state->path_count);
            s_last_path_valid = HIDO_FALSE;
        }
        return;
    }
    s_last_path_valid = HIDO_TRUE;

    if (_state->pose_valid == HIDO_FALSE)
    {
        if (s_last_pose_valid)
        {
            HIDO_Debug2("[MC_TGT]WARNING: target_valid=FALSE, pose_valid=FALSE\r\n");
            s_last_pose_valid = HIDO_FALSE;
        }
        return;
    }
    s_last_pose_valid = HIDO_TRUE;

    if (_state->imu_valid == HIDO_TRUE && _dt_s > 0.0f)
    {
@@ -545,7 +619,9 @@
        break;
    }

    _out->turn_rate = -_out->turn_rate;
    /* 不再取反：yaw_rate_cmd（数学坐标系，正=CCW）直接作为turn_rate输出
     * motion_control_task.c中，正值=左转(逆时针)，负值=右转(顺时针)，与数学坐标系一致 */
    // _out->turn_rate = -_out->turn_rate;  // 已注释：此取反导致方向错误
    _out->stage = _state->stage;
    _out->pos_enu[0] = _state->pos[0];
    _out->pos_enu[1] = _state->pos[1];
@@ -553,17 +629,39 @@
    _out->heading_deg = _state->heading_deg;
    _out->pitch_deg = _state->pitch_deg;
    _out->roll_deg = _state->roll_deg;
    static HIDO_BOOL s_last_target_valid = HIDO_FALSE;
    static E_MCStage s_last_stage_for_target = MC_STAGE_IDLE;
    
    if (_state->stage == MC_STAGE_FOLLOW_PATH || _state->stage == MC_STAGE_GOTO_START)
    {
        _out->target_valid = HIDO_TRUE;
        _out->target_xy[0] = _state->current_target_xy[0];
        _out->target_xy[1] = _state->current_target_xy[1];
        
        if (!s_last_target_valid)
        {
            int x_int = (int)_out->target_xy[0];
            int x_frac = (int)(fabsf(_out->target_xy[0] - x_int) * 100);
            int y_int = (int)_out->target_xy[1];
            int y_frac = (int)(fabsf(_out->target_xy[1] - y_int) * 100);
            HIDO_Debug2("[MC_TGT]target_valid: FALSE->TRUE, stage=%d, target=(%d.%02d,%d.%02d)\r\n",
                        _state->stage, x_int, x_frac, y_int, y_frac);
        }
    }
    else
    {
        _out->target_valid = HIDO_FALSE;
        _out->target_xy[0] = 0.0f;
        _out->target_xy[1] = 0.0f;
        
        if (s_last_target_valid || _state->stage != s_last_stage_for_target)
        {
            HIDO_Debug2("[MC_TGT]target_valid: TRUE->FALSE, stage=%d->%d (will output 0,0)\r\n",
                        s_last_stage_for_target, _state->stage);
        }
    }
    
    s_last_target_valid = _out->target_valid;
    s_last_stage_for_target = _state->stage;
}