diff --git a/app/gl_data.c b/app/gl_data.c index 88680dd..5d0835d 100644 --- a/app/gl_data.c +++ b/app/gl_data.c @@ -111,5 +111,6 @@ bool is_turn1_r; float rptsn[PT_MAXLEN][2]; int32_t rptsn_num; float aim_distance; +float aim_judge_far=0.3f; track_type_e track_type = TRACK_RIGHT; \ No newline at end of file diff --git a/app/gl_data.h b/app/gl_data.h index 04444c5..eba5dad 100644 --- a/app/gl_data.h +++ b/app/gl_data.h @@ -122,6 +122,7 @@ extern bool is_turn1_r; extern float rptsn[PT_MAXLEN][2]; extern int32_t rptsn_num; extern float aim_distance; +extern float aim_judge_far; extern track_type_e track_type; diff --git a/app/gl_tracking.c b/app/gl_tracking.c index eca0aa7..bfe05e6 100644 --- a/app/gl_tracking.c +++ b/app/gl_tracking.c @@ -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; + } + }