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