diff --git a/app/gl_get_corners.c b/app/gl_get_corners.c index bf5d14a..6e150fa 100644 --- a/app/gl_get_corners.c +++ b/app/gl_get_corners.c @@ -2,27 +2,29 @@ #include "math.h" #include "gl_headfile.h" - - -void get_corners() { +void get_corners() +{ Lpt0_found = Lpt1_found = false; Lpt_in0_found = Lpt_in1_found = false; - is_straight0 = pts_resample_left_count > 1.0 / RESAMPLEDIST; - is_straight1 = pts_resample_right_count > 1.0 / RESAMPLEDIST; + is_straight0 = pts_resample_left_count > 1.0 / RESAMPLEDIST; + is_straight1 = pts_resample_right_count > 1.0 / RESAMPLEDIST; + is_turn0 = is_turn1 = 0; for (int i = 0; i < pts_resample_left_count; i++) { if (angle_new_left[i] == 0) continue; - int im1 = clip(i - (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_left_count - 1); - int ip1 = clip(i + (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_left_count - 1); + int im1 = clip(i - (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_left_count - 1); + int ip1 = clip(i + (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_left_count - 1); float conf = fabs(angle_left[i]) - (fabs(angle_left[im1]) + fabs(angle_left[ip1])) / 2; - //L 角点阈值 + // L 角点阈值 if (Lpt0_found == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.5 / RESAMPLEDIST) { Lpt0_rpts0s_id = i; - Lpt0_found = true; + Lpt0_found = true; // 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 (conf > (7. / 180. * PI32) && i < 0.8 / RESAMPLEDIST) { + is_straight0 = false; + } if (Lpt0_found == true && is_straight0 == false) break; } @@ -41,38 +43,34 @@ void get_corners() { Lpt0_found_barrier_in = Lpt1_found_barrier_in = false; for (int i = 0; i < pts_resample_left_count; i++) { if (angle_new_left_barrier[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); + 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_barrier[i]) - (fabs(angle_left_barrier[im1]) + fabs(angle_left_barrier[ip1])) / 2; - //L 角点阈值 + // 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; + Lpt0_found_barrier = true; } } - if(Lpt0_found_barrier){ - float angle1 = calculate_vector_angle(pts_resample_left[Lpt0_rpts0s_id_barrier][1],pts_resample_left[Lpt0_rpts0s_id_barrier][0],pts_resample_left[Lpt0_rpts0s_id_barrier+5][1],pts_resample_left[Lpt0_rpts0s_id_barrier+5][0]); - if(angle1 < 85.) { - Lpt0_found_barrier_in = true; - Lpt0_found_barrier = false; + if (Lpt0_found_barrier) { + float angle1 = calculate_vector_angle(pts_resample_left[Lpt0_rpts0s_id_barrier][1], pts_resample_left[Lpt0_rpts0s_id_barrier][0], pts_resample_left[Lpt0_rpts0s_id_barrier + 5][1], pts_resample_left[Lpt0_rpts0s_id_barrier + 5][0]); + if (angle1 < 85.) { + Lpt0_found_barrier_in = true; + Lpt0_found_barrier = false; Lpt0_found_barrier_in_id = Lpt0_rpts0s_id_barrier; } } - - - for (int i = 0; i < pts_resample_right_count; i++) { if (angle_new_right[i] == 0) continue; - int im1 = clip(i - (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_right_count - 1); - int ip1 = clip(i + (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_right_count - 1); + int im1 = clip(i - (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_right_count - 1); + int ip1 = clip(i + (int)round(ANGLEDIST / 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 == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.5 / RESAMPLEDIST) { Lpt1_rpts1s_id = i; - Lpt1_found = true; + Lpt1_found = true; // transform(pts_resample_right[Lpt1_rpts1s_id][1],pts_resample_right[Lpt1_rpts1s_id][0],&Lpt1[1],&Lpt1[0]); } @@ -91,21 +89,20 @@ void get_corners() { for (int i = 0; i < pts_resample_right_count; i++) { if (angle_new_right_barrier[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); + 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_barrier[i]) - (fabs(angle_right_barrier[im1]) + fabs(angle_right_barrier[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; + Lpt1_found_barrier = true; } } - if(Lpt1_found_barrier){ - float angle2 = calculate_vector_angle(pts_resample_right[Lpt1_rpts1s_id_barrier][1],pts_resample_right[Lpt1_rpts1s_id_barrier][0],pts_resample_right[Lpt1_rpts1s_id_barrier+5][1],pts_resample_right[Lpt1_rpts1s_id_barrier+5][0]); - if(angle2 > 100.) { - Lpt1_found_barrier_in = true; - Lpt1_found_barrier = false; + if (Lpt1_found_barrier) { + float angle2 = calculate_vector_angle(pts_resample_right[Lpt1_rpts1s_id_barrier][1], pts_resample_right[Lpt1_rpts1s_id_barrier][0], pts_resample_right[Lpt1_rpts1s_id_barrier + 5][1], pts_resample_right[Lpt1_rpts1s_id_barrier + 5][0]); + if (angle2 > 100.) { + Lpt1_found_barrier_in = true; + Lpt1_found_barrier = false; Lpt1_found_barrier_in_id = Lpt1_rpts1s_id_barrier; } } @@ -114,9 +111,4 @@ void get_corners() { { state_type = TURN_RIGHT_STATE; } - - - } - - diff --git a/app/page/page_rtcam.c b/app/page/page_rtcam.c index 0ed4a59..baf14c4 100644 --- a/app/page/page_rtcam.c +++ b/app/page/page_rtcam.c @@ -28,8 +28,8 @@ static void Setup() ips200_show_string(5, 165, "pts_l:"); ips200_show_string(5, 185, "pts_r:"); - ips200_show_string(5, 205, "con_l:"); - ips200_show_string(5, 224, "con_r:"); + ips200_show_string(5, 205, "str_l:"); + ips200_show_string(5, 224, "str_r:"); ips200_show_string(100, 165, "lpt0: "); ips200_show_string(100, 185, "lpt1: "); ips200_show_string(100, 205, "lptin0: "); @@ -55,14 +55,14 @@ static void Loop() Show_Marked_Image(); ips200_show_uint(60, 165, pts_inv_l_count, 3); ips200_show_uint(60, 185, pts_inv_r_count, 3); - ips200_show_uint(60, 205, Lpt0_found, 3); - ips200_show_uint(60, 224, Lpt1_found, 3); + ips200_show_uint(60, 205, is_straight0, 3); + ips200_show_uint(60, 224, is_straight1, 3); ips200_show_uint(160, 165, Lpt0_found_barrier, 3); ips200_show_uint(160, 185, Lpt1_found_barrier, 3); ips200_show_uint(160, 205, Lpt0_found_barrier_in, 3); ips200_show_uint(160, 224, Lpt1_found_barrier_in, 3); ips200_show_uint(200, 165, barrier_type, 3); - ips200_show_uint(200, 185, time_barrier, 5); + ips200_show_uint(200, 185, state_type, 5); } /**