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-29 10:50:37 +08:00
parent 902d639475
commit 28636012f6
9 changed files with 145 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
app/gl_tracking.c
View File

@@ -124,6 +124,7 @@ void MidLineTrack()
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);
// 远预锚点位置-
int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1);
@@ -131,6 +132,33 @@ void MidLineTrack()
// 近锚点位置
int aim_idx_near = clip(round(0.09 / RESAMPLEDIST), 0, rptsn_num - 1);
int gap_1 = fabs(rptsn[3 * (rptsn_num / 4)][1] - rptsn[0][1]);
float dx1 = rptsn[3 * (rptsn_num / 4)][0] - rptsn[aim_idx][0];
float dy1 = rptsn[3 * (rptsn_num / 4)][1] - rptsn[aim_idx][1];
float dn1 = Q_sqrt(dx1 * dx1 + dy1 * dy1);
float dx2 = rptsn[aim_idx][0] - rptsn[aim_idx_near][0];
float dy2 = rptsn[aim_idx][1] - rptsn[aim_idx_near][1];
float dn2 = Q_sqrt(dx2 * dx2 + dy2 * dy2);
float c1 = dx1 / dn1;
float s1 = dy1 / dn1;
float c2 = dx2 / dn2;
float s2 = dy2 / dn2;
float angle_1= atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1);
ips114_show_float(120, 80, angle_1, 3, 4);
if (-0.3 <= angle_1 <= 0.3 )
{
state_type = TURN_STATE;
}
else
{
state_type = STRAIGHT_STATE;
}
if (circle_type == CIRCLE_LEFT_IN && circle_type == CIRCLE_LEFT_OUT && circle_type == CIRCLE_RIGHT_IN && circle_type == CIRCLE_RIGHT_OUT)
{
state_type = TURN_STATE;
}
// 计算远锚点偏差值
float dx = rptsn[aim_idx][1] - cx;
float dy = cy - rptsn[aim_idx][0] + 0.2f * PIXPERMETER;