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

feat: 增加动态预瞄点

Browse Source
This commit is contained in:
Glz
2024-03-22 20:28:49 +08:00
parent 97fc4f9b21
commit fba4476d35
9 changed files with 59 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
app/gl_common.h
View File

@@ -16,6 +16,7 @@
#define PIXPERMETER (70.0f)
#define RESAMPLEDIST (0.02f)
#define ANGLEDIST (0.2f)
#define ANGLEDIST_barrier (0.06f)
#define ROADWIDTH (0.45f)
#define FRAMENONE (1)
#define FRAMETOLEFT (5)

1
app/gl_cross.c
View File

@@ -15,7 +15,6 @@ void CheckCross() {
void RunCross() {
bool Xfound = Lpt0_found && Lpt1_found;
aim_distance = 0.4;
//检测到十字，先按照近线走
if (cross_type == CROSS_BEGIN) {
if (Lpt0_found) {

4
app/gl_data.c
View File

@@ -66,8 +66,12 @@ int far_angle_right_num;
int Lpt0_rpts0s_id;
int Lpt1_rpts1s_id;
int Lpt0_rpts0s_id_barrier;
int Lpt1_rpts1s_id_barrier;
bool Lpt0_found;
bool Lpt1_found;
bool Lpt0_found_barrier;
bool Lpt1_found_barrier;
int Lpt1[2];
int Lpt0[2];
int far_Lpt1[2];

4
app/gl_data.h
View File

@@ -86,8 +86,12 @@ extern int far_Lpt0[2];
extern int Lpt_in0_rpts0s_id;
extern int Lpt_in1_rpts1s_id;
extern int Lpt0_rpts0s_id_barrier;
extern int Lpt1_rpts1s_id_barrier;
extern bool Lpt_in0_found;
extern bool Lpt_in1_found;
extern bool Lpt0_found_barrier;
extern bool Lpt1_found_barrier;
extern int Lpt_in1[2];
extern int Lpt_in0[2];

63
app/gl_get_corners.c
View File

@@ -19,31 +19,34 @@ void get_corners() {
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]); //待优化，是否用到，无用则删
// 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 (Lpt0_found == true && is_straight0 == false) break;
}
// if(Lpt0_found){
// float angle1 = calculate_vector_angle(pts_resample_left[Lpt0_rpts0s_id][1],pts_resample_left[Lpt0_rpts0s_id][0],pts_resample_left[Lpt0_rpts0s_id+5][1],pts_resample_left[Lpt0_rpts0s_id+5][0]);
// if(angle1 < 85.) {
// Lpt_in0_found = true;
// Lpt0_found = false;
// Lpt_in0_rpts0s_id = Lpt0_rpts0s_id;
// }
// }
// else{
// is_turn0 = is_curve(angle_left ,clip(angle_left_num - 10, 0,angle_left_num),0.05);
// if(is_turn0){
// state_type = TURN_STATE;
// }
// }
if(is_straight0){
state_type = STRAIGHT_STATE;
}
Lpt0_found_barrier = Lpt1_found_barrier = false;
for (int i = 0; i < pts_resample_left_count; i++) {
if (angle_new_left[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);
float conf = fabs(angle_left[i]) - (fabs(angle_left[im1]) + fabs(angle_left[ip1])) / 2;
//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;
}
}
for (int i = 0; i < pts_resample_right_count; i++) {
@@ -56,29 +59,29 @@ void get_corners() {
if (Lpt1_found == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.5 / RESAMPLEDIST) {
Lpt1_rpts1s_id = i;
Lpt1_found = true;
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 > (7. / 180. * PI32) && i < 0.8 / RESAMPLEDIST) is_straight1 = false;
if (Lpt1_found == true && is_straight1 == false) break;
}
// if(Lpt1_found){
// float angle2 = calculate_vector_angle(pts_resample_right[Lpt1_rpts1s_id][1],pts_resample_right[Lpt1_rpts1s_id][0],pts_resample_right[Lpt1_rpts1s_id+5][1],pts_resample_right[Lpt1_rpts1s_id+5][0]);
// if(angle2 > 100.) {
// Lpt_in1_found = true;
// Lpt1_found = false;
// Lpt_in1_rpts1s_id = Lpt1_rpts1s_id;
// }
// }
//else{
// is_turn1 = is_curve(angle_right, clip(angle_right_num - 10,0 ,angle_right_num), 0.05);
// if(is_turn1){
// state_type = TURN_STATE;
// }
//}
if(is_straight1){
state_type = STRAIGHT_STATE;
}
for (int i = 0; i < pts_resample_right_count; i++) {
if (angle_new_right[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);
float conf = fabs(angle_right[i]) - (fabs(angle_right[im1]) + fabs(angle_right[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;
}
}
}

11
app/gl_tracking.c
View File

@@ -22,6 +22,17 @@ void tracking()
}
}
void aim_distance_select(void)
{
if (cross_type != CROSS_NONE) {
aim_distance = 0.4f;
} else if (circle_type != CIRCLE_NONE) {
aim_distance = 0.2f;
} else {
aim_distance = COMMON_AIM;
}
}
void ElementJudge()
{
CheckGarage();

1
app/gl_tracking.h
View File

@@ -6,6 +6,7 @@ extern int32_t mid_track_count;
extern float pure_angle;
extern float dx_near;
void aim_distance_select(void);
void tracking(void);
void ElementJudge(void);
void ElementRun(void);

2
app/main.c
View File

@@ -67,7 +67,7 @@ int main(void)
state_type = COMMON_STATE;
img_processing();
get_corners();
aim_distance = COMMON_AIM;
aim_distance_select();
tracking();
ElementJudge();
ElementRun();

8
app/page/page_ui_widget.c
View File

@@ -152,10 +152,10 @@ void Show_Marked_Image(void)
// 确认边线数组在显示前不会清空
for (uint i = 0; i < PT_MAXLEN; i++) {
// 寻近线逆透视后边线数组
uint16_t l_x = START_X + (uint16_t)((float)pts_inv_l[i][1] * horizontal_zoom_rate);
uint16_t l_y = START_Y + (uint16_t)((float)pts_inv_l[i][0] * vertical_zoom_rate);
uint16_t r_x = START_X + (uint16_t)((float)pts_inv_r[i][1] * horizontal_zoom_rate);
uint16_t r_y = START_Y + (uint16_t)((float)pts_inv_r[i][0] * vertical_zoom_rate);
uint16_t l_x = START_X + (uint16_t)((float)pts_resample_left[i][1] * horizontal_zoom_rate);
uint16_t l_y = START_Y + (uint16_t)((float)pts_resample_left[i][0] * vertical_zoom_rate);
uint16_t r_x = START_X + (uint16_t)((float)pts_resample_right[i][1] * horizontal_zoom_rate);
uint16_t r_y = START_Y + (uint16_t)((float)pts_resample_right[i][0] * vertical_zoom_rate);
// 寻远线逆透视后边线数组
// uint16_t far_l_x = START_X + (uint16_t)((float)pts_far_inv_r[i][1] * horizontal_zoom_rate);
// uint16_t far_l_y = START_Y + (uint16_t)((float)pts_far_inv_r[i][0] * vertical_zoom_rate);