diff --git a/app/gl_common.h b/app/gl_common.h index 50d17ec..2c9f2fe 100644 --- a/app/gl_common.h +++ b/app/gl_common.h @@ -13,7 +13,7 @@ #define SELFADAPT_KERNELSIZE (7) // 巡线区域核大小 #define FILTER_KERNELSIZE (7) // 滤波核大小 #define SELFADAPT_OFFSET (8) // 适应性块大小 -#define PIXPERMETER (70) +#define PIXPERMETER (70.0f) #define RESAMPLEDIST (0.02f) #define ANGLEDIST (0.2f) #define ROADWIDTH (0.45f) @@ -22,6 +22,8 @@ #define FRAMETORIGHT (5) #define PIXEL_PER_METER (100) +#define PI32 (3.1415926535898f) + int32_t limit(int32_t x, int32_t low, int32_t up); int clip(int x, int low, int up); float fclip(float x, float low, float up); diff --git a/app/gl_cross.c b/app/gl_cross.c index 0f2c920..8523fab 100644 --- a/app/gl_cross.c +++ b/app/gl_cross.c @@ -195,7 +195,7 @@ void cross_farline(){ float conf = fabs(far_angle_left[i]) - (fabs(far_angle_left[im1]) + fabs(far_angle_left[ip1])) / 2; //L角点阈值 - if (far_Lpt0_found == false && (66. / 180. * PI) < conf && conf < (140. / 180. * PI) && i < 0.7 / RESAMPLEDIST) { + if (far_Lpt0_found == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.7 / RESAMPLEDIST) { far_Lpt0_rpts0s_id = i; far_Lpt0_found = true; } @@ -209,7 +209,7 @@ void cross_farline(){ float conf = fabs(far_angle_right[i]) - (fabs(far_angle_right[im1]) + fabs(far_angle_right[ip1])) / 2; - if (far_Lpt1_found == false && (66. / 180. * PI) < conf && conf < 140. / 180. * PI && i < 0.7 / RESAMPLEDIST) { + if (far_Lpt1_found == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.7 / RESAMPLEDIST) { far_Lpt1_rpts1s_id = i; far_Lpt1_found = true; } diff --git a/app/gl_garage.c b/app/gl_garage.c index ff95769..967eed3 100644 --- a/app/gl_garage.c +++ b/app/gl_garage.c @@ -7,47 +7,42 @@ enum garage_type_e garage_type = GARAGE_NONE; float (*garage_rpts)[2]; int garage_rpts_num; -float calculate_vector_angle(float x1, float y1, float x2, float y2) { +float calculate_vector_angle(float x1, float y1, float x2, float y2) +{ float dx = x2 - x1; float dy = y2 - y1; - float vector_length = sqrt(dx*dx + dy*dy); + float vector_length = sqrt(dx * dx + dy * dy); float angle_radians = acos(dx / vector_length); float angle_degrees = angle_radians * 180 / M_PI; return angle_degrees; } -void CheckGarage() { - int change_num = 0; +void CheckGarage() +{ + int change_num = 0; int check_garage_h = 60; - for (int check_garage_w = 50; check_garage_w < IMAGE_W - 50; check_garage_w++) - { + 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)) - { + (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)) { change_num++; } - } - - if (change_num > 14) - { + if (change_num > 14) { garage_type = GARAGE_FOUND; - //printf("跳变点的数量为：%d\r\n", change_num); + // printf("跳变点的数量为：%d\r\n", change_num); } - - change_num = 0; + change_num = 0; } -void RunGarage(){ - - if (garage_type == GARAGE_FOUND) - { +void RunGarage() +{ + + if (garage_type == GARAGE_FOUND) { printf("识别到车库\r\n"); - garage_type == GARAGE_NONE; + garage_type = GARAGE_NONE; // TFIXME 原来是 garage_type == GARAGE_NONE，确认更改后无问题 } - } \ No newline at end of file diff --git a/app/gl_get_corners.c b/app/gl_get_corners.c index 2828d50..642df07 100644 --- a/app/gl_get_corners.c +++ b/app/gl_get_corners.c @@ -16,13 +16,13 @@ void get_corners() { float conf = fabs(angle_left[i]) - (fabs(angle_left[im1]) + fabs(angle_left[ip1])) / 2; //L角点阈值 - if (Lpt0_found == false && (66. / 180. * PI) < conf && conf < (140. / 180. * PI) && 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_found = true; transform(pts_resample_left[Lpt0_rpts0s_id][1],pts_resample_left[Lpt0_rpts0s_id][0],&Lpt0[1],&Lpt0[0]); //待优化，是否用到，无用则删 } //长直道阈值 - if (conf > (7. / 180. * PI) && 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){ @@ -53,13 +53,13 @@ void get_corners() { float conf = fabs(angle_right[i]) - (fabs(angle_right[im1]) + fabs(angle_right[ip1])) / 2; - if (Lpt1_found == false && (66. / 180. * PI) < conf && conf < 140. / 180. * PI && 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_found = true; transform(pts_resample_right[Lpt1_rpts1s_id][1],pts_resample_right[Lpt1_rpts1s_id][0],&Lpt1[1],&Lpt1[0]); } - if (conf > (7. / 180. * PI) && i < 0.8 / RESAMPLEDIST) is_straight1 = false; + if (conf > (7. / 180. * PI32) && i < 0.8 / RESAMPLEDIST) is_straight1 = false; if (Lpt1_found == true && is_straight1 == false) break; } // if(Lpt1_found){ diff --git a/app/gl_tracking.c b/app/gl_tracking.c index aa7b1df..9f5ef1a 100644 --- a/app/gl_tracking.c +++ b/app/gl_tracking.c @@ -107,20 +107,20 @@ void MidLineTrack() int aim_idx_near = clip(round(0.09 / RESAMPLEDIST), 0, rptsn_num - 1); // 计算远锚点偏差值 - float dx = rptsn[aim_idx][1] - cx; - float dy = cy - rptsn[aim_idx][0] + 0.2 * PIXPERMETER; - float dn = sqrt(dx * dx + dy * dy); - float error = -atan2f(dx, dy) * 180 / PI; + float dx = rptsn[aim_idx][1] - cx; + float dy = cy - rptsn[aim_idx][0] + 0.2 * PIXPERMETER; + float dn = sqrt(dx * dx + dy * dy); + // float error = -atan2f(dx, dy) * 180 / PI32; // 计算近锚点偏差值 dx_near = rptsn[aim_idx_near][1] - cx; // float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER; // float dn_near = sqrt(dx_near * dx_near + dy_near * dy_near); - // float error_near = -atan2f(dx_near, dy_near) * 180 / PI; + // float error_near = -atan2f(dx_near, dy_near) * 180 / PI32; // //考虑近点 - // near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI * 180 ; + // near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI32 * 180 ; // //考虑远点 - pure_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx/ dn / dn) / PI * 180 ; + pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f; } } diff --git a/app/page/page_ui_widget.c b/app/page/page_ui_widget.c index c68f9f5..95351dd 100644 --- a/app/page/page_ui_widget.c +++ b/app/page/page_ui_widget.c @@ -157,15 +157,15 @@ void Show_Marked_Image(void) uint16_t r_x = START_X + (uint16_t)((float)pts_inv_l[i][1] * horizontal_zoom_rate); uint16_t r_y = START_Y + (uint16_t)((float)pts_inv_l[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); - uint16_t far_r_x = START_X + (uint16_t)((float)pts_far_inv_l[i][1] * horizontal_zoom_rate); - uint16_t far_r_y = START_Y + (uint16_t)((float)pts_far_inv_l[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); + // uint16_t far_r_x = START_X + (uint16_t)((float)pts_far_inv_l[i][1] * horizontal_zoom_rate); + // uint16_t far_r_y = START_Y + (uint16_t)((float)pts_far_inv_l[i][0] * vertical_zoom_rate); //寻近线中线数组 - uint16_t mid_x_l = START_X + (uint16_t)(mid_left[i][1] * horizontal_zoom_rate); - uint16_t mix_y_l = START_Y + (uint16_t)(mid_left[i][0] * vertical_zoom_rate); - uint16_t mid_x_r = START_X + (uint16_t)(mid_right[i][1] * horizontal_zoom_rate); - uint16_t mix_y_r = START_Y + (uint16_t)(mid_right[i][0] * vertical_zoom_rate); + // uint16_t mid_x_l = START_X + (uint16_t)(mid_left[i][1] * horizontal_zoom_rate); + // uint16_t mix_y_l = START_Y + (uint16_t)(mid_left[i][0] * vertical_zoom_rate); + // uint16_t mid_x_r = START_X + (uint16_t)(mid_right[i][1] * horizontal_zoom_rate); + // uint16_t mix_y_r = START_Y + (uint16_t)(mid_right[i][0] * vertical_zoom_rate); uint16_t mid_x = START_X + (uint16_t)(mid_track[i][1] * horizontal_zoom_rate); uint16_t mix_y = START_Y + (uint16_t)(mid_track[i][0] * vertical_zoom_rate);