From 7fdc4fa80d5d04b5936fc1bdd617b64c6ae9ef37 Mon Sep 17 00:00:00 2001
From: lizhanwei <Dana_Lee1016@126.com>
Date: 星期一, 26 四月 2021 13:17:00 +0800
Subject: [PATCH] Merge branch 'master' of https://gitee.com/endian11/DriveJudge
---
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鍓嶈疆鏄惁姝ｅ悜瓒婅繃宸﹀彸鎺у埗绾�
@@ -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