From 148254bb1dc170db320bcb208ca79b0e252751d8 Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期三, 28 四月 2021 19:26:00 +0800
Subject: [PATCH] 修改科目三道路识别问题
---
lib/src/main/cpp/test_items/park_edge.cpp | 102 ++++++++++++++++++++++++++++++++++++++++++++-------
1 files changed, 88 insertions(+), 14 deletions(-)
diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp
index cf155ed..4a4160c 100644
--- a/lib/src/main/cpp/test_items/park_edge.cpp
+++ b/lib/src/main/cpp/test_items/park_edge.cpp
@@ -12,6 +12,7 @@
#include "../test_common/car_sensor.h"
#include "../master/comm_if.h"
#include "area_exam.h"
+#include "../test_common/odo_graph.h"
#include <vector>
#include <cstdlib>
@@ -33,16 +34,20 @@
static bool reportParkFail;
static uint32_t stopTimepoint = 0;
-static bool occurCrashRedLine1, occurCrashRedLine2;
+static bool occurCrashRedLine1, occurCrashRedLine2, occurCrashRedLine3;
static int prevMoveStatus, storeMoveStatusBeforeStop;
static int parkStatus;
static int gearAtStop;
static bool occurMoveBack, checkPark, parkSuccess, checkLight;
static uint32_t moveBackTimePoint;
static int testStatus;
+static int exitAreaCfm;
+static int currGear;
+static double odo;
static bool CrashRedLine1(const Polygon *map, const car_model *car);
static bool CrashRedLine2(const Polygon *map, const car_model *car);
+static bool CrashRedLine3(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);
static bool ExitParkArea2(const Polygon *map, const car_model *car);
@@ -54,7 +59,7 @@
testStatus = TESTING;
mapIndex = index;
- occurCrashRedLine1 = occurCrashRedLine2 = false; // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑
+ occurCrashRedLine1 = occurCrashRedLine2 = occurCrashRedLine3 = false; // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑
reportExamTimeout = false;
reportParkFail = false;
prevMoveStatus = moveStatus;
@@ -64,6 +69,10 @@
checkPark = false;
checkLight = false;
gearAtStop = -1;
+ stopTimepoint = 0;
+ odo = ReadOdo();
+ currGear = ReadCarStatus(GEAR);
+ exitAreaCfm = 0;
PlayTTS("鎮ㄥ凡杩涘叆渚ф柟鍋滆溅鍖哄煙", NULL);
// 浠呭綋鍙戠敓鍊掕溅锛屾墠鎰忓懗鐫�椤圭洰寮�濮�
@@ -78,24 +87,55 @@
vector<double> dtox;
vector<Line> line_set;
Line distance_line;
- bool is_gear_r = false;
+
+ 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 (gear_change) {
+ // 妫�鏌ヤ笂涓�娆℃尅浣嶇殑琛岄┒璺濈锛岃繃灏忓氨鏀惧純锛岄伩寮�瀛﹀憳鍘熷湴鎸傛尅閲嶈瘯
+ double run_distance = ReadOdo() - odo;
+
+ DEBUG("2娆℃尅浣嶈繍琛岃窛绂� %f", run_distance);
+
+ if (run_distance < 1) {
+ gear_change = false;
+ DEBUG("2娆℃尅浣嶈繍琛岃窛绂昏繃灏忥紝蹇界暐");
+ }
+ odo = ReadOdo();
+ }
// 棣栨鎸傚�掓尅锛� 鎵嶆剰鍛崇潃椤圭洰寮�濮�
- if (testStatus == TESTING) {
- if (ReadCarStatus(GEAR) == GEAR_R) {
- is_gear_r = true;
+ if (testStatus == TESTING && gear_change) {
+ if (currGear == GEAR_R) {
if (!occurMoveBack) {
+ DEBUG("棣栨渚ф柟鍋滆溅");
+ } else {
+ DEBUG("鍐嶆渚ф柟鍋滆溅");
+ }
+
+ {
+ checkPark = false;
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;
+ checkLight = false;
if (EnterParking(map, car)) {
parkStatus = 1;
parkSuccess = true;
@@ -129,6 +169,17 @@
}
} else {
occurCrashRedLine2 = false;
+ }
+
+ if (CrashRedLine3(map, car)) {
+ if (!occurCrashRedLine3 && !occurMoveBack && moveStatus == 1) {
+ // 杞﹁韩鍘嬪簱浣嶇嚎锛屾瘡娆℃墸10鍒�
+ AddExamFault(20407, rtkTime);
+ DEBUG("杞﹁韩鍘嬪簱浣嶇嚎");
+ occurCrashRedLine3 = true;
+ }
+ } else {
+ occurCrashRedLine3 = false;
}
if (ExitParkArea2(map, car)) {
@@ -166,9 +217,13 @@
if (testStatus == TESTING && !occurMoveBack && ExitParkArea(map, car)) {
// 鍏ュ簱鍚庝竴鐩村墠杩涳紝杞﹀ご绉诲嚭椹剁绾�
- AddExamFault(10103, rtkTime);
- DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
- testStatus = TEST_FAIL;
+ if (++exitAreaCfm >= 4) { // 閬垮厤淇″彿婕傜Щ閫犳垚鐨勮鍒�
+ AddExamFault(10103, rtkTime);
+ DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
+ testStatus = TEST_FAIL;
+ }
+ } else {
+ exitAreaCfm = 0;
}
if (occurMoveBack) {
@@ -187,17 +242,19 @@
parkStatus = 0;
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
storeMoveStatusBeforeStop = prevMoveStatus;
- gearAtStop = is_gear_r ? 1 : 0;
+ 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);
- } else if (prevMoveStatus == 0) {
+ DEBUG("鍋滆溅鏃舵尅浣� = %d", gearAtStop);
+ } else if (prevMoveStatus == 0 && stopTimepoint > 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);
+ DEBUG("鍐嶆绉诲姩鏃舵尅浣� = %d", currGear == GEAR_R ? 1 : 0);
if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria)
&& occurMoveBack
- && gearAtStop == (is_gear_r ? 1 : 0)) {
+ && gearAtStop == (currGear == GEAR_R ? 1 : 0)) {
// 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
AddExamFault(20406, rtkTime);
DEBUG("鍋滆溅瓒呮椂");
@@ -326,15 +383,32 @@
}
}
+ free(car_body.point);
+ return ret;
+}
+
+static bool CrashRedLine3(const Polygon *map, const car_model *car) {
+ bool ret = false;
+
if (!occurMoveBack) {
// 鍊掕溅鍓嶏紝杞﹁韩涓嶅緱鍘嬪簱浣嶈櫄绾�
+ Polygon car_body;
+ Line red_line;
+
+ 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->carXY[car->body[i]];
+ }
+
MakeLine(&red_line, &map->point[2], &map->point[5]);
if (IntersectionOf(red_line, &car_body) != GM_None) {
ret = true;
}
+
+ free(car_body.point);
}
- free(car_body.point);
return ret;
}
--
Gitblit v1.8.0