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

yincheng.zhong

2025-12-10 8d0d8dec18cc9170f3fb82a4dba18160dd8e5233

 STM32H743/FML/motion_control.c

@@ -20,6 +20,8 @@
#define DEG2RAD                (0.017453292519943295769236907684886f)
#define RAD2DEG                (57.295779513082320876798154814105f)
#define MC_CLAMP(v, lo, hi)    ((v) < (lo) ? (lo) : (((v) > (hi)) ? (hi) : (v)))
/* 当航向误差逼近 ±π 时的判决窗口（约 0.5°） */
#define MC_HEADING_PI_EPS      (0.00872664626f)

/* 将角度约束到 [-pi, pi]，避免航向跳变 */
static float mc_wrap_angle(float angle)
@@ -35,9 +37,44 @@
    return angle;
}

/* 在误差接近 ±π 时保持上一周期的转向方向，避免来回反转 */
static float mc_heading_error_with_hysteresis(MC_State *state, float desired_heading)
{
    float err = mc_wrap_angle(desired_heading - state->heading_rad);
    float abs_err = fabsf(err);
    float pi_delta = (float)M_PI - abs_err;

    if (pi_delta <= MC_HEADING_PI_EPS)
    {
        float preferred_sign = state->last_turn_sign;
        if (preferred_sign == 0.0f)
        {
            preferred_sign = (err >= 0.0f) ? 1.0f : -1.0f;
        }
        float old_err = err;
        err = preferred_sign * abs_err;
        state->last_turn_sign = preferred_sign;
        
    }
    else if (abs_err > 1.0e-6f)
    {
        state->last_turn_sign = (err > 0.0f) ? 1.0f : -1.0f;
    }

    return err;
}

/* 读取路径点，索引越界时截到末尾 */
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;
@@ -66,48 +103,84 @@
    return dist;
}

/* 搜索当前位置最近的路径点 */
static HIDO_UINT32 mc_find_nearest(const MC_State *state)
/* 数学坐标（东为 0°，逆时针为正）→ 罗盘坐标（北为 0°，顺时针为正） */
static float mc_math_rad_to_compass_deg(float heading_rad)
{
    float best_dist = 1.0e9f;
    HIDO_UINT32 best_idx = state->nearest_index;
    float pos[2] = {state->pos[0], state->pos[1]};

    for (HIDO_UINT32 i = 0; i < state->path_count; i++)
    float compass_deg = 90.0f - (heading_rad * RAD2DEG);
    while (compass_deg >= 360.0f)
    {
        float point[2];
        mc_get_point(state, i, point);
        float dist_sq = mc_distance_sq(point, pos);
        if (dist_sq < best_dist)
        {
            best_dist = dist_sq;
            best_idx = i;
        }
        compass_deg -= 360.0f;
    }
    return best_idx;
    while (compass_deg < 0.0f)
    {
        compass_deg += 360.0f;
    }
    return compass_deg;
}

/* 沿路径累积距离，直到达到前视目标 */
/* 罗盘坐标（北为 0°，顺时针为正）→ 数学坐标（东为 0°，逆时针为正） */
static float mc_compass_deg_to_math_rad(float heading_deg)
{
    float math_deg = 90.0f - heading_deg;
    return mc_wrap_angle(math_deg * DEG2RAD);
}

/* 判断车辆是否已经"到达"某个路径点
 * 标准：距离该点小于阈值（0.5米）即为到达 */
static HIDO_BOOL mc_is_point_reached(const MC_State *state, HIDO_UINT32 point_idx)
{
    if (point_idx >= state->path_count)
    {
        return HIDO_FALSE;
    }
    
    float pos[2] = {state->pos[0], state->pos[1]};
    float point[2];
    mc_get_point(state, point_idx, point);
    
    float dist = mc_distance(pos, point);
    
    /* 定义"到达"阈值为0.5米 */
    const float REACH_THRESHOLD_M = 0.5f;
    
    return (dist < REACH_THRESHOLD_M) ? HIDO_TRUE : HIDO_FALSE;
}

/* 搜索当前位置最近的路径点（只允许向前推进，必须到达才推进）
 * 类似航点导航：必须先到达航点0，才能导航到航点1 */
static HIDO_UINT32 mc_find_nearest(const MC_State *state)
{
    HIDO_UINT32 current_idx = state->nearest_index;
    
    /* 检查是否到达当前点 */
    if (mc_is_point_reached(state, current_idx) == HIDO_TRUE)
    {
        /* 到达了，推进到下一个点（如果不是最后一个） */
        if (current_idx < state->path_count - 1U)
        {
            return current_idx + 1U;
        }
    }
    
    /* 否则保持当前索引不变 */
    return current_idx;
}

/* 沿路径累积距离，直到达到前视目标
 * 注意：lookahead点始终至少是start_idx的下一个点，确保顺序导航 */
static HIDO_UINT32 mc_find_lookahead(const MC_State *state, HIDO_UINT32 start_idx, float lookahead_m)
{
    float accum = 0.0f;
    float current[2];
    mc_get_point(state, start_idx, current);

    for (HIDO_UINT32 i = start_idx; i < state->path_count - 1U; i++)
    /* 如果已经是最后一个点，直接返回 */
    if (start_idx >= state->path_count - 1U)
    {
        float next[2];
        mc_get_point(state, i + 1U, next);
        float seg = mc_distance(current, next);
        accum += seg;
        if (accum >= lookahead_m)
        {
            return i + 1U;
        }
        current[0] = next[0];
        current[1] = next[1];
        return state->path_count - 1U;
    }
    return state->path_count - 1U;
    
    /* lookahead点至少是start_idx的下一个点（顺序导航：必须先经过下一个点） */
    HIDO_UINT32 next_idx = start_idx + 1U;
    
    /* 简化逻辑：直接返回下一个点，确保按顺序访问每个航点 */
    return next_idx;
}

/* 计算带符号横向误差（左正右负） */
@@ -166,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;
@@ -175,11 +268,13 @@
    float vec_y = start_point[1] - state->pos[1];
    float dist = mc_distance(start_point, state->pos);

    float desired_heading = arm_atan2_f32(vec_y, vec_x);
    float heading_err = mc_wrap_angle(desired_heading - state->heading_rad);
    float desired_heading;
    arm_status status = arm_atan2_f32(vec_y, vec_x, &desired_heading);
    out->target_heading_deg = mc_math_rad_to_compass_deg(desired_heading);
    float heading_err = mc_heading_error_with_hysteresis(state, desired_heading);
    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);

    
    float forward = 0.0f;
    if (dist > state->config.start_pos_tolerance_m)
    {
@@ -193,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;
@@ -207,17 +304,107 @@
/* 阶段：纯追踪 + 航向/横向误差补偿 */
static void mc_compute_follow_path(MC_State *state, float dt_s, MC_Output *out)
{
    /* 安全检查：路径必须至少有1个点 */
    if (state->path_count == 0U)
    {
        out->forward_mps = 0.0f;
        out->turn_rate = 0.0f;
        out->active = HIDO_FALSE;
        return;
    }
    
    HIDO_UINT32 nearest_idx = mc_find_nearest(state);
    
    /* 安全检查：nearest_idx 必须在有效范围内 */
    if (nearest_idx >= state->path_count)
    {
        nearest_idx = state->path_count - 1U;
    }
    state->nearest_index = nearest_idx;

    float lookahead = state->config.lookahead_min_m;
    float speed_ratio = MC_CLAMP(state->speed_mps / state->config.max_forward_mps, 0.0f, 1.0f);
    lookahead += (state->config.lookahead_max_m - state->config.lookahead_min_m) * speed_ratio;
    HIDO_UINT32 lookahead_idx = mc_find_lookahead(state, nearest_idx, lookahead);
    
    /* 安全检查：lookahead_idx 必须在有效范围内 */
    if (lookahead_idx >= state->path_count)
    {
        lookahead_idx = state->path_count - 1U;
    }
    state->lookahead_index = lookahead_idx;
    
    /* 调试：每20帧输出一次路径跟踪状态 */
    static HIDO_UINT32 s_path_log_idx = 0U;
    static HIDO_UINT32 s_last_nearest = 0U;
    static HIDO_UINT32 s_last_lookahead = 0U;
    
    /* 如果索引发生变化，立即输出LOG */
    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)
    {
        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);
    
    /* 检测目标点变化并记录 */
    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;
@@ -226,8 +413,10 @@

    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);
    float heading_err = mc_wrap_angle(desired_heading - state->heading_rad);
    float desired_heading;
    arm_atan2_f32(vec_y, vec_x, &desired_heading);
    out->target_heading_deg = mc_math_rad_to_compass_deg(desired_heading);
    float heading_err = mc_heading_error_with_hysteresis(state, desired_heading);
    float heading_err_rate = (heading_err - state->last_heading_err) / (dt_s > 0.0f ? dt_s : 0.013f);
    state->last_heading_err = heading_err;

@@ -236,7 +425,7 @@
                       + state->config.heading_kd * heading_err_rate
                       + state->config.xtrack_kp * cross_track;
    yaw_rate_cmd = MC_CLAMP(yaw_rate_cmd, -state->config.max_turn_rate, state->config.max_turn_rate);

    
    float forward = state->config.base_speed_mps
                  - state->config.heading_speed_scale * fabsf(heading_err)
                  - state->config.xtrack_speed_scale * fabsf(cross_track);
@@ -290,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;
    }

@@ -300,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 位姿及航向/速度 */
@@ -313,8 +507,12 @@
    _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 old_heading_deg = _state->heading_deg;
    float gps_heading_rad = mc_compass_deg_to_math_rad(_gprmi->m_fHeadingAngle);
    _state->heading_rad = gps_heading_rad;
    _state->heading_deg = mc_math_rad_to_compass_deg(_state->heading_rad);
    
    float old_heading_rad = mc_compass_deg_to_math_rad(old_heading_deg);
    _state->pitch_deg = _gprmi->m_fPitchAngle;
    _state->roll_deg = _gprmi->m_fRollAngle;
    _state->vel[0] = _gprmi->m_fEastVelocity;
@@ -337,11 +535,16 @@
        return;
    }

    _state->yaw_rate_rad = _gpimu->m_fGyroZ * DEG2RAD;
    /* GyroZ符号定义：顺时针正，逆时针负（从上往下看Z轴，右手螺旋定则）
     * 数学坐标系：东为0°，逆时针为正（CCW>0）
     * 所以：GyroZ正值（顺时针）→ yaw_rate_rad负值，GyroZ负值（逆时针）→ yaw_rate_rad正值
     * 需要取反：yaw_rate_rad = -GyroZ * DEG2RAD */
    _state->yaw_rate_rad = -(_gpimu->m_fGyroZ * DEG2RAD);
    _state->imu_valid = HIDO_TRUE;
    
}

/* 入口：根据阶段计算控制量 */
/* 入口：根据阶段计算控制量 */ 
HIDO_VOID MC_Compute(MC_State *_state, float _dt_s, MC_Output *_out)
{
    if (_state == HIDO_NULL || _out == HIDO_NULL)
@@ -354,21 +557,52 @@
    _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)
    {
        float old_heading_rad = _state->heading_rad;
        float old_heading_deg = _state->heading_deg;
        float delta_rad = _state->yaw_rate_rad * _dt_s;
        _state->heading_rad = mc_wrap_angle(_state->heading_rad + delta_rad);
        float new_heading_rad = _state->heading_rad;
        _state->heading_deg = mc_math_rad_to_compass_deg(_state->heading_rad);
        
        (void)new_heading_rad;
    }

    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,11 +613,15 @@
        break;
    case MC_STAGE_FINISHED:
        _out->active = HIDO_FALSE;
        _out->target_heading_deg = _state->heading_deg;
        break;
    default:
        break;
    }

    /* 不再取反：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];
@@ -391,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;
}