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 +++++++++++++++++++-
lib/src/main/cpp/driver_test.cpp | 10
lib/src/main/cpp/test_items/park_edge.cpp | 134 +++++++++++++---
lib/src/main/cpp/test_items/stop_and_start.cpp | 27 +++
lib/src/main/cpp/native-lib.cpp | 2
lib/src/main/cpp/test_items/area_exam.cpp | 123 ++++++++++-----
app/src/main/java/safeluck/drive/evaluation/fragment/MapFragment.java | 7
7 files changed, 376 insertions(+), 93 deletions(-)
diff --git a/app/src/main/java/safeluck/drive/evaluation/fragment/MapFragment.java b/app/src/main/java/safeluck/drive/evaluation/fragment/MapFragment.java
index 2a5fd54..2fc7362 100644
--- a/app/src/main/java/safeluck/drive/evaluation/fragment/MapFragment.java
+++ b/app/src/main/java/safeluck/drive/evaluation/fragment/MapFragment.java
@@ -672,7 +672,7 @@
path.lineTo((float) (base_x + (map[k][i][0]) * scale_x), (float) (base_y + (map[k][i][1]) * scale_y));
}
path.close();
- } else if (map[k].length == 9) {
+ } else if (map[k].length == 9 || map[k].length == 10) {
path.moveTo((float) (base_x + (map[k][0][0] - min_x) * scale_x), (float) (base_y + (map[k][0][1] - min_y) * scale_y));
path.lineTo((float) (base_x + (map[k][8][0] - min_x) * scale_x), (float) (base_y + (map[k][8][1] - min_y) * scale_y));
@@ -687,6 +687,11 @@
path.moveTo((float) (base_x + (map[k][7][0] - min_x) * scale_x), (float) (base_y + (map[k][7][1] - min_y) * scale_y));
path.lineTo((float) (base_x + (map[k][8][0] - min_x) * scale_x), (float) (base_y + (map[k][8][1] - min_y) * scale_y));
+
+ if (map[k].length == 10) {
+ path.moveTo((float) (base_x + (map[k][8][0] - min_x) * scale_x), (float) (base_y + (map[k][8][1] - min_y) * scale_y));
+ path.lineTo((float) (base_x + (map[k][9][0] - min_x) * scale_x), (float) (base_y + (map[k][9][1] - min_y) * scale_y));
+ }
} else {
path.moveTo((float) (base_x + (map[k][0][0] - min_x) * scale_x), (float) (base_y + (map[k][0][1] - min_y) * scale_y));
for (int i = 1; i < map[k].length; i++) {
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 884cab2..f6c9ee7 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -871,10 +871,12 @@
if (ExamType == TEST_TYPE_ROAD_DUMMY_LIGHT) {
if (exam_dummy_light == 0) {
- StartPrepare();
-// StartDummyLightExam(DummyLightContent, DummyLightContentSize, rtkTime);
- exam_dummy_light = 1;
- DEBUG("寮�濮嬩笂杞﹀噯澶�");
+// StartPrepare();
+//// StartDummyLightExam(DummyLightContent, DummyLightContentSize, rtkTime);
+// exam_dummy_light = 1;
+// DEBUG("寮�濮嬩笂杞﹀噯澶�");
+
+ exam_dummy_light = 2; // 棰戦棴涓婅溅鍑嗗
} else if (exam_dummy_light == 2) {
DEBUG("寮�濮嬬伅鍏夎�冭瘯");
StartDummyLightExam(DummyLightContent, DummyLightContentSize, rtkTime);
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index 7c14d93..4d2fcdb 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/lib/src/main/cpp/native-lib.cpp
@@ -28,7 +28,7 @@
const int RTK_PLATFORM_PORT = 12125;
const uint8_t phone[] = {0x20,0x19,0x10,0x15,0x00,0x00,0x00,0x01};
-const char *VIRTUAL_RTK_IP = "192.168.16.212";
+const char *VIRTUAL_RTK_IP = "192.168.43.76";
const int VIRTUAL_RTK_PORT = 9002;
static pthread_mutex_t tts_mutex = PTHREAD_MUTEX_INITIALIZER;
diff --git a/lib/src/main/cpp/test_items/area_exam.cpp b/lib/src/main/cpp/test_items/area_exam.cpp
index f4dcce9..8c95819 100644
--- a/lib/src/main/cpp/test_items/area_exam.cpp
+++ b/lib/src/main/cpp/test_items/area_exam.cpp
@@ -226,48 +226,66 @@
return i;
}
if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
- // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
- if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
- Line enterLine1, enterLine2;
+ Line triggerLine;
- MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
- MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
+ MakeLine(&triggerLine, &(mapList[i].map.point[1]), &(mapList[i].map.point[0]));
- if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
- DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
- score[i]++;
- }
- // 杞﹀熬椤剁偣鍦ㄥ満鍦板唴
- if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &mapList[i].map) == GM_Containment) {
- Line enterLine1, enterLine2;
+ if (CrashTriggerLine(triggerLine, car, CarModelList))
+ return i;
- MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
- MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
+ MakeLine(&triggerLine, &(mapList[i].map.point[7]), &(mapList[i].map.point[6]));
- if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine1) > 0.1 &&
- DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine2) > 0.1)
- score[i]++;
- }
+ if (CrashTriggerLine(triggerLine, car, CarModelList))
+ return i;
+
+// // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
+// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
+// Line enterLine1, enterLine2;
+//
+// MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+// MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
+//
+// if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
+// DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
+// score[i]++;
+// }
+// // 杞﹀熬椤剁偣鍦ㄥ満鍦板唴
+// if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &mapList[i].map) == GM_Containment) {
+// Line enterLine1, enterLine2;
+//
+// MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+// MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
+//
+// if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine1) > 0.1 &&
+// DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine2) > 0.1)
+// score[i]++;
+// }
}
if (mapList[i].type == MAP_TYPE_PART_EDGE) {
- // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
- if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
- Line enterLine;
+ Line triggerLine;
- MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+ MakeLine(&triggerLine, &(mapList[i].map.point[1]), &(mapList[i].map.point[0]));
- if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
- score[i]++;
- }
- // 杞﹀熬椤剁偣鍦ㄥ満鍦板唴
- if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &mapList[i].map) == GM_Containment) {
- Line enterLine;
-
- MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
-
- if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine) > 0.1)
- score[i]++;
- }
+ if (CrashTriggerLine(triggerLine, car, CarModelList))
+ return i;
+// // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
+// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
+// Line enterLine;
+//
+// MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+//
+// if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
+// score[i]++;
+// }
+// // 杞﹀熬椤剁偣鍦ㄥ満鍦板唴
+// if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &mapList[i].map) == GM_Containment) {
+// Line enterLine;
+//
+// MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+//
+// if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine) > 0.1)
+// score[i]++;
+// }
}
if (mapList[i].type == MAP_TYPE_TURN_90) {
// 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾�
@@ -328,7 +346,7 @@
}
if (mapList[index].type == MAP_TYPE_STOP_START) {
// 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
- double x9, y9, xo, yo;
+ double x10, y10, x9, y9, xo, yo;
bool enter = false;
@@ -338,17 +356,33 @@
x9 = 2*xo - mapList[index].map.point[8].X;
y9 = 2*yo - mapList[index].map.point[8].Y;
+ if (mapList[index].map.num > 9) {
+ // 鏋勯�犺櫄鎷熺殑鍙充笂瑙掔偣
+ xo = (mapList[index].map.point[9].X + mapList[index].map.point[7].X) / 2;
+ yo = (mapList[index].map.point[9].Y + mapList[index].map.point[7].Y) / 2;
+ x10 = 2*xo - mapList[index].map.point[8].X;
+ y10 = 2*yo - mapList[index].map.point[8].Y;
+ }
+
Polygon map;
map.num = 4;
map.point = (PointF *) malloc(map.num * sizeof(PointF));
- map.point[0] = mapList[index].map.point[0];
- map.point[1] = mapList[index].map.point[8];
- map.point[2] = mapList[index].map.point[7];
- map.point[3].X = x9;
- map.point[3].Y = y9;
-
+ if (mapList[index].map.num <= 9) {
+ map.point[0] = mapList[index].map.point[0];
+ map.point[1] = mapList[index].map.point[8];
+ map.point[2] = mapList[index].map.point[7];
+ map.point[3].X = x9;
+ map.point[3].Y = y9;
+ } else {
+ map.point[0] = mapList[index].map.point[0];
+ map.point[1] = mapList[index].map.point[9];
+ map.point[2].X = x10;
+ map.point[2].Y = y10;
+ map.point[3].X = x9;
+ map.point[3].Y = y9;
+ }
// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
Polygon carBody;
@@ -422,9 +456,12 @@
MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]);
- if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection || IntersectionOf(trace2_line, triggerLine) == GM_Intersection) &&
+ if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection &&
IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
- DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) {
+ DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) ||
+ (IntersectionOf(trace2_line, triggerLine) == GM_Intersection &&
+ IntersectionOfLine(p1, p2, car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]) == -1 &&
+ DistanceOf(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) ) {
// 纰板埌瑙﹀彂绾�
DEBUG("纰版挒瑙﹀彂绾� 寮曞彂鍦板浘");
trigger = true;
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;
+}
diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp
index e7b9366..cf155ed 100644
--- a/lib/src/main/cpp/test_items/park_edge.cpp
+++ b/lib/src/main/cpp/test_items/park_edge.cpp
@@ -20,6 +20,12 @@
using namespace std;
+enum {
+ TESTING,
+ TEST_FAIL, // 鍥犺Е鍙戞煇浜涜鍒欙紝鍦ㄨ溅韬湭瀹屽叏绔嬪満鎯呭喌涓嬶紝鎻愬墠缁堟閮ㄥ垎娴嬭瘯
+ TEST_FINISH
+};
+
const uint32_t CHECK_PARK_DELAY = 400;
static int mapIndex = 0;
@@ -30,9 +36,10 @@
static bool occurCrashRedLine1, occurCrashRedLine2;
static int prevMoveStatus, storeMoveStatusBeforeStop;
static int parkStatus;
-static bool occurMoveBack, parkSuccess;
+static int gearAtStop;
+static bool occurMoveBack, checkPark, parkSuccess, checkLight;
static uint32_t moveBackTimePoint;
-static bool testing = false;
+static int testStatus;
static bool CrashRedLine1(const Polygon *map, const car_model *car);
static bool CrashRedLine2(const Polygon *map, const car_model *car);
@@ -44,7 +51,7 @@
{
DEBUG("杩涘叆渚ф柟鍋滆溅鍦哄湴");
- testing = true;
+ testStatus = TESTING;
mapIndex = index;
occurCrashRedLine1 = occurCrashRedLine2 = false; // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑
@@ -54,12 +61,16 @@
parkSuccess = false;
parkStatus = 0;
occurMoveBack = false;
+ checkPark = false;
+ checkLight = false;
+ gearAtStop = -1;
+ PlayTTS("鎮ㄥ凡杩涘叆渚ф柟鍋滆溅鍖哄煙", NULL);
// 浠呭綋鍙戠敓鍊掕溅锛屾墠鎰忓懗鐫�椤圭洰寮�濮�
- if (moveStatus == -1) {
+ /*if (moveStatus == -1) {
occurMoveBack = true;
moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- }
+ }*/
}
int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime)
@@ -67,9 +78,39 @@
vector<double> dtox;
vector<Line> line_set;
Line distance_line;
+ bool is_gear_r = false;
+
+ // 棣栨鎸傚�掓尅锛� 鎵嶆剰鍛崇潃椤圭洰寮�濮�
+ if (testStatus == TESTING) {
+ if (ReadCarStatus(GEAR) == GEAR_R) {
+ is_gear_r = true;
+ if (!occurMoveBack) {
+ occurMoveBack = true;
+ moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss,
+ rtkTime->mss * 10); // 寮�濮嬭鏃�
+ MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
+ }
+ } else {
+ is_gear_r = false;
+ if (occurMoveBack && !checkPark) {
+ // 妫�鏌ヨ溅韬叆搴撴儏鍐�
+ DEBUG("妫�鏌ヨ溅韬叆搴撴儏鍐�");
+ checkPark = true;
+ if (EnterParking(map, car)) {
+ parkStatus = 1;
+ parkSuccess = true;
+ } else {
+ // 鍋滄鍚庯紝杞﹁韩鍑虹嚎锛屼笉鍚堟牸
+ AddExamFault(20401, rtkTime);
+ parkSuccess = false;
+ DEBUG("绉诲簱涓嶅叆");
+ }
+ }
+ }
+ }
if (CrashRedLine1(map, car)) {
- if (!occurCrashRedLine1 && occurMoveBack) {
+ if (!occurCrashRedLine1 /*&& occurMoveBack*/) {
// 杞﹁疆鍘嬭竟绾匡紝姣忔鎵�10鍒�
AddExamFault(20403, rtkTime);
DEBUG("杞﹁疆鍘嬭竟绾�");
@@ -80,7 +121,7 @@
}
if (CrashRedLine2(map, car)) {
- if (!occurCrashRedLine2 && occurMoveBack) {
+ if (!occurCrashRedLine2 /*&& occurMoveBack*/) {
// 杞﹁韩鍘嬪簱浣嶇嚎锛屾瘡娆℃墸10鍒�
AddExamFault(20404, rtkTime);
DEBUG("杞﹁韩鍘嬪簱浣嶇嚎");
@@ -90,14 +131,20 @@
occurCrashRedLine2 = false;
}
- if (ExitParkArea(map, car) || ExitParkArea2(map, car)) {
- if (!parkSuccess && occurMoveBack && !reportParkFail) {
+ if (ExitParkArea2(map, car)) {
+ /*if (!parkSuccess && occurMoveBack && !reportParkFail) {
// 鐩存帴椹剁娴嬭瘯鍖猴紝璁や负绉诲簱涓嶅叆
AddExamFault(10103, rtkTime);
reportParkFail = true;
DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
+ }*/
+ if (occurMoveBack && !checkPark) {
+ // 鍊掕溅鐩存帴椹剁娴嬭瘯鍖�
+ AddExamFault(10103, rtkTime);
+ DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
}
- testing = false;
+
+ testStatus = TEST_FINISH;
goto TEST_END;
}
@@ -117,6 +164,13 @@
DistanceOfTire2X(dtox, car, line_set);
MA_SendDistance(dtox[0], dtox[1]);
+ if (testStatus == TESTING && !occurMoveBack && ExitParkArea(map, car)) {
+ // 鍏ュ簱鍚庝竴鐩村墠杩涳紝杞﹀ご绉诲嚭椹剁绾�
+ AddExamFault(10103, rtkTime);
+ DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
+ testStatus = TEST_FAIL;
+ }
+
if (occurMoveBack) {
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
@@ -133,15 +187,33 @@
parkStatus = 0;
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
storeMoveStatusBeforeStop = prevMoveStatus;
-
+ gearAtStop = is_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);
- } else {
+ } else if (prevMoveStatus == 0) {
DEBUG("缁х画琛岄┒ %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
DEBUG("鍋滆溅鏃堕棿 %ld", tp - stopTimepoint);
- if (moveStatus == storeMoveStatusBeforeStop) {
+ if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria)
+ && occurMoveBack
+ && gearAtStop == (is_gear_r ? 1 : 0)) {
+ // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
+ AddExamFault(20406, rtkTime);
+ DEBUG("鍋滆溅瓒呮椂");
+ }
+
+ if (moveStatus == 1 && checkPark && !checkLight) {
+ // 鍦ㄨ繖閲屾鏌ヨ浆鍚戠伅鐘舵��
+ checkLight = true;
+ if (ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT) {
+ // 涓嶅紑杞悜鐏紝鎵�10鍒�
+ AddExamFault(20405, rtkTime);
+ DEBUG("鏈紑鍚浆鍚戠伅");
+ }
+ }
+
+ /*if (moveStatus == storeMoveStatusBeforeStop) {
// 鍚屾柟鍚戝啀鍚姩锛岀户缁垽鏂槸鍚﹀仠杞﹁秴鏃�
if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria) && occurMoveBack) {
// 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
@@ -172,18 +244,18 @@
DEBUG("鏈紑鍚浆鍚戠伅");
}
}
- }
+ }*/
- if (moveStatus == -1 && !occurMoveBack) {
+ /* if (moveStatus == -1 && !occurMoveBack) {
DEBUG("寮�濮嬪�掕溅");
occurMoveBack = true;
moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
- }
+ }*/
}
prevMoveStatus = moveStatus;
- } else if (moveStatus == 0 && parkStatus == 0) {
+ } /*else if (moveStatus == 0 && parkStatus == 0) {
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
if (tp - stopTimepoint >= CHECK_PARK_DELAY) {
@@ -194,13 +266,15 @@
parkStatus = -1;
}
}
- }
+ }*/
TEST_END:
- if (!testing && occurMoveBack) {
+ if (testStatus == TEST_FINISH) {
+ DEBUG("渚ф柟鍋滆溅缁撴潫");
MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 0);
+ return 0;
}
- return testing ? 1 : 0;
+ return 1;
}
// 杞﹁疆鏄惁鍘嬮亾璺竟绾�
@@ -234,7 +308,7 @@
bool ret = false;
Line red_line;
- const int red_lines[][2] = {{2, 3}, {3, 4}, {4, 5}};
+ const int red_lines[][2] = {{0, 7}, {2, 3}, {3, 4}, {4, 5}};
Polygon car_body;
@@ -249,6 +323,14 @@
if (IntersectionOf(red_line, &car_body) != GM_None) {
ret = true;
break;
+ }
+ }
+
+ if (!occurMoveBack) {
+ // 鍊掕溅鍓嶏紝杞﹁韩涓嶅緱鍘嬪簱浣嶈櫄绾�
+ MakeLine(&red_line, &map->point[2], &map->point[5]);
+ if (IntersectionOf(red_line, &car_body) != GM_None) {
+ ret = true;
}
}
@@ -282,14 +364,12 @@
return succ;
}
-// 鏁翠釜杞﹁締閮借椹惰繃鍓嶅簱浣嶇嚎
+// 杞﹀ご瑕侀┒杩囧墠搴撲綅绾�
static bool ExitParkArea(const Polygon *map, const car_model *car)
{
- for (int i = 0; i < car->bodyNum; ++i) {
- if (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->body[i]]) != -1)
- return false;
- }
- return true;
+ if (IntersectionOfLine(map->point[6], map->point[7], car->carXY[ car->axial[AXIAL_FRONT] ]) == -1)
+ return true;
+ return false;
}
static bool ExitParkArea2(const Polygon *map, const car_model *car)
diff --git a/lib/src/main/cpp/test_items/stop_and_start.cpp b/lib/src/main/cpp/test_items/stop_and_start.cpp
index 12e8000..672c0ad 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/stop_and_start.cpp
@@ -216,14 +216,27 @@
bool ret = false;
Line red_line;
- const int red_lines[][2] = {{0, 8}};
+ int red_lines[2][2];
+ int red_line_num = 0;
+
+ if (map->num == 10) {
+ red_lines[0][0] = 0;
+ red_lines[0][1] = 8;
+ red_lines[1][0] = 8;
+ red_lines[1][1] = 9;
+ red_line_num = 2;
+ } else {
+ red_lines[0][0] = 0;
+ red_lines[0][1] = 8;
+ red_line_num = 1;
+ }
Line frontAxle, rearAxle;
MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
- for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
+ for (int i = 0; i < red_line_num; ++i) {
MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
IntersectionOf(red_line, rearAxle) == GM_Intersection) {
@@ -285,11 +298,19 @@
Polygon carBody, map2;
PointF vPoint = Calc3Point(map->point[8], map->point[0], DistanceOf(map->point[8], map->point[7]), 'R');
+ PointF vPoint2;
+
+ if (map->num == 10) {
+ vPoint2 = Calc3Point(map->point[8], map->point[9], DistanceOf(map->point[8], map->point[7]), 'L');
+ }
map2.num = 4;
map2.point = (PointF *)malloc(map2.num * sizeof(PointF));
- MakePolygon(&map2, {vPoint, map->point[0], map->point[7], map->point[8]});
+ if (map->num == 9)
+ MakePolygon(&map2, {vPoint, map->point[0], map->point[8], map->point[7]});
+ else
+ MakePolygon(&map2, {vPoint, map->point[0], map->point[9], vPoint2});
carBody.num = car->bodyNum;
carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
--
Gitblit v1.8.0