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

4
app/gl_circle.c
View File

@@ -124,8 +124,8 @@ void RunCircle()
} }
} else if (circle_type == CIRCLE_RIGHT_RUNNING) // 正常巡线，寻外圆左线 } else if (circle_type == CIRCLE_RIGHT_RUNNING) // 正常巡线，寻外圆左线
{ {
// track_type = TRACK_RIGHT; track_type = TRACK_RIGHT;
track_type = TRACK_LEFT; // 看看加一个如果丢线才切换 //track_type = TRACK_LEFT; // 看看加一个如果丢线才切换
if (Lpt0_found) // 外环存在拐点，可再加拐点距离判据 (左 L 点) if (Lpt0_found) // 外环存在拐点，可再加拐点距离判据 (左 L 点)
{ {
pts_resample_left_count = mid_left_count = Lpt0_rpts0s_id; pts_resample_left_count = mid_left_count = Lpt0_rpts0s_id;

6
app/gl_data.c
View File

@@ -103,8 +103,10 @@ bool is_straight1;
bool is_far_straight0; bool is_far_straight0;
bool is_far_straight1; bool is_far_straight1;
bool is_turn0; bool is_turn0_l;
bool is_turn1; bool is_turn1_l;
bool is_turn0_r;
bool is_turn1_r;
float rptsn[PT_MAXLEN][2]; float rptsn[PT_MAXLEN][2];
int32_t rptsn_num; int32_t rptsn_num;

6
app/gl_data.h
View File

@@ -114,8 +114,10 @@ extern int Lpt1_found_barrier_in_id;
extern int32_t Lpt0_found_count; extern int32_t Lpt0_found_count;
extern int32_t Lpt1_found_count; extern int32_t Lpt1_found_count;
extern bool is_turn0; extern bool is_turn0_l;
extern bool is_turn1; extern bool is_turn1_l;
extern bool is_turn0_r;
extern bool is_turn1_r;
extern float rptsn[PT_MAXLEN][2]; extern float rptsn[PT_MAXLEN][2];
extern int32_t rptsn_num; extern int32_t rptsn_num;

73
app/gl_garage.c
View File

@@ -40,28 +40,69 @@ float calculate_vector_angle(float x1, float y1, float x2, float y2)
void CheckGarage() void CheckGarage()
{ {
int change_num = 0; //变量标志位
int check_garage_h = 60; int banmaxian_hangshu = 0;//斑马线行数
for (int check_garage_w = 50; check_garage_w < IMAGE_W - 50; check_garage_w++) {
if ((GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w) < FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w + 1) >= FIX_BINTHRESHOLD) || //从下往上扫描
(GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w) >= FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w + 1) < FIX_BINTHRESHOLD)) { for (int y = BEGINH_L - 6; y >= BEGINH_L - 9; y--)
change_num++; {
int banmaxian_kuandu=0;
//int banmaxian_hangshu=0;
int banmaxian_geshu=0;
//从右往左扫描
for (int x =130; x >=20; x--)
{
int baidian_heng=0;
//扫描到黑色，就进判断
if (GET_PIX_1C(mt9v03x_image_copy[0], y, x + 2) > FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], y, x + 1) > FIX_BINTHRESHOLD
&& GET_PIX_1C(mt9v03x_image_copy[0], y, x) < FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], y, x - 1) < FIX_BINTHRESHOLD)
{
for (int a = x; a > x - 30; a--)//从黑色点向左侧扫描
{
//找到白色点
if(GET_PIX_1C(mt9v03x_image_copy[0], y, a) > FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], y, a - 1) > FIX_BINTHRESHOLD)
{
//记录白色点的位置，跳出循环
baidian_heng=a;
break;
}
}//斑马线宽度等于黑白点的差
banmaxian_kuandu=x-baidian_heng;
}
else
{ //斑马线的宽度在4~8之间认为它成立为斑马线黑色块
if (banmaxian_kuandu >= 2 && banmaxian_kuandu <= 6)
{
//斑马线黑色块++
banmaxian_geshu++;
//斑马线色块宽度清零，进行下一个黑色块的扫描计算
banmaxian_kuandu = 0;
}
else
{
//如果不满足对黑色块的认为要求就直接清零，去计算下一个黑色块
banmaxian_kuandu = 0;
}
}
}
//如果色块的个数在6~9之间则认为这一行的斑马线满足要求，在去扫下一行
if (banmaxian_geshu >= 2 ){banmaxian_hangshu++;}
} }
} //如果有大于等于4行的有效斑马线
if(banmaxian_hangshu>=2)
if (change_num > 14) { {
garage_type = GARAGE_FOUND; //斑马线标准位置1
// printf("跳变点的数量为：%d\r\n", change_num); garage_type = GARAGE_FOUND;
} }
else{garage_type = GARAGE_NONE;}
change_num = 0;
} }
void RunGarage() void RunGarage()
{ {
if (garage_type == GARAGE_FOUND) { if (garage_type == GARAGE_FOUND) {
printf("识别到车库\r\n"); printf("识别到车库\r\n");
garage_type = GARAGE_NONE; // TFIXME 原来是 garage_type == GARAGE_NONE，确认更改后无问题 //garage_type = GARAGE_NONE; // TFIXME 原来是 garage_type == GARAGE_NONE，确认更改后无问题
} }
} }

9
app/gl_garage.h
View File

@@ -5,15 +5,6 @@
enum garage_type_e { enum garage_type_e {
GARAGE_NONE, // 非车库模式 GARAGE_NONE, // 非车库模式
GARAGE_FOUND, GARAGE_FOUND,
GARAGE_OUT_LEFT,
GARAGE_OUT_RIGHT, // 出库，陀螺仪转过45°，即出库完毕
GARAGE_FOUND_LEFT,
GARAGE_FOUND_RIGHT, // 发现车库，即斑马线+单侧L角点(未使用)
GARAGE_IN_LEFT,
GARAGE_IN_RIGHT, // 进库，发现车库后判断第几次，从而决定是否进库
GARAGE_PASS_LEFT,
GARAGE_PASS_RIGHT, // 不进库，发现车库后判断第几次，从而决定是否进库
GARAGE_STOP // 进库完毕，停车
}; };
extern enum garage_type_e garage_type; extern enum garage_type_e garage_type;

67
app/gl_get_corners.c
View File

@@ -8,13 +8,13 @@ void get_corners()
Lpt_in0_found = Lpt_in1_found = false; Lpt_in0_found = Lpt_in1_found = false;
is_straight0 = pts_resample_left_count > 1.0 / RESAMPLEDIST; is_straight0 = pts_resample_left_count > 1.0 / RESAMPLEDIST;
is_straight1 = pts_resample_right_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++) { for (int i = 0; i < pts_resample_left_count; i++) {
if (angle_new_left[i] == 0) continue; if (angle_new_left[i] == 0) continue;
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); // ips114_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;
@@ -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]); //待优化，是否用到，无用则删 // 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; is_straight0 = false;
} }
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);
} // 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) //if (is_turn0)
//{ //{
//state_type = TURN_LEFT_STATE; //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 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;
@@ -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]); // 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 (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);
} // 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++) { for (int i = 0; i < pts_resample_right_count; i++) {
if (angle_new_right_barrier[i] == 0) continue; if (angle_new_right_barrier[i] == 0) continue;
@@ -108,8 +131,12 @@ void get_corners()
} }
} }
if (is_turn1 && is_turn0) // if (is_turn0_l && is_turn1_l)
{ // {
state_type = TURN_LEFT_STATE; // state_type = TURN_LEFT_STATE;
} // }
// if (is_turn0_r && is_turn1_r)
// {
// state_type = TURN_RIGHT_STATE;
// }
} }

3
app/gl_state.h
View File

@@ -3,8 +3,7 @@
enum state_type_e { enum state_type_e {
COMMON_STATE, COMMON_STATE,
TURN_LEFT_STATE, TURN_STATE,
TURN_RIGHT_STATE,
STRAIGHT_STATE, STRAIGHT_STATE,
}; };

28
app/gl_tracking.c
View File

@@ -125,12 +125,40 @@ void MidLineTrack()
rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]); rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]);
GetLinesResample(mid_track + begin_id, mid_track_count - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER); GetLinesResample(mid_track + begin_id, mid_track_count - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER);
// 远预锚点位置- // 远预锚点位置-
int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1); int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1);
// 近锚点位置 // 近锚点位置
int aim_idx_near = clip(round(0.09 / RESAMPLEDIST), 0, rptsn_num - 1); int aim_idx_near = clip(round(0.09 / RESAMPLEDIST), 0, rptsn_num - 1);
int gap_1 = fabs(rptsn[3 * (rptsn_num / 4)][1] - rptsn[0][1]);
float dx1 = rptsn[3 * (rptsn_num / 4)][0] - rptsn[aim_idx][0];
float dy1 = rptsn[3 * (rptsn_num / 4)][1] - rptsn[aim_idx][1];
float dn1 = Q_sqrt(dx1 * dx1 + dy1 * dy1);
float dx2 = rptsn[aim_idx][0] - rptsn[aim_idx_near][0];
float dy2 = rptsn[aim_idx][1] - rptsn[aim_idx_near][1];
float dn2 = Q_sqrt(dx2 * dx2 + dy2 * dy2);
float c1 = dx1 / dn1;
float s1 = dy1 / dn1;
float c2 = dx2 / dn2;
float s2 = dy2 / dn2;
float angle_1= atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1);
ips114_show_float(120, 80, angle_1, 3, 4);
if (-0.3 <= angle_1 <= 0.3 )
{
state_type = TURN_STATE;
}
else
{
state_type = STRAIGHT_STATE;
}
if (circle_type == CIRCLE_LEFT_IN && circle_type == CIRCLE_LEFT_OUT && circle_type == CIRCLE_RIGHT_IN && circle_type == CIRCLE_RIGHT_OUT)
{
state_type = TURN_STATE;
}
// 计算远锚点偏差值 // 计算远锚点偏差值
float dx = rptsn[aim_idx][1] - cx; float dx = rptsn[aim_idx][1] - cx;
float dy = cy - rptsn[aim_idx][0] + 0.2f * PIXPERMETER; float dy = cy - rptsn[aim_idx][0] + 0.2f * PIXPERMETER;

4
app/page/page_rtcam.c
View File

@@ -53,8 +53,8 @@ static void Exit()
static void Loop() static void Loop()
{ {
Show_Marked_Image(); Show_Marked_Image();
ips200_show_uint(60, 165, pts_inv_l_count, 3); ips200_show_uint(60, 165, pts_resample_left_count, 3);
ips200_show_uint(60, 185, pts_inv_r_count, 3); ips200_show_uint(60, 185, pts_resample_right_count, 3);
ips200_show_uint(60, 205, is_straight0, 3); ips200_show_uint(60, 205, is_straight0, 3);
ips200_show_uint(60, 224, is_straight1, 3); ips200_show_uint(60, 224, is_straight1, 3);
ips200_show_uint(160, 165, Lpt0_found_barrier, 3); ips200_show_uint(160, 165, Lpt0_found_barrier, 3);