路径跟随初步调通

yincheng.zhong

2025-11-25 427d0b6ee0966fcf19a75914a47360969e00e456

 STM32H743/FML/motion_control.c

@@ -55,10 +55,6 @@
        err = preferred_sign * abs_err;
        state->last_turn_sign = preferred_sign;
        
        /* DEBUG: 输出滞后逻辑触发 */
        extern void HIDO_Debug2(const char *format, ...);
        HIDO_Debug2("[MC_DBG] HYSTERESIS: pi_delta=%.4f old_err=%.4f new_err=%.4f sign=%.1f\r\n",
                    pi_delta, old_err, err, preferred_sign);
    }
    else if (abs_err > 1.0e-6f)
    {
@@ -121,50 +117,64 @@
    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)
{
    float best_dist = 1.0e9f;
    HIDO_UINT32 best_idx = state->nearest_index;
    float pos[2] = {state->pos[0], state->pos[1]};
    HIDO_UINT32 current_idx = state->nearest_index;

    for (HIDO_UINT32 i = 0; i < state->path_count; i++)
    /* 检查是否到达当前点 */
    if (mc_is_point_reached(state, current_idx) == HIDO_TRUE)
    {
        float point[2];
        mc_get_point(state, i, point);
        float dist_sq = mc_distance_sq(point, pos);
        if (dist_sq < best_dist)
        /* 到达了，推进到下一个点（如果不是最后一个） */
        if (current_idx < state->path_count - 1U)
        {
            best_dist = dist_sq;
            best_idx = i;
            return current_idx + 1U;
        }
    }
    return best_idx;
}

/* 沿路径累积距离，直到达到前视目标 */
    /* 否则保持当前索引不变 */
    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;
}

    /* lookahead点至少是start_idx的下一个点（顺序导航：必须先经过下一个点） */
    HIDO_UINT32 next_idx = start_idx + 1U;
    
    /* 简化逻辑：直接返回下一个点，确保按顺序访问每个航点 */
    return next_idx;
}

/* 计算带符号横向误差（左正右负） */
static float mc_cross_track_error(const MC_State *state, HIDO_UINT32 nearest_idx)
{
@@ -230,54 +240,13 @@
    float vec_y = start_point[1] - state->pos[1];
    float dist = mc_distance(start_point, state->pos);

    /* DEBUG: 在调用atan2前输出参数 */
    extern void HIDO_Debug2(const char *format, ...);
    HIDO_Debug2("[MC_DBG] ATAN2_IN: vec_y=%.6f vec_x=%.6f\r\n", vec_y, vec_x);
    
    /* 直接调用atan2f测试 */
    extern float atan2f(float y, float x);
    float test_direct = atan2f(vec_y, vec_x);
    HIDO_Debug2("[MC_DBG] Direct atan2f(vec_y, vec_x) = %.6f\r\n", test_direct);
    
    /* 使用新的DSP库API: arm_atan2_f32(y, x, result) */
    float desired_heading;
    arm_status status = arm_atan2_f32(vec_y, vec_x, &desired_heading);
    if (status != ARM_MATH_SUCCESS) {
        HIDO_Debug2("[MC_DBG] WARNING: arm_atan2_f32 failed with status %d\r\n", status);
    }
    
    /* DEBUG: 测试不同的输出方式 */
    float test_val = desired_heading;
    int int_part = (int)test_val;
    float frac_part = (test_val - (float)int_part) * 1000.0f;
    HIDO_Debug2("[MC_DBG] ATAN2_OUT: arm_atan2_f32=%d.%03d rad\r\n", 
                int_part, (int)frac_part);
    HIDO_Debug2("[MC_DBG] ATAN2_OUT: in degrees = %.2f\r\n", 
                desired_heading * RAD2DEG);
    
    /* 测试已知值 */
    float test_atan2;
    arm_atan2_f32(1.0f, 1.0f, &test_atan2);
    float test_direct2 = atan2f(1.0f, 1.0f);
    HIDO_Debug2("[MC_DBG] TEST: arm_atan2(1,1)=%.6f atan2f(1,1)=%.6f (should be ~0.785)\r\n", 
                test_atan2, test_direct2);
    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);
    
    /* DEBUG: 输出goto_start控制计算 */
    extern void HIDO_Debug2(const char *format, ...);
    HIDO_Debug2("[MC_DBG] GOTO: pos=(%.2f,%.2f) tgt=(%.2f,%.2f) vec=(%.2f,%.2f) des_hdg_rad=%.4f\r\n",
                state->pos[0], state->pos[1], start_point[0], start_point[1],
                vec_x, vec_y, desired_heading);
    HIDO_Debug2("[MC_DBG] GOTO: cur_hdg=%.2f tgt_hdg=%.2f err=%.4f turn_sign=%.1f yaw_cmd=%.4f\r\n",
                mc_math_rad_to_compass_deg(state->heading_rad),
                out->target_heading_deg,
                heading_err,
                state->last_turn_sign,
                yaw_rate_cmd);

    float forward = 0.0f;
    if (dist > state->config.start_pos_tolerance_m)
    {
@@ -305,15 +274,83 @@
/* 阶段：纯追踪 + 航向/横向误差补偿 */
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);
    state->current_target_xy[0] = target[0];
@@ -337,16 +374,6 @@
                       + state->config.xtrack_kp * cross_track;
    yaw_rate_cmd = MC_CLAMP(yaw_rate_cmd, -state->config.max_turn_rate, state->config.max_turn_rate);
    
    /* DEBUG: 输出follow_path控制计算 */
    extern void HIDO_Debug2(const char *format, ...);
    HIDO_Debug2("[MC_DBG] FOLLOW: cur_hdg=%.2f tgt_hdg=%.2f err=%.4f turn_sign=%.1f yaw_cmd=%.4f xtrack=%.3f\r\n",
                mc_math_rad_to_compass_deg(state->heading_rad),
                out->target_heading_deg,
                heading_err,
                state->last_turn_sign,
                yaw_rate_cmd,
                cross_track);

    float forward = state->config.base_speed_mps
                  - state->config.heading_speed_scale * fabsf(heading_err)
                  - state->config.xtrack_speed_scale * fabsf(cross_track);
@@ -428,13 +455,7 @@
    _state->heading_rad = gps_heading_rad;
    _state->heading_deg = mc_math_rad_to_compass_deg(_state->heading_rad);
    
    /* DEBUG: 输出GPS更新信息（包含弧度值以便调试） */
    extern void HIDO_Debug2(const char *format, ...);
    float old_heading_rad = mc_compass_deg_to_math_rad(old_heading_deg);
    HIDO_Debug2("[MC_DBG] GPS: old_hdg=%.2f(old_rad=%.6f) gps_hdg=%.2f(gps_rad=%.6f) new_hdg=%.2f(new_rad=%.6f)\r\n",
                old_heading_deg, old_heading_rad,
                _gprmi->m_fHeadingAngle, gps_heading_rad,
                _state->heading_deg, _state->heading_rad);
    _state->pitch_deg = _gprmi->m_fPitchAngle;
    _state->roll_deg = _gprmi->m_fRollAngle;
    _state->vel[0] = _gprmi->m_fEastVelocity;
@@ -464,10 +485,6 @@
    _state->yaw_rate_rad = -(_gpimu->m_fGyroZ * DEG2RAD);
    _state->imu_valid = HIDO_TRUE;
    
    /* DEBUG: 输出IMU更新信息 */
    extern void HIDO_Debug2(const char *format, ...);
    HIDO_Debug2("[MC_DBG] IMU: GyroZ=%.2f deg/s, yaw_rate_rad=%.4f\r\n", 
                _gpimu->m_fGyroZ, _state->yaw_rate_rad);
}

/* 入口：根据阶段计算控制量 */ 
@@ -493,17 +510,6 @@
        return;
    }

    /* DEBUG: 输出计算开始状态（每50次输出一次，避免刷屏） */
    static HIDO_UINT32 dbg_counter = 0;
    dbg_counter++;
    if (dbg_counter % 50 == 0)
    {
        extern void HIDO_Debug2(const char *format, ...);
        HIDO_Debug2("[MC_DBG] COMPUTE: pose_valid=%d imu_valid=%d dt=%.4f hdg=%.2f yaw_rate=%.4f\r\n",
                    _state->pose_valid, _state->imu_valid, _dt_s, 
                    _state->heading_deg, _state->yaw_rate_rad);
    }

    if (_state->imu_valid == HIDO_TRUE && _dt_s > 0.0f)
    {
        float old_heading_rad = _state->heading_rad;
@@ -513,12 +519,7 @@
        float new_heading_rad = _state->heading_rad;
        _state->heading_deg = mc_math_rad_to_compass_deg(_state->heading_rad);
        
        /* DEBUG: 输出积分过程（包含弧度值以便调试） */
        extern void HIDO_Debug2(const char *format, ...);
        HIDO_Debug2("[MC_DBG] INTEG: dt=%.4f yaw_rate=%.4f delta_rad=%.6f old_rad=%.6f new_rad=%.6f old_hdg=%.2f new_hdg=%.2f delta_deg=%.2f\r\n",
                    _dt_s, _state->yaw_rate_rad, delta_rad, old_heading_rad, new_heading_rad,
                    old_heading_deg, _state->heading_deg, 
                    _state->heading_deg - old_heading_deg);
        (void)new_heading_rad;
    }

    if (_state->stage == MC_STAGE_IDLE)