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

Merge branch 'master' of http://git.brisky.space:441/btl143/firmware_clover

Browse Source
This commit is contained in:
Glz
2024-03-26 22:45:05 +08:00
parent a500e2f917 05a2a18804
commit 8bc484488e
2 changed files with 38 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
app/gl_get_corners.c
View File

@@ -2,27 +2,29 @@
#include "math.h" #include "math.h"
#include "gl_headfile.h" #include "gl_headfile.h"
void get_corners()
{
void get_corners() {
Lpt0_found = Lpt1_found = false; Lpt0_found = Lpt1_found = false;
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;
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;
//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]); //待优化，是否用到，无用则删
} }
//长直道阈值 // 长直道阈值
if (conf > (7. / 180. * PI32) && i < 0.8 / RESAMPLEDIST) is_straight0 = false; if (conf > (7. / 180. * PI32) && i < 0.8 / RESAMPLEDIST) {
is_straight0 = false;
}
if (Lpt0_found == true && is_straight0 == false) break; if (Lpt0_found == true && is_straight0 == false) break;
} }
@@ -41,35 +43,31 @@ void get_corners() {
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++) {
if (angle_new_left_barrier[i] == 0) continue; if (angle_new_left_barrier[i] == 0) continue;
int im1 = clip(i - (int) round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_left_count - 1); 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); int ip1 = clip(i + (int)round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_left_count - 1);
float conf = fabs(angle_left_barrier[i]) - (fabs(angle_left_barrier[im1]) + fabs(angle_left_barrier[ip1])) / 2; float conf = fabs(angle_left_barrier[i]) - (fabs(angle_left_barrier[im1]) + fabs(angle_left_barrier[ip1])) / 2;
//L 角点阈值 // L 角点阈值
if (Lpt0_found_barrier == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.5 / RESAMPLEDIST) { if (Lpt0_found_barrier == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.5 / RESAMPLEDIST) {
Lpt0_rpts0s_id_barrier = i; Lpt0_rpts0s_id_barrier = i;
Lpt0_found_barrier = true; Lpt0_found_barrier = true;
} }
} }
if(Lpt0_found_barrier){ if (Lpt0_found_barrier) {
float angle1 = calculate_vector_angle(pts_resample_left[Lpt0_rpts0s_id_barrier][1],pts_resample_left[Lpt0_rpts0s_id_barrier][0],pts_resample_left[Lpt0_rpts0s_id_barrier+5][1],pts_resample_left[Lpt0_rpts0s_id_barrier+5][0]); float angle1 = calculate_vector_angle(pts_resample_left[Lpt0_rpts0s_id_barrier][1], pts_resample_left[Lpt0_rpts0s_id_barrier][0], pts_resample_left[Lpt0_rpts0s_id_barrier + 5][1], pts_resample_left[Lpt0_rpts0s_id_barrier + 5][0]);
if(angle1 < 85.) { if (angle1 < 85.) {
Lpt0_found_barrier_in = true; Lpt0_found_barrier_in = true;
Lpt0_found_barrier = false; Lpt0_found_barrier = false;
Lpt0_found_barrier_in_id = Lpt0_rpts0s_id_barrier; Lpt0_found_barrier_in_id = Lpt0_rpts0s_id_barrier;
} }
} }
for (int i = 0; i < pts_resample_right_count; i++) { for (int i = 0; i < pts_resample_right_count; i++) {
if (angle_new_right[i] == 0) continue; if (angle_new_right[i] == 0) continue;
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;
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;
Lpt1_found = true; Lpt1_found = true;
@@ -91,19 +89,18 @@ void get_corners() {
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;
int im1 = clip(i - (int) round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_right_count - 1); 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); int ip1 = clip(i + (int)round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_right_count - 1);
float conf = fabs(angle_right_barrier[i]) - (fabs(angle_right_barrier[im1]) + fabs(angle_right_barrier[ip1])) / 2; float conf = fabs(angle_right_barrier[i]) - (fabs(angle_right_barrier[im1]) + fabs(angle_right_barrier[ip1])) / 2;
if (Lpt1_found_barrier == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.5 / RESAMPLEDIST) { if (Lpt1_found_barrier == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.5 / RESAMPLEDIST) {
Lpt1_rpts1s_id_barrier = i; Lpt1_rpts1s_id_barrier = i;
Lpt1_found_barrier = true; Lpt1_found_barrier = true;
} }
} }
if(Lpt1_found_barrier){ if (Lpt1_found_barrier) {
float angle2 = calculate_vector_angle(pts_resample_right[Lpt1_rpts1s_id_barrier][1],pts_resample_right[Lpt1_rpts1s_id_barrier][0],pts_resample_right[Lpt1_rpts1s_id_barrier+5][1],pts_resample_right[Lpt1_rpts1s_id_barrier+5][0]); float angle2 = calculate_vector_angle(pts_resample_right[Lpt1_rpts1s_id_barrier][1], pts_resample_right[Lpt1_rpts1s_id_barrier][0], pts_resample_right[Lpt1_rpts1s_id_barrier + 5][1], pts_resample_right[Lpt1_rpts1s_id_barrier + 5][0]);
if(angle2 > 100.) { if (angle2 > 100.) {
Lpt1_found_barrier_in = true; Lpt1_found_barrier_in = true;
Lpt1_found_barrier = false; Lpt1_found_barrier = false;
Lpt1_found_barrier_in_id = Lpt1_rpts1s_id_barrier; Lpt1_found_barrier_in_id = Lpt1_rpts1s_id_barrier;
@@ -114,9 +111,4 @@ void get_corners() {
{ {
state_type = TURN_RIGHT_STATE; state_type = TURN_RIGHT_STATE;
} }
} }

10
app/page/page_rtcam.c
View File

@@ -28,8 +28,8 @@ static void Setup()
ips200_show_string(5, 165, "pts_l:"); ips200_show_string(5, 165, "pts_l:");
ips200_show_string(5, 185, "pts_r:"); ips200_show_string(5, 185, "pts_r:");
ips200_show_string(5, 205, "con_l:"); ips200_show_string(5, 205, "str_l:");
ips200_show_string(5, 224, "con_r:"); ips200_show_string(5, 224, "str_r:");
ips200_show_string(100, 165, "lpt0: "); ips200_show_string(100, 165, "lpt0: ");
ips200_show_string(100, 185, "lpt1: "); ips200_show_string(100, 185, "lpt1: ");
ips200_show_string(100, 205, "lptin0: "); ips200_show_string(100, 205, "lptin0: ");
@@ -55,14 +55,14 @@ 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_inv_l_count, 3);
ips200_show_uint(60, 185, pts_inv_r_count, 3); ips200_show_uint(60, 185, pts_inv_r_count, 3);
ips200_show_uint(60, 205, Lpt0_found, 3); ips200_show_uint(60, 205, is_straight0, 3);
ips200_show_uint(60, 224, Lpt1_found, 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);
ips200_show_uint(160, 185, Lpt1_found_barrier, 3); ips200_show_uint(160, 185, Lpt1_found_barrier, 3);
ips200_show_uint(160, 205, Lpt0_found_barrier_in, 3); ips200_show_uint(160, 205, Lpt0_found_barrier_in, 3);
ips200_show_uint(160, 224, Lpt1_found_barrier_in, 3); ips200_show_uint(160, 224, Lpt1_found_barrier_in, 3);
ips200_show_uint(200, 165, barrier_type, 3); ips200_show_uint(200, 165, barrier_type, 3);
ips200_show_uint(200, 185, time_barrier, 5); ips200_show_uint(200, 185, state_type, 5);
} }
/** /**