diff --git a/app/gl_common.h b/app/gl_common.h index 730c5b7..7e22801 100644 --- a/app/gl_common.h +++ b/app/gl_common.h @@ -16,6 +16,7 @@ #define PIXPERMETER (70.0f) #define RESAMPLEDIST (0.02f) #define ANGLEDIST (0.2f) +#define ANGLEDIST_barrier (0.06f) #define ROADWIDTH (0.45f) #define FRAMENONE (1) #define FRAMETOLEFT (5) diff --git a/app/gl_cross.c b/app/gl_cross.c index 8523fab..497ffe9 100644 --- a/app/gl_cross.c +++ b/app/gl_cross.c @@ -15,7 +15,6 @@ void CheckCross() { void RunCross() { bool Xfound = Lpt0_found && Lpt1_found; - aim_distance = 0.4; //检测到十字，先按照近线走 if (cross_type == CROSS_BEGIN) { if (Lpt0_found) { diff --git a/app/gl_data.c b/app/gl_data.c index 539abc0..a5c7b25 100644 --- a/app/gl_data.c +++ b/app/gl_data.c @@ -66,8 +66,12 @@ int far_angle_right_num; int Lpt0_rpts0s_id; int Lpt1_rpts1s_id; +int Lpt0_rpts0s_id_barrier; +int Lpt1_rpts1s_id_barrier; bool Lpt0_found; bool Lpt1_found; +bool Lpt0_found_barrier; +bool Lpt1_found_barrier; int Lpt1[2]; int Lpt0[2]; int far_Lpt1[2]; diff --git a/app/gl_data.h b/app/gl_data.h index 3048f34..b4243f0 100644 --- a/app/gl_data.h +++ b/app/gl_data.h @@ -86,8 +86,12 @@ extern int far_Lpt0[2]; extern int Lpt_in0_rpts0s_id; extern int Lpt_in1_rpts1s_id; +extern int Lpt0_rpts0s_id_barrier; +extern int Lpt1_rpts1s_id_barrier; extern bool Lpt_in0_found; extern bool Lpt_in1_found; +extern bool Lpt0_found_barrier; +extern bool Lpt1_found_barrier; extern int Lpt_in1[2]; extern int Lpt_in0[2]; diff --git a/app/gl_get_corners.c b/app/gl_get_corners.c index 642df07..b15e057 100644 --- a/app/gl_get_corners.c +++ b/app/gl_get_corners.c @@ -19,31 +19,34 @@ void get_corners() { if (Lpt0_found == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.5 / RESAMPLEDIST) { Lpt0_rpts0s_id = i; Lpt0_found = true; - transform(pts_resample_left[Lpt0_rpts0s_id][1],pts_resample_left[Lpt0_rpts0s_id][0],&Lpt0[1],&Lpt0[0]); //待优化，是否用到，无用则删 + // transform(pts_resample_left[Lpt0_rpts0s_id][1],pts_resample_left[Lpt0_rpts0s_id][0],&Lpt0[1],&Lpt0[0]); //待优化，是否用到，无用则删 } //长直道阈值 if (conf > (7. / 180. * PI32) && i < 0.8 / RESAMPLEDIST) is_straight0 = false; if (Lpt0_found == true && is_straight0 == false) break; } - // if(Lpt0_found){ - // float angle1 = calculate_vector_angle(pts_resample_left[Lpt0_rpts0s_id][1],pts_resample_left[Lpt0_rpts0s_id][0],pts_resample_left[Lpt0_rpts0s_id+5][1],pts_resample_left[Lpt0_rpts0s_id+5][0]); - // if(angle1 < 85.) { - // Lpt_in0_found = true; - // Lpt0_found = false; - // Lpt_in0_rpts0s_id = Lpt0_rpts0s_id; - // } - // } - // else{ - // is_turn0 = is_curve(angle_left ,clip(angle_left_num - 10, 0,angle_left_num),0.05); - // if(is_turn0){ - // state_type = TURN_STATE; - // } - // } + if(is_straight0){ state_type = STRAIGHT_STATE; } + Lpt0_found_barrier = Lpt1_found_barrier = false; + for (int i = 0; i < pts_resample_left_count; i++) { + if (angle_new_left[i] == 0) continue; + int im1 = clip(i - (int) round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_left_count - 1); + int ip1 = clip(i + (int) round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_left_count - 1); + float conf = fabs(angle_left[i]) - (fabs(angle_left[im1]) + fabs(angle_left[ip1])) / 2; + + //L角点阈值 + if (Lpt0_found_barrier == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.5 / RESAMPLEDIST) { + Lpt0_rpts0s_id_barrier = i; + Lpt0_found_barrier = true; + } + + } + + for (int i = 0; i < pts_resample_right_count; i++) { @@ -56,29 +59,29 @@ void get_corners() { if (Lpt1_found == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.5 / RESAMPLEDIST) { Lpt1_rpts1s_id = i; Lpt1_found = true; - transform(pts_resample_right[Lpt1_rpts1s_id][1],pts_resample_right[Lpt1_rpts1s_id][0],&Lpt1[1],&Lpt1[0]); + // transform(pts_resample_right[Lpt1_rpts1s_id][1],pts_resample_right[Lpt1_rpts1s_id][0],&Lpt1[1],&Lpt1[0]); } if (conf > (7. / 180. * PI32) && i < 0.8 / RESAMPLEDIST) is_straight1 = false; if (Lpt1_found == true && is_straight1 == false) break; } - // if(Lpt1_found){ - // float angle2 = calculate_vector_angle(pts_resample_right[Lpt1_rpts1s_id][1],pts_resample_right[Lpt1_rpts1s_id][0],pts_resample_right[Lpt1_rpts1s_id+5][1],pts_resample_right[Lpt1_rpts1s_id+5][0]); - // if(angle2 > 100.) { - // Lpt_in1_found = true; - // Lpt1_found = false; - // Lpt_in1_rpts1s_id = Lpt1_rpts1s_id; - // } - // } - //else{ - // is_turn1 = is_curve(angle_right, clip(angle_right_num - 10,0 ,angle_right_num), 0.05); - // if(is_turn1){ - // state_type = TURN_STATE; - // } - //} + if(is_straight1){ state_type = STRAIGHT_STATE; } + + for (int i = 0; i < pts_resample_right_count; i++) { + if (angle_new_right[i] == 0) continue; + int im1 = clip(i - (int) round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_right_count - 1); + int ip1 = clip(i + (int) round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_right_count - 1); + float conf = fabs(angle_right[i]) - (fabs(angle_right[im1]) + fabs(angle_right[ip1])) / 2; + + + if (Lpt1_found_barrier == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.5 / RESAMPLEDIST) { + Lpt1_rpts1s_id_barrier = i; + Lpt1_found_barrier = true; + } + } } diff --git a/app/gl_tracking.c b/app/gl_tracking.c index b0e3713..570a676 100644 --- a/app/gl_tracking.c +++ b/app/gl_tracking.c @@ -22,6 +22,17 @@ void tracking() } } +void aim_distance_select(void) +{ + if (cross_type != CROSS_NONE) { + aim_distance = 0.4f; + } else if (circle_type != CIRCLE_NONE) { + aim_distance = 0.2f; + } else { + aim_distance = COMMON_AIM; + } +} + void ElementJudge() { CheckGarage(); diff --git a/app/gl_tracking.h b/app/gl_tracking.h index 70dd50d..3af06cc 100644 --- a/app/gl_tracking.h +++ b/app/gl_tracking.h @@ -6,6 +6,7 @@ extern int32_t mid_track_count; extern float pure_angle; extern float dx_near; +void aim_distance_select(void); void tracking(void); void ElementJudge(void); void ElementRun(void); diff --git a/app/main.c b/app/main.c index 805c459..3bd8dc2 100644 --- a/app/main.c +++ b/app/main.c @@ -67,7 +67,7 @@ int main(void) state_type = COMMON_STATE; img_processing(); get_corners(); - aim_distance = COMMON_AIM; + aim_distance_select(); tracking(); ElementJudge(); ElementRun(); diff --git a/app/page/page_ui_widget.c b/app/page/page_ui_widget.c index ab95cb3..3e58956 100644 --- a/app/page/page_ui_widget.c +++ b/app/page/page_ui_widget.c @@ -152,10 +152,10 @@ void Show_Marked_Image(void) // 确认边线数组在显示前不会清空 for (uint i = 0; i < PT_MAXLEN; i++) { // 寻近线逆透视后边线数组 - uint16_t l_x = START_X + (uint16_t)((float)pts_inv_l[i][1] * horizontal_zoom_rate); - uint16_t l_y = START_Y + (uint16_t)((float)pts_inv_l[i][0] * vertical_zoom_rate); - uint16_t r_x = START_X + (uint16_t)((float)pts_inv_r[i][1] * horizontal_zoom_rate); - uint16_t r_y = START_Y + (uint16_t)((float)pts_inv_r[i][0] * vertical_zoom_rate); + uint16_t l_x = START_X + (uint16_t)((float)pts_resample_left[i][1] * horizontal_zoom_rate); + uint16_t l_y = START_Y + (uint16_t)((float)pts_resample_left[i][0] * vertical_zoom_rate); + uint16_t r_x = START_X + (uint16_t)((float)pts_resample_right[i][1] * horizontal_zoom_rate); + uint16_t r_y = START_Y + (uint16_t)((float)pts_resample_right[i][0] * vertical_zoom_rate); // 寻远线逆透视后边线数组 // uint16_t far_l_x = START_X + (uint16_t)((float)pts_far_inv_r[i][1] * horizontal_zoom_rate); // uint16_t far_l_y = START_Y + (uint16_t)((float)pts_far_inv_r[i][0] * vertical_zoom_rate);