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

日常更新

Browse Source
This commit is contained in:
井明江
2024-04-03 16:20:56 +08:00
parent 9e23f1968d
commit 2ac6683128
3 changed files with 12 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
app/gl_tracking.c
View File

@@ -127,16 +127,17 @@ void MidLineTrack()
// 远预锚点位置-
int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1);
int aim_idx_judge = clip(round(aim_judge_far / RESAMPLEDIST), 0, rptsn_num - 1);
// 近锚点位置
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 dx1 = rptsn[3 * (rptsn_num / 4)][0] - rptsn[aim_idx_judge][0];
float dy1 = rptsn[3 * (rptsn_num / 4)][1] - rptsn[aim_idx_judge][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 dx2 = rptsn[aim_idx_judge][0] - rptsn[aim_idx_near][0];
float dy2 = rptsn[aim_idx_judge][1] - rptsn[aim_idx_near][1];
float dn2 = Q_sqrt(dx2 * dx2 + dy2 * dy2);
float c1 = dx1 / dn1;
float s1 = dy1 / dn1;
@@ -185,4 +186,9 @@ void MidLineTrack()
if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_LEFT_OUT || circle_type == CIRCLE_RIGHT_IN || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING) {
state_type = TURN_STATE;
}
if (cross_type == CROSS_BEGIN || cross_type == CROSS_IN)
{
state_type = STRAIGHT_STATE;
}
}