From 24428a1b69afcf113883e9cede1159443201a1b8 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 02 四月 2020 18:57:07 +0800
Subject: [PATCH] 进出科目二区域调整
---
lib/src/main/cpp/test_items/driving_curve.cpp | 30 ++++
lib/src/main/cpp/driver_test.cpp | 157 ++++++++++++++++---------
lib/src/main/cpp/test_items/park_edge.h | 2
lib/src/main/cpp/test_items/park_bottom.h | 2
lib/src/main/cpp/test_items/stop_and_start.cpp | 40 +++++-
lib/src/main/cpp/driver_test.h | 8 +
lib/src/main/cpp/test_items/driving_curve.h | 2
lib/src/main/cpp/master/comm_if.h | 2
lib/src/main/cpp/test_items/park_bottom.cpp | 38 +++++-
lib/src/main/cpp/test_items/turn_a90.cpp | 12 +
lib/src/main/cpp/test_items2/drive_straight.cpp | 9 +
lib/src/main/cpp/test_items/park_edge.cpp | 20 ++
lib/src/main/cpp/master/comm_if.cpp | 18 +++
lib/src/main/cpp/test_items/turn_a90.h | 2
lib/src/main/cpp/test_items/stop_and_start.h | 2
15 files changed, 256 insertions(+), 88 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 0af9a65..9f47645 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -40,12 +40,6 @@
using namespace std;
-#define MAP_TYPE_PARK_BUTTOM 1
-#define MAP_TYPE_STOP_START 2
-#define MAP_TYPE_PART_EDGE 3
-#define MAP_TYPE_CURVE 4
-#define MAP_TYPE_TURN_90 5
-
#define RTK_INVALID 0
#define RTK_SINGLE_POINT 1
#define RTK_DIFF 2
@@ -117,7 +111,8 @@
static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
static bool UpdateCarCoord(double &spd, int &mov, int &idx);
-static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum);
+static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList);
+static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum);
static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum);
static int GetMapId(int index, const struct map_list *mapList, int mapNum);
static int GetMapType(int index, const struct map_list *mapList, int mapNum);
@@ -650,7 +645,7 @@
RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
brief.qf = RtkBuffer[index].qf;
- brief.map_id = GetMapId(CurrExamMapIndex, MapList, MapNum);
+ brief.map_id = -1;//GetMapId(CurrExamMapIndex, MapList, MapNum);
brief.move = move;
brief.speed = speed * 3.6;
brief.heading = RtkBuffer[index].heading;
@@ -712,18 +707,18 @@
}
if (CurrExamMapIndex < 0) {
if (CurrEnterMapIndex < 0) {
- CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum);
+ CurrEnterMapIndex = EnterMap(CarModel, CarModelList, MapList, MapNum);
if (CurrEnterMapIndex >= 0) {
- DEBUG("鍙戦�佽繘鍏ュ満鍦版姤鍛� %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
- MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);
+// DEBUG("鍙戦�佽繘鍏ュ満鍦版姤鍛� %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
+// MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);
CurrExamMapIndex = CurrEnterMapIndex;
CurrExamStatus = EXAM_AREA_START;
}
} else {
if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
- DEBUG("鍙戦�佺寮�鍦哄湴鎶ュ憡 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
- MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
+// DEBUG("鍙戦�佺寮�鍦哄湴鎶ュ憡 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
+// MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
CurrEnterMapIndex = -1;
}
}
@@ -894,33 +889,33 @@
DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴");
MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- StartParkBottom(move, rtkTime);
+ StartParkBottom(CurrExamMapIndex, move, rtkTime);
CurrExamStatus = EXAM_AREA_RUN;
break;
case MAP_TYPE_STOP_START:
DEBUG("杩涘叆涓婂潯璧锋鍦哄湴");
MA_SendDebugInfo("杩涘叆涓婂潯璧锋鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- StartSAS(move, rtkTime);
+ StartSAS(CurrExamMapIndex, move, rtkTime);
CurrExamStatus = EXAM_AREA_RUN;
break;
case MAP_TYPE_PART_EDGE:
DEBUG("杩涘叆渚ф柟浣嶅仠杞﹀満鍦�");
MA_SendDebugInfo("杩涘叆渚ф柟浣嶅仠杞﹀満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- StartParkEdge(move, rtkTime);
+ StartParkEdge(CurrExamMapIndex, move, rtkTime);
CurrExamStatus = EXAM_AREA_RUN;
break;
case MAP_TYPE_CURVE:
DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴");
MA_SendDebugInfo("杩涘叆鏇茬嚎琛岄┒鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- StartDrivingCurve(move, rtkTime);
+ StartDrivingCurve(CurrExamMapIndex, move, rtkTime);
CurrExamStatus = EXAM_AREA_RUN;
break;
case MAP_TYPE_TURN_90:
DEBUG("杩涘叆鐩磋杞集鍦哄湴");
MA_SendDebugInfo("杩涘叆鐩磋杞集鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- StartTurnA90(move, azimuth, rtkTime);
+ StartTurnA90(CurrExamMapIndex, move, azimuth, rtkTime);
CurrExamStatus = EXAM_AREA_RUN;
break;
default:break;
@@ -1145,15 +1140,79 @@
return true;
}
-static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum)
+static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
- // 杞︾殑鏈�鍓嶇偣鏄惁杩涘叆鍦板浘
+ bool trigger = false;
+
+ if (CarModelList.size() < 5)
+ return trigger;
+
+ Polygon trace, trace2;
+ int pn = 0;
+
+ trace2.num = trace.num = 5;
+ trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);
+ trace2.point = (PointF *) malloc(sizeof(PointF) * trace2.num);
+
+ list<car_model *>::iterator iter = CarModelList.begin();
+
+ car_model *c1 = *iter;
+
+ trace.point[pn] = c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]];
+ trace2.point[pn++] = c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]];
+
+ ++iter;
+
+ while (iter != CarModelList.end() && pn < trace.num) {
+ car_model *c2 = *iter;
+
+ uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
+
+ if (tdiff >= D_SEC(1)) {
+ trace.point[pn] = c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]];
+ trace2.point[pn++] = c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]];
+ c1 = c2;
+ }
+ ++iter;
+ }
+
+ PointF p1, p2;
+
+ p1.X = triggerLine.X1;
+ p1.Y = triggerLine.Y1;
+ p2.X = triggerLine.X2;
+ p2.Y = triggerLine.Y2;
+
+ int pp = 0;
+ for (int p = 1; p < pn; ++p) {
+ Line trace_line, trace2_line;
+
+ 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) &&
+ IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
+ DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) {
+ // 纰板埌瑙﹀彂绾�
+ DEBUG("纰版挒瑙﹀彂绾� 寮曞彂鍦板浘");
+ trigger = true;
+ goto SEARCH_TRIGGER_LINE_END;
+ }
+ }
+
+SEARCH_TRIGGER_LINE_END:
+ free(trace.point);
+
+ return trigger;
+}
+
+static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum)
+{
for (int i = 0; i < mapNum && car != NULL; ++i) {
+ // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾�
if (mapList[i].type == MAP_TYPE_STOP_START) {
// 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
double x9, y9, xo, yo;
-
- bool enter = false;
xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;
@@ -1161,31 +1220,18 @@
x9 = 2*xo - mapList[i].map.point[8].X;
y9 = 2*yo - mapList[i].map.point[8].Y;
- Polygon map;
+ Line triggerLine;
- map.num = 4;
- map.point = (PointF *) malloc(map.num * sizeof(PointF));
+ triggerLine.X1 = mapList[i].map.point[0].X;
+ triggerLine.Y1 = mapList[i].map.point[0].Y;
+ triggerLine.X2 = x9;
+ triggerLine.Y2 = y9;
- map.point[0] = mapList[i].map.point[0];
- map.point[1] = mapList[i].map.point[8];
- map.point[2] = mapList[i].map.point[7];
- map.point[3].X = x9;
- map.point[3].Y = y9;
-
- if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) {
- Line enterLine1;
-
- MakeLine(&enterLine1, &(map.point[0]), &(map.point[3]));
-
- if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
- enter = true;
- }
-
- free(map.point);
-
- if (enter) return i;
+ if (CrashTriggerLine(triggerLine, car, CarModelList))
+ 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;
@@ -1198,6 +1244,7 @@
}
}
if (mapList[i].type == MAP_TYPE_PART_EDGE) {
+ // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
Line enterLine;
@@ -1208,26 +1255,20 @@
}
}
if (mapList[i].type == MAP_TYPE_TURN_90) {
- if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
- Line enterLine1;
+ // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾�
+ Line triggerLine;
- MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+ MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
- if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
- return i;
- }
+ if (CrashTriggerLine(triggerLine, car, CarModelList))
+ return i;
}
if (mapList[i].type == MAP_TYPE_CURVE) {
- Line startLine;
- Line carAxial;
+ Line triggerLine;
- MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]);
- MakeLine(&carAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]);
-
- if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
- if (DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], startLine) > 0.1)
- return i;
- }
+ MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]);
+ if (CrashTriggerLine(triggerLine, car, CarModelList))
+ return i;
}
}
return -1;
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 684e996..0cc5aa0 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -21,7 +21,13 @@
#define TIRE_OUTSIDE 0
#define TIRE_INSIDE 1
#define AXIAL_FRONT 0
-#define AXIAL_REAR 1
+#define AXIAL_REAR 1
+
+#define MAP_TYPE_PARK_BUTTOM 1
+#define MAP_TYPE_STOP_START 2
+#define MAP_TYPE_PART_EDGE 3
+#define MAP_TYPE_CURVE 4
+#define MAP_TYPE_TURN_90 5
using namespace std;
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index ceaf567..90b4b1a 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -60,12 +60,15 @@
#define ID_MS_SYS_SHUTDOWN 0x8010
#define ID_SM_LIGHT_EXAM_REQ 0x0012
#define ID_MS_LIGHT_EXAM_RES 0x8012
+#define ID_SM_DISTANCE 0x0020
+
#define MA_OUT_GPS_BRIEF 0x0001
#define MA_OUT_RTK_BRIEF 0x0002
#define MA_OUT_CAR_BRIEF 0x0004
#define MA_OUT_RTCM_IND 0x0008
#define MA_OUT_DBG_INFO 0x0010
+
static int OnOff = 0;//0xFFFF;
@@ -352,6 +355,21 @@
SendMsgToMainProcIndep(ID_SM_RTK_BRIEF, sb.GetString());
}
+void MA_SendDistance(double l, double r)
+{
+ StringBuffer sb;
+ Writer<StringBuffer> writer(sb);
+
+ writer.StartObject();
+ writer.Key("left");
+ writer.Double(l);
+ writer.Key("right");
+ writer.Double(r);
+ writer.EndObject();
+
+ SendMsgToMainProcIndep(ID_SM_DISTANCE, sb.GetString());
+}
+
void MA_SendExamWrong(vector<ExamFault> &ExamFaultList)
{
StringBuffer sb;
diff --git a/lib/src/main/cpp/master/comm_if.h b/lib/src/main/cpp/master/comm_if.h
index 8e23f95..80d93a9 100644
--- a/lib/src/main/cpp/master/comm_if.h
+++ b/lib/src/main/cpp/master/comm_if.h
@@ -97,4 +97,6 @@
void MA_EnterMap(int map_id, int type, int enter);
void MA_ExamLight(void);
+void MA_SendDistance(double l, double r);
+
#endif //MYAPPLICATION2_COMM_IF_H
diff --git a/lib/src/main/cpp/test_items/driving_curve.cpp b/lib/src/main/cpp/test_items/driving_curve.cpp
index 7a33a68..24ad383 100644
--- a/lib/src/main/cpp/test_items/driving_curve.cpp
+++ b/lib/src/main/cpp/test_items/driving_curve.cpp
@@ -7,6 +7,8 @@
#include "../driver_test.h"
#include "../common/apptimer.h"
#include "../utils/xconvert.h"
+#include "../master/comm_if.h"
+#include "../native-lib.h"
#include <vector>
#include <cstdlib>
@@ -18,6 +20,7 @@
const uint32_t STOP_CAR_TIME = D_SEC(2);
static bool testing = false;
+static int mapIndex = 0;
static uint32_t stopTimepoint = 0;
static bool reportStopCarTimeout;
@@ -32,13 +35,14 @@
static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
-static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone);
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who);
-void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime)
+void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime)
{
DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴");
testing = true;
+ mapIndex = index;
prevMoveDirect = moveDirect;
if (moveDirect == 0) {
@@ -48,12 +52,14 @@
crashRedLine = false;
scanWindow.leftStart = scanWindow.leftEnd = scanWindow.rightStart = scanWindow.rightEnd = 0;
+
+ MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 1);
}
int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
Polygon tireRect;
-
+ int who = 0;
MakePolygon(&tireRect, {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
car->carXY[car->right_rear_tire[TIRE_OUTSIDE]],
@@ -71,12 +77,17 @@
DEBUG("scanWindow leftStart %d leftEnd %d rightStart %d rightEnd %d", scanWindow.leftStart, scanWindow.leftEnd, scanWindow.rightStart, scanWindow.rightEnd);
- if (CrashRedLine(map, map2, car, &scanWindow)) {
+ if (CrashRedLine(map, map2, car, &scanWindow, who)) {
if (!crashRedLine) {
crashRedLine = true;
// 杞﹁疆鍘嬭竟绾匡紝涓嶅悎鏍�
AddExamFault(27, rtkTime);
DEBUG("杞﹁疆鍘嬭竟绾�");
+ if (who == 1) {
+ PlayTTS("鍘嬪乏鏇茬嚎");
+ } else if (who == 2) {
+ PlayTTS("鍘嬪彸鏇茬嚎");
+ }
}
} else {
crashRedLine = false;
@@ -105,6 +116,11 @@
TEST_END:
CleanPolygon(&tireRect);
+
+ if (!testing) {
+ MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 0);
+ }
+
return testing ? 1 : 0;
}
@@ -276,7 +292,7 @@
}
// 杞﹁疆鏄惁鍘嬭竟绾�
-static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone)
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who)
{
bool ret = false;
@@ -290,9 +306,11 @@
for (int e = zone->leftStart - 1; e >= zone->leftEnd; --e) {
MakeLine(&redLine, &map->point[s], &map->point[e]);
if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
+ who = 1;
return true;
}
if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
+ who = 1;
return true;
}
s = e;
@@ -302,9 +320,11 @@
for (int e = zone->rightStart - 1; e >= zone->rightEnd; --e) {
MakeLine(&redLine, &map2->point[s], &map2->point[e]);
if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
+ who = 2;
return true;
}
if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
+ who = 2;
return true;
}
s = e;
diff --git a/lib/src/main/cpp/test_items/driving_curve.h b/lib/src/main/cpp/test_items/driving_curve.h
index 22c7181..e9f39fc 100644
--- a/lib/src/main/cpp/test_items/driving_curve.h
+++ b/lib/src/main/cpp/test_items/driving_curve.h
@@ -10,7 +10,7 @@
using namespace std;
-void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime);
+void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime);
bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model *car);
int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime);
diff --git a/lib/src/main/cpp/test_items/park_bottom.cpp b/lib/src/main/cpp/test_items/park_bottom.cpp
index b0a598b..3a59252 100644
--- a/lib/src/main/cpp/test_items/park_bottom.cpp
+++ b/lib/src/main/cpp/test_items/park_bottom.cpp
@@ -9,6 +9,7 @@
#include "../jni_log.h"
#include "../driver_test.h"
#include "../utils/xconvert.h"
+#include "../master/comm_if.h"
#include <vector>
#include <cstdlib>
@@ -27,7 +28,8 @@
const int PARK_TIMEOUT = D_SEC(210);
-static bool testing = false;
+static bool testing = false, reverseCar = false;
+static int mapIndex = 0;
const uint32_t STOP_CAR_TIME = D_SEC(2);
static uint32_t stopTimepoint;
@@ -44,13 +46,15 @@
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);
+static bool CrashRedLine(const Polygon *map, const car_model *car, int &who);
static bool ExitParkArea(const Polygon *map, const car_model *car);
-void StartParkBottom(int moveDirect, const struct RtkTime *rtkTime)
+void StartParkBottom(int index, int moveDirect, const struct RtkTime *rtkTime)
{
DEBUG("StartParkBottom");
testing = true;
+ reverseCar = false;
+ mapIndex = index;
memset(carray, 0, sizeof(carray));
memset(darray, 0, sizeof(darray));
memset(parkStatus, 0, sizeof(parkStatus));
@@ -69,12 +73,13 @@
{
char crossCtrlLine = 0;
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
+ int who = 0;
if (ExitParkArea(map, car)) {
DEBUG("绂诲紑鍦哄湴");
// 绂诲紑鍦哄湴
testing = false;
- if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail) {
+ if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail && reverseCar) {
// 鍊掑簱涓嶅叆锛屼笉鍚堟牸
reportParkFail = true;
AddExamFault(8, rtkTime);
@@ -83,12 +88,18 @@
goto TEST_END;
}
- if (CrashRedLine(map, car)) {
+
+ if (CrashRedLine(map, car, who)) {
if (!occurCrashRedLine) {
occurCrashRedLine = true;
// 杞﹁韩鍑虹嚎锛屼笉鍚堟牸
AddExamFault(7, rtkTime);
DEBUG("杞﹁疆鍘嬬嚎");
+ if (who == 1) {
+ PlayTTS("鍘嬪乏搴撲綅绾�");
+ } else if (who == 2) {
+ PlayTTS("鍘嬪彸搴撲綅绾�");
+ }
}
} else {
occurCrashRedLine = false;
@@ -153,6 +164,10 @@
}
} else if (moveDirect == -1) {
// 鍒囨崲涓哄�掕溅
+ if (!reverseCar) {
+ reverseCar = true;
+ MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
+ }
if (darray[parkCount] == 0) {
if (!crossCtrlLineSw) {
// 鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎
@@ -160,7 +175,7 @@
DEBUG("鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎");
}
darray[parkCount] = 1;
- firstReverseTimepoint = tp;
+ firstReverseTimepoint = tp; // 棣栨鍊掕溅锛屾墠鎰忓懗鐫�姝ら」鐩紑濮�
}
} else {
// 鍒囨崲涓哄墠杩�
@@ -209,6 +224,9 @@
}
TEST_END:
+ if (!testing && reverseCar) {
+ MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 0);
+ }
return testing ? 1 : 0;
}
@@ -258,7 +276,7 @@
return succ;
}
-static bool CrashRedLine(const Polygon *map, const car_model *car)
+static bool CrashRedLine(const Polygon *map, const car_model *car, int &who)
{
bool ret = false;
@@ -276,6 +294,12 @@
for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
MakeLine(&red_line, &(map->point[red_lines[i][0]]), &(map->point[red_lines[i][1]]));
if (IntersectionOf(red_line, &car_body) != GM_None) {
+ if (i == 2 || i == 1) {
+ who = 1;
+ } else if (i == 4 || i == 5) {
+ who = 2;
+ }
+
ret = true;
break;
}
diff --git a/lib/src/main/cpp/test_items/park_bottom.h b/lib/src/main/cpp/test_items/park_bottom.h
index be36967..69cd14e 100644
--- a/lib/src/main/cpp/test_items/park_bottom.h
+++ b/lib/src/main/cpp/test_items/park_bottom.h
@@ -11,7 +11,7 @@
using namespace std;
-void StartParkBottom(int moveStatus, const struct RtkTime *rtkTime);
+void StartParkBottom(int index, int moveStatus, const struct RtkTime *rtkTime);
int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
#endif //RTKDRIVERTEST_PARK_BOTTOM_H
diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp
index 207d9e7..921ce34 100644
--- a/lib/src/main/cpp/test_items/park_edge.cpp
+++ b/lib/src/main/cpp/test_items/park_edge.cpp
@@ -10,6 +10,7 @@
#include "../native-lib.h"
#include "../utils/xconvert.h"
#include "../test_common/car_sensor.h"
+#include "../master/comm_if.h"
#include <vector>
#include <cstdlib>
@@ -21,7 +22,10 @@
const int PARK_TIMEOUT = D_SEC(90);
const uint32_t STOP_CAR_TIME = D_SEC(2);
+static int mapIndex = 0;
static bool reportExamTimeout;
+static bool reportParkFail;
+
static uint32_t stopTimepoint = 0;
static bool occurCrashRedLine1, occurCrashRedLine2;
static int prevMoveStatus, storeMoveStatusBeforeStop;
@@ -36,15 +40,16 @@
static bool ExitParkArea(const Polygon *map, const car_model *car);
static bool ExitParkArea2(const Polygon *map, const car_model *car);
-void StartParkEdge(int moveStatus, const struct RtkTime *rtkTime)
+void StartParkEdge(int index, int moveStatus, const struct RtkTime *rtkTime)
{
DEBUG("杩涘叆渚ф柟鍋滆溅鍦哄湴");
testing = true;
+ mapIndex = index;
occurCrashRedLine1 = occurCrashRedLine2 = false; // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑
reportExamTimeout = false;
-
+ reportParkFail = false;
prevMoveStatus = moveStatus;
parkSuccess = false;
parkStatus = 0;
@@ -80,9 +85,10 @@
}
if (ExitParkArea(map, car) || ExitParkArea2(map, car)) {
- if (!parkSuccess || !occurMoveBack) {
+ if (!parkSuccess && occurMoveBack && !reportParkFail) {
// 鐩存帴椹剁娴嬭瘯鍖猴紝璁や负绉诲簱涓嶅叆
AddExamFault(21, rtkTime);
+ reportParkFail = true;
DEBUG("鐩存帴椹剁娴嬭瘯鍖�,绉诲簱涓嶅叆");
}
testing = false;
@@ -130,9 +136,10 @@
}
}
- if (!parkSuccess) {
+ if (!parkSuccess && !reportParkFail) {
// 鍋滄鍚庯紝杞﹁韩鍑虹嚎锛屼笉鍚堟牸
AddExamFault(21, rtkTime);
+ reportParkFail = true;
DEBUG("绉诲簱涓嶅叆");
}
@@ -149,6 +156,7 @@
DEBUG("寮�濮嬪�掕溅");
occurMoveBack = true;
moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
+ MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
}
}
@@ -165,7 +173,11 @@
}
}
}
+
TEST_END:
+ if (!testing && occurMoveBack) {
+ MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 0);
+ }
return testing ? 1 : 0;
}
diff --git a/lib/src/main/cpp/test_items/park_edge.h b/lib/src/main/cpp/test_items/park_edge.h
index 752e909..8916ff8 100644
--- a/lib/src/main/cpp/test_items/park_edge.h
+++ b/lib/src/main/cpp/test_items/park_edge.h
@@ -11,7 +11,7 @@
using namespace std;
-void StartParkEdge(int moveStatus, const struct RtkTime *rtkTime);
+void StartParkEdge(int index, int moveStatus, const struct RtkTime *rtkTime);
int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
#endif //RTKDRIVERTEST_PARK_EDGE_H
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 91a9b0b..b57ceec 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/stop_and_start.cpp
@@ -12,6 +12,7 @@
#include "../common/apptimer.h"
#include "../utils/xconvert.h"
#include "../test_common/car_sensor.h"
+#include "../master/comm_if.h"
#define DEBUG(fmt, args...) LOGD("<stop_and_start> <%s>: " fmt, __func__, ##args)
@@ -31,6 +32,7 @@
static PointF stopPoint;
+static int mapIndex = 0;
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool stopCar = false;
@@ -46,11 +48,11 @@
static double DistanceOfTire2Edge(const Polygon *map, const car_model *car);
static bool ExitTestArea(const Polygon *map, const car_model *car);
-void StartSAS(int moveDirect, const struct RtkTime *rtkTime)
+void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime)
{
DEBUG("杩涘叆鍧¤捣椤圭洰");
testing = true;
-
+ mapIndex = index;
prevMoveDirect = moveDirect;
if (moveDirect == 0) {
@@ -63,6 +65,8 @@
slideNormalDistance = false;
reportSlideFault = false;
reportStartTimeout = false;
+
+ MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 1);
}
int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
@@ -81,6 +85,12 @@
if (ExitTestArea(map, car)) {
// 椹剁娴嬭瘯鍖�
testing = false;
+ }
+
+ {
+ double dis2 = DistanceOfTire2Edge(map, car);
+
+ MA_SendDistance(-1, dis2);
}
if (prevMoveDirect != moveDirect) {
@@ -172,6 +182,10 @@
}
}
+ if (!testing) {
+ MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 0);
+ }
+
return testing ? 1 : 0;
}
@@ -245,10 +259,22 @@
// 鏁翠釜杞﹁締閮借椹剁璇ユ祴璇曞尯鍩�
static bool ExitTestArea(const Polygon *map, const car_model *car)
{
- // 鍦�8->7绾跨殑鍙充晶
- for (int i = 0; i < car->bodyNum; ++i) {
- if (IntersectionOfLine(map->point[8], map->point[7], car->carXY[car->body[i]]) != -1)
- return false;
+ // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
+ bool ret = false;
+
+ Polygon carBody;
+
+ carBody.num = car->bodyNum;
+ carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
+ for (int i = 0; i < carBody.num; ++i) {
+ carBody.point[i] = car->carXY[car->body[i]];
}
- return true;
+
+ if (IntersectionOf(&carBody, map) == GM_None) {
+ ret = true;
+ }
+
+ free(carBody.point);
+
+ return ret;
}
diff --git a/lib/src/main/cpp/test_items/stop_and_start.h b/lib/src/main/cpp/test_items/stop_and_start.h
index b3f69cb..894a7e3 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.h
+++ b/lib/src/main/cpp/test_items/stop_and_start.h
@@ -9,7 +9,7 @@
using namespace std;
-void StartSAS(int moveDirect, const struct RtkTime *rtkTime);
+void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime);
int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
#endif //RTKDRIVERTEST_STOP_AND_START_H
diff --git a/lib/src/main/cpp/test_items/turn_a90.cpp b/lib/src/main/cpp/test_items/turn_a90.cpp
index 6ee18df..4b3ba68 100644
--- a/lib/src/main/cpp/test_items/turn_a90.cpp
+++ b/lib/src/main/cpp/test_items/turn_a90.cpp
@@ -10,6 +10,7 @@
#include "../utils/xconvert.h"
#include "../defs.h"
#include "../test_common/car_sensor.h"
+#include "../master/comm_if.h"
#include <vector>
#include <cstdlib>
@@ -21,6 +22,7 @@
const uint32_t STOP_CAR_TIME = D_SEC(2);
static bool testing;
+static int mapIndex;
static int enterAreaHeading;
static bool turnLeftFinished;
@@ -33,7 +35,7 @@
static bool CrashRedLine(const Polygon *map, const car_model *car);
static bool ExitTestArea(const Polygon *map, const car_model *car);
-void StartTurnA90(int moveDirect, double heading, const struct RtkTime *rtkTime)
+void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime)
{
DEBUG("杩涘叆鐩磋杞集鍦哄湴");
testing = true;
@@ -45,6 +47,9 @@
reportStopCarTimeout = false;
crashRedLine = false;
turnLeftFinished = false;
+ mapIndex = index;
+
+ MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 1);
}
int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime)
@@ -118,6 +123,11 @@
if (turnLeftFinished) {
}
+
+ if (!testing) {
+ MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 0);
+ }
+
TEST_END:
return testing? 1:0;
}
diff --git a/lib/src/main/cpp/test_items/turn_a90.h b/lib/src/main/cpp/test_items/turn_a90.h
index f6e88b2..857285d 100644
--- a/lib/src/main/cpp/test_items/turn_a90.h
+++ b/lib/src/main/cpp/test_items/turn_a90.h
@@ -10,7 +10,7 @@
using namespace std;
-void StartTurnA90(int moveDirect, double heading, const struct RtkTime *rtkTime);
+void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime);
int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime);
#endif //RTKDRIVERTEST_TURN_A90_H
diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp
index 16a8832..8db23e0 100644
--- a/lib/src/main/cpp/test_items2/drive_straight.cpp
+++ b/lib/src/main/cpp/test_items2/drive_straight.cpp
@@ -7,6 +7,7 @@
#include "../native-lib.h"
#include "../jni_log.h"
#include "road_exam.h"
+#include "../master/comm_if.h"
#include <cmath>
#define DEBUG(fmt, args...) LOGD("<drive_straight> <%s>: " fmt, __func__, ##args)
@@ -43,6 +44,14 @@
MakeLine(&road_edge, &RoadMapList[index].roadEdgeLine[0].point[0], &RoadMapList[index].roadEdgeLine[0].point[1]);
+ {
+ double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], road_edge);
+
+ double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], road_edge);
+
+ MA_SendDistance(-1, (l1+l2)/2.0);
+ }
+
if (!crossStartLine) {
PointF p1, p2;
--
Gitblit v1.8.0