From 577627c0696c3c08726da3fae1ed36d04e66a944 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E4=BA=95=E6=98=8E=E6=B1=9F?= <246462502@qq.com> Date: Fri, 8 Mar 2024 20:58:16 +0800 Subject: [PATCH] =?UTF-8?q?fix:=20=E4=BF=AE=E5=A4=8D=E5=9B=BE=E5=83=8Fbug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .eide/eide.json | 3 +- app/gl_common.h | 4 +- app/gl_tracking.c | 197 ++++++++++---------- libraries/zf_device/zf_device_mt9v03x_dvp.h | 2 +- 4 files changed, 100 insertions(+), 106 deletions(-) diff --git a/.eide/eide.json b/.eide/eide.json index 59c83c9..21b0c8d 100644 --- a/.eide/eide.json +++ b/.eide/eide.json @@ -60,7 +60,8 @@ "libraries/zf_driver", "3rd-lib/crc16", "app/page", - "3rd-lib/lwrb/inc" + "3rd-lib/lwrb/inc", + "app/tiny_frame" ], "libList": [ "libraries/zf_device" diff --git a/app/gl_common.h b/app/gl_common.h index 2385846..4118f93 100644 --- a/app/gl_common.h +++ b/app/gl_common.h @@ -3,8 +3,8 @@ #define IMAGE_H (MT9V03X_H) #define IMAGE_W (MT9V03X_W) -#define BEGINH_L (60) -#define BEGINH_R (60) +#define BEGINH_L (55) +#define BEGINH_R (55) #define BEGINW_L (-18) #define BEGINW_R (-12) #define PT_MAXLEN (75) diff --git a/app/gl_tracking.c b/app/gl_tracking.c index 4fd08a2..6108d5c 100644 --- a/app/gl_tracking.c +++ b/app/gl_tracking.c @@ -8,126 +8,119 @@ float dx_near; float (*rpts)[2]; int rpts_num; - - -void tracking(){ +void tracking() +{ if (pts_resample_left_count < pts_resample_right_count / 2 && pts_resample_left_count < 50) { - track_type = TRACK_RIGHT; - } else if (pts_resample_right_count < pts_resample_left_count / 2 && pts_resample_right_count < 58) { - track_type = TRACK_LEFT; - } else if (pts_resample_left_count < 20 && pts_resample_right_count > pts_resample_left_count) { - track_type = TRACK_RIGHT; - } else if (pts_resample_right_count < 20 && pts_resample_left_count > pts_resample_right_count) { - track_type = TRACK_LEFT; + track_type = TRACK_RIGHT; + } else if (pts_resample_right_count < pts_resample_left_count / 2 && pts_resample_right_count < 58) { + track_type = TRACK_LEFT; + } else if (pts_resample_left_count < 20 && pts_resample_right_count > pts_resample_left_count) { + track_type = TRACK_RIGHT; + } else if (pts_resample_right_count < 20 && pts_resample_left_count > pts_resample_right_count) { + track_type = TRACK_LEFT; + } +} + +void ElementJudge() +{ + CheckGarage(); + if (garage_type == GARAGE_NONE) { + CheckCross(); + if (cross_type == CROSS_NONE) { + CheckCircle(); } - -} - - - -void ElementJudge() { - CheckGarage(); - if (garage_type == GARAGE_NONE) { - CheckCross(); - if (cross_type == CROSS_NONE) { - CheckCircle(); } - } - if (garage_type != GARAGE_NONE) - { - cross_type = CROSS_NONE; - circle_type = CIRCLE_NONE; - } - + if (garage_type != GARAGE_NONE) { + cross_type = CROSS_NONE; + circle_type = CIRCLE_NONE; + } } -void ElementRun() { - if (garage_type != GARAGE_NONE) { - RunGarage(); - } +void ElementRun() +{ + if (garage_type != GARAGE_NONE) { + RunGarage(); + } - else if (cross_type != CROSS_NONE) { - RunCross(); - } + else if (cross_type != CROSS_NONE) { + RunCross(); + } - else if (circle_type != CIRCLE_NONE) { - RunCircle(); - } + else if (circle_type != CIRCLE_NONE) { + RunCircle(); + } } -void MidLineTrack() { - if (cross_type == CROSS_IN) { - if (track_type == TRACK_LEFT) { - mid_track = mid_left; // 这是为了预先分配内存 - GetMidLine_Left(pts_far_resample_left + far_Lpt0_rpts0s_id, pts_far_resample_left_count - far_Lpt0_rpts0s_id, mid_left, (int) round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER * ROADWIDTH / 2); - mid_track_count = pts_far_resample_left_count - far_Lpt0_rpts0s_id; +void MidLineTrack() +{ + if (cross_type == CROSS_IN) { + if (track_type == TRACK_LEFT) { + mid_track = mid_left; // 这是为了预先分配内存 + GetMidLine_Left(pts_far_resample_left + far_Lpt0_rpts0s_id, pts_far_resample_left_count - far_Lpt0_rpts0s_id, mid_left, (int)round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER * ROADWIDTH / 2); + mid_track_count = pts_far_resample_left_count - far_Lpt0_rpts0s_id; + } else { + mid_track = mid_right; // 这是为了预先分配内存 + GetMidLine_Right(pts_far_resample_right + far_Lpt1_rpts1s_id, pts_far_resample_right_count - far_Lpt1_rpts1s_id, mid_right, (int)round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER * ROADWIDTH / 2); + mid_track_count = pts_far_resample_right_count - far_Lpt1_rpts1s_id; + } } else { - mid_track = mid_right; // 这是为了预先分配内存 - GetMidLine_Right(pts_far_resample_right + far_Lpt1_rpts1s_id, pts_far_resample_right_count - far_Lpt1_rpts1s_id, mid_right, (int) round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER * ROADWIDTH / 2); - mid_track_count =pts_far_resample_right_count - far_Lpt1_rpts1s_id; + if (track_type == TRACK_LEFT) { + mid_track = mid_left; + mid_track_count = mid_left_count; + } else { + mid_track = mid_right; + mid_track_count = mid_right_count; + } } - } else { - if (track_type == TRACK_LEFT) { - mid_track = mid_left; - mid_track_count = mid_left_count; - } else { - mid_track = mid_right; - mid_track_count = mid_right_count; + + // 车轮对应点 (纯跟踪起始点) + float cx = InverseMapW[(int)(IMAGE_H * 0.75f)][70]; + float cy = InverseMapH[(int)(IMAGE_H * 0.75f)][70]; + + // 找最近点 (起始点中线归一化) + float min_dist = 1e10; + + int begin_id = -1; + for (int i = 0; i < mid_track_count; i++) { + float dx = mid_track[i][1] - cx; + float dy = mid_track[i][0] - cy; + float dist = sqrt(dx * dx + dy * dy); + if (dist < min_dist) { + min_dist = dist; + begin_id = i; + } } - } - // 车轮对应点 (纯跟踪起始点) - float cx = InverseMapW[(int) (IMAGE_H * 0.75f)][90]; - float cy = InverseMapH[(int) (IMAGE_H * 0.75f)][90]; + if (begin_id >= 0 && mid_track_count - begin_id >= 3) { + // 归一化中线 + mid_track[begin_id][0] = cy; + mid_track[begin_id][1] = cx; + rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]); + GetLinesResample(mid_track + begin_id, mid_track_count - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER); - // 找最近点 (起始点中线归一化) - float min_dist = 1e10; + // 远预锚点位置- + int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1); - int begin_id = -1; - for (int i = 0; i < mid_track_count; i++) { - float dx = mid_track[i][1] - cx; - float dy = mid_track[i][0] - cy; - float dist = sqrt(dx * dx + dy * dy); - if (dist < min_dist) { - min_dist = dist; - begin_id = i; - } - } + // 近锚点位置 + int aim_idx_near = clip(round(0.11 / RESAMPLEDIST), 0, rptsn_num - 1); - if (begin_id >= 0 && mid_track_count - begin_id >= 3){ - // 归一化中线 - mid_track[begin_id][0] = cy; - mid_track[begin_id][1] = cx; - rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]); - GetLinesResample(mid_track + begin_id, mid_track_count - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER ); + // 计算远锚点偏差值 + 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; - // 远预锚点位置 - int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1); + // 计算近锚点偏差值 + 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; - //近锚点位置 - int aim_idx_near = clip(round(0.15 / 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_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; - - // //考虑近点 - // near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI * 180 ; - // //考虑远点 - // pure_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx/ dn / dn) / PI * 180 ; - - - - } + // //考虑近点 + // near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI * 180 ; + // //考虑远点 + // pure_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx/ dn / dn) / PI * 180 ; + } } - diff --git a/libraries/zf_device/zf_device_mt9v03x_dvp.h b/libraries/zf_device/zf_device_mt9v03x_dvp.h index 2c7333e..7598d94 100644 --- a/libraries/zf_device/zf_device_mt9v03x_dvp.h +++ b/libraries/zf_device/zf_device_mt9v03x_dvp.h @@ -104,7 +104,7 @@ // ͷƫݺԶƫƣüƫ #define MT9V03X_UD_OFFSET_DEF (0 ) // ͼƫ ֵ ƫ ֵ ƫ Ϊ 120 240 480 ʱ޷ƫ // ͷƫݺԶƫƣüƫ -#define MT9V03X_GAIN_DEF (64 ) // ͼ Χ [16-64] عʱ̶¸ıͼ̶ +#define MT9V03X_GAIN_DEF (32 ) // ͼ Χ [16-64] عʱ̶¸ıͼ̶ #define MT9V03X_PCLK_MODE_DEF (1 ) // ʱģʽ Χ [0-1] Ĭϣ0 ѡΪ[0źţ1ź] // ͨΪ 0ʹ CH32V307 DVP ӿڻ STM32 DCMI ӿڲɼҪΪ 1 // MT9V034 V1.5 Լϰ汾ָ֧