更新

.eide/eide.json

@@ -65,10 +65,9 @@
         "libList": [
           "libraries/zf_device"
         ],
-        "sourceDirList": [],
         "defineList": []
       }
     }
   },
-  "version": "3.3"
+  "version": "3.4"
 }

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)

app/gl_tracking.c

@@ -1,12 +1,15 @@
 #include "zf_common_headfile.h"
 #include "gl_headfile.h"
 #include "jj_blueteeth.h"
+#include <float.h>
 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 neary = (int)mid_track[0][0];
-    int nearx = mid_track[0][1];
+    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)) {
+    if (last_state == STRAIGHT_STATE && (state_type == TURN_STATE || state_type == CIRCLE_STATE)) {
-        turn_flag = 1;     
+        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;

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;

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:");

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);
         }
     }
 }

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  页面事件

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);
         }