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

1
app/gl_data.c
View File

@@ -111,5 +111,6 @@ bool is_turn1_r;
float rptsn[PT_MAXLEN][2]; float rptsn[PT_MAXLEN][2];
int32_t rptsn_num; int32_t rptsn_num;
float aim_distance; float aim_distance;
float aim_judge_far=0.3f;
track_type_e track_type = TRACK_RIGHT; track_type_e track_type = TRACK_RIGHT;

1
app/gl_data.h
View File

@@ -122,6 +122,7 @@ extern bool is_turn1_r;
extern float rptsn[PT_MAXLEN][2]; extern float rptsn[PT_MAXLEN][2];
extern int32_t rptsn_num; extern int32_t rptsn_num;
extern float aim_distance; extern float aim_distance;
extern float aim_judge_far;
extern track_type_e track_type; extern track_type_e track_type;

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 = 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 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]); 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 dx1 = rptsn[3 * (rptsn_num / 4)][0] - rptsn[aim_idx_judge][0];
float dy1 = rptsn[3 * (rptsn_num / 4)][1] - rptsn[aim_idx][1]; float dy1 = rptsn[3 * (rptsn_num / 4)][1] - rptsn[aim_idx_judge][1];
float dn1 = Q_sqrt(dx1 * dx1 + dy1 * dy1); float dn1 = Q_sqrt(dx1 * dx1 + dy1 * dy1);
float dx2 = rptsn[aim_idx][0] - rptsn[aim_idx_near][0]; float dx2 = rptsn[aim_idx_judge][0] - rptsn[aim_idx_near][0];
float dy2 = rptsn[aim_idx][1] - rptsn[aim_idx_near][1]; float dy2 = rptsn[aim_idx_judge][1] - rptsn[aim_idx_near][1];
float dn2 = Q_sqrt(dx2 * dx2 + dy2 * dy2); float dn2 = Q_sqrt(dx2 * dx2 + dy2 * dy2);
float c1 = dx1 / dn1; float c1 = dx1 / dn1;
float s1 = dy1 / 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) { 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; state_type = TURN_STATE;
} }
if (cross_type == CROSS_BEGIN || cross_type == CROSS_IN)
{
state_type = STRAIGHT_STATE;
}
} }