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/driver_test.cpp | 23 +++
lib/src/main/cpp/test_common/odo_graph.cpp | 6
lib/src/main/cpp/test_items/stop_and_start.cpp | 4
lib/src/main/cpp/test_items/area_exam.cpp | 79 +++++++++---
app/src/main/java/safeluck/drive/evaluation/fragment/RoadDriveMapFragmentab.java | 3
lib/src/main/cpp/test_common/Geometry.cpp | 9 +
lib/src/main/cpp/test_items/park_bottom.cpp | 36 ++++-
lib/src/main/cpp/test_items/park_edge.cpp | 102 ++++++++++++++--
lib/src/main/cpp/master/comm_if.cpp | 1
lib/src/main/cpp/test_items2/road_exam.cpp | 25 +++
app/src/main/java/safeluck/drive/evaluation/fragment/RoadDriveMapFragmentaa.java | 4
lib/src/main/cpp/test_common/Geometry.h | 1
lib/src/main/cpp/test_common/car_sensor.cpp | 3
lib/src/main/cpp/rtk_platform/platform.cpp | 6
14 files changed, 250 insertions(+), 52 deletions(-)
diff --git a/app/src/main/java/safeluck/drive/evaluation/fragment/RoadDriveMapFragmentaa.java b/app/src/main/java/safeluck/drive/evaluation/fragment/RoadDriveMapFragmentaa.java
index e422d48..290f0f6 100644
--- a/app/src/main/java/safeluck/drive/evaluation/fragment/RoadDriveMapFragmentaa.java
+++ b/app/src/main/java/safeluck/drive/evaluation/fragment/RoadDriveMapFragmentaa.java
@@ -11,6 +11,7 @@
import android.graphics.Path;
import android.os.Bundle;
import android.text.TextUtils;
+import android.util.DebugUtils;
import android.util.Log;
import android.view.LayoutInflater;
import android.view.SurfaceHolder;
@@ -890,6 +891,9 @@
paint.setStyle(Paint.Style.STROKE);
paint.setColor(Color.GREEN);
paint.setStrokeWidth(2.0f);
+ paint.setPathEffect(null);
+
+ Log.d(TAG, "鐢昏溅浣�");
Path carpath = new Path();
// 鐢昏溅浣�
diff --git a/app/src/main/java/safeluck/drive/evaluation/fragment/RoadDriveMapFragmentab.java b/app/src/main/java/safeluck/drive/evaluation/fragment/RoadDriveMapFragmentab.java
index 1c98ed6..f175a96 100644
--- a/app/src/main/java/safeluck/drive/evaluation/fragment/RoadDriveMapFragmentab.java
+++ b/app/src/main/java/safeluck/drive/evaluation/fragment/RoadDriveMapFragmentab.java
@@ -89,7 +89,7 @@
}
private SurfaceView mSurfaceView;
- private static final String TAG = "RoadDriveFragmentaa";
+ private static final String TAG = "RoadDriveFragmentab";
private SurfaceHolder holder;
Path path = new Path();
private Canvas canvas;
@@ -889,6 +889,7 @@
paint.setStyle(Paint.Style.STROKE);
paint.setColor(Color.GREEN);
paint.setStrokeWidth(2.0f);
+ paint.setPathEffect(null);
Path carpath = new Path();
// 鐢昏溅浣�
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index f6c9ee7..ab63bb9 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -381,6 +381,29 @@
// RoadMap.examScheme.assign(scheme.begin(), scheme.end());
DEBUG("寰楀埌鏂扮殑璺�冨湴鍥� 璺暟閲� %d 鐗规畩鍖哄煙鏁伴噺 %d 鍏朵粬绂佹绾挎暟閲� %d 椤圭洰鏁伴噺 %d", RoadMap.roads.size(), RoadMap.specialAreas.size(), RoadMap.forbidLines.size(), RoadMap.examScheme.size());
+
+ for (int i = 0; i < RoadMap.roads.size(); ++i) {
+ DEBUG("璺� id = %d", RoadMap.roads[i].id);
+ DEBUG("宸﹁竟绾挎鏁� %d", RoadMap.roads[i].leftEdge.size());
+ for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) {
+ int n = RoadMap.roads[i].leftEdge[j].points.size();
+
+ DEBUG("\t褰撳墠宸﹁竟绾挎 绫诲瀷 %d 鐐规暟 %d", RoadMap.roads[i].leftEdge[j].character, n);
+ for (int k = 0; k < n; ++k) {
+ DEBUG("\t\t鐐瑰潗鏍� %d: %f, %f", k, RoadMap.roads[i].leftEdge[j].points[k].X, RoadMap.roads[i].leftEdge[j].points[k].Y);
+ }
+ }
+
+ DEBUG("鍙宠竟绾挎鏁� %d", RoadMap.roads[i].rightEdge.size());
+ for (int j = 0; j < RoadMap.roads[i].rightEdge.size(); ++j) {
+ int n = RoadMap.roads[i].rightEdge[j].points.size();
+
+ DEBUG("\t褰撳墠鍙宠竟绾挎 绫诲瀷 %d 鐐规暟 %d", RoadMap.roads[i].rightEdge[j].character, n);
+ for (int k = 0; k < n; ++k) {
+ DEBUG("\t\t鐐瑰潗鏍� %d: %f, %f", k, RoadMap.roads[i].rightEdge[j].points[k].X, RoadMap.roads[i].rightEdge[j].points[k].Y);
+ }
+ }
+ }
}
void SetRoadExamScheme(vector<scheme_t> &scheme)
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index 1240336..4bed281 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -938,6 +938,7 @@
vector<stop_line_t> crossing;
+
for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2) {
stop_line_t temp;
if (!itr2->IsObject()) {
diff --git a/lib/src/main/cpp/rtk_platform/platform.cpp b/lib/src/main/cpp/rtk_platform/platform.cpp
index ec41199..11139bc 100644
--- a/lib/src/main/cpp/rtk_platform/platform.cpp
+++ b/lib/src/main/cpp/rtk_platform/platform.cpp
@@ -604,8 +604,10 @@
sensor.surround3 = data[x++];
sensor.surround4 = data[x++];
- if (sensor.clutch == 1)
- sensor.gear = 0;
+// DEBUG("绂诲悎 = %d", sensor.clutch);
+
+ // if (sensor.clutch == 1)
+ // sensor.gear = 0;
UpdateSensor(&sensor);
break;
diff --git a/lib/src/main/cpp/test_common/Geometry.cpp b/lib/src/main/cpp/test_common/Geometry.cpp
index bc05767..0864ff2 100644
--- a/lib/src/main/cpp/test_common/Geometry.cpp
+++ b/lib/src/main/cpp/test_common/Geometry.cpp
@@ -403,6 +403,15 @@
return newPoint;
}
+PointF centerOfTwoPoint(PointF p1, PointF p2) {
+ PointF center;
+
+ center.X = (p1.X + p2.X) / 2;
+ center.Y = (p1.Y + p2.Y) / 2;
+
+ return center;
+}
+
// All in
bool InsidePolygon(const Polygon *t1, const Polygon *t2) {
for (int i = 0; i < t1->num; ++i) {
diff --git a/lib/src/main/cpp/test_common/Geometry.h b/lib/src/main/cpp/test_common/Geometry.h
index bd34181..7248273 100644
--- a/lib/src/main/cpp/test_common/Geometry.h
+++ b/lib/src/main/cpp/test_common/Geometry.h
@@ -55,6 +55,7 @@
double DeltaYaw(double yaw1, double yaw2);
double CalculateAngle(Line base, Line dest);
PointF rotatePoint(PointF oldPoint, PointF centre, double degree);
+PointF centerOfTwoPoint(PointF p1, PointF p2);
bool InsidePolygon(const Polygon *t1, const Polygon *t2);
bool PartInsidePolygon(const Polygon *t1, const Polygon *t2);
bool OutsidePolygon(const Polygon *t1, const Polygon *t2);
diff --git a/lib/src/main/cpp/test_common/car_sensor.cpp b/lib/src/main/cpp/test_common/car_sensor.cpp
index 3c462b4..51d43c4 100644
--- a/lib/src/main/cpp/test_common/car_sensor.cpp
+++ b/lib/src/main/cpp/test_common/car_sensor.cpp
@@ -202,6 +202,9 @@
if (s->surround4 != Sensor.surround4 && s->surround4 != '#') {
SensorChanged(SENSOR_SURROUND_CAR_4, BX(s->surround4));
}
+
+// DEBUG("鎸′綅 鑰佺殑 %d 褰撳墠 %d", Sensor.gear, s->gear);
+
if (s->gear != Sensor.gear && s->gear != '#') {
// SensorChanged(GEAR, GEAR_N + s->gear);
DEBUG("鐘舵�佹敼鍙� 鎸′綅 %d", s->gear);
diff --git a/lib/src/main/cpp/test_common/odo_graph.cpp b/lib/src/main/cpp/test_common/odo_graph.cpp
index 977fc23..87381a9 100644
--- a/lib/src/main/cpp/test_common/odo_graph.cpp
+++ b/lib/src/main/cpp/test_common/odo_graph.cpp
@@ -24,13 +24,13 @@
}
void UpdataOdo(double speed, int moveDirect, const struct RtkTime *rtkTime) {
- // 琛岄┒璺濈锛屼笉鍖呭惈鍊掕溅
- if (odoCnt == 0 && moveDirect == 1) {
+ // 琛岄┒璺濈锛屽惈鍊掕溅
+ if (odoCnt == 0 && moveDirect != 0) {
odoPrevSpeed = speed;
odoCnt = 1;
odoTimer = *rtkTime;
} else if (odoCnt == 1) {
- if (moveDirect == 1) {
+ if (moveDirect != 0) {
uint32_t tm = TimeGetDiff(rtkTime, &odoTimer);
if (tm >= D_SEC(1)) {
odoGraph += ((double) tm) * (odoPrevSpeed + speed) / 2.0 / 1000.0;
diff --git a/lib/src/main/cpp/test_items/area_exam.cpp b/lib/src/main/cpp/test_items/area_exam.cpp
index 8c95819..1eba15d 100644
--- a/lib/src/main/cpp/test_items/area_exam.cpp
+++ b/lib/src/main/cpp/test_items/area_exam.cpp
@@ -22,8 +22,8 @@
static int CurrExamMapIndex = -1;
static int CurrEnterMapIndex = -1;
-static void DetectEnterOrExitMap(const car_model *CarModel, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
-static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
+static void DetectEnterOrExitMap(int moveDirect, const car_model *CarModel, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
+static int EnterMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
static bool ExitMap(const car_model *car, int index, LIST_AREA_MAP &mapList);
static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList);
static void ExecuteExam(int index, LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int move, double azimuth, const struct RtkTime* rtkTime);
@@ -43,7 +43,7 @@
{
UpdataOdo(speed, moveDirect, rtkTime);
- DetectEnterOrExitMap(car, CarModelList, AreaMapList);
+ DetectEnterOrExitMap(moveDirect, car, CarModelList, AreaMapList);
ExecuteExam(CurrExamMapIndex, AreaMapList, car, CarModelList, speed, moveDirect, azimuth, rtkTime);
}
@@ -103,11 +103,11 @@
array.push_back(rd);
}
-static void DetectEnterOrExitMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
+static void DetectEnterOrExitMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
{
if (CurrExamMapIndex < 0) {
if (CurrEnterMapIndex < 0) {
- CurrEnterMapIndex = EnterMap(car, CarModelList, mapList);
+ CurrEnterMapIndex = EnterMap(moveDirect, car, CarModelList, mapList);
if (CurrEnterMapIndex >= 0) {
DEBUG("杩涘叆鏌愪釜瀛愰」鐩� idx = %d", CurrEnterMapIndex);
CurrExamMapIndex = CurrEnterMapIndex;
@@ -199,7 +199,7 @@
}
}
-static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
+static int EnterMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
{
vector<int> score(mapList.size(), 0); // 鍦哄湴閲嶅悎鏃讹紝杞﹀ご灏鹃兘鍦ㄥ唴鐨勪紭鍏�
@@ -222,7 +222,7 @@
triggerLine.X2 = x9;
triggerLine.Y2 = y9;
- if (CrashTriggerLine(triggerLine, car, CarModelList))
+ if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
return i;
}
if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
@@ -230,14 +230,33 @@
MakeLine(&triggerLine, &(mapList[i].map.point[1]), &(mapList[i].map.point[0]));
- if (CrashTriggerLine(triggerLine, car, CarModelList))
+ if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
return i;
MakeLine(&triggerLine, &(mapList[i].map.point[7]), &(mapList[i].map.point[6]));
- if (CrashTriggerLine(triggerLine, car, CarModelList))
+ if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
return i;
+ // 鍏ㄨ溅鍦ㄥ満鍦板唴
+ if (moveDirect == 0) {
+ Polygon carBody;
+ bool ret = false;
+ 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]];
+ }
+
+ if (IntersectionOf(&carBody, &mapList[i].map) == GM_Containment) {
+ ret = true;
+ }
+
+ free(carBody.point);
+
+ if (ret)
+ return i;
+ }
// // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
// Line enterLine1, enterLine2;
@@ -266,8 +285,28 @@
MakeLine(&triggerLine, &(mapList[i].map.point[1]), &(mapList[i].map.point[0]));
- if (CrashTriggerLine(triggerLine, car, CarModelList))
+ if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
return i;
+
+ // 鍏ㄨ溅鍦ㄥ満鍦板唴
+ if (moveDirect == 0) {
+ Polygon carBody;
+ bool ret = false;
+ 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]];
+ }
+
+ if (IntersectionOf(&carBody, &mapList[i].map) == GM_Containment) {
+ ret = true;
+ }
+
+ free(carBody.point);
+
+ if (ret)
+ return i;
+ }
// // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
// Line enterLine;
@@ -293,7 +332,7 @@
MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
- if (CrashTriggerLine(triggerLine, car, CarModelList))
+ if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
return i;
}
if (mapList[i].type == MAP_TYPE_CURVE) {
@@ -301,7 +340,7 @@
Line triggerLine;
MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]);
- if (CrashTriggerLine(triggerLine, car, CarModelList))
+ if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
return i;
}
}
@@ -424,8 +463,8 @@
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]];
+ trace.point[pn] = centerOfTwoPoint(c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]], c1->carXY[c1->right_front_tire[TIRE_OUTSIDE]]);
+ trace2.point[pn++] = centerOfTwoPoint(c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]], c1->carXY[c1->right_rear_tire[TIRE_OUTSIDE]]);
++iter;
@@ -435,8 +474,8 @@
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]];
+ trace.point[pn] = centerOfTwoPoint(c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]], c2->carXY[c2->right_front_tire[TIRE_OUTSIDE]]);
+ trace2.point[pn++] = centerOfTwoPoint(c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]], c2->carXY[c2->right_rear_tire[TIRE_OUTSIDE]]);
c1 = c2;
}
++iter;
@@ -457,11 +496,11 @@
MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]);
if ((IntersectionOf(trace_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) ||
+ IntersectionOfLine(p1, p2, centerOfTwoPoint(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], car->carXY[car->right_front_tire[TIRE_OUTSIDE]])) == -1 &&
+ DistanceOf(centerOfTwoPoint(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]), triggerLine) > 0.1) ||
(IntersectionOf(trace2_line, triggerLine) == GM_Intersection &&
- IntersectionOfLine(p1, p2, car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]) == -1 &&
- DistanceOf(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) ) {
+ IntersectionOfLine(p1, p2, centerOfTwoPoint(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], car->carXY[car->right_rear_tire[TIRE_OUTSIDE]])) == -1 &&
+ DistanceOf(centerOfTwoPoint(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]), triggerLine) > 0.1) ) {
// 纰板埌瑙﹀彂绾�
DEBUG("纰版挒瑙﹀彂绾� 寮曞彂鍦板浘");
trigger = true;
diff --git a/lib/src/main/cpp/test_items/park_bottom.cpp b/lib/src/main/cpp/test_items/park_bottom.cpp
index 7c2124c..d1bb652 100644
--- a/lib/src/main/cpp/test_items/park_bottom.cpp
+++ b/lib/src/main/cpp/test_items/park_bottom.cpp
@@ -12,6 +12,7 @@
#include "../master/comm_if.h"
#include "area_exam.h"
#include "../test_common/car_sensor.h"
+#include "../test_common/odo_graph.h"
#include <vector>
#include <cstdlib>
@@ -55,7 +56,8 @@
static int gearAtStop;
static int currGear;
static char prevCrossedCtrlLine;
-
+static double odo;
+static int exitAreaCfm;
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, int &who);
@@ -84,6 +86,9 @@
currGear = ReadCarStatus(GEAR);
prevCrossedCtrlLine = 0;
+ stopTimepoint = 0;
+ exitAreaCfm = 0;
+ odo = ReadOdo();
PlayTTS("鎮ㄥ凡杩涘叆鍊掕溅鍏ュ簱鍖哄煙", NULL);
}
@@ -108,6 +113,19 @@
} 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 && gear_change) {
@@ -159,9 +177,13 @@
}
if (testStatus == TESTING && parkCount < 2 && AllTireExitParkArea(map, car)) {
- testStatus = TEST_FAIL;
- AddExamFault(10103, rtkTime);
- DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
+ if (++exitAreaCfm >= 4) { // 閬垮厤淇″彿婕傜Щ閫犳垚鐨勮鍒�
+ testStatus = TEST_FAIL;
+ AddExamFault(10103, rtkTime);
+ DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
+ }
+ } else {
+ exitAreaCfm = 0;
}
if (ExitParkArea(map, car)) {
@@ -215,7 +237,7 @@
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);
DEBUG("鍋滆溅鏃舵尅浣� = %d", gearAtStop);
- } else if (prevMoveDirect == 0) {
+ } else if (prevMoveDirect == 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);
DEBUG("鍋滆溅鏃堕棿 %ld", tp - stopTimepoint);
@@ -456,7 +478,7 @@
return ret;
}
-// 鍙屽墠杞拰浠讳竴鍚庤疆涓嶅湪鍖哄煙
+// 鍙屽墠杞拰鍙屽悗杞笉鍦ㄥ尯鍩�
static bool AllTireExitParkArea(const Polygon *map, const car_model *car)
{
int tireExitNum = 0;
@@ -477,7 +499,7 @@
tireExitNum++;
}
- if (tireExitNum >= 3) {
+ if (tireExitNum >= 4) {
return true;
}
return false;
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;
}
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 672c0ad..135f74b 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/stop_and_start.cpp
@@ -14,6 +14,7 @@
#include "../test_common/car_sensor.h"
#include "../master/comm_if.h"
#include "area_exam.h"
+#include "../defs.h"
#define DEBUG(fmt, args...) LOGD("<stop_and_start> <%s>: " fmt, __func__, ##args)
@@ -287,7 +288,8 @@
double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], edge);
- return (l1+l2)/2.0;
+// return (l1+l2)/2.0;
+ return MAX(l1, l2); // 鍙栨渶杩滅殑
}
// 鏁翠釜杞﹁締閮借椹剁璇ユ祴璇曞尯鍩�
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index c946ecc..003f787 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -412,24 +412,31 @@
vector<PointF> roadOutLine;
Polygon area;
+// DEBUG("CalcRoadIndex %d 鍋氱偣 %d 鏈夌偣 %d", index, RoadMap.roads[index].leftEdge.size(), RoadMap.roads[index].rightEdge.size());
+
for (int i = 0; i < RoadMap.roads[index].leftEdge.size(); ++i) {
for (int j = 0; j < RoadMap.roads[index].leftEdge[i].points.size(); ++j) {
if (changeSegment && roadOutLine.size() > 0 && IsSamePoint(roadOutLine.back(), RoadMap.roads[index].leftEdge[i].points[j])) {
+// DEBUG("閲嶅鐐�");
continue;
}
changeSegment = false;
roadOutLine.push_back(RoadMap.roads[index].leftEdge[i].points[j]);
+// DEBUG("鍋氱偣鍔犲叆 point (%f, %f)", RoadMap.roads[index].leftEdge[i].points[j].X, RoadMap.roads[index].leftEdge[i].points[j].Y);
}
changeSegment = true;
}
for (int i = 0; i < RoadMap.roads[index].rightEdge.size(); ++i) {
for (int j = RoadMap.roads[index].rightEdge[i].points.size() - 1; j >= 0; --j) {
- if (changeSegment && roadOutLine.size() > 0 && IsSamePoint(roadOutLine.back(), RoadMap.roads[index].leftEdge[i].points[j])) {
+ if (changeSegment && roadOutLine.size() > 0 && IsSamePoint(roadOutLine.back(), RoadMap.roads[index].rightEdge[i].points[j])) {
+// DEBUG("閲嶅鐐�");
continue;
}
changeSegment = false;
roadOutLine.push_back(RoadMap.roads[index].rightEdge[i].points[j]);
+
+// DEBUG("鏈夌偣鍔犲叆 point (%f, %f)", RoadMap.roads[index].rightEdge[i].points[j].X, RoadMap.roads[index].rightEdge[i].points[j].Y);
}
changeSegment = true;
}
@@ -440,6 +447,12 @@
for (int i = 0; i < area.num; ++i) {
area.point[i] = roadOutLine[i];
}
+
+// {
+// for (int x = 0; x < area.num; ++x) {
+// DEBUG("road %d point %d (%f, %f)", index, x, area.point[x].X, area.point[x].Y);
+// }
+// }
if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &area) == GM_Containment) {
free(area.point);
@@ -1401,8 +1414,8 @@
// 鎸′綅涓嶅尮閰嶈秴鏃�
if (currGearError && prevGearError) {
- DEBUG("鎸′綅閿欒澧炲姞 %ld姣 褰撳墠鎸′綅 %d 鏃堕�� %f", TimeGetDiff(rtkTime, &gearErrorTimePoint),
- ReadCarStatus(GEAR), ConvertMs2KMh(speed));
+// DEBUG("鎸′綅閿欒澧炲姞 %ld姣 褰撳墠鎸′綅 %d 鏃堕�� %f", TimeGetDiff(rtkTime, &gearErrorTimePoint),
+// ReadCarStatus(GEAR), ConvertMs2KMh(speed));
gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
}
if (gearErrorTime > examParam.gear_speed_error_cumulative_time) {
@@ -1562,6 +1575,7 @@
DEBUG("瀵煎悜绫诲瀷鍒囨崲 %d", Lane.guide);
}
}
+ DEBUG("currExamMapIndex = %d Lane.no = %d Lane.guide = %d (%f, %f)", currExamMapIndex, Lane.no, Lane.guide, car->carXY[car->axial[AXIAL_FRONT]].X, car->carXY[car->axial[AXIAL_FRONT]].Y);
if (currExamMapIndex >= 0 && Lane.guide == 0) {
BigStraightRoadFree = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);
@@ -1714,7 +1728,10 @@
item = EntryItem(roadIndex, RoadMap, car, CarModelList, forward, moveDirect);
}
- if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 3) {
+ if (RoadExamStatus == RoadExamStatus && item == 6) {
+ PlayTTS("鍓嶆柟浼氳溅", NULL);
+ }
+ else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 3) {
StartDriveStraightExam();
RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
} else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 4) {
--
Gitblit v1.8.0