#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 dx_near; float (*rpts)[2]; int rpts_num; float last_pure_angle = 0.0f; // 计算最小二乘法斜率的函数 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 = cricle_aim; } else if (circle_type != CIRCLE_NONE) { aim_distance = cross_aim; } else if (barrier_type != BARRIER_NONE) { aim_distance = barrier_aim; } else { aim_distance = common_aim; } } void ElementJudge() { CheckGarage(); if (garage_type == GARAGE_NONE) { CheckCross(); if (cross_type == CROSS_NONE) { CheckBarrier(); if (barrier_type == BARRIER_NONE) { CheckCircle(); } } } 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(); } } 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]; // float slope = leastSquaresSlope(mid_track, mid_track_count); int neary = mid_track[0][0]; int nearx = mid_track[0][1]; // float near_distance = calculateX(a_x, a_y, slope, cy); int w1 = (int)cx; int h1 = (int)cy; int near_x1 = 0; int near_x2 = 0; if (GET_PIX_1C(mt9v03x_image_copy[0], h1, w1) >= FIX_BINTHRESHOLD) { for(;w1>10; w1--) { if(GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - 1) < FIX_BINTHRESHOLD) { near_x1 = w1; break; } } for(;w1<130; w1++) { if(GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 + 1) < FIX_BINTHRESHOLD) { near_x2 = w1; break; } } } else { if (GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - BEGINW_R) < FIX_BINTHRESHOLD) { for(;w1>10; w1--) { if(GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - 1) > FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - 2) > FIX_BINTHRESHOLD) { near_x1 = w1; near_x2 = cx; break; } } } if (GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 + BEGINW_R) < FIX_BINTHRESHOLD) { for(;w1<130; w1++) { if(GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 + 1) > FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 + 2) > FIX_BINTHRESHOLD) { near_x1 = cx; near_x2 = w1; break; } } } } // 找最近点 (起始点中线归一化) 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(0.09 / 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 - 10; } else if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING ) { dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset; } else { if (fabs(cx - near_x1) > fabs(cx - near_x2)) { dx_near = near_x2 - cx - 45; temp_near = dx_near; } if (fabs(cx - near_x1) < fabs(cx - near_x2)) { dx_near = near_x1 - cx + 45; temp_near = dx_near; } if (fabs(cx - near_x1) == fabs(cx - near_x2)) { dx_near = 0; temp_near = dx_near; } //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; } else { pure_angle = last_pure_angle; } } // // 计算近锚点偏差值 // 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; // // //考虑近点 // // near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI32 * 180 ; // // //考虑远点 // pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f; } 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) { state_type = CIRCLE_STATE; } if (cross_type == CROSS_BEGIN || cross_type == CROSS_IN) { state_type = STRAIGHT_STATE; } }