检查障碍测试

app/gl_barrier.c

@@ -0,0 +1,15 @@
+#include "zf_common_headfile.h"
+#include "gl_headfile.h"
+
+enum barrier_type_e barrier_type = BARRIER_NONE;
+
+
+void CheckBarrier() {
+    if (barrier_type == BARRIER_NONE && Lpt0_found_barrier_in && !Lpt1_found_barrier_in && is_straight1) {
+        barrier_type = BARRIER_LEFT_BEGIN;
+    }
+    if (barrier_type == BARRIER_NONE && !Lpt0_found_barrier_in && Lpt1_found_barrier_in && is_straight0) {
+        barrier_type = BARRIER_RIGHT_BEGIN;
+    }
+}
+

app/gl_barrier.h

@@ -0,0 +1,19 @@
+#ifndef BARRIER
+#define BARRIER
+
+enum barrier_type_e {
+  BARRIER_NONE,
+  BARRIER_LEFT_BEGIN,
+  BARRIER_RIGHT_BEGIN,
+  BARRIER_LEFT_RUNNING,
+  BARRIER_RIGHT_RUNNING,
+  BARRIER_LEFT_OUT,
+  BARRIER_RIGHT_OUT,
+};
+
+extern enum barrier_type_e barrier_type;
+
+void CheckBarrier(void);
+
+
+#endif /* COMMON_H_ */

app/gl_get_corners.c

@@ -32,6 +32,7 @@ void get_corners() {
 
 
     Lpt0_found_barrier = Lpt1_found_barrier = false;
+    Lpt0_found_barrier_in = Lpt1_found_barrier_in = false;
     for (int i = 0; i < pts_resample_left_count; i++) {
         if (angle_new_left_barrier[i] == 0) continue;
         int im1 = clip(i - (int) round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_left_count - 1);

app/gl_headfile.h

@@ -13,5 +13,6 @@
 #include "gl_cross.h"
 #include "gl_data.h"
 #include "math.h"
+#include "gl_barrier.h"
 
 #endif /* STATE_H_ */

app/gl_tracking.c

@@ -39,7 +39,10 @@ void ElementJudge()
     if (garage_type == GARAGE_NONE) {
         CheckCross();
         if (cross_type == CROSS_NONE) {
-            CheckCircle();
+            CheckBarrier();
+            if (barrier_type == BARRIER_NONE){
+                CheckCircle();
+            }
         }
     }
 
@@ -50,7 +53,13 @@ void ElementJudge()
     if (cross_type != CROSS_NONE)
     {
         circle_type = CIRCLE_NONE;
+        barrier_type = BARRIER_NONE;
     }
+    if (barrier_type != BARRIER_NONE)
+    {
+        circle_type = CIRCLE_NONE;
+    }
+    
     
 }