138 lines
4.4 KiB
C
138 lines
4.4 KiB
C
#include "zf_common_headfile.h"
|
|
#include "gl_headfile.h"
|
|
|
|
float (*mid_track)[2];
|
|
int32_t mid_track_count;
|
|
float pure_angle;
|
|
float dx_near;
|
|
float (*rpts)[2];
|
|
int rpts_num;
|
|
|
|
void tracking()
|
|
{
|
|
|
|
if (pts_resample_left_count < pts_resample_right_count / 2 && pts_resample_left_count < 50) {
|
|
track_type = TRACK_RIGHT;
|
|
} else if (pts_resample_right_count < pts_resample_left_count / 2 && pts_resample_right_count < 58) {
|
|
track_type = TRACK_LEFT;
|
|
} else if (pts_resample_left_count < 20 && pts_resample_right_count > pts_resample_left_count) {
|
|
track_type = TRACK_RIGHT;
|
|
} else if (pts_resample_right_count < 20 && pts_resample_left_count > pts_resample_right_count) {
|
|
track_type = TRACK_LEFT;
|
|
}
|
|
}
|
|
|
|
void aim_distance_select(void)
|
|
{
|
|
if (cross_type != CROSS_NONE) {
|
|
aim_distance = 0.4f;
|
|
} else if (circle_type != CIRCLE_NONE) {
|
|
aim_distance = 0.2f;
|
|
} else {
|
|
aim_distance = COMMON_AIM;
|
|
}
|
|
}
|
|
|
|
void ElementJudge()
|
|
{
|
|
CheckGarage();
|
|
if (garage_type == GARAGE_NONE) {
|
|
CheckCross();
|
|
if (cross_type == CROSS_NONE) {
|
|
CheckCircle();
|
|
}
|
|
}
|
|
|
|
if (garage_type != GARAGE_NONE) {
|
|
cross_type = CROSS_NONE;
|
|
circle_type = CIRCLE_NONE;
|
|
}
|
|
}
|
|
|
|
void ElementRun()
|
|
{
|
|
if (garage_type != GARAGE_NONE) {
|
|
RunGarage();
|
|
}
|
|
|
|
else if (cross_type != CROSS_NONE) {
|
|
RunCross();
|
|
}
|
|
|
|
else if (circle_type != CIRCLE_NONE) {
|
|
RunCircle();
|
|
}
|
|
}
|
|
|
|
void MidLineTrack()
|
|
{
|
|
if (cross_type == CROSS_IN) {
|
|
if (track_type == TRACK_LEFT) {
|
|
mid_track = mid_left; // 这是为了预先分配内存
|
|
GetMidLine_Left(pts_far_resample_left + far_Lpt0_rpts0s_id, pts_far_resample_left_count - far_Lpt0_rpts0s_id, mid_left, (int)round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER * ROADWIDTH / 2);
|
|
mid_track_count = pts_far_resample_left_count - far_Lpt0_rpts0s_id;
|
|
} else {
|
|
mid_track = mid_right; // 这是为了预先分配内存
|
|
GetMidLine_Right(pts_far_resample_right + far_Lpt1_rpts1s_id, pts_far_resample_right_count - far_Lpt1_rpts1s_id, mid_right, (int)round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER * ROADWIDTH / 2);
|
|
mid_track_count = pts_far_resample_right_count - far_Lpt1_rpts1s_id;
|
|
}
|
|
} else {
|
|
if (track_type == TRACK_LEFT) {
|
|
mid_track = mid_left;
|
|
mid_track_count = mid_left_count;
|
|
} else {
|
|
mid_track = mid_right;
|
|
mid_track_count = mid_right_count;
|
|
}
|
|
}
|
|
|
|
// 车轮对应点 (纯跟踪起始点)
|
|
float cx = InverseMapW[(int)(IMAGE_H * 0.75f)][70];
|
|
float cy = InverseMapH[(int)(IMAGE_H * 0.75f)][70];
|
|
|
|
// 找最近点 (起始点中线归一化)
|
|
float min_dist = 1e10;
|
|
|
|
int begin_id = -1;
|
|
for (int i = 0; i < mid_track_count; i++) {
|
|
float dx = mid_track[i][1] - cx;
|
|
float dy = mid_track[i][0] - cy;
|
|
float dist = Q_sqrt(dx * dx + dy * dy);
|
|
if (dist < min_dist) {
|
|
min_dist = dist;
|
|
begin_id = i;
|
|
}
|
|
}
|
|
|
|
if (begin_id >= 0 && mid_track_count - begin_id >= 3) {
|
|
// 归一化中线
|
|
mid_track[begin_id][0] = cy;
|
|
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);
|
|
|
|
// 近锚点位置
|
|
int aim_idx_near = clip(round(0.09 / RESAMPLEDIST), 0, rptsn_num - 1);
|
|
|
|
// 计算远锚点偏差值
|
|
float dx = rptsn[aim_idx][1] - cx;
|
|
float dy = cy - rptsn[aim_idx][0] + 0.2f * PIXPERMETER;
|
|
float dn = (float)Q_sqrt(dx * dx + dy * dy);
|
|
// float error = -atan2f(dx, dy) * 180 / PI32;
|
|
|
|
// 计算近锚点偏差值
|
|
dx_near = rptsn[aim_idx_near][1] - cx;
|
|
// float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
|
|
// float dn_near = Q_sqrt(dx_near * dx_near + dy_near * dy_near);
|
|
// float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;
|
|
|
|
// //考虑近点
|
|
// near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI32 * 180 ;
|
|
// //考虑远点
|
|
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
|
|
}
|
|
}
|