更新

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