From 2ea9a1440259ef77e5af3776c022b4473ac34e09 Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期三, 05 二月 2020 22:28:34 +0800
Subject: [PATCH] 修改了侧方位停车。
---
lib/src/main/cpp/test_items/park_edge.cpp | 274 ++++++++++++++++++++++++------------------------------
1 files changed, 123 insertions(+), 151 deletions(-)
diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp
index 6b1d625..c24b64c 100644
--- a/lib/src/main/cpp/test_items/park_edge.cpp
+++ b/lib/src/main/cpp/test_items/park_edge.cpp
@@ -3,182 +3,161 @@
//
#include "park_edge.h"
+#include "../jni_log.h"
#include "../Geometry.h"
#include "../driver_test.h"
#include "../common/apptimer.h"
#include "../native-lib.h"
+#include "../utils/xconvert.h"
#include <vector>
#include <cstdlib>
-using namespace std;
+#define DEBUG(fmt, args...) LOGD("<park_edge> <%s>: " fmt, __func__, ##args)
-enum {
- ARRIVED_START,
- PARK_CAR,
- START_CAR
-};
+using namespace std;
const int PARK_TIMEOUT = 90;
const uint32_t STOP_CAR_TIME = D_SEC(2);
-static int prev_run_status;
-static int runStatusBeforeStop;
-static uint32_t stopTimepoint = 0;
-static bool parkTimeout;
+static uint64_t stopTimepoint = 0;
static bool occurCrashRedLine1, occurCrashRedLine2;
-static bool PETesting = false;
-static int currTarget;
-static bool carStopEvent; // 涓�斿仠杞︽爣璁�
-static bool carParkSuccess; // 鏄惁鍋滃湪搴撲綅
-static int leaveParkCnt; // 杞﹁締绂诲紑搴撲綅鍒氬彂鐢熺殑鏃讹紝妫�鏌ユ槸鍚﹀紑鍚浆鍚戠伅
+static int prevMoveStatus, storeMoveStatusBeforeStop;
+static int parkStatus;
+static bool occurMoveBack, parkSuccess;
+static uint32_t moveBackTimePoint;
-static void ParkEdgeTimeout(union sigval sig);
-static bool CrashRedLine1(const Polygon *map, const car_model_cache_t *car);
-static bool CrashRedLine2(const Polygon *map, const car_model_cache_t *car);
-static bool EnterParking(const Polygon *map, const car_model_cache_t *car);
-static bool ExitParkArea(const Polygon *map, const car_model_cache_t *car);
-static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car);
+static bool CrashRedLine1(const Polygon *map, const car_model *car);
+static bool CrashRedLine2(const Polygon *map, const car_model *car);
+static bool EnterParking(const Polygon *map, const car_model *car);
+static bool ExitParkArea(const Polygon *map, const car_model *car);
-bool EnterParkEdgeArea(const Polygon *car, const Polygon *tire, const Polygon *map)
+void StartParkEdge(int moveStatus, const struct RtkTime *rtkTime)
{
- if (IntersectionOf(tire->point[0], map) == GM_Containment &&
- IntersectionOf(tire->point[1], map) == GM_Containment &&
- IntersectionOf(tire->point[4], map) == GM_Containment &&
- IntersectionOf(tire->point[5], map) == GM_Containment) {
- return true;
- }
- return false;
-}
-
-void StartParkEdge(void)
-{
- prev_run_status = 0;
- parkTimeout = false;
occurCrashRedLine1 = occurCrashRedLine2 = false; // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑
- PETesting = true;
- currTarget = ARRIVED_START;
+
+ prevMoveStatus = moveStatus;
+ parkSuccess = false;
+ parkStatus = 0;
+ occurMoveBack = false;
+ if (moveStatus == -1) {
+ occurMoveBack = true;
+ moveBackTimePoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
+ }
+
+ DEBUG("杩涘叆渚ф柟鍋滆溅鍦哄湴");
}
-void StopParkEdge(void)
-{
- AppTimer_delete(ParkEdgeTimeout);
- PETesting = false;
-}
-
-int TestParkEdge(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status)
+int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime)
{
int status = 0;
- if (!PETesting)
- return 0;
-
- if (currTarget >= PARK_CAR) {
- if (CrashRedLine1(map, car)) {
- if (!occurCrashRedLine1) {
- occurCrashRedLine1 = true;
- // 杞﹁疆鍘嬭竟绾匡紝姣忔鎵�10鍒�
- err.push_back(23);
- }
- } else {
- occurCrashRedLine1 = false;
+ if (CrashRedLine1(map, car)) {
+ if (!occurCrashRedLine1) {
+ // 杞﹁疆鍘嬭竟绾匡紝姣忔鎵�10鍒�
+ AddExamFault(23, rtkTime);
+ DEBUG("杞﹁疆鍘嬭竟绾�");
}
+ occurCrashRedLine1 = true;
+ } else {
+ occurCrashRedLine1 = false;
+ }
- if (CrashRedLine2(map, car)) {
- if (!occurCrashRedLine2) {
- occurCrashRedLine2 = true;
- // 杞﹁韩鍘嬪簱浣嶇嚎锛屾瘡娆℃墸10鍒�
- err.push_back(24);
- }
- } else {
- occurCrashRedLine2 = false;
+ if (CrashRedLine2(map, car)) {
+ if (!occurCrashRedLine2) {
+ // 杞﹁韩鍘嬪簱浣嶇嚎锛屾瘡娆℃墸10鍒�
+ AddExamFault(24, rtkTime);
+ DEBUG("杞﹁韩鍘嬪簱浣嶇嚎");
}
+ occurCrashRedLine2 = true;
+ } else {
+ occurCrashRedLine2 = false;
+ }
- if (parkTimeout) {
+ if (ExitParkArea(map, car)) {
+ if (!parkSuccess || !occurMoveBack) {
+ // 鐩存帴椹剁娴嬭瘯鍖猴紝璁や负绉诲簱涓嶅叆
+ AddExamFault(21, rtkTime);
+ DEBUG("鐩存帴椹剁娴嬭瘯鍖�,绉诲簱涓嶅叆");
+ status = -1;
+ } else {
+ status = 1;
+ }
+ }
+
+ if (occurMoveBack) {
+ uint32_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
+
+ if (tp - moveBackTimePoint >= PARK_TIMEOUT) {
// 瓒呮椂90绉掞紝涓嶅悎鏍�
- err.push_back(22);
+ AddExamFault(22, rtkTime);
+ DEBUG("绉诲簱90绉掕秴鏃�");
status = -1;
}
-
- if (prev_run_status != run_status) {
- if (run_status == 0) {
- // 杞﹀仠浜�
- runStatusBeforeStop = prev_run_status;
- stopTimepoint = AppTimer_GetTickCount();
- } else {
- // 杞﹀姩浜嗭紝涓斿拰鍋滆溅鍓嶇殑杩愯鐘舵�佷竴鑷�
- if (runStatusBeforeStop == run_status && AppTimer_GetTickCount() - stopTimepoint > STOP_CAR_TIME) {
- // 涓�斿仠杞︼紝鎵�5鍒�
- err.push_back(26);
- }
- }
- }
}
- if (currTarget == ARRIVED_START) {
- if (ExitTestArea(map, car)) {
- // 鐩存帴椹剁娴嬭瘯鍖哄煙
- status = -2;
- } else
- if (run_status < 0) {
- AppTimer_add(ParkEdgeTimeout, D_SEC(PARK_TIMEOUT));
- currTarget = PARK_CAR;
- TextOsd(0, "寮�濮嬪�掑簱");
- }
- } else if (currTarget == PARK_CAR) {
- if (run_status < 0) {
- carStopEvent = false;
- } else if (run_status == 0) {
- // 绔嬪嵆妫�鏌ユ槸鍚﹀仠杞﹀埌浣嶏紝涔熻鏄腑閫斿仠杞︼紝鍏堜笉绠★紝寰呭彂鐢熷墠杩涗簨浠跺悗锛屽啀鏂畾鏄惁鍋滆溅鍒颁綅
- if (!carStopEvent) {
- carStopEvent = true;
- carParkSuccess = EnterParking(map, car);
- }
+ if (moveStatus != prevMoveStatus) {
+ if (moveStatus == 0) {
+ DEBUG("鍋滆溅浜�");
+ parkStatus = 0;
+ stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
+ stopTimepoint = stopTimepoint * 1000 + rtkTime->mss*10;
+ storeMoveStatusBeforeStop = prevMoveStatus;
} else {
- if (carStopEvent) {
- if (!carParkSuccess) {
- // 涓嶅悎鏍硷細杞﹁韩鍑虹嚎
- err.push_back(21);
- status = -1;
+ DEBUG("缁х画琛岄┒");
+ uint64_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
+ tp = tp * 1000 + rtkTime->mss * 10;
+
+ if (moveStatus == storeMoveStatusBeforeStop) {
+ // 鍚屾柟鍚戝啀鍚姩锛岀户缁垽鏂槸鍚﹀仠杞﹁秴鏃�
+ if (tp - stopTimepoint >= STOP_CAR_TIME) {
+ // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
+ AddExamFault(26, rtkTime);
+ DEBUG("鍋滆溅瓒呮椂");
+ }
+ } else {
+ // 鍊掕溅鍒囨崲鍒板墠杩�
+ if (moveStatus == 1 && tp - stopTimepoint >= STOP_CAR_TIME) {
+ if (parkStatus == 0) {
+
+ }
+
+ if (!parkSuccess) {
+ // 鍋滄鍚庯紝杞﹁韩鍑虹嚎锛屼笉鍚堟牸
+ AddExamFault(21, rtkTime);
+ DEBUG("绉诲簱涓嶅叆");
+ status = -1;
+ }
}
}
- carStopEvent = false;
- leaveParkCnt = 0;
- currTarget = START_CAR;
- TextOsd(0, "寮�濮嬪嚭搴�");
- }
- } else if (currTarget == START_CAR) {
- if (run_status > 0) {
- leaveParkCnt++;
- if (leaveParkCnt == 1) {
- // 鏈紑鍚浆鍚戠伅锛屾墸10鍒�
- err.push_back(25);
+ if (moveStatus == -1 && !occurMoveBack) {
+ DEBUG("寮�濮嬪�掕溅");
+ occurMoveBack = true;
+ moveBackTimePoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
}
+ }
- if (ExitParkArea(map, car)) {
- // 椤圭洰瀹屾垚
- status = 1;
- TextOsd(0, "椤圭洰瀹屾垚");
+ prevMoveStatus = moveStatus;
+ } else if (moveStatus == 0 && parkStatus == 0) {
+ uint64_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
+ tp = tp * 1000 + rtkTime->mss * 10;
+
+ if (tp - stopTimepoint >= STOP_CAR_TIME) {
+ if (EnterParking(map, car)) {
+ parkStatus = 1;
+ parkSuccess = true;
+ } else {
+ parkStatus = -1;
}
}
}
- if (status != 0) {
- StopParkEdge();
- }
-
- prev_run_status = run_status;
return status;
}
-static void ParkEdgeTimeout(union sigval sig) {
- AppTimer_delete(ParkEdgeTimeout);
- parkTimeout = true;
-}
-
// 杞﹁疆鏄惁鍘嬮亾璺竟绾�
-static bool CrashRedLine1(const Polygon *map, const car_model_cache_t *car)
+static bool CrashRedLine1(const Polygon *map, const car_model *car)
{
bool ret = false;
@@ -187,8 +166,8 @@
Line frontAxle, rearAxle;
- MakeLine(&frontAxle, &car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->front_right_tire[TIRE_OUTSIDE]]);
- MakeLine(&rearAxle, &car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]]);
+ 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) {
MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
@@ -203,7 +182,7 @@
}
// 杞﹁韩鏄惁鍘嬪簱浣嶇嚎
-static bool CrashRedLine2(const Polygon *map, const car_model_cache_t *car)
+static bool CrashRedLine2(const Polygon *map, const car_model *car)
{
bool ret = false;
@@ -212,10 +191,10 @@
Polygon car_body;
- car_body.num = car->desc->body_num;
+ car_body.num = car->bodyNum;
car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
for (int i = 0; i < car_body.num; ++i) {
- car_body.point[i] = car->points[car->desc->body[i]];
+ car_body.point[i] = car->carXY[car->body[i]];
}
for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
@@ -230,16 +209,18 @@
return ret;
}
-static bool EnterParking(const Polygon *map, const car_model_cache_t *car) {
+static bool EnterParking(const Polygon *map, const car_model *car) {
+ DEBUG("妫�鏌ュ仠杞﹀埌浣�...");
+
bool succ = false;
Polygon parking;
Polygon car_body;
- car_body.num = car->desc->body_num;
+ car_body.num = car->bodyNum;
car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
for (int i = 0; i < car_body.num; ++i) {
- car_body.point[i] = car->points[car->desc->body[i]];
+ car_body.point[i] = car->carXY[car->body[i]];
}
MakePolygon(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]});
@@ -251,24 +232,15 @@
CleanPolygon(&parking);
free(car_body.point);
+ DEBUG("妫�鏌ュ仠杞﹀埌浣嶇粨鏉�");
return succ;
}
// 鏁翠釜杞﹁締閮借椹惰繃鍓嶅簱浣嶇嚎
-static bool ExitParkArea(const Polygon *map, const car_model_cache_t *car)
+static bool ExitParkArea(const Polygon *map, const car_model *car)
{
- for (int i = 0; i < car->point_num; ++i) {
- if (IntersectionOfLine(map->point[4], map->point[5], car->points[i]) != -1)
- return false;
- }
- return true;
-}
-
-// 鏁翠釜杞﹁締閮借椹惰繃鏈�鍓嶇鎺у埗绾�
-static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car)
-{
- for (int i = 0; i < car->point_num; ++i) {
- if (IntersectionOfLine(map->point[6], map->point[7], car->points[i]) != -1)
+ 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;
--
Gitblit v1.8.0