QDAC-firmware/app/gl_tracking.c

#include "zf_common_headfile.h"
#include "gl_headfile.h"
#include "jj_blueteeth.h"
float (*mid_track)[2];
int32_t mid_track_count;
float pure_angle;
float pure_angle_half;
float dx_near;
float curvature;
float (*rpts)[2];
int rpts_num;
float last_pure_angle = 0.0f;
int8_t turn_flag      = 0;
// 计算最小二乘法斜率的函数
float leastSquaresSlope(float points[][2], int n)
{
    float sum_x         = 0;
    float sum_y         = 0;
    float sum_xy        = 0;
    float sum_x_squared = 0;

    // 计算各项和
    for (int i = 0; i < n; i++) {
        sum_x += points[i][1];
        sum_y += points[i][0];
        sum_xy += points[i][1] * points[i][0];
        sum_x_squared += points[i][1] * points[i][1];
    }

    // 计算斜率
    float numerator   = (float)n * sum_xy - sum_x * sum_y;
    float denominator = (float)n * sum_x_squared - sum_x * sum_x;

    if (denominator == 0) {
        // 避免除以零错误
        printf("Error: denominator is zero.\n");
        return 0;
    }

    float temp = denominator / numerator;

    return temp;
}

float calculateX(float a_x, float a_y, float slope, float b_y)
{
    float b_x = a_x - (b_y - a_y) * slope;
    return b_x;
}

void tracking()
{

    if (pts_resample_left_count < pts_resample_right_count / 2 && pts_resample_left_count < 50) {
        track_type = TRACK_RIGHT;
    } else if (pts_resample_right_count < pts_resample_left_count / 2 && pts_resample_right_count < 58) {
        track_type = TRACK_LEFT;
    } else if (pts_resample_left_count < 20 && pts_resample_right_count > pts_resample_left_count) {
        track_type = TRACK_RIGHT;
    } else if (pts_resample_right_count < 20 && pts_resample_left_count > pts_resample_right_count) {
        track_type = TRACK_LEFT;
    }
}

void aim_distance_select(void)
{
    if (cross_type != CROSS_NONE) {
        aim_distance = cross_aim;
    } else if (circle_type != CIRCLE_NONE) {
        aim_distance = cricle_aim;
    } else if (barrier_type != BARRIER_NONE) {
        aim_distance = barrier_aim;
    }
}

void ElementJudge()
{
    CheckGarage();
    if (garage_type == GARAGE_NONE) {
        CheckCross();
        if (cross_type == CROSS_NONE) {
            CheckBarrier();
            if (barrier_type == BARRIER_NONE) {
                CheckCircle();
                if (circle_type == CIRCLE_NONE) {
                    Check_s();
                }
            }
        }
    }

    if (garage_type != GARAGE_NONE) {
        cross_type  = CROSS_NONE;
        circle_type = CIRCLE_NONE;
    }
    if (cross_type != CROSS_NONE) {
        circle_type  = CIRCLE_NONE;
        barrier_type = BARRIER_NONE;
    }
    if (barrier_type != BARRIER_NONE) {
        circle_type = CIRCLE_NONE;
    }
}

void ElementRun()
{
    if (garage_type != GARAGE_NONE) {
        RunGarage();
    }

    else if (cross_type != CROSS_NONE) {
        RunCross();
    }

    else if (circle_type != CIRCLE_NONE) {
        RunCircle();
    } else if (barrier_type != BARRIER_NONE) {
        RunBarrier();
    } else if (s_type != S_NONE) {
        RunS();
    }
}

void MidLineTrack()
{
    if (cross_type == CROSS_IN) {
        if (track_type == TRACK_LEFT) {
            mid_track = mid_left; // 这是为了预先分配内存
            GetMidLine_Left(pts_far_resample_left + far_Lpt0_rpts0s_id, pts_far_resample_left_count - far_Lpt0_rpts0s_id, mid_left, (int)round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER * ROADWIDTH / 2);
            mid_track_count = pts_far_resample_left_count - far_Lpt0_rpts0s_id;
        } else {
            mid_track = mid_right; // 这是为了预先分配内存
            GetMidLine_Right(pts_far_resample_right + far_Lpt1_rpts1s_id, pts_far_resample_right_count - far_Lpt1_rpts1s_id, mid_right, (int)round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER * ROADWIDTH / 2);
            mid_track_count = pts_far_resample_right_count - far_Lpt1_rpts1s_id;
        }
    } else {
        if (track_type == TRACK_LEFT) {
            mid_track       = mid_left;
            mid_track_count = mid_left_count;
        } else {
            mid_track       = mid_right;
            mid_track_count = mid_right_count;
        }
    }

    // 车轮对应点 (纯跟踪起始点)
    float cx = InverseMapW[(int)(IMAGE_H * 0.8f)][70];
    float cy = InverseMapH[(int)(IMAGE_H * 0.8f)][70];

    int neary = mid_track[0][0];
    int nearx = mid_track[0][1];


    // 找最近点 (起始点中线归一化)
    float min_dist = 1e10;

    int begin_id = -1;
    for (int i = 0; i < mid_track_count; i++) {
        float dx   = mid_track[i][1] - cx;
        float dy   = mid_track[i][0] - cy;
        float dist = Q_sqrt(dx * dx + dy * dy);
        if (dist < min_dist) {
            min_dist = dist;
            begin_id = i;
        }
    }

    if (begin_id >= 0 && mid_track_count - begin_id >= 3) {
        // 归一化中线
        mid_track[begin_id][0] = cy;
        mid_track[begin_id][1] = cx;
        rptsn_num              = sizeof(rptsn) / sizeof(rptsn[0]);
        GetLinesResample(mid_track + begin_id, mid_track_count - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER);

        // 远预锚点位置-
        int aim_idx       = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1);
        int aim_idx_judge = clip(round(aim_judge_far / RESAMPLEDIST), 0, mid_track_count - 1);

        // 近锚点位置
        int aim_idx_near = clip(round(aim_distance / 2 / RESAMPLEDIST), 0, rptsn_num - 1);

        float dx1     = mid_track[3 * (mid_track_count / 4)][1] - mid_track[aim_idx_judge][1];
        float dy1     = mid_track[3 * (mid_track_count / 4)][0] - mid_track[aim_idx_judge][0];
        float dn1     = Q_sqrt(dx1 * dx1 + dy1 * dy1);
        float dx2     = mid_track[aim_idx_judge][1] - nearx;
        float dy2     = mid_track[aim_idx_judge][0] - neary;
        float dn2     = Q_sqrt(dx2 * dx2 + dy2 * dy2);
        float c1      = dx1 / dn1;
        float s1      = dy1 / dn1;
        float c2      = dx2 / dn2;
        float s2      = dy2 / dn2;
        float angle_1 = atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1);

        if (angle_1 >= 0.2f || angle_1 <= -0.2f) {
            state_type = TURN_STATE;
        } else {
            state_type = STRAIGHT_STATE;
        }

        // 计算远锚点偏差值
        float dx        = rptsn[aim_idx][1] - cx;
        float dy        = cy - rptsn[aim_idx][0]; // + 0.2f * PIXPERMETER;
        float dn        = (dx * dx + dy * dy);
        float temp_near = 0;
        if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
            dx_near    = mid_track[aim_idx_near][1] - cx + barrier_offset;
            pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f - 20;
        } else if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
            dx_near    = mid_track[aim_idx_near][1] - cx - barrier_offset;
            pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f + 20;
        } else {
            // pure_angle  = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
            if (dy > 0) {
                pure_angle      = -atanf(dx / dy) / PI32 * 180.0f;
                last_pure_angle = pure_angle;
                //last_pure_angle_half = pure_angle_half;
            } else {
                pure_angle = last_pure_angle;
                //pure_angle_half = last_pure_angle_half;
            }
        }


        // // 计算近锚点偏差值
        // dx_near = rptsn[aim_idx_near][1] - cx;
        // // float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
        // // float dn_near = Q_sqrt(dx_near * dx_near + dy_near * dy_near);
        // // float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;

    }
    if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_LEFT_OUT || circle_type == CIRCLE_RIGHT_IN || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING || circle_type == CIRCLE_LEFT_BEGIN || circle_type == CIRCLE_RIGHT_BEGIN) {
        state_type = CIRCLE_STATE;
    }
    if (cross_type == CROSS_BEGIN || cross_type == CROSS_IN) {
        state_type = STRAIGHT_STATE;
    }
    last_state = state_type;
    if (state_type == STRAIGHT_STATE) {
        aim_distance = straight_aim;
    } else if (state_type == TURN_STATE) {
        aim_distance = mid_aim;
    }
    // }else{
    //     aim_distance = common_aim;
    // }

    if (last_state == STRAIGHT_STATE && (state_type == TURN_STATE  || state_type == CIRCLE_STATE)) {
        turn_flag = 1;     
        timer_clear(TIM_3);
        timer_start(TIM_3);
    }
    if (turn_flag == 1) {
        aim_distance = turn_aim;
   
        uint16 ti = timer_get(TIM_3);
        if (ti >= 2000) {
            turn_flag = 0;
            timer_stop(TIM_3);
            timer_clear(TIM_3);
        }
    }
}