日常更新

app/gl_cross.c

@@ -5,23 +5,37 @@ enum cross_type_e cross_type = CROSS_NONE;
 bool far_Lpt0_found, far_Lpt1_found;
 int32_t Both_Boder_None_Cross;
 int far_Lpt0_rpts0s_id, far_Lpt1_rpts1s_id;
+int cross_easy = 0;
+int cross_open = 1;
 
-void CheckCross() {
+void CheckCross()
+{
     bool Xfound = Lpt0_found && Lpt1_found;
+    if (cross_open == 0) {
         if (cross_type == CROSS_NONE && Xfound) {
             cross_type = CROSS_BEGIN;
         }
     }
+    if (cross_open == 1) {
+        if (cross_type == CROSS_NONE && Xfound && cross_easy == 0) {
+            cross_type = CROSS_BEGIN;
+        }
+        if (cross_type == CROSS_NONE && cross_easy == 1 && (Lpt0_found || Lpt1_found)) {
+            cross_type = CROSS_BEGIN;
+        }
+    }
+}
 
-void RunCross() {
+void RunCross()
+{
     bool Xfound = Lpt0_found && Lpt1_found;
     // 检测到十字，先按照近线走
+    if (cross_easy == 0) {
         if (cross_type == CROSS_BEGIN) {
             if (Lpt0_found) {
                 mid_left_count = pts_resample_left_count = Lpt0_rpts0s_id;
                 track_type                               = TRACK_LEFT;
-        }
+            } else if (Lpt1_found) {
-        else if(Lpt1_found) {
                 mid_right_count = pts_resample_right_count = Lpt1_rpts1s_id;
                 track_type                                 = TRACK_RIGHT;
             }
@@ -30,7 +44,6 @@ void RunCross() {
             if ((Xfound && (Lpt0_rpts0s_id < 0.2 / RESAMPLEDIST && Lpt1_rpts1s_id < 0.2 / RESAMPLEDIST)) || (pts_resample_left_count < 20 && pts_resample_right_count < 20)) {
                 cross_type = CROSS_IN;
             }
-        
         }
         // 远线控制进十字,begin_y渐变靠近防丢线
         if (cross_type == CROSS_IN) {
@@ -43,17 +56,63 @@ void RunCross() {
             if (Both_Boder_None_Cross > 2 && pts_resample_left_count > 20 && pts_resample_right_count > 20) {
                 cross_type            = CROSS_NONE;
                 Both_Boder_None_Cross = 0;
+                cross_easy            = 1;
+            }
+            if (far_Lpt1_found) {
+                track_type = TRACK_RIGHT;
+            } else if (far_Lpt0_found) {
+                track_type = TRACK_LEFT;
+            } else if (Both_Boder_None_Cross > 0 && pts_resample_right_count < 5) {
+                track_type = TRACK_RIGHT;
+            } else if (Both_Boder_None_Cross > 0 && pts_resample_left_count < 5) {
+                track_type = TRACK_LEFT;
             }
-        if (far_Lpt1_found) { track_type = TRACK_RIGHT; }
-        else if (far_Lpt0_found) { track_type = TRACK_LEFT; }
-        else if (Both_Boder_None_Cross > 0 && pts_resample_right_count < 5) { track_type = TRACK_RIGHT; }
-        else if (Both_Boder_None_Cross > 0 && pts_resample_left_count < 5) { track_type = TRACK_LEFT; }
-
         }
     }
 
+    if (cross_easy == 1) {
+        if (cross_type == CROSS_BEGIN) {
+            if (Lpt0_found) {
+                mid_left_count = pts_resample_left_count = Lpt0_rpts0s_id;
+                track_type                               = TRACK_LEFT;
+            } else if (Lpt1_found) {
+                mid_right_count = pts_resample_right_count = Lpt1_rpts1s_id;
+                track_type                                 = TRACK_RIGHT;
+            }
 
-void cross_farline(){
+            // 近角点过少，进入远线控制
+            if ((Xfound && (Lpt0_rpts0s_id < 0.2 / RESAMPLEDIST && Lpt1_rpts1s_id < 0.2 / RESAMPLEDIST)) || (pts_resample_left_count < 20 && pts_resample_right_count < 20)) {
+                cross_type = CROSS_IN;
+            }
+        }
+        // 远线控制进十字,begin_y渐变靠近防丢线
+        if (cross_type == CROSS_IN) {
+            // 寻远线,算法与近线相同
+            cross_farline();
+
+            if (pts_resample_left_count < 5 && pts_resample_right_count < 5) {
+                Both_Boder_None_Cross++;
+            }
+            if (Both_Boder_None_Cross > 2 && pts_resample_left_count > 20 && pts_resample_right_count > 20) {
+                cross_type            = CROSS_NONE;
+                Both_Boder_None_Cross = 0;
+                cross_easy            = 0;
+            }
+            if (far_Lpt1_found) {
+                track_type = TRACK_RIGHT;
+            } else if (far_Lpt0_found) {
+                track_type = TRACK_LEFT;
+            } else if (Both_Boder_None_Cross > 0 && pts_resample_right_count < 5) {
+                track_type = TRACK_RIGHT;
+            } else if (Both_Boder_None_Cross > 0 && pts_resample_left_count < 5) {
+                track_type = TRACK_LEFT;
+            }
+        }
+    }
+}
+
+void cross_farline()
+{
     int cross_width = 4;
     int far_y1 = 0, far_y2 = 0;
     int far_x1 = 0, far_x2 = 0;
@@ -61,16 +120,14 @@ void cross_farline(){
     pts_far_left_count = sizeof(pts_far_left) / sizeof(pts_far_left[0]);
 
     int w1 = IMAGE_W / 2 - BEGINW_R, h1 = BEGINH_L;
-    for(;w1>cross_width*2; w1--) 
+    for (; w1 > cross_width * 2; w1--) {
-    {
         if (GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - 1) < FIX_BINTHRESHOLD) {
             far_x1 = w1 - cross_width;
             break;
         }
     }
     /*如果一行全为白色没写*/
-    if (far_x1 <= 4)
+    if (far_x1 <= 4) {
-    {
         far_x1 = 4;
     }
 
@@ -84,22 +141,20 @@ void cross_farline(){
     }
     if (GET_PIX_1C(mt9v03x_image_copy[0], far_y1, far_x1) >= FIX_BINTHRESHOLD) {
         SearchLineAdaptive_Left(mt9v03x_image_copy[0], SELFADAPT_KERNELSIZE, SELFADAPT_OFFSET, far_y1, far_x1, pts_far_left, &pts_far_left_count);
-    }
+    } else
-    else pts_far_left_count = 0;
+        pts_far_left_count = 0;
 
     int w2 = IMAGE_W / 2 + BEGINW_L, h2 = BEGINH_R;
     white_found         = false;
     pts_far_right_count = sizeof(pts_far_right) / sizeof(pts_far_right[0]);
-    for(;w2 < IMAGE_W - cross_width*2; w2++) 
+    for (; w2 < IMAGE_W - cross_width * 2; w2++) {
-    {
         if (GET_PIX_1C(mt9v03x_image_copy[0], h2, w2 + 1) < FIX_BINTHRESHOLD) {
             far_x2 = w2 + cross_width;
             break;
         }
     }
     /*如果一行全为白色没写*/
-    if (far_x2 >= 136)
+    if (far_x2 >= 136) {
-    {
         far_x2 = 136;
     }
 
@@ -113,8 +168,8 @@ void cross_farline(){
     }
     if (GET_PIX_1C(mt9v03x_image_copy[0], far_y2, far_x2) >= FIX_BINTHRESHOLD) {
         SearchLineAdaptive_Right(mt9v03x_image_copy[0], SELFADAPT_KERNELSIZE, SELFADAPT_OFFSET, far_y2, far_x2, pts_far_right, &pts_far_right_count);
-    }
+    } else
-    else pts_far_right_count = 0;
+        pts_far_right_count = 0;
 
     // 透视变换
     for (int i = 0; i < pts_far_left_count; i++) {
@@ -141,7 +196,6 @@ void cross_farline(){
     pts_far_resample_right_count = sizeof(pts_far_resample_right) / sizeof(pts_far_resample_right[0]);
     GetLinesResample(pts_far_filter_r, pts_far_filter_r_count, pts_far_resample_right, &pts_far_resample_right_count, RESAMPLEDIST * PIXPERMETER);
 
-
     int valid_far_points_count = 0;
 
     for (int i = 0; i < pts_far_resample_left_count; i++) {
@@ -200,19 +254,15 @@ void cross_farline(){
         }
     }
 
-
     for (int i = 0; i < pts_far_resample_right_count; i++) {
         if (far_angle_new_right[i] == 0) continue;
         int im1    = clip(i - (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_right_count - 1);
         int ip1    = clip(i + (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_right_count - 1);
         float conf = fabs(far_angle_right[i]) - (fabs(far_angle_right[im1]) + fabs(far_angle_right[ip1])) / 2;
 
-
         if (far_Lpt1_found == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.7 / RESAMPLEDIST) {
             far_Lpt1_rpts1s_id = i;
             far_Lpt1_found     = true;
         }
-
     }
-
 }

app/gl_cross.h

@@ -10,6 +10,7 @@ enum cross_type_e {
 extern enum cross_type_e cross_type;
 extern int32_t Both_Boder_None_Cross;
 extern int far_Lpt0_rpts0s_id, far_Lpt1_rpts1s_id;
+extern int cross_open;
 
 void CheckCross(void);
 void RunCross(void);