diff --git a/app/gl_get_corners.c b/app/gl_get_corners.c index c2339bb..c4604e9 100644 --- a/app/gl_get_corners.c +++ b/app/gl_get_corners.c @@ -14,9 +14,10 @@ void get_corners() 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; - ips114_show_float(120, 80, conf, 3, 4); - // L 角点阈值 - if (Lpt0_found == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.5 / RESAMPLEDIST) { + // 114_show_float(120, 80, conf, 3, 4); + // L 角点阈值 + 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]); //待优化,是否用到,无用则删 @@ -28,16 +29,13 @@ void get_corners() if (Lpt0_found == true && is_straight0 == false) break; } - if (Lpt0_found == false) - { - is_turn0 = is_curve(angle_left ,clip(angle_left_num - 10, 0,angle_left_num),0.05); + if (Lpt0_found == false) { + is_turn0 = is_curve(angle_left, clip(angle_left_num - 10, 0, angle_left_num), 0.05); } - //if (is_turn0) + // if (is_turn0) //{ - //state_type = TURN_LEFT_STATE; + // state_type = TURN_LEFT_STATE; //} - - Lpt0_found_barrier = Lpt1_found_barrier = false; Lpt0_found_barrier_in = Lpt1_found_barrier_in = false; @@ -67,7 +65,7 @@ void get_corners() 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; - ips114_show_float(100, 100, conf, 3, 4); + // ips114_show_float(100, 100, conf, 3, 4); if (Lpt1_found == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.5 / RESAMPLEDIST) { Lpt1_rpts1s_id = i; @@ -79,13 +77,12 @@ void get_corners() if (Lpt1_found == true && is_straight1 == false) break; } - if(is_straight1 && is_straight0){ + if (is_straight1 && is_straight0) { state_type = STRAIGHT_STATE; } - if (Lpt1_found == false) - { - is_turn1 = is_curve(angle_right ,clip(angle_right_num - 10, 0,angle_right_num),0.05); + if (Lpt1_found == false) { + is_turn1 = is_curve(angle_right, clip(angle_right_num - 10, 0, angle_right_num), 0.05); } for (int i = 0; i < pts_resample_right_count; i++) { @@ -108,8 +105,7 @@ void get_corners() } } - if (is_turn1 && is_turn0) - { + if (is_turn1 && is_turn0) { state_type = TURN_LEFT_STATE; } }