From a086ce17e46f15a8a6b4516e4d100164a1c5738f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E4=BA=95=E6=98=8E=E6=B1=9F?= <246462502@qq.com>
Date: Mon, 27 May 2024 21:17:40 +0800
Subject: [PATCH] =?UTF-8?q?=E6=97=A5=E5=B8=B8?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/by_frame.c    |  2 +-
 app/gl_tracking.c | 10 +++++++++-
 app/gl_tracking.h |  1 +
 app/main.c        |  2 +-
 4 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/app/by_frame.c b/app/by_frame.c
index 6370cdb..c790c28 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 183e291..812c27c 100644
--- a/app/gl_tracking.c
+++ b/app/gl_tracking.c
@@ -4,10 +4,12 @@
 float (*mid_track)[2];
 int32_t mid_track_count;
 float pure_angle;
+float pure_angle_half;
 float dx_near;
 float (*rpts)[2];
 int rpts_num;
 float last_pure_angle = 0.0f;
+float last_pure_angle_half = 0.0f;
 
 // 计算最小二乘法斜率的函数
 float leastSquaresSlope(float points[][2], int n)
@@ -222,7 +224,7 @@ void MidLineTrack()
         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 aim_idx_near = clip(round(aim_distance / 2 / RESAMPLEDIST), 0, rptsn_num - 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];
@@ -245,6 +247,9 @@ void MidLineTrack()
         float dx = rptsn[aim_idx][1] - cx;
         float dy = cy - rptsn[aim_idx][0]; // + 0.2f * PIXPERMETER;
         float dn = (dx * dx + dy * dy);
+        float dx_half = rptsn[aim_idx_near][1] - cx;
+        float dy_half = cy - rptsn[aim_idx_near][0]; // + 0.2f * PIXPERMETER;
+        float dn_half = (dx_half * dx_half + dy_half * dy_half);
         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;
@@ -273,9 +278,12 @@ void MidLineTrack()
             //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;
+                pure_angle_half = -atanf(dx_half / dy_half) / PI32 * 180.0f;
                 last_pure_angle = pure_angle;
+                last_pure_angle_half = pure_angle_half;
             } else {
                 pure_angle = last_pure_angle;
+                pure_angle_half = last_pure_angle_half;
             }
         }
 
diff --git a/app/gl_tracking.h b/app/gl_tracking.h
index 3af06cc..dddf9f8 100644
--- a/app/gl_tracking.h
+++ b/app/gl_tracking.h
@@ -4,6 +4,7 @@
 extern float (*mid_track)[2];
 extern int32_t mid_track_count;
 extern float pure_angle;
+extern float pure_angle_half;
 extern float dx_near;
 
 void aim_distance_select(void);
diff --git a/app/main.c b/app/main.c
index 0262f36..b9fe526 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 = dx_near;
+        tiny_frame_param[1].f32 = pure_angle_half;
         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) {