From 3d3a5aa436645e5af1a4877338319ff8274e0346 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 19 三月 2020 18:04:46 +0800
Subject: [PATCH] 通相检查
---
lib/src/main/cpp/test_items2/road_exam.h | 5 +
lib/src/main/cpp/driver_test.cpp | 44 ++++----
lib/src/main/cpp/test_common/car_sensor.h | 14 +-
lib/src/main/cpp/utils/xconvert.h | 2
lib/src/main/cpp/utils/xconvert.cpp | 2
lib/src/main/cpp/test_items2/road_exam.cpp | 206 +++++++++++++++++++++++++++++++++++++----
lib/src/main/cpp/driver_test.h | 8 +
7 files changed, 229 insertions(+), 52 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 58c440f..8a9c7cd 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -55,17 +55,6 @@
#define MAP_LIST_SIZE 32
enum {
- TEST_NONE,
- TEST_PARK_EDGE,
- TEST_PARK_BOTTOM,
- TEST_TUNE_90,
- TEST_S_CURVE,
- TEST_SLOPE,
- TEST_SIMPLE,
- TEST_END
-};
-
-enum {
TEST_TYPE_AREA = 2,
TEST_TYPE_ROAD_DUMMY_LIGHT,
TEST_TYPE_ROAD_TRUE_LIGHT
@@ -258,8 +247,8 @@
if (map.triggerLine != NULL) {
for (int j = 0; j < map.triggerLineNum; ++j) {
- if (map.triggerLine[j].point != NULL)
- free(map.triggerLine[j].point);
+ if (map.triggerLine[j].line.point != NULL)
+ free(map.triggerLine[j].line.point);
}
free(map.triggerLine);
}
@@ -305,6 +294,8 @@
newMap.redLine[i].point[j] = RoadMapPoints.point[redLines[i][j]];
}
}
+ } else {
+ newMap.redLine = NULL;
}
if ((newMap.redAreaNum = redAreas.size()) > 0) {
@@ -318,6 +309,8 @@
newMap.redArea[i].point[j] = RoadMapPoints.point[redAreas[i][j]];
}
}
+ } else {
+ newMap.redArea = NULL;
}
if ((newMap.greenLineNum = greenLines.size()) > 0) {
@@ -331,20 +324,25 @@
newMap.greenLine[i].point[j] = RoadMapPoints.point[greenLines[i][j]];
}
}
+ } else {
+ newMap.greenLine = NULL;
}
if ((newMap.triggerLineNum = triggerLines.size()) > 0) {
- newMap.triggerLine = (Polygon *) malloc(sizeof(Polygon) * newMap.triggerLineNum);
+ newMap.triggerLine = (struct trigger_line_t *) malloc(sizeof(struct trigger_line_t) * newMap.triggerLineNum);
for (int i = 0; i < newMap.triggerLineNum; ++i) {
- newMap.triggerLine[i].num = triggerLines[i].size();
- newMap.triggerLine[i].point = (PointF *) malloc(sizeof(PointF) * newMap.triggerLine[i].num);
+ newMap.triggerLine[i].line.num = triggerLines[i].size() - 1;
+ newMap.triggerLine[i].line.point = (PointF *) malloc(sizeof(PointF) * newMap.triggerLine[i].line.num);
- for (int j = 0; j < newMap.triggerLine[i].num; ++j) {
- newMap.triggerLine[i].point[j] = RoadMapPoints.point[triggerLines[i][j]];
+ newMap.triggerLine[i].triggerMapId = triggerLines[i][0];
+ for (int j = 0; j < newMap.triggerLine[i].line.num; ++j) {
+ newMap.triggerLine[i].line.point[j] = RoadMapPoints.point[triggerLines[i][j+1]];
}
}
- }
+ } else {
+ newMap.triggerLine = NULL;
+ };
RoadMapList.push_back(newMap);
}
@@ -480,7 +478,7 @@
return;
}
- if (MapNum == 0) {
+ if (MapNum == 0 && type == TEST_TYPE_AREA) {
err = true;
MA_SendExamStatus(0, -1);
}
@@ -491,6 +489,10 @@
if (DummyLightContent == NULL && type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
err = true;
MA_SendExamStatus(0, -3);
+ }
+ if (type != TEST_TYPE_AREA && (RoadMapPoints.num == 0 || RoadMapPoints.point == NULL || RoadMapList.size() == 0)) {
+ err = true;
+ MA_SendExamStatus(0, -1);
}
if (!err) {
@@ -657,7 +659,7 @@
if (ReadCarStatus(ENGINE_START) == ENGINE_START_ACTIVE) {
if (!engineStart) {
engineStart = true;
- if (ReadCarStatus(SHIFT) != 'N') {
+ if (ReadCarStatus(GEAR) != 'N') {
// 涓嶆槸绌烘尅鐐圭伀锛屼笉鍚堟牸
if (ExamType == TEST_TYPE_AREA)
AddExamFault(3, rtkTime);
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 85b0594..7bff0a9 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -84,6 +84,11 @@
int wrong_id;
};
+struct trigger_line_t {
+ int triggerMapId;
+ Polygon line;
+};
+
struct road_exam_map {
int id;
int type;
@@ -100,10 +105,11 @@
Polygon *redArea;
int triggerLineNum;
- Polygon *triggerLine;
+ trigger_line_t *triggerLine;
Line startLine;
Line endLine;
+ int flagStop; // 鍒拌揪寮�濮嬬嚎鍓嶏紝鏄惁闇�瑕佸仠杞�
};
typedef vector<struct road_exam_map> LIST_ROAD_MAP;
diff --git a/lib/src/main/cpp/test_common/car_sensor.h b/lib/src/main/cpp/test_common/car_sensor.h
index 290f6bd..749dc26 100644
--- a/lib/src/main/cpp/test_common/car_sensor.h
+++ b/lib/src/main/cpp/test_common/car_sensor.h
@@ -10,7 +10,7 @@
enum {
OBD_SPEED,
ENGINE_RPM,
- SHIFT,
+ GEAR,
TURN_SIGNAL_LAMP,
DIPPED_BEAM_LAMP,
FOG_LAMP,
@@ -36,13 +36,13 @@
FOG_LIGHT,
INSERT_SEATBELT,
ENGINE_START_ACTIVE,
- SHIFT_N,
+ GEAR_N,
SHIFT_R,
- SHIFT_1,
- SHIFT_2,
- SHIFT_3,
- SHIFT_4,
- SHIFT_5
+ GEAR_1,
+ GEAR_2,
+ GEAR_3,
+ GEAR_4,
+ GEAR_5
};
void CarSensorInit(void);
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 80da3de..fd723b9 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -9,6 +9,7 @@
#include "../jni_log.h"
#include "../defs.h"
#include "../test_common/car_sensor.h"
+#include "../native-lib.h"
#include <vector>
#include <list>
@@ -37,6 +38,8 @@
static uint32_t stopTimepoint = 0;
static bool reportStopCarOnRedArea;
static PointF stopPoint;
+static bool prevGearError = false;
+static bool prevGearNSlide = false;
static bool slideLongDistance;
static bool slideNormalDistance;
@@ -49,22 +52,34 @@
int msec;
} crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;
+static struct drive_timer gearErrorTimePoint;
+static struct drive_timer gearNSlideTimePoint;
+
+static int gearErrorTime;
+static int gearNSlideTime;
+
+static int currExamMapIndex;
+
+static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(5);
+static const uint32_t GEAR_ERROR_TIMEOUT = D_SEC(15);
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 const int SPEED_SHIFT_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};
+static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};
-static void Rtk2DirveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
+static void Rtk2DriveTimer(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);
+static int CrashTriggerLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList);
+static int FindMapIndexById(int id, LIST_ROAD_MAP &RoadMapList);
-void Init(void)
+void InitRoadExam(void)
{
crashGreenCmpTime.hour = -1;
occurCrashRedLine = false;
@@ -81,10 +96,17 @@
occurSlide = false;
slideLongDistance = false;
slideNormalDistance = false;
+ prevGearError = false;
+ gearErrorTime = 0;
+ prevGearNSlide = false;
+ gearNSlideTime = 0;
+
+ currExamMapIndex = -1;
}
void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
+ // 瓒呴�熸娴�
if (moveDirect != 0 && speed > MAX_SPEED) {
if (!occurOverSpeed) {
occurOverSpeed = true;
@@ -96,28 +118,78 @@
}
// 鎸′綅鍖归厤妫�娴�
- switch (ReadCarStatus(SHIFT)) {
- case SHIFT_N:
+ bool currGearError = false;
+ bool currGearNSlide = false;
+
+ switch (ReadCarStatus(GEAR)) {
+ case GEAR_N:
if (moveDirect != 0) {
-
+ // 绌烘。婊戣
+ currGearNSlide = true;
}
break;
- case SHIFT_1:
- if (ConvertMs2KMs(speed) < SPEED_SHIFT_TABLE[0][0] || ConvertMs2KMs(speed) > SPEED_SHIFT_TABLE[0][1]) {
-
+ case GEAR_1:
+ if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) {
+ currGearError = true;
}
break;
- case SHIFT_2:
+ case GEAR_2:
+ if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) {
+ currGearError = true;
+ }
break;
- case SHIFT_3:
+ case GEAR_3:
+ if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) {
+ currGearError = true;
+ }
break;
- case SHIFT_4:
+ case GEAR_4:
+ if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) {
+ currGearError = true;
+ }
break;
- case SHIFT_5:
+ case GEAR_5:
+ if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) {
+ currGearError = true;
+ }
break;
default:break;
}
+ // 绌烘。婊戣瓒呮椂
+ if (currGearNSlide && prevGearNSlide) {
+ gearNSlideTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
+ gearNSlideTimePoint.hour, gearNSlideTimePoint.min, gearNSlideTimePoint.sec, gearNSlideTimePoint.msec*10);
+ }
+ if (gearNSlideTime > GEAR_N_SLIDE_TIMEOUT) {
+ // 绌烘。婊戣瓒�5绉掞紝涓嶅悎鏍�
+ DEBUG("鎸′綅婊戣锛岃秴杩�5绉�");
+ AddExamFault(8, rtkTime);
+ gearNSlideTime = 0;
+ }
+ prevGearNSlide = currGearNSlide;
+ if (prevGearNSlide) {
+ Rtk2DriveTimer(gearNSlideTimePoint, rtkTime);
+ } else {
+ gearNSlideTime = 0;
+ }
+ // 鎸′綅涓嶅尮閰嶈秴鏃�
+ if (currGearError && prevGearError) {
+ gearErrorTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
+ gearErrorTimePoint.hour, gearErrorTimePoint.min, gearErrorTimePoint.sec, gearErrorTimePoint.msec*10);
+ }
+ if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
+ // 绱15绉掞紝鎸′綅-杞﹂�熶笉鍖归厤锛屼笉鍚堟牸
+ DEBUG("鎸′綅閿欒瓒呰繃15绉�");
+ AddExamFault(6, rtkTime);
+ gearErrorTime = 0;
+ }
+
+ prevGearError = currGearError;
+ if (prevGearError) {
+ Rtk2DriveTimer(gearErrorTimePoint, rtkTime);
+ }
+
// 璧锋鍚庢粦
if (moveDirect != prevMoveDirect) {
if (moveDirect == 0) {
@@ -171,13 +243,13 @@
case LEFT_TURN_LIGHT:
if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
currTurnSignalStatus = LEFT_TURN_LIGHT;
- Rtk2DirveTimer(turnSignalChangeTime, rtkTime);
+ Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
}
break;
case RIGHT_TURN_LIGHT:
if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
currTurnSignalStatus = RIGHT_TURN_LIGHT;
- Rtk2DirveTimer(turnSignalChangeTime, rtkTime);
+ Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
}
break;
default:
@@ -226,7 +298,7 @@
reportTurnSignalError = false;
}
-
+ // 鎾炵孩绾�
if (CrashRedLine(RoadMapList, car)) {
if (!occurCrashRedLine) {
// 杞﹁締琛岄┒涓獞杞ц溅閬撲腑蹇冨疄绾挎垨鑰呰溅閬撹竟缂樺疄绾匡紝涓嶅悎鏍�
@@ -237,14 +309,14 @@
occurCrashRedLine = false;
}
+ // 鎾炵豢绾�
static PointF p1, p2;
-
if (CrashGreenLine(RoadMapList, car, p1, p2)) {
// 鍘嬭櫄绾�
if (moveDirect != 0) {
if (checkCrashGreenTimeout == 0) {
checkCrashGreenTimeout = 1;
- Rtk2DirveTimer(crashGreenRunTime, rtkTime); // 杩愬姩涓帇铏氱嚎鐨勫紑濮嬫椂闂寸偣
+ Rtk2DriveTimer(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);
@@ -265,7 +337,7 @@
if (!occurCrashGreenLine) {
occurCrashGreenLine = true;
// 璁板綍寮�濮嬪帇绾跨殑鏃堕棿锛屼笉纭畾鏄惁鏈夊彉閬撴剰鍥撅紝寰呯‘璁ゅ彉閬撳悗鍐嶅鐞嗕箣
- Rtk2DirveTimer(crashGreenStartTime, rtkTime);
+ Rtk2DriveTimer(crashGreenStartTime, rtkTime);
turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus;
}
@@ -300,7 +372,7 @@
}
// 璁板綍鏈鍙橀亾鏃堕棿鐐�
- Rtk2DirveTimer(crashGreenCmpTime, rtkTime);
+ Rtk2DriveTimer(crashGreenCmpTime, rtkTime);
// 妫�鏌ュ彉閬撳墠锛屾槸鍚︽彁鍓嶈浆鍚戠伅
if (inter == 1) {
@@ -323,9 +395,22 @@
occurCrashGreenLine = false;
checkCrashGreenTimeout = 0;
}
+
+ // 瑙﹀彂绾挎娴�
+ if (currExamMapIndex == -1) {
+ currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList);
+ if (currExamMapIndex != -1) {
+ DEBUG("杩涘叆璺�冨瓙鍦板浘 index = %d id = %d item = %d", currExamMapIndex, RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type);
+ if (!RoadMapList[currExamMapIndex].tts.empty()) {
+ PlayTTS(RoadMapList[currExamMapIndex].tts.c_str(), 0);
+ }
+ }
+ } else {
+
+ }
}
-static void Rtk2DirveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
+static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
{
tm.hour = rtkTime->hh;
tm.min = rtkTime->mm;
@@ -507,3 +592,82 @@
}
return false;
}
+
+/************************************************************
+ * 妫�娴嬭溅杈嗘槸鍚﹁Е鍙戝瓙鑰冮」鍦板浘
+ * @param RoadMapList
+ * @param car
+ * @param CarModelList
+ * @return
+ */
+static int CrashTriggerLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList)
+{
+ int map_idx = -1;
+
+ if (CarModelList.size() < 5)
+ return map_idx;
+
+ Polygon trace;
+
+ trace.num = 5;
+ trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);
+
+ list<car_model *>::iterator iter = CarModelList.begin();
+
+ int pn = 0;
+ while (iter != CarModelList.end() && pn < trace.num) {
+ trace.point[pn++] = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
+ ++iter;
+ }
+
+ for (int i = 0; i < RoadMapList.size(); ++i) {
+ if (RoadMapList[i].type == 100) {
+
+ // 姣忔潯绾块兘妫�娴�
+ for (int j = 0; j < RoadMapList[i].triggerLineNum; ++j) {
+ Line trigger_line;
+
+ int kp = 0;
+
+ // 瑙﹀彂绾夸竴鑸簲璇ュ彧鏈夐灏�2鐐癸紙id, p1, p2锛�
+ for (int k = 1; k < RoadMapList[i].triggerLine[j].line.num; ++k) {
+ MakeLine(&trigger_line, &RoadMapList[i].triggerLine[j].line.point[kp], &RoadMapList[i].triggerLine[j].line.point[k]);
+
+ int pp = 1;
+ for (int p = 2; p < pn; ++p) {
+ Line trace_line;
+ MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
+
+ if (IntersectionOf(trace_line, trigger_line) == GM_Intersection &&
+ IntersectionOfLine(trace.point[pp], trace.point[p], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
+ DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], trace_line) > 0.1) {
+ // 纰板埌瑙﹀彂绾�
+ map_idx = FindMapIndexById(RoadMapList[i].triggerLine[j].triggerMapId, RoadMapList);
+ goto SEARCH_TRIGGER_LINE_END;
+ }
+
+ pp = p;
+ }
+
+ kp = k;
+ }
+ }
+ break;
+ }
+ }
+
+SEARCH_TRIGGER_LINE_END:
+ free(trace.point);
+
+ return map_idx;
+}
+
+static int FindMapIndexById(int id, LIST_ROAD_MAP &RoadMapList)
+{
+ for (int i = 0; i < RoadMapList.size(); ++i) {
+ if (RoadMapList[i].id == id) {
+ return i;
+ }
+ }
+ return -1;
+}
diff --git a/lib/src/main/cpp/test_items2/road_exam.h b/lib/src/main/cpp/test_items2/road_exam.h
index 697d0c5..7cbff05 100644
--- a/lib/src/main/cpp/test_items2/road_exam.h
+++ b/lib/src/main/cpp/test_items2/road_exam.h
@@ -5,4 +5,9 @@
#ifndef MYAPPLICATION2_ROAD_EXAM_H
#define MYAPPLICATION2_ROAD_EXAM_H
+#include "../driver_test.h"
+
+void InitRoadExam(void);
+void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
+
#endif //MYAPPLICATION2_ROAD_EXAM_H
diff --git a/lib/src/main/cpp/utils/xconvert.cpp b/lib/src/main/cpp/utils/xconvert.cpp
index a4e524b..0d07d85 100644
--- a/lib/src/main/cpp/utils/xconvert.cpp
+++ b/lib/src/main/cpp/utils/xconvert.cpp
@@ -23,7 +23,7 @@
return ((double)kmh) * 1000.0 / 3600.0;
}
-double ConvertMs2KMs(double ms)
+double ConvertMs2KMh(double ms)
{
return ms * 3600.0 / 1000.0;
}
diff --git a/lib/src/main/cpp/utils/xconvert.h b/lib/src/main/cpp/utils/xconvert.h
index dbbe7b4..9821df7 100644
--- a/lib/src/main/cpp/utils/xconvert.h
+++ b/lib/src/main/cpp/utils/xconvert.h
@@ -8,7 +8,7 @@
#include <cstdint>
double ConvertKMh2Ms(int kmh);
-double ConvertMs2KMs(double ms);
+double ConvertMs2KMh(double ms);
void ConvertPhoneNum(uint8_t *dst, int length, const char *src);
void ConvertHex2String(char *str, const uint8_t *hex, int length);
--
Gitblit v1.8.0