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

fix: 修复图像bug

Browse Source
This commit is contained in:
井明江
2024-03-08 20:58:16 +08:00
parent a0076d760e
commit 577627c069
4 changed files with 100 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.eide/eide.json
View File

@@ -60,7 +60,8 @@
"libraries/zf_driver", "libraries/zf_driver",
"3rd-lib/crc16", "3rd-lib/crc16",
"app/page", "app/page",
"3rd-lib/lwrb/inc" "3rd-lib/lwrb/inc",
"app/tiny_frame"
], ],
"libList": [ "libList": [
"libraries/zf_device" "libraries/zf_device"

4
app/gl_common.h
View File

@@ -3,8 +3,8 @@
#define IMAGE_H (MT9V03X_H) #define IMAGE_H (MT9V03X_H)
#define IMAGE_W (MT9V03X_W) #define IMAGE_W (MT9V03X_W)
#define BEGINH_L (60) #define BEGINH_L (55)
#define BEGINH_R (60) #define BEGINH_R (55)
#define BEGINW_L (-18) #define BEGINW_L (-18)
#define BEGINW_R (-12) #define BEGINW_R (-12)
#define PT_MAXLEN (75) #define PT_MAXLEN (75)

197
app/gl_tracking.c
View File

@@ -8,126 +8,119 @@ float dx_near;
float (*rpts)[2]; float (*rpts)[2];
int rpts_num; int rpts_num;
void tracking()
{
void tracking(){
if (pts_resample_left_count < pts_resample_right_count / 2 && pts_resample_left_count < 50) { if (pts_resample_left_count < pts_resample_right_count / 2 && pts_resample_left_count < 50) {
track_type = TRACK_RIGHT; track_type = TRACK_RIGHT;
} else if (pts_resample_right_count < pts_resample_left_count / 2 && pts_resample_right_count < 58) { } else if (pts_resample_right_count < pts_resample_left_count / 2 && pts_resample_right_count < 58) {
track_type = TRACK_LEFT; track_type = TRACK_LEFT;
} else if (pts_resample_left_count < 20 && pts_resample_right_count > pts_resample_left_count) { } else if (pts_resample_left_count < 20 && pts_resample_right_count > pts_resample_left_count) {
track_type = TRACK_RIGHT; track_type = TRACK_RIGHT;
} else if (pts_resample_right_count < 20 && pts_resample_left_count > pts_resample_right_count) { } else if (pts_resample_right_count < 20 && pts_resample_left_count > pts_resample_right_count) {
track_type = TRACK_LEFT; 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() { void ElementRun()
if (garage_type != GARAGE_NONE) { {
RunGarage(); if (garage_type != GARAGE_NONE) {
} RunGarage();
}
else if (cross_type != CROSS_NONE) { else if (cross_type != CROSS_NONE) {
RunCross(); RunCross();
} }
else if (circle_type != CIRCLE_NONE) { else if (circle_type != CIRCLE_NONE) {
RunCircle(); RunCircle();
} }
} }
void MidLineTrack() { void MidLineTrack()
if (cross_type == CROSS_IN) { {
if (track_type == TRACK_LEFT) { if (cross_type == CROSS_IN) {
mid_track = mid_left; // 这是为了预先分配内存 if (track_type == TRACK_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 = mid_left; // 这是为了预先分配内存
mid_track_count = pts_far_resample_left_count - far_Lpt0_rpts0s_id; 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 { } else {
mid_track = mid_right; // 这是为了预先分配内存 if (track_type == TRACK_LEFT) {
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 = mid_left;
mid_track_count =pts_far_resample_right_count - far_Lpt1_rpts1s_id; 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; float cx = InverseMapW[(int)(IMAGE_H * 0.75f)][70];
mid_track_count = mid_left_count; float cy = InverseMapH[(int)(IMAGE_H * 0.75f)][70];
} else {
mid_track = mid_right; // 找最近点 (起始点中线归一化)
mid_track_count = mid_right_count; 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;
}
} }
}
// 车轮对应点 (纯跟踪起始点) if (begin_id >= 0 && mid_track_count - begin_id >= 3) {
float cx = InverseMapW[(int) (IMAGE_H * 0.75f)][90]; // 归一化中线
float cy = InverseMapH[(int) (IMAGE_H * 0.75f)][90]; 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++) { int aim_idx_near = clip(round(0.11 / RESAMPLEDIST), 0, rptsn_num - 1);
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;
}
}
if (begin_id >= 0 && mid_track_count - begin_id >= 3){ // 计算远锚点偏差值
// 归一化中线 dx = rptsn[aim_idx][1] - cx;
mid_track[begin_id][0] = cy; // float dy = cy - rptsn[aim_idx][0] + 0.2 * PIXPERMETER;
mid_track[begin_id][1] = cx; // float dn = sqrt(dx * dx + dy * dy);
rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]); // float error = -atan2f(dx, dy) * 180 / PI;
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); 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); // 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 ;
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 ;
}
} }

2
libraries/zf_device/zf_device_mt9v03x_dvp.h
View File

@@ -104,7 +104,7 @@
// 摄像头收偏移数据后会自动计算最大偏移，如果超出则设置计算出来的最大偏移 // 摄像头收偏移数据后会自动计算最大偏移，如果超出则设置计算出来的最大偏移
#define MT9V03X_UD_OFFSET_DEF (0 ) // 图像上下偏移量 正值 上偏移 负值 下偏移 行为 120 240 480 时无法设置偏移 #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：输出消隐信号] #define MT9V03X_PCLK_MODE_DEF (1 ) // 像素时钟模式 范围 [0-1] 默认：0 可选参数为：[0：不输出消隐信号，1：输出消隐信号]
// 通常都设置为 0，如果使用 CH32V307 的 DVP 接口或 STM32 的 DCMI 接口采集需要设置为 1 // 通常都设置为 0，如果使用 CH32V307 的 DVP 接口或 STM32 的 DCMI 接口采集需要设置为 1
// 仅总钻风 MT9V034 V1.5 以及以上版本支持该命令 // 仅总钻风 MT9V034 V1.5 以及以上版本支持该命令