From 57e132c5352d312eadfcba33bcfa5c0b1a2ee8b7 Mon Sep 17 00:00:00 2001 From: Glz <354338225@qq.com> Date: Tue, 18 Jun 2024 16:21:19 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .eide/eide.json | 3 +- app/by_frame.c | 2 +- app/gl_tracking.c | 87 ++++++++++++++++++++++++++++++++++----- app/gl_tracking.h | 1 + app/jj_param.c | 2 +- app/main.c | 4 +- app/page/page_dparam.c | 1 + app/page/page_ui_widget.c | 4 +- 8 files changed, 85 insertions(+), 19 deletions(-) diff --git a/.eide/eide.json b/.eide/eide.json index 59c83c9..9925e4c 100644 --- a/.eide/eide.json +++ b/.eide/eide.json @@ -65,10 +65,9 @@ "libList": [ "libraries/zf_device" ], - "sourceDirList": [], "defineList": [] } } }, - "version": "3.3" + "version": "3.4" } \ No newline at end of file diff --git a/app/by_frame.c b/app/by_frame.c index c790c28..6370cdb 100644 --- a/app/by_frame.c +++ b/app/by_frame.c @@ -16,7 +16,7 @@ void by_frame_init(void) { lwrb_init(&lwrb_ctx, frame_buffer_recv, sizeof(frame_buffer_recv)); // lwrb 最大元素数量为 buff 大小减一 uart_init(BY_FRAME_UART_INDEX, BY_FRAME_UART_BAUDRATE, BY_FRAME_UART_TX_PIN, BY_FRAME_UART_RX_PIN); - // uart_rx_interrupt(BY_FRAME_UART_INDEX, ENABLE); + uart_rx_interrupt(BY_FRAME_UART_INDEX, ENABLE); } void by_frame_send(uint32_t *data_array) diff --git a/app/gl_tracking.c b/app/gl_tracking.c index 3664bab..6adcd06 100644 --- a/app/gl_tracking.c +++ b/app/gl_tracking.c @@ -1,12 +1,15 @@ #include "zf_common_headfile.h" #include "gl_headfile.h" #include "jj_blueteeth.h" +#include float (*mid_track)[2]; int32_t mid_track_count; float pure_angle; float pure_angle_half; +float curvature; float dx_near; float curvature; +float last_curvature; float (*rpts)[2]; int rpts_num; float last_pure_angle = 0.0f; @@ -42,6 +45,39 @@ float leastSquaresSlope(float points[][2], int n) return temp; } +// 计算曲率的函数 +// 计算曲率的函数 +float calculate_curvature(float x[], float y[], int n) +{ + float total_curvature = 0.0; + + for (int i = 1; i < n - 1; i++) { + float x1 = x[i - 1], y1 = y[i - 1]; + float x2 = x[i], y2 = y[i]; + float x3 = x[i + 1], y3 = y[i + 1]; + + float dx1 = x2 - x1; + float dy1 = y2 - y1; + float dx2 = x3 - x2; + float dy2 = y3 - y2; + + float dx = (dx1 + dx2) / 2; + float dy = (dy1 + dy2) / 2; + + float ddx = x3 - 2 * x2 + x1; + float ddy = y3 - 2 * y2 + y1; + + float numerator = dx * ddy - dy * ddx; + float denominator = powf(dx * dx + dy * dy, 1.5f); + + if (fabs(denominator) > FLT_EPSILON) { + total_curvature += numerator / denominator; + } + } + + return total_curvature / (float)(n - 2); +} + float calculateX(float a_x, float a_y, float slope, float b_y) { float b_x = a_x - (b_y - a_y) * slope; @@ -117,7 +153,7 @@ void ElementRun() } else if (barrier_type != BARRIER_NONE) { RunBarrier(); } else if (s_type != S_NONE) { - RunS(); + // RunS(); } } @@ -141,15 +177,29 @@ void MidLineTrack() mid_track = mid_right; mid_track_count = mid_right_count; } + float x[mid_track_count / 2]; + float y[mid_track_count / 2]; + + for (int i = 0; i < mid_track_count / 2; i++) { + x[i] = mid_track[i][1]; + y[i] = mid_track[i][0]; + } + + if (mid_track_count <= 4) { + curvature = last_curvature; + /* code */ + } else { + curvature = calculate_curvature(x, y, mid_track_count / 2); + } + last_curvature = curvature; } // 车轮对应点 (纯跟踪起始点) float cx = InverseMapW[(int)(IMAGE_H * 0.8f)][70]; float cy = InverseMapH[(int)(IMAGE_H * 0.8f)][70]; - int neary = mid_track[0][0]; - int nearx = mid_track[0][1]; - + int neary = (int)mid_track[0][0]; + int nearx = (int)mid_track[0][1]; // 找最近点 (起始点中线归一化) float min_dist = 1e10; @@ -172,6 +222,23 @@ void MidLineTrack() rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]); GetLinesResample(mid_track + begin_id, mid_track_count - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER); + if (cross_type == CROSS_IN) { + float x[rptsn_num / 2]; + float y[rptsn_num / 2]; + + for (int i = 0; i < rptsn_num / 2; i++) { + x[i] = rptsn[i][1]; + y[i] = rptsn[i][0]; + } + if (rptsn_num <= 4) { + curvature = last_curvature; + /* code */ + } else { + curvature = calculate_curvature(x, y, rptsn_num / 2); + } + last_curvature = curvature; + } + // 远预锚点位置- 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); @@ -213,20 +280,18 @@ void MidLineTrack() if (dy > 0) { pure_angle = -atanf(dx / dy) / PI32 * 180.0f; last_pure_angle = pure_angle; - //last_pure_angle_half = pure_angle_half; + // last_pure_angle_half = pure_angle_half; } else { pure_angle = last_pure_angle; - //pure_angle_half = last_pure_angle_half; + // pure_angle_half = last_pure_angle_half; } } - // // 计算近锚点偏差值 // 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; - } 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 || circle_type == CIRCLE_LEFT_BEGIN || circle_type == CIRCLE_RIGHT_BEGIN) { state_type = CIRCLE_STATE; @@ -244,14 +309,14 @@ void MidLineTrack() // aim_distance = common_aim; // } - if (last_state == STRAIGHT_STATE && (state_type == TURN_STATE || state_type == CIRCLE_STATE)) { - turn_flag = 1; + if (last_state == STRAIGHT_STATE && (state_type == TURN_STATE || state_type == CIRCLE_STATE)) { + turn_flag = 1; timer_clear(TIM_3); timer_start(TIM_3); } if (turn_flag == 1) { aim_distance = turn_aim; - + uint16 ti = timer_get(TIM_3); if (ti >= 2000) { turn_flag = 0; diff --git a/app/gl_tracking.h b/app/gl_tracking.h index 3dc01c3..39720fe 100644 --- a/app/gl_tracking.h +++ b/app/gl_tracking.h @@ -6,6 +6,7 @@ extern int32_t mid_track_count; extern float pure_angle; extern float pure_angle_half; extern float dx_near; +extern float curvature; extern int8_t turn_flag ; extern float curvature; diff --git a/app/jj_param.c b/app/jj_param.c index d40bb57..d5fb726 100644 --- a/app/jj_param.c +++ b/app/jj_param.c @@ -24,7 +24,7 @@ void jj_param_eeprom_init(void) PARAM_REG(aim_mid, &mid_aim, EFLOAT, 1, "mid:"); PARAM_REG(offset_barrier, &barrier_offset, EFLOAT, 1, "offs:"); PARAM_REG(delta_x, &pure_angle, EFLOAT, 2, "far:"); - PARAM_REG(delta_y, &dx_near, EFLOAT, 2, "near:"); + PARAM_REG(delta_y, &curvature, EFLOAT, 2, "cur:"); PARAM_REG(aim_straight, &straight_aim, EFLOAT, 1, "str:"); PARAM_REG(aim_turn, &turn_aim, EFLOAT, 1, "tur:"); PARAM_REG(baoguang, &FIX_BINTHRESHOLD, EFLOAT, 1, "bao:"); diff --git a/app/main.c b/app/main.c index a818e63..b6e056f 100644 --- a/app/main.c +++ b/app/main.c @@ -57,7 +57,7 @@ int main(void) Page_Run(); by_buzzer_run(); tiny_frame_param[0].f32 = pure_angle; - tiny_frame_param[1].f32 = pure_angle_half; + tiny_frame_param[1].f32 = curvature; tiny_frame_param[2].u8[0] = (uint8_t)state_type; // 0: 无状态 1: 弯道 2: 直行 tiny_frame_param[2].u8[1] = (uint8_t)garage_type; // 0: 无车库 1: 有车库 if (mt9v03x_finish_flag) { @@ -73,7 +73,7 @@ int main(void) ElementJudge(); ElementRun(); MidLineTrack(); - //bt_printf("type= %d", circle_type); + //bt_printf("type= %d", (int)curvature); } } } diff --git a/app/page/page_dparam.c b/app/page/page_dparam.c index b9e7b6b..6971125 100644 --- a/app/page/page_dparam.c +++ b/app/page/page_dparam.c @@ -55,6 +55,7 @@ static void Loop() ips200_show_int(90, 18 + 74, cross_type, 1); ips200_show_int(90, 18 + 92, circle_type, 1); ips200_show_int(90, 18 + 110, s_type, 1); + ips200_show_float(90, 18 + 128, curvature, 4, 5); } /** * @brief 页面事件 diff --git a/app/page/page_ui_widget.c b/app/page/page_ui_widget.c index 3e58956..8f7209d 100644 --- a/app/page/page_ui_widget.c +++ b/app/page/page_ui_widget.c @@ -171,10 +171,10 @@ void Show_Marked_Image(void) uint16_t mix_y = START_Y + (uint16_t)(rptsn[i][0] * vertical_zoom_rate); // 近线数组显示 - if (i < pts_inv_l_count) { + if (i < pts_resample_left_count) { ips200_draw_rect(l_x, l_y, l_x + 2, l_y + 1, RGB565_RED); } - if (i < pts_inv_r_count) { + if (i < pts_resample_right_count) { ips200_draw_rect(r_x - 2, r_y, r_x, r_y + 1, RGB565_BLUE); }