From 08e28643083ff1bee05ad872c98c708dc16dda35 Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期一, 26 四月 2021 13:07:04 +0800
Subject: [PATCH] 适应河南测试反馈,倒库,侧方,判定由挡位改变触发,坡起增加上方点。

---
 lib/src/main/cpp/test_items/park_bottom.cpp |  166 ++++++++++++++++++++++++++++++++++++++++++++++++++----
 1 files changed, 152 insertions(+), 14 deletions(-)

diff --git a/lib/src/main/cpp/test_items/park_bottom.cpp b/lib/src/main/cpp/test_items/park_bottom.cpp
index 12e80fc..7c2124c 100644
--- a/lib/src/main/cpp/test_items/park_bottom.cpp
+++ b/lib/src/main/cpp/test_items/park_bottom.cpp
@@ -11,6 +11,7 @@
 #include "../utils/xconvert.h"
 #include "../master/comm_if.h"
 #include "area_exam.h"
+#include "../test_common/car_sensor.h"
 #include <vector>
 #include <cstdlib>
 
@@ -27,7 +28,14 @@
     THIRD_TOUCH_CTRL_LINE
 };
 
-static bool testing = false, reverseCar = false;
+enum {
+    TESTING,
+    TEST_FAIL,          // 鍥犺Е鍙戞煇浜涜鍒欙紝鍦ㄨ溅韬湭瀹屽叏绔嬪満鎯呭喌涓嬶紝鎻愬墠缁堟閮ㄥ垎娴嬭瘯
+    TEST_FINISH
+};
+
+static int testStatus;
+static bool reverseCar = false;
 static int mapIndex = 0;
 
 const uint32_t CHECK_PARK_DELAY = 400;
@@ -44,15 +52,21 @@
 static int darray[3];
 static int parkStatus[3];
 
+static int gearAtStop;
+static int currGear;
+static char prevCrossedCtrlLine;
+
 static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car);
 static bool EnterParking(const Polygon *map, const car_model *car);
 static bool CrashRedLine(const Polygon *map, const car_model *car, int &who);
 static bool ExitParkArea(const Polygon *map, const car_model *car);
+static bool AllTireExitParkArea(const Polygon *map, const car_model *car);
 
 void StartParkBottom(int index, int moveDirect, const struct RtkTime *rtkTime)
 {
     DEBUG("StartParkBottom");
-    testing = true;
+    testStatus = TESTING;
+
     reverseCar = false;
     mapIndex = index;
     memset(carray, 0, sizeof(carray));
@@ -67,6 +81,11 @@
     crossCtrlLineSw = false;
     reportParkFail = false;
     occurCrashRedLine = false;
+
+    currGear = ReadCarStatus(GEAR);
+    prevCrossedCtrlLine = 0;
+
+    PlayTTS("鎮ㄥ凡杩涘叆鍊掕溅鍏ュ簱鍖哄煙", NULL);
 }
 
 int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
@@ -78,16 +97,83 @@
     vector<Line> line_set;
     Line distance_line;
 
+    bool gear_change = false;
+    int gear = ReadCarStatus(GEAR);
+
+    if (gear == GEAR_R) {
+        if (currGear != GEAR_R) {
+            gear_change = true;
+            currGear = GEAR_R;
+        }
+    } else if (currGear == GEAR_R) {
+        gear_change = true;
+        currGear = gear;
+    }
+
+    if (testStatus == TESTING && gear_change) {
+        if (currGear == GEAR_R) {
+            // 鎸傚�掓尅,妫�娴嬫槸鍚﹁繃鎺у埗绾�
+            DEBUG("寮�濮嬫寕鍊掓尅");
+            if (!reverseCar) {
+                DEBUG("寮�濮嬮杞叆搴�");
+                reverseCar = true;
+                MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
+                firstReverseTimepoint = tp;             // 寮�濮�210绉掕鏃�
+            }
+            crossCtrlLine = CrossCtrlLine(map, car, carPrev);
+
+            if (parkCount >= 2) {
+                DEBUG("寮�濮嬫杞叆搴�");
+                parkCount = 0;
+                MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
+                firstReverseTimepoint = tp;             // 寮�濮�210绉掕鏃�
+            }
+
+            if (crossCtrlLine == 0) {
+                // 鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎
+                AddExamFault(20104, rtkTime);
+                DEBUG("鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎");
+            } else if (crossCtrlLine == prevCrossedCtrlLine) {
+                // 閲嶅璺ㄨ秺鍚屼竴鎺у埗绾匡紝涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮忥紝涓嶅悎鏍�
+                AddExamFault(20101, rtkTime);
+                DEBUG("涓嶆寜瑙勫畾绾胯矾锛岄『搴忓舰寮忥紝 鍚� %c 渚�", prevCrossedCtrlLine);
+            } else {
+                prevCrossedCtrlLine = crossCtrlLine;
+                DEBUG("寮�濮� %c 渚� 鍊掑簱", prevCrossedCtrlLine);
+            }
+        } else {
+            // 浠庡�掓尅绉诲嚭锛屾娴嬫槸鍚﹀叆搴�
+            DEBUG("浠庡�掓尅绉诲嚭");
+
+            parkCount++;
+
+            DEBUG("搴撲綅妫�鏌ユ鏁� = %d", parkCount);
+
+            if (EnterParking(map, car)) {
+                DEBUG("鍊掑簱鎴愬姛");
+            } else {
+                AddExamFault(20103, rtkTime);
+                DEBUG("鍊掑簱涓嶅叆");
+            }
+        }
+    }
+    
+    if (testStatus == TESTING && parkCount < 2 && AllTireExitParkArea(map, car)) {
+        testStatus = TEST_FAIL;
+        AddExamFault(10103, rtkTime);
+        DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
+    }
+    
     if (ExitParkArea(map, car)) {
         DEBUG("绂诲紑鍦哄湴");
         // 绂诲紑鍦哄湴
-        testing = false;
-        if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail && reverseCar) {
+        testStatus = TEST_FINISH;
+        /*if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail && reverseCar) {
             // 鍊掑簱涓嶅叆锛屼笉鍚堟牸
             reportParkFail = true;
             AddExamFault(20103, rtkTime);
             DEBUG("鍊掑簱涓嶅叆");
-        }
+        }*/
         goto TEST_END;
     }
 
@@ -108,22 +194,45 @@
     MA_SendDistance(dtox[0], dtox[1]);
 
     if (CrashRedLine(map, car, who)) {
-        if (!occurCrashRedLine && reverseCar) {
+        if (!occurCrashRedLine /*&& reverseCar*/) {
             occurCrashRedLine = true;
             // 杞﹁韩鍑虹嚎锛屼笉鍚堟牸
             AddExamFault(10116, rtkTime);
             DEBUG("杞﹁疆鍘嬬嚎");
-            if (who == 1) {
+            /*if (who == 1) {
                 PlayTTS("鍘嬪乏搴撲綅绾�", NULL);
             } else if (who == 2) {
                 PlayTTS("鍘嬪彸搴撲綅绾�", NULL);
-            }
+            }*/
         }
     } else {
         occurCrashRedLine = false;
     }
 
-    crossCtrlLine = CrossCtrlLine(map, car, carPrev);
+    if (moveDirect != prevMoveDirect) {
+        if (moveDirect == 0) {
+            stopTimepoint = tp;
+            gearAtStop = (currGear == GEAR_R ? 1 : 0);
+            DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+            DEBUG("鍋滆溅鏃舵尅浣� = %d", gearAtStop);
+        } else if (prevMoveDirect == 0) {
+            DEBUG("缁х画琛岄┒ %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+
+            DEBUG("鍋滆溅鏃堕棿 %ld", tp - stopTimepoint);
+            DEBUG("鍐嶆绉诲姩鏃舵尅浣� = %d", currGear == GEAR_R ? 1 : 0);
+
+            if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_bottom_pause_criteria)
+                && gearAtStop == (currGear == GEAR_R ? 1 : 0)) {
+                // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
+                AddExamFault(20106, rtkTime);
+                DEBUG("涓�斿仠杞�");
+            }
+        }
+
+        prevMoveDirect = moveDirect;
+    }
+
+/*    crossCtrlLine = CrossCtrlLine(map, car, carPrev);
     if (crossCtrlLine > 0 && !crossCtrlLineSw) {
         crossCtrlLineSw = true;
         if (parkCount == 0) {
@@ -143,14 +252,14 @@
             } else {
                 // 绂诲紑娴嬭瘯鍖猴紝鍋滄璁℃椂
                 DEBUG("绂诲紑娴嬭瘯鍖猴紝鍋滄璁℃椂");
-                testing = false;
+                testStatus = false;
                 goto TEST_END;
             }
             carray[2] = crossCtrlLine;
         }
     }
 
-    if (testing && darray[0] > 0 && tp - firstReverseTimepoint >= examParam.park_bottom_limit_time) {
+    if (testStatus && darray[0] > 0 && tp - firstReverseTimepoint >= examParam.park_bottom_limit_time) {
         // 瀹屾垚瓒呮椂锛屼笉鍚堟牸
         if (!reportExamTimeout) {
             reportExamTimeout = true;
@@ -235,13 +344,15 @@
             }
             checkPartStatus = false;
         }
-    }
+    }*/
 
 TEST_END:
-    if (!testing && reverseCar) {
+    if (testStatus == TEST_FINISH) {
+        DEBUG("鍊掑簱缁撴潫");
         MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 0);
+        return 0;
     }
-    return testing ? 1 : 0;
+    return 1;
 }
 
 // 妫�娴�2鍓嶈疆鏄惁姝e悜瓒婅繃宸﹀彸鎺у埗绾�
@@ -344,3 +455,30 @@
 
     return ret;
 }
+
+// 鍙屽墠杞拰浠讳竴鍚庤疆涓嶅湪鍖哄煙
+static bool AllTireExitParkArea(const Polygon *map, const car_model *car)
+{
+    int tireExitNum = 0;
+
+    if (IntersectionOf(car->carXY[ car->left_front_tire[TIRE_OUTSIDE] ], map) == GM_None) {
+        tireExitNum++;
+    }
+
+    if (IntersectionOf(car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ], map) == GM_None) {
+        tireExitNum++;
+    }
+
+    if (IntersectionOf(car->carXY[ car->left_rear_tire[TIRE_OUTSIDE] ], map) == GM_None) {
+        tireExitNum++;
+    }
+
+    if (IntersectionOf(car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ], map) == GM_None) {
+        tireExitNum++;
+    }
+
+    if (tireExitNum >= 3) {
+        return true;
+    }
+    return false;
+}

--
Gitblit v1.8.0