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

增加直道,弯道判断,识别车库

Browse Source
This commit is contained in:
Glz
2024-03-29 10:50:37 +08:00
parent 902d639475
commit 28636012f6
9 changed files with 145 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
app/gl_get_corners.c
View File

@@ -8,13 +8,13 @@ void get_corners()
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_turn0 = is_turn1 = 0;
// is_turn0_l = is_turn1_l = is_turn0_r = is_turn1_r = 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);
float conf = fabs(angle_left[i]) - (fabs(angle_left[im1]) + fabs(angle_left[ip1])) / 2;
ips114_show_float(120, 80, conf, 3, 4);
// 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) {
Lpt0_rpts0s_id = i;
@@ -22,16 +22,28 @@ void get_corners()
// transform(pts_resample_left[Lpt0_rpts0s_id][1],pts_resample_left[Lpt0_rpts0s_id][0],&Lpt0[1],&Lpt0[0]); //待优化，是否用到，无用则删
}
// 长直道阈值
if (conf > (6. / 180. * PI32) && i < 1.2 / RESAMPLEDIST) {
if (conf > (6. / 180. * PI32) && i < 0.8 / RESAMPLEDIST) {
is_straight0 = false;
}
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);
// int gap = 0;
// gap = pts_resample_left[clip(pts_resample_left_count - 30, 0, pts_resample_left_count - 1)][1] - pts_resample_left[0][1];
// ips200_show_uint(200, 205, gap, 3);
// if (gap <= -10)
// {
// is_turn0_l = true;
// }
// if (gap >= 10)
// {
// is_turn1_l = true;
// }
// }
//if (is_turn0)
//{
//state_type = TURN_LEFT_STATE;
@@ -67,7 +79,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;
@@ -75,18 +87,29 @@ void get_corners()
// transform(pts_resample_right[Lpt1_rpts1s_id][1],pts_resample_right[Lpt1_rpts1s_id][0],&Lpt1[1],&Lpt1[0]);
}
if (conf > (6. / 180. * PI32) && i < 1.2 / RESAMPLEDIST) is_straight1 = false;
if (conf > (6. / 180. * PI32) && i < 0.8 / RESAMPLEDIST) is_straight1 = false;
if (Lpt1_found == true && is_straight1 == false) break;
}
if(is_straight1 && is_straight0){
state_type = STRAIGHT_STATE;
}
// 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);
// int gap = 0;
// gap = pts_resample_right[clip(pts_resample_right_count - 30, 0, pts_resample_right_count - 1)][1] - pts_resample_right[0][1];
// ips200_show_uint(200, 224, gap, 3);
// if (gap <= -10)
// {
// is_turn1_l = true;
// }
// if (gap >= 10)
// {
// is_turn1_r = true;
// }
// }
for (int i = 0; i < pts_resample_right_count; i++) {
if (angle_new_right_barrier[i] == 0) continue;
@@ -108,8 +131,12 @@ void get_corners()
}
}
if (is_turn1 && is_turn0)
{
state_type = TURN_LEFT_STATE;
}
// if (is_turn0_l && is_turn1_l)
// {
// state_type = TURN_LEFT_STATE;
// }
// if (is_turn0_r && is_turn1_r)
// {
// state_type = TURN_RIGHT_STATE;
// }
}