From 2d0741f8e56e4f24126ea8448551e64560c45b4e Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 18 三月 2020 18:41:57 +0800
Subject: [PATCH] 通用测试
---
lib/src/main/cpp/test_items2/road_exam.cpp | 273 +++++++++++++++++++++++++++++++++++++++++++++++++++---
1 files changed, 256 insertions(+), 17 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 6826a10..c0b82e3 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -7,27 +7,54 @@
#include "../utils/xconvert.h"
#include "../common/apptimer.h"
#include "../jni_log.h"
+#include "../defs.h"
+#include "../test_common/car_sensor.h"
+
+#include <vector>
+#include <list>
+#include <string>
#define DEBUG(fmt, args...) LOGD("<road_exam> <%s>: " fmt, __func__, ##args)
+
+using namespace std;
+
+#define TURN_CHECK_CNT 5
+
+static const int TURN_THRESHOLD = 10;
+static const int TURN_CHECK_INTERVAL = D_SEC(1);
static bool occurCrashRedLine;
static bool occurCrashGreenLine;
static bool occurOverSpeed;
static int checkCrashGreenTimeout;
static char carIntersectionOfGreenLine;
+static int currTurnSignalStatus;
+static int turnSignalStatusWhenCrashGreenLine;
+static bool reportTurnSignalError;
+static int prevMoveDirect;
+static uint32_t stopTimepoint = 0;
+static bool reportStopCarOnRedArea;
+static PointF stopPoint;
-struct {
+static struct drive_timer {
int hour;
int min;
int sec;
int msec;
-} crashGreenRunTime, crashGreenCmpTime;
+} crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;
+static const uint32_t STOP_CAR_TIME = D_SEC(2);
static const uint32_t CHANGE_ROAD_MIN_INTERVAL = D_SEC(10);
static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10);
+static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3);
+
static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0;
+static void Rtk2DirveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
+static char isTurn(int currYaw, int prevYaw);
+static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);
static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car);
+static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car);
static bool CrashGreenLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, PointF &p1, PointF &p2);
void Init(void)
@@ -38,11 +65,17 @@
occurOverSpeed = false;
checkCrashGreenTimeout = 0;
carIntersectionOfGreenLine = 0;
+
+ reportTurnSignalError = false;
+ currTurnSignalStatus = OFF_LIGHT;
+ prevMoveDirect = 0;
+
+ reportStopCarOnRedArea = false;
}
-void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, double speed, int moveStatus, const struct RtkTime *rtkTime)
+void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
- if (speed > MAX_SPEED) {
+ if (moveDirect != 0 && speed > MAX_SPEED) {
if (!occurOverSpeed) {
occurOverSpeed = true;
// 瓒呴�燂紝涓嶅悎鏍�
@@ -51,6 +84,89 @@
} else {
occurOverSpeed = false;
}
+
+ if (moveDirect != prevMoveDirect) {
+ if (moveDirect == 0) {
+ stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
+ reportStopCarOnRedArea = false;
+
+ DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+ } else if (moveDirect == -1) {
+
+ }
+ prevMoveDirect = moveDirect;
+ } else if (moveDirect == 0) {
+ uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
+
+ if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) {
+ // 鍋滆溅瓒�2绉掞紝鍋滃湪绾㈠尯
+ AddExamFault(16, rtkTime);
+ DEBUG("涓�斿仠杞�");
+ reportStopCarOnRedArea = true;
+ }
+
+ stopPoint = car->basePoint;
+ }
+
+ switch (ReadCarStatus(TURN_SIGNAL_LAMP)) {
+ case LEFT_TURN_LIGHT:
+ if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
+ currTurnSignalStatus = LEFT_TURN_LIGHT;
+ Rtk2DirveTimer(turnSignalChangeTime, rtkTime);
+ }
+ break;
+ case RIGHT_TURN_LIGHT:
+ if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
+ currTurnSignalStatus = RIGHT_TURN_LIGHT;
+ Rtk2DirveTimer(turnSignalChangeTime, rtkTime);
+ }
+ break;
+ default:
+ currTurnSignalStatus = ReadCarStatus(TURN_SIGNAL_LAMP);
+ break;
+ }
+
+
+ // 妫�鏌ユ槸鍚︽寔缁浆鍚�
+ char turnDirect = CheckCarTurn(CarModelList);
+ if (turnDirect == 'L') {
+ if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
+ if (!reportTurnSignalError) {
+ DEBUG("娌℃墦宸﹁浆鐏�");
+ // 娌℃墦宸﹁浆鐏紝涓嶅悎鏍�
+ AddExamFault(13, rtkTime);
+ reportTurnSignalError = true;
+ }
+ } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
+ turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
+ if (!reportTurnSignalError) {
+ DEBUG("杞悜鐏椂闂翠笉瓒�");
+ // 涓嶈冻3绉掞紝涓嶅悎鏍�
+ AddExamFault(14, rtkTime);
+ reportTurnSignalError = true;
+ }
+ }
+ } else if (turnDirect == 'R') {
+ if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
+ if (!reportTurnSignalError) {
+ DEBUG("娌℃墦鍙宠浆鐏�");
+ // 娌℃墦鍙宠浆鐏紝涓嶅悎鏍�
+ AddExamFault(13, rtkTime);
+ reportTurnSignalError = true;
+ }
+ } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
+ turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
+ if (!reportTurnSignalError) {
+ DEBUG("杞悜鐏椂闂翠笉瓒�");
+ // 涓嶈冻3绉掞紝涓嶅悎鏍�
+ AddExamFault(14, rtkTime);
+ reportTurnSignalError = true;
+ }
+ }
+ } else {
+ reportTurnSignalError = false;
+ }
+
if (CrashRedLine(RoadMapList, car)) {
if (!occurCrashRedLine) {
@@ -65,13 +181,11 @@
static PointF p1, p2;
if (CrashGreenLine(RoadMapList, car, p1, p2)) {
- if (moveStatus != 0) {
+ // 鍘嬭櫄绾�
+ if (moveDirect != 0) {
if (checkCrashGreenTimeout == 0) {
checkCrashGreenTimeout = 1;
- crashGreenRunTime.hour = rtkTime->hh;
- crashGreenRunTime.min = rtkTime->mm;
- crashGreenRunTime.sec = rtkTime->ss;
- crashGreenRunTime.msec = rtkTime->mss;
+ Rtk2DirveTimer(crashGreenRunTime, rtkTime); // 杩愬姩涓帇铏氱嚎鐨勫紑濮嬫椂闂寸偣
} else if (checkCrashGreenTimeout == 1) {
uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10);
@@ -91,7 +205,9 @@
// 妫�娴嬫槸鍚�3绉掑墠鏈夊紑鍚搴斾箣杞悜鐏�
if (!occurCrashGreenLine) {
occurCrashGreenLine = true;
-
+ // 璁板綍寮�濮嬪帇绾跨殑鏃堕棿锛屼笉纭畾鏄惁鏈夊彉閬撴剰鍥撅紝寰呯‘璁ゅ彉閬撳悗鍐嶅鐞嗕箣
+ Rtk2DirveTimer(crashGreenStartTime, rtkTime);
+ turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus;
}
// p1 ---------------> p2
@@ -104,14 +220,14 @@
carIntersectionOfGreenLine = 'L';
}
} else {
+ // 涓嶅啀鍘嬭櫄绾�
if (occurCrashGreenLine) {
int inter = IntersectionOfLine(p1, p2, car->basePoint);
- // 鏈夎繃绾垮姩浣�
+ // 瀹屾垚璺ㄧ嚎鍔ㄤ綔
if ((inter == 1 && carIntersectionOfGreenLine == 'R') ||
(inter == -1 && carIntersectionOfGreenLine == 'L')) {
- DEBUG("璺ㄨ秺铏氱嚎");
-
+ // 姣旇緝涓婃璺ㄧ嚎鏃堕棿
if (crashGreenCmpTime.hour >= 0) {
uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
rtkTime->mss * 10,
@@ -124,15 +240,102 @@
}
}
- crashGreenCmpTime.hour = rtkTime->hh;
- crashGreenCmpTime.min = rtkTime->mm;
- crashGreenCmpTime.sec = rtkTime->ss;
- crashGreenCmpTime.msec = rtkTime->mss;
+ // 璁板綍鏈鍙橀亾鏃堕棿鐐�
+ Rtk2DirveTimer(crashGreenCmpTime, rtkTime);
+
+ // 妫�鏌ュ彉閬撳墠锛屾槸鍚︽彁鍓嶈浆鍚戠伅
+ if (inter == 1) {
+ // 鍚戝乏渚у彉閬�
+ DEBUG("鍚戝乏渚у彉閬�");
+ if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
+ // 娌℃墦鐏紝涓嶅悎鏍�
+ AddExamFault(13, rtkTime);
+ }
+ } else {
+ // 鍚戝彸渚у彉閬�
+ DEBUG("鍚戝彸渚у彉閬�");
+ if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
+ // 娌℃墦鐏紝涓嶅悎鏍�
+ AddExamFault(13, rtkTime);
+ }
+ }
}
}
occurCrashGreenLine = false;
checkCrashGreenTimeout = 0;
}
+}
+
+static void Rtk2DirveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
+{
+ tm.hour = rtkTime->hh;
+ tm.min = rtkTime->mm;
+ tm.sec = rtkTime->ss;
+ tm.msec = rtkTime->mss;
+}
+
+static char isTurn(int currYaw, int prevYaw)
+{
+ if (ABS(currYaw - prevYaw) > 180) {
+ currYaw = 360 - ABS(currYaw-prevYaw);
+ } else {
+ currYaw = ABS(currYaw - prevYaw);
+ }
+
+ if (currYaw >= TURN_THRESHOLD) {
+ if((( currYaw + 360 - prevYaw) % 360) < 180) {
+ DEBUG("鍙宠浆");
+ return 'R';
+ } else {
+ DEBUG("宸﹁浆");
+ return 'L';
+ }
+ }
+
+ return 0;
+}
+
+static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)
+{
+ // 鏈�杩�3绉掑唴锛屾瘡绉掔殑瑙掑害宸ぇ浜�10搴︼紝涓旀柟鍚戠浉鍚岋紝杩炵画3绉掞紝璁や负杞悜
+ if (CarModelList.size() < 1)
+ return false;
+
+ list<car_model *>::iterator iter = CarModelList.begin();
+
+ car_model *c1 = *iter, *c2;
+
+ ++iter;
+
+ char turn[TURN_CHECK_CNT] = {0};
+ int checkCnt = 0;
+
+ while (iter != CarModelList.end()) {
+ 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 >= TURN_CHECK_INTERVAL) {
+ turn[checkCnt++] = isTurn((int)c1->yaw, (int)c2->yaw);
+ c1 = c2;
+
+ if (checkCnt == TURN_CHECK_CNT)
+ break;
+ }
+
+ ++iter;
+ }
+
+ int i = 0;
+ for (; checkCnt == TURN_CHECK_CNT && i < TURN_CHECK_CNT-1; ++i) {
+ if (turn[i] != turn[i+1])
+ break;
+ }
+
+ if (i == TURN_CHECK_CNT-1)
+ return turn[0];
+
+ return 0;
}
/*****************************************************
@@ -170,6 +373,42 @@
return false;
}
+/**********************************************************
+ * 鎸夋暣涓溅韬槸鍚﹁鐩栬绠�
+ * @param RoadMapList
+ * @param car
+ * @return
+ */
+static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car)
+{
+ 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]];
+ }
+
+ for (int i = 0; i < RoadMapList.size(); ++i) {
+ if (RoadMapList[i].type == 100) {
+
+ // 姣忔潯绾㈠尯閮芥娴�
+ for (int j = 0; j < RoadMapList[i].redAreaNum; ++j) {
+ if (IntersectionOf(&carBody, &RoadMapList[i].redArea[j]) != GM_None) {
+ ret = true;
+ }
+ }
+ break;
+ }
+ }
+
+ free(carBody.point);
+
+ return ret;
+}
+
/**************************************************
* 杞﹁疆瑙︾閬撹矾铏氱嚎銆傛娴嬭椹舵椂闂磋秴闀匡紱鍙橀亾鎯呭喌锛�
* @param RoadMapList
--
Gitblit v1.8.0