解决了必须坐标点重复才能稳定跟踪的问题，原因是算法里面适配之前坐标点重复的BUG，写了前视跳点和路径跳点。改成不跳点就好了。

yincheng.zhong

2025-12-11 26eb4faf2fb7b37f33a204d6a35814c725221e68

 STM32H743/FML/motion_control.c

@@ -141,7 +141,7 @@
    float dist = mc_distance(pos, point);
    
    /* 定义"到达"阈值为0.5米 */
    const float REACH_THRESHOLD_M = 0.5f;
    const float REACH_THRESHOLD_M = 0.1f;
    
    return (dist < REACH_THRESHOLD_M) ? HIDO_TRUE : HIDO_FALSE;
}
@@ -180,7 +180,7 @@
    HIDO_UINT32 next_idx = start_idx + 1U;
    
    /* 简化逻辑：直接返回下一个点，确保按顺序访问每个航点 */
    return next_idx;
    return start_idx;
}

/* 计算带符号横向误差（左正右负） */
@@ -191,8 +191,17 @@
        return 0.0f;
    }

    HIDO_UINT32 idx0 = (nearest_idx == 0U) ? 0U : nearest_idx - 1U;
    HIDO_UINT32 idx1 = (nearest_idx + 1U < state->path_count) ? nearest_idx + 1U : nearest_idx;
    HIDO_UINT32 idx0, idx1;
    if (nearest_idx == 0U)
    {
        idx0 = 0U;
        idx1 = (state->path_count > 1U) ? 1U : 0U;
    }
    else
    {
        idx0 = nearest_idx - 1U;
        idx1 = nearest_idx;
    }

    float p0[2];
    float p1[2];
@@ -240,24 +249,8 @@
    float start_point[2];
    mc_get_point(state, 0U, start_point);
    
    /* 检测目标点变化并记录 */
    /* 检测目标点变化并记录 - 已移除LOG */
    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];
@@ -336,47 +329,17 @@
    
    /* 调试：每20帧输出一次路径跟踪状态 */
    static HIDO_UINT32 s_path_log_idx = 0U;
    static HIDO_UINT32 s_last_nearest = 0U;
    static HIDO_UINT32 s_last_lookahead = 0U;
    static HIDO_UINT32 s_last_nearest = 0xFFFFFFFF;
    static HIDO_UINT32 s_last_lookahead = 0xFFFFFFFF;
    
    /* 如果索引发生变化，立即输出LOG */
    /* 路径点调试LOG - 已移除 */
//    static HIDO_UINT32 s_last_nearest = 0xFFFFFFFF;
//    static HIDO_UINT32 s_last_lookahead = 0xFFFFFFFF;
    HIDO_BOOL index_changed = (nearest_idx != s_last_nearest) || (lookahead_idx != s_last_lookahead);
    
    if ((s_path_log_idx++ % 20U) == 0U || index_changed == HIDO_TRUE)
    if (index_changed)
    {
        float nearest_pt[2];
        mc_get_point(state, nearest_idx, nearest_pt);
        float dist_to_nearest = mc_distance(nearest_pt, state->pos);
        HIDO_BOOL reached = mc_is_point_reached(state, nearest_idx);
        
        /* 使用整数表示法打印浮点数 */
        int pos_x_int = (int)state->pos[0];
        int pos_x_frac = (int)(fabsf(state->pos[0] - pos_x_int) * 100);
        int pos_y_int = (int)state->pos[1];
        int pos_y_frac = (int)(fabsf(state->pos[1] - pos_y_int) * 100);
        int near_x_int = (int)nearest_pt[0];
        int near_x_frac = (int)(fabsf(nearest_pt[0] - near_x_int) * 100);
        int near_y_int = (int)nearest_pt[1];
        int near_y_frac = (int)(fabsf(nearest_pt[1] - near_y_int) * 100);
        int dn_int = (int)dist_to_nearest;
        int dn_frac = (int)((dist_to_nearest - dn_int) * 100);
        
        if (index_changed == HIDO_TRUE)
        {
            HIDO_Debug2("[MC_PATH]CHG pos=(%d.%02d,%d.%02d) near=%u->%u look=%u->%u dn=%d.%02d reach=%d\r\n",
                        pos_x_int, pos_x_frac, pos_y_int, pos_y_frac,
                        s_last_nearest, nearest_idx,
                        s_last_lookahead, lookahead_idx,
                        dn_int, dn_frac, reached);
        }
        else
        {
            HIDO_Debug2("[MC_PATH] near=%u(%d.%02d,%d.%02d) look=%u dn=%d.%02d reach=%d\r\n",
                        nearest_idx, near_x_int, near_x_frac, near_y_int, near_y_frac,
                        lookahead_idx,
                        dn_int, dn_frac, reached);
        }
        
        s_last_nearest = nearest_idx;
        s_last_lookahead = lookahead_idx;
    }
@@ -384,26 +347,7 @@
    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);
    }
    /* 检测目标点变化并记录 - 已移除LOG */
    
    state->current_target_xy[0] = target[0];
    state->current_target_xy[1] = target[1];
@@ -421,11 +365,15 @@
    state->last_heading_err = heading_err;

    float cross_track = mc_cross_track_error(state, nearest_idx);
    float yaw_rate_cmd = state->config.heading_kp * heading_err
                       + state->config.heading_kd * heading_err_rate
                       + state->config.xtrack_kp * cross_track;
    float p_term = state->config.heading_kp * heading_err;
    float d_term = state->config.heading_kd * heading_err_rate;
    float x_term = state->config.xtrack_kp * cross_track;
    
    float yaw_rate_cmd = p_term + d_term + x_term;
    yaw_rate_cmd = MC_CLAMP(yaw_rate_cmd, -state->config.max_turn_rate, state->config.max_turn_rate);
    
    /* 调试LOG：每10帧打印一次PID控制详情，帮助分析死区/不转向问题 - 已移除 */
    
    float forward = state->config.base_speed_mps
                  - state->config.heading_speed_scale * fabsf(heading_err)
                  - state->config.xtrack_speed_scale * fabsf(cross_track);
@@ -474,13 +422,11 @@
    _cfg->xtrack_speed_scale = MC_CFG_XTRACK_SPEED_SCALE;
}

/* 初始化控制器，绑定路径与配置 */
/* MC_Init */
HIDO_VOID MC_Init(MC_State *_state, const MC_Config *_cfg, const float *_path_xy, HIDO_UINT32 _point_count)
{
    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;
    }

@@ -491,9 +437,6 @@
    _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 位姿及航向/速度 */
@@ -565,8 +508,6 @@
    {
        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;
@@ -577,7 +518,6 @@
    {
        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;
@@ -640,12 +580,6 @@
        
        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
@@ -656,8 +590,6 @@
        
        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);
        }
    }