转向问题解决，方向定义问题，开始走了

yincheng.zhong

2025-11-24 275b03224aa6170d4dc8c661c1cd949dd88c1fcb

 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,6 +37,37 @@
    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;
        
        /* 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)
    {
        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])
{
@@ -66,6 +99,28 @@
    return dist;
}

/* 数学坐标（东为 0°，逆时针为正）→ 罗盘坐标（北为 0°，顺时针为正） */
static float mc_math_rad_to_compass_deg(float heading_rad)
{
    float compass_deg = 90.0f - (heading_rad * RAD2DEG);
    while (compass_deg >= 360.0f)
    {
        compass_deg -= 360.0f;
    }
    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);
}

/* 搜索当前位置最近的路径点 */
static HIDO_UINT32 mc_find_nearest(const MC_State *state)
{
@@ -175,11 +230,53 @@
    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);
    out->target_heading_deg = desired_heading * RAD2DEG;
    float heading_err = mc_wrap_angle(desired_heading - state->heading_rad);
    /* 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)
@@ -227,9 +324,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);
    out->target_heading_deg = desired_heading * RAD2DEG;
    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;

@@ -238,6 +336,16 @@
                       + 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);
    
    /* 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)
@@ -315,17 +423,18 @@
    _state->pos[0] = _enu[0];
    _state->pos[1] = _enu[1];
    _state->pos[2] = _enu[2];
    float gps_heading_rad = mc_wrap_angle((_gprmi->m_fHeadingAngle) * DEG2RAD);
    if (_state->imu_valid == HIDO_TRUE)
    {
        float heading_err = mc_wrap_angle(gps_heading_rad - _state->heading_rad);
        _state->heading_rad = mc_wrap_angle(_state->heading_rad + heading_err * MC_CFG_GPS_HEADING_BLEND);
    }
    else
    {
        _state->heading_rad = gps_heading_rad;
    }
    _state->heading_deg = _state->heading_rad * RAD2DEG;
    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);
    
    /* 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;
@@ -348,11 +457,20 @@
        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;
    
    /* 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);
}

/* 入口：根据阶段计算控制量 */
/* 入口：根据阶段计算控制量 */ 
HIDO_VOID MC_Compute(MC_State *_state, float _dt_s, MC_Output *_out)
{
    if (_state == HIDO_NULL || _out == HIDO_NULL)
@@ -375,10 +493,32 @@
        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)
    {
        _state->heading_rad = mc_wrap_angle(_state->heading_rad + _state->yaw_rate_rad * _dt_s);
        _state->heading_deg = _state->heading_rad * RAD2DEG;
        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);
        
        /* 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);
    }

    if (_state->stage == MC_STAGE_IDLE)
@@ -404,6 +544,7 @@
        break;
    }

    _out->turn_rate = -_out->turn_rate;
    _out->stage = _state->stage;
    _out->pos_enu[0] = _state->pos[0];
    _out->pos_enu[1] = _state->pos[1];