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

分辨率更新

Browse Source
This commit is contained in:
Glz
2024-03-08 19:58:36 +08:00
parent 690401acf1
commit aa69134eb7
9 changed files with 208 additions and 268 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
app/gl_tracking.c
View File

@@ -3,8 +3,8 @@
float (*mid_track)[2];
int32_t mid_track_count;
float near_angle;
float pure_angle;
float dx;
float dx_near;
float (*rpts)[2];
int rpts_num;
@@ -79,8 +79,8 @@ void MidLineTrack() {
}
// 车轮对应点 (纯跟踪起始点)
float cx = InverseMapW[(int) (IMAGE_H * 0.75f)][100];
float cy = InverseMapH[(int) (IMAGE_H * 0.75f)][100];
float cx = InverseMapW[(int) (IMAGE_H * 0.75f)][90];
float cy = InverseMapH[(int) (IMAGE_H * 0.75f)][90];
// 找最近点 (起始点中线归一化)
float min_dist = 1e10;
@@ -107,24 +107,24 @@ void MidLineTrack() {
int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1);
//近锚点位置
int aim_idx_near = clip(round(0.25 / RESAMPLEDIST), 0, rptsn_num - 1);
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 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 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 ;