From f7a18ec4494b9c5c9ef3fd440bbf68ffc6425e18 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 08 十二月 2022 15:40:56 +0800
Subject: [PATCH] 智慧驾培首次提交
---
lib/src/main/cpp/test_items/stop_and_start.cpp | 42 +++++++++++++++++++++++-------------------
1 files changed, 23 insertions(+), 19 deletions(-)
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 135f74b..7639775 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/stop_and_start.cpp
@@ -77,6 +77,7 @@
int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
+ static double distanceToStopLine = 0, distanceToEdge = 0;
if (!testing)
return 0;
@@ -100,18 +101,6 @@
testing = false;
}
- // 璺濈妫�娴�
- {
- vector<double> dtox;
- vector<Line> line_set;
- Line distance_line;
-
- MakeLine(&distance_line, &map->point[0], &map->point[8]);
- line_set.push_back(distance_line);
- DistanceOfTire2X(dtox, car, line_set);
- MA_SendDistance(dtox[0], dtox[1]);
- }
-
if (prevMoveDirect != moveDirect) {
if (moveDirect == 0) {
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
@@ -132,26 +121,26 @@
stopPoint = car->carXY[car->body[0]];
- double dis1 = DistanceOfHead2Stopline(map, car);
- double dis2 = DistanceOfTire2Edge(map, car);
+ distanceToStopLine = DistanceOfHead2Stopline(map, car);
+ distanceToEdge = DistanceOfTire2Edge(map, car);
- DEBUG("DIS1 = %f DIS2 = %f", dis1, dis2);
+ DEBUG("DIS1 = %f DIS2 = %f", distanceToStopLine, distanceToEdge);
- if (dis1 > examParam.ramp_stoppoint_red_distance) {
+ if (distanceToStopLine > examParam.ramp_stoppoint_red_distance) {
// 璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背
AddExamFault(20301, rtkTime);
DEBUG("璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背锛屼笉鍚堟牸");
- } else if (fabs(dis1) > EPSILON) {
+ } else if (fabs(distanceToStopLine) > EPSILON) {
// 鍓嶄繚闄╂病鏈変綅浜庡仠姝㈠甫鍐咃紝浣嗘病鏈夎秴鍑�50鍘樼背锛屾墸10鍒�
AddExamFault(20304, rtkTime);
DEBUG("鍓嶄繚闄╂病鏈変綅浜庡仠姝㈠甫鍐咃紝浣嗘病鏈夎秴鍑�50鍘樼背");
}
- if (dis2 > examParam.ramp_edge_red_distance) {
+ if (distanceToEdge > examParam.ramp_edge_red_distance) {
// 璺濈杈圭嚎瓒呭嚭50鍘樼背,涓嶅悎鏍�
AddExamFault(20302, rtkTime);
DEBUG("璺濈杈圭嚎瓒呭嚭50鍘樼背");
- } else if (dis2 > examParam.ramp_edge_yellow_distance) {
+ } else if (distanceToEdge > examParam.ramp_edge_yellow_distance) {
// 璺濈杈圭嚎瓒呭嚭30鍘樼背锛屾墸10鍒�
AddExamFault(20305, rtkTime);
DEBUG("璺濈杈圭嚎瓒呭嚭30鍘樼背");
@@ -161,6 +150,20 @@
if (stopCar && !handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
handBreakActive = true;
}
+ }
+
+ if (!stopCar) {
+ // 璺濈妫�娴�
+ vector<double> dtox;
+ vector<Line> line_set;
+ Line distance_line;
+
+ MakeLine(&distance_line, &map->point[0], &map->point[8]);
+ line_set.push_back(distance_line);
+ DistanceOfTire2X(dtox, car, line_set);
+ MA_SendDistance(dtox[0], dtox[1]);
+ } else {
+ MA_SendDistance(distanceToStopLine, distanceToEdge);
}
// 鍒ゆ柇璧锋鍚庢粦鐘舵��
@@ -289,6 +292,7 @@
double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], edge);
// return (l1+l2)/2.0;
+
return MAX(l1, l2); // 鍙栨渶杩滅殑
}
--
Gitblit v1.8.0