路径点不重复的控制算法测试通了，但是好像不如之前路径点重复的版本好。增加了蓝牙接口部分。准备移植外包的MQTT。

yincheng.zhong

10 天以前 4adc78553c8d48ff122506195fa33641134bd7b1

 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;
@@ -172,7 +180,7 @@
    HIDO_UINT32 next_idx = start_idx + 1U;
    
    /* 简化逻辑：直接返回下一个点，确保按顺序访问每个航点 */
    return next_idx;
    return start_idx;
}

/* 计算带符号横向误差（左正右负） */
@@ -183,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];
@@ -231,6 +248,10 @@
{
    float start_point[2];
    mc_get_point(state, 0U, start_point);
    
    /* 检测目标点变化并记录 - 已移除LOG */
    static HIDO_UINT32 s_goto_log_idx = 0U;
    
    state->current_target_xy[0] = start_point[0];
    state->current_target_xy[1] = start_point[1];
    out->target_valid = HIDO_TRUE;
@@ -260,7 +281,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;
@@ -306,53 +329,26 @@
    
    /* 调试：每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;
    }

    float target[2];
    mc_get_point(state, lookahead_idx, target);
    
    /* 检测目标点变化并记录 - 已移除LOG */
    
    state->current_target_xy[0] = target[0];
    state->current_target_xy[1] = target[1];
    out->target_valid = HIDO_TRUE;
@@ -369,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);
@@ -422,7 +422,7 @@
    _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)
@@ -500,15 +500,29 @@
    _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)
        {
            s_last_path_valid = HIDO_FALSE;
        }
        return;
    }
    s_last_path_valid = HIDO_TRUE;

    if (_state->pose_valid == HIDO_FALSE)
    {
        if (s_last_pose_valid)
        {
            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 +559,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 +569,31 @@
    _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)
        {
        }
    }
    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)
        {
        }
    }
    
    s_last_target_valid = _out->target_valid;
    s_last_stage_for_target = _state->stage;
}