btl143/QDAC-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

删除显示

Browse Source
This commit is contained in:
井明江
2024-03-28 19:57:17 +08:00
parent 902d639475
commit 1a9d9e4f9d
1 changed files with 13 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
app/gl_get_corners.c
View File

@@ -14,9 +14,10 @@ void get_corners()
int im1 = 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); 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; float conf = fabs(angle_left[i]) - (fabs(angle_left[im1]) + fabs(angle_left[ip1])) / 2;
ips114_show_float(120, 80, conf, 3, 4); // 114_show_float(120, 80, conf, 3, 4);
// L 角点阈值 // L 角点阈值
if (Lpt0_found == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.5 / RESAMPLEDIST) { if (Lpt0_found == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.5 / RESAMPLEDIST)
{
Lpt0_rpts0s_id = i; 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]); //待优化，是否用到，无用则删 // transform(pts_resample_left[Lpt0_rpts0s_id][1],pts_resample_left[Lpt0_rpts0s_id][0],&Lpt0[1],&Lpt0[0]); //待优化，是否用到，无用则删
@@ -28,17 +29,14 @@ void get_corners()
if (Lpt0_found == true && is_straight0 == false) break; if (Lpt0_found == true && is_straight0 == false) break;
} }
if (Lpt0_found == false) if (Lpt0_found == false) {
{ is_turn0 = is_curve(angle_left, clip(angle_left_num - 10, 0, angle_left_num), 0.05);
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 = Lpt1_found_barrier = false;
Lpt0_found_barrier_in = Lpt1_found_barrier_in = false; Lpt0_found_barrier_in = Lpt1_found_barrier_in = false;
for (int i = 0; i < pts_resample_left_count; i++) { for (int i = 0; i < pts_resample_left_count; i++) {
@@ -67,7 +65,7 @@ void get_corners()
int im1 = 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); 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; 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) { if (Lpt1_found == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.5 / RESAMPLEDIST) {
Lpt1_rpts1s_id = i; Lpt1_rpts1s_id = i;
@@ -79,13 +77,12 @@ void get_corners()
if (Lpt1_found == true && is_straight1 == false) break; if (Lpt1_found == true && is_straight1 == false) break;
} }
if(is_straight1 && is_straight0){ if (is_straight1 && is_straight0) {
state_type = STRAIGHT_STATE; state_type = STRAIGHT_STATE;
} }
if (Lpt1_found == false) if (Lpt1_found == false) {
{ is_turn1 = is_curve(angle_right, clip(angle_right_num - 10, 0, angle_right_num), 0.05);
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++) { 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; state_type = TURN_LEFT_STATE;
} }
} }