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-05-24 20:42:56 +08:00
parent 2ac6683128
commit d4afb39f8f
8 changed files with 324 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files

168
app/gl_tracking.c
View File

@@ -7,6 +7,43 @@ float pure_angle;
float dx_near;
float (*rpts)[2];
int rpts_num;
float last_pure_angle = 0.0f;
// 计算最小二乘法斜率的函数
float leastSquaresSlope(float points[][2], int n)
{
float sum_x = 0;
float sum_y = 0;
float sum_xy = 0;
float sum_x_squared = 0;
// 计算各项和
for (int i = 0; i < n; i++) {
sum_x += points[i][1];
sum_y += points[i][0];
sum_xy += points[i][1] * points[i][0];
sum_x_squared += points[i][1] * points[i][1];
}
// 计算斜率
float numerator = (float)n * sum_xy - sum_x * sum_y;
float denominator = (float)n * sum_x_squared - sum_x * sum_x;
if (denominator == 0) {
// 避免除以零错误
printf("Error: denominator is zero.\n");
return 0;
}
float temp = denominator / numerator;
return temp;
}
float calculateX(float a_x, float a_y, float slope, float b_y) {
float b_x = a_x - (b_y - a_y) * slope;
return b_x;
}
void tracking()
{
@@ -101,8 +138,63 @@ void MidLineTrack()
}
// 车轮对应点 (纯跟踪起始点)
float cx = InverseMapW[(int)(IMAGE_H * 0.75f)][70];
float cy = InverseMapH[(int)(IMAGE_H * 0.75f)][70];
float cx = InverseMapW[(int)(IMAGE_H * 0.8f)][70];
float cy = InverseMapH[(int)(IMAGE_H * 0.8f)][70];
// float slope = leastSquaresSlope(mid_track, mid_track_count);
int neary = mid_track[0][0];
int nearx = mid_track[0][1];
// float near_distance = calculateX(a_x, a_y, slope, cy);
int w1 = (int)cx;
int h1 = (int)cy;
int near_x1 = 0;
int near_x2 = 0;
if (GET_PIX_1C(mt9v03x_image_copy[0], h1, w1) >= FIX_BINTHRESHOLD)
{
for(;w1>10; w1--)
{
if(GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - 1) < FIX_BINTHRESHOLD) {
near_x1 = w1;
break;
}
}
for(;w1<130; w1++)
{
if(GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 + 1) < FIX_BINTHRESHOLD) {
near_x2 = w1;
break;
}
}
}
else
{
if (GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - BEGINW_R) < FIX_BINTHRESHOLD)
{
for(;w1>10; w1--)
{
if(GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - 1) > FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - 2) > FIX_BINTHRESHOLD) {
near_x1 = w1;
near_x2 = cx;
break;
}
}
}
if (GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 + BEGINW_R) < FIX_BINTHRESHOLD)
{
for(;w1<130; w1++)
{
if(GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 + 1) > FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 + 2) > FIX_BINTHRESHOLD) {
near_x1 = cx;
near_x2 = w1;
break;
}
}
}
}
// 找最近点 (起始点中线归一化)
float min_dist = 1e10;
@@ -126,50 +218,64 @@ void MidLineTrack()
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_judge = clip(round(aim_judge_far / 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, mid_track_count - 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_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_judge][0] - rptsn[aim_idx_near][0];
float dy2 = rptsn[aim_idx_judge][1] - rptsn[aim_idx_near][1];
float dx1 = mid_track[3 * (mid_track_count / 4)][1] - mid_track[aim_idx_judge][1];
float dy1 = mid_track[3 * (mid_track_count / 4)][0] - mid_track[aim_idx_judge][0];
float dn1 = Q_sqrt(dx1 * dx1 + dy1 * dy1);
float dx2 = mid_track[aim_idx_judge][1] - nearx;
float dy2 = mid_track[aim_idx_judge][0] - neary;
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 (angle_1 >= 0.3f || angle_1 <= -0.3f) {
if (angle_1 >= 0.2f || angle_1 <= -0.2f) {
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 || circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING)
//{
// state_type = TURN_STATE;
//}
// 计算远锚点偏差值
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;
bt_printf("%d", barrier_type);
if (barrier_type == BARRIER_LEFT_BEGIN) {
dx_near = rptsn[aim_idx_near][1] - cx + barrier_offset;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
} else if (barrier_type == BARRIER_RIGHT_BEGIN) {
dx_near = rptsn[aim_idx_near][1] - cx - barrier_offset;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
float dy = cy - rptsn[aim_idx][0]; // + 0.2f * PIXPERMETER;
float dn = (dx * dx + dy * dy);
float temp_near = 0;
if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f - 10;
} else if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING ) {
dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset;
} else {
dx_near = rptsn[aim_idx_near][1] - cx;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
if (fabs(cx - near_x1) > fabs(cx - near_x2))
{
dx_near = near_x2 - cx - 45;
temp_near = dx_near;
}
if (fabs(cx - near_x1) < fabs(cx - near_x2))
{
dx_near = near_x1 - cx + 45;
temp_near = dx_near;
}
if (fabs(cx - near_x1) == fabs(cx - near_x2))
{
dx_near = 0;
temp_near = dx_near;
}
//pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
if (dy > 0) {
pure_angle = -atanf(dx / dy) / PI32 * 180.0f;
last_pure_angle = pure_angle;
} else {
pure_angle = last_pure_angle;
}
}
// // 计算近锚点偏差值
@@ -184,11 +290,9 @@ void MidLineTrack()
// pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
}
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 = CIRCLE_STATE;
}
if (cross_type == CROSS_BEGIN || cross_type == CROSS_IN)
{
if (cross_type == CROSS_BEGIN || cross_type == CROSS_IN) {
state_type = STRAIGHT_STATE;
}
}