From 4bd08f0355b6b2cf3c027202d5ad301b4e182953 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 31 三月 2023 17:16:22 +0800
Subject: [PATCH] 科目二修改
---
lib/src/main/cpp/test_items/stop_and_start.cpp | 314 ++++++++++++++++++++++++----------------------------
1 files changed, 146 insertions(+), 168 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 7639775..0037350 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/stop_and_start.cpp
@@ -2,6 +2,21 @@
// Created by YY on 2019/10/31.
//
+// |9
+// |
+// |
+// |
+// |
+// 7-----------------|8
+// 6_________________|5
+// 3_________________|4
+// |
+// 2-----------------|1
+// |
+// |
+// |
+// |0
+
#include <cstdlib>
#include <vector>
#include <cmath>
@@ -21,7 +36,7 @@
using namespace std;
-const uint32_t STOP_CAR_TIME = D_SEC(1);
+
const double EPSILON = 1e-3;
@@ -29,62 +44,66 @@
static PointF stopPoint;
-static int mapIndex = 0;
-static int prevMoveDirect;
-static uint32_t stopTimepoint = 0;
-static bool stopCar = false;
-static uint32_t stopCarTime;
+static bool check1 = false;
+
+static bool stopConfirm;
+static bool restartComplete = false;
static bool occurCrashRedLine = false;
-static bool slideLongDistance = false;
+
static bool slideNormalDistance = false;
static bool reportSlideFault = false;
-static bool reportStartTimeout = false;
static bool handBreakActive = false;
-static bool CrashRedLine(const Polygon *map, const car_model *car);
-static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car);
-static double DistanceOfTire2Edge(const Polygon *map, const car_model *car);
-static bool ExitTestArea(const Polygon *map, const car_model *car);
-void StartSAS(int index, const Polygon *map, const car_model *car, int moveDirect, const struct RtkTime *rtkTime)
+static bool CrashRedLine(prime_t &prime);
+static double DistanceOfHead2Stopline(prime_t &prime);
+static double DistanceOfTire2Edge(prime_t &prime);
+static bool ExitTestArea(prime_t &prime);
+
+void StartSAS(prime_t &prime)
{
- double yawEdge = YawOf(map->point[8], map->point[0]);
-
- if (moveDirect < 0 || DeltaYaw(car->yaw, yawEdge) >= 90.0) {
- testing = false;
- return;
- }
-
DEBUG("杩涘叆鍧¤捣椤圭洰");
testing = true;
- mapIndex = index;
- prevMoveDirect = moveDirect;
-
- if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- }
-
+ stopConfirm = false;
+ restartComplete = false;
occurCrashRedLine = false;
- stopCar = false;
- slideLongDistance = false;
+ check1 = false;
slideNormalDistance = false;
reportSlideFault = false;
- reportStartTimeout = false;
handBreakActive = false;
- MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 1);
+ MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 1);
}
-int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
+static void StoppingTimeout(apptimer_var_t val) {
+ // 璧锋鏃堕棿瓒呰繃30绉掞紝涓嶅悎鏍�
+ AddExamFault(20303);
+ DEBUG("璧锋鏃堕棿瓒呰繃30绉�");
+}
+
+static void StopConfirm(apptimer_var_t val) {
+ stopConfirm = true;
+}
+
+void MotionChange(move_status_t mv)
+{
+ AppTimer_delete(StopConfirm);
+
+ if (mv == STOP) {
+ AppTimer_add(StopConfirm, D_SEC(1));
+ } else {
+
+ }
+}
+
+int TestSAS(prime_t &prime)
{
static double distanceToStopLine = 0, distanceToEdge = 0;
- if (!testing)
- return 0;
- if (CrashRedLine(map, car)) {
+ if (CrashRedLine(prime)) {
// 杞﹁疆鍘嬬嚎锛屼笉鍚堟牸
if (!occurCrashRedLine) {
- AddExamFault(10116, rtkTime);
+ AddExamFault(10116);
DEBUG("杞﹁疆鍘嬬嚎");
}
occurCrashRedLine = true;
@@ -92,158 +111,125 @@
occurCrashRedLine = false;
}
- if (ExitTestArea(map, car)) {
- // 椹剁娴嬭瘯鍖�
- if (!stopCar) {
- // 涓嶅仠杞︾洿鎺ョ寮�
- AddExamFault(10103, rtkTime);
- }
- testing = false;
- }
+ // 妫�娴嬪埌鍋滆溅
+ if (prime.pMotion->move == STOP && stopConfirm) {
+ if (!check1) {
+ check1 = true;
+ // 瀛樺偍鍋滄鐐�
+ stopPoint = prime.pModeling->base_point;
+ // 寮�濮嬪仠杞﹁鏃�
+ AppTimer_delete(StoppingTimeout);
+ AppTimer_add(StoppingTimeout, examParam.ramp_start_car_limit_time);
- if (prevMoveDirect != moveDirect) {
- if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- } else if (stopCar && !handBreakActive) {
- // 妫�鏌ユ槸鍚︽媺浣忔墜鍒�
- handBreakActive = true;
- AddExamFault(20306, rtkTime);
- DEBUG("娌℃媺鎵嬪埞");
- }
- 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 && !stopCar) {
- // 杩欓噷鍒ゆ柇鍋滆溅鐘舵��
- stopCar = true;
- stopCarTime = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
-
- stopPoint = car->carXY[car->body[0]];
-
- distanceToStopLine = DistanceOfHead2Stopline(map, car);
- distanceToEdge = DistanceOfTire2Edge(map, car);
+ // 妫�鏌ヨ溅澶村拰鍋滆溅甯︾殑璺濈
+ distanceToStopLine = DistanceOfHead2Stopline(prime);
+ distanceToEdge = DistanceOfTire2Edge(prime);
DEBUG("DIS1 = %f DIS2 = %f", distanceToStopLine, distanceToEdge);
if (distanceToStopLine > examParam.ramp_stoppoint_red_distance) {
// 璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背
- AddExamFault(20301, rtkTime);
+ AddExamFault(20301);
DEBUG("璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背锛屼笉鍚堟牸");
} else if (fabs(distanceToStopLine) > EPSILON) {
// 鍓嶄繚闄╂病鏈変綅浜庡仠姝㈠甫鍐咃紝浣嗘病鏈夎秴鍑�50鍘樼背锛屾墸10鍒�
- AddExamFault(20304, rtkTime);
+ AddExamFault(20304);
DEBUG("鍓嶄繚闄╂病鏈変綅浜庡仠姝㈠甫鍐咃紝浣嗘病鏈夎秴鍑�50鍘樼背");
}
if (distanceToEdge > examParam.ramp_edge_red_distance) {
// 璺濈杈圭嚎瓒呭嚭50鍘樼背,涓嶅悎鏍�
- AddExamFault(20302, rtkTime);
+ AddExamFault(20302);
DEBUG("璺濈杈圭嚎瓒呭嚭50鍘樼背");
} else if (distanceToEdge > examParam.ramp_edge_yellow_distance) {
// 璺濈杈圭嚎瓒呭嚭30鍘樼背锛屾墸10鍒�
- AddExamFault(20305, rtkTime);
+ AddExamFault(20305);
DEBUG("璺濈杈圭嚎瓒呭嚭30鍘樼背");
}
}
- if (stopCar && !handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
+ // 鍋滆溅鍚庯紝妫�鏌ユ墜鍒规媺璧锋儏鍐�
+ if (!handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
handBreakActive = true;
}
}
- if (!stopCar) {
- // 璺濈妫�娴�
- vector<double> dtox;
- vector<Line> line_set;
- Line distance_line;
+ if (prime.pMotion->move != STOP && stopConfirm) {
+ // 杞﹁締浠庡仠姝㈢姸鎬佸啀娆＄Щ鍔�
+ double juli = DistanceOf(prime.pModeling->base_point, stopPoint);
- 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);
- }
-
- // 鍒ゆ柇璧锋鍚庢粦鐘舵��
- if (stopCar) {
- if (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) == 1) {
- // 鍙戠敓鍚庢粦
- double slideDistance = DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]);
-
- if (slideDistance > examParam.ramp_slide_yellow_distance) {
+ if (juli > examParam.ramp_slide_yellow_distance) {
+ double deg = YawOf(stopPoint, prime.pModeling->base_point);
+ deg = fabs(prime.pModeling->yaw - deg);
+ if (deg > 180) {
+ deg = 360 - deg;
+ }
+ if (deg < 90) {
+ // 杞﹁締涓婂潯
+ if (!restartComplete) {
+ restartComplete = true;
+ AppTimer_delete(StoppingTimeout);
+ if (slideNormalDistance && !reportSlideFault) {
+ // 鍚庢粦瓒呰繃10鍘樼背锛屼絾娌¤秴杩�30鍘樼背
+ DEBUG("鍚庢粦瓒呰繃10鍘樼背锛屼絾娌¤秴杩�30鍘樼背");
+ reportSlideFault = true;
+ AddExamFault(10204);
+ }
+ if (!handBreakActive) {
+ // 妫�鏌ユ槸鍚︽媺浣忔墜鍒�
+ DEBUG("娌℃媺鎵嬪埞");
+ handBreakActive = true;
+ AddExamFault(20306);
+ }
+ }
+ } else {
+ // 杞﹁締鍚庢粦
slideNormalDistance = true;
- }
-
- if (slideDistance > examParam.ramp_slide_red_distance && !slideLongDistance && !reportSlideFault) {
- // 鍚庢粦瓒呰繃30鍘樼背, 涓嶅悎鏍�
- AddExamFault(10106, rtkTime);
- DEBUG("鍚庢粦瓒呰繃30鍘樼背");
- slideLongDistance = true;
- reportSlideFault = true;
- }
- }
-
-
- if (!reportStartTimeout && (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) != -1 ||
- DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) < 0.1)) {
- if (TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10) - stopCarTime > examParam.ramp_start_car_limit_time) {
- // 璧锋鏃堕棿瓒呰繃30绉掞紝涓嶅悎鏍�
- AddExamFault(20303, rtkTime);
- DEBUG("璧锋鏃堕棿瓒呰繃30绉�");
- reportStartTimeout = true;
- }
- }
-
- if (IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->axial[AXIAL_REAR]]) == -1) {
- // 杞﹀熬椹惰繃鍋滄鏉�
- if (slideNormalDistance && !slideLongDistance && !reportSlideFault) {
- reportSlideFault = true;
- // 鍚庢粦瓒呰繃10鍘樼背锛屼絾娌¤秴杩�30鍘樼背
- AddExamFault(10204, rtkTime);
- DEBUG("鍚庢粦瓒呰繃10鍘樼背锛屼絾娌¤秴杩�30鍘樼背");
+ if (juli > examParam.ramp_slide_red_distance && !reportSlideFault) {
+ // 鍚庢粦瓒呰繃30鍘樼背, 涓嶅悎鏍�
+ DEBUG("鍚庢粦瓒呰繃30鍘樼背");
+ reportSlideFault = true;
+ AddExamFault(10106);
+ }
}
}
}
- if (!testing) {
- MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 0);
+ if (ExitTestArea(prime)) {
+ // 椹剁娴嬭瘯鍖�
+ if (!stopConfirm) {
+ // 涓嶅仠杞︾洿鎺ョ寮�
+ AddExamFault(10103);
+ }
+ MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 0);
}
return testing ? 1 : 0;
}
// 杞﹁疆鏄惁鍘嬭竟绾�
-static bool CrashRedLine(const Polygon *map, const car_model *car)
+static bool CrashRedLine(prime_t &prime)
{
bool ret = false;
- Line red_line;
- int red_lines[2][2];
- int red_line_num = 0;
+ int red_lines[][2] = {{0, 1}, {1, 4}, {4, 5}, {5, 8}, {8, 9}};
- if (map->num == 10) {
- red_lines[0][0] = 0;
- red_lines[0][1] = 8;
- red_lines[1][0] = 8;
- red_lines[1][1] = 9;
- red_line_num = 2;
- } else {
- red_lines[0][0] = 0;
- red_lines[0][1] = 8;
- red_line_num = 1;
- }
Line frontAxle, rearAxle;
- MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
- MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
+ // 浠呯湅杞﹁疆澶栦晶
+ MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
+ prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
+ MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
+ prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);
- for (int i = 0; i < red_line_num; ++i) {
- MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
+ for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
+ Line red_line;
+ MAKE_LINE(red_line, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[red_lines[i][0]],
+ prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[red_lines[i][1]]);
+
if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
- IntersectionOf(red_line, rearAxle) == GM_Intersection) {
+ IntersectionOf(red_line, rearAxle) == GM_Intersection) {
ret = true;
break;
}
@@ -252,28 +238,21 @@
return ret;
}
-static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car)
+static double DistanceOfHead2Stopline(prime_t &prime)
{
double dis = 0.0;
- int rel1 = IntersectionOfLine(map->point[4], map->point[3], car->carXY[car->body[0]]);
- int rel2 = IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->body[0]]);
+ Line upper_edge, lower_edge;
- DEBUG("%d %d %f, %f", car->body[0], car->axial[AXIAL_FRONT], car->carXY[car->body[0]].X, car->carXY[car->body[0]].Y);
- DEBUG("rel1 %d rel2 %d", rel1, rel2);
+ MAKE_LINE(upper_edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[5],
+ prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[6]);
+ MAKE_LINE(lower_edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[4],
+ prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[3]);
- if (rel1 == 1) {
- Line line1;
-
- MakeLine(&line1, &map->point[4], &map->point[3]);
-
- dis = DistanceOf(car->carXY[car->body[0]], line1);
- } else if (rel2 == -1) {
- Line line2;
-
- MakeLine(&line2, &map->point[5], &map->point[6]);
-
- dis = DistanceOf(car->carXY[car->body[0]], line2);
+ if (IntersectionOfLine(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge) == REL_POS_RIGHT) {
+ dis = DistanceOf(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge);
+ } else if (IntersectionOfLine(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], lower_edge) == REL_POS_LEFT) {
+ dis = DistanceOf(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], lower_edge);
}
DEBUG("DistanceOfHead2Stopline dis %f", dis);
@@ -281,28 +260,27 @@
return dis;
}
-static double DistanceOfTire2Edge(const Polygon *map, const car_model *car)
+static double DistanceOfTire2Edge(prime_t &prime)
{
Line edge;
- MakeLine(&edge, &map->point[0], &map->point[8]);
+ MAKE_LINE(edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[0],
+ prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[8]);
- double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], edge);
+ double l1 = DistanceOf(prime.pModeling->points[prime.pModel->right_front_tire[TIRE_OUTSIDE]], edge);
- double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], edge);
-
-// return (l1+l2)/2.0;
+ double l2 = DistanceOf(prime.pModeling->points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]], edge);
return MAX(l1, l2); // 鍙栨渶杩滅殑
}
// 鏁翠釜杞﹁締閮借椹剁璇ユ祴璇曞尯鍩�
-static bool ExitTestArea(const Polygon *map, const car_model *car)
+static bool ExitTestArea(prime_t &prime)
{
// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
bool ret = false;
- Polygon carBody, map2;
+ /*Polygon carBody, map2;
PointF vPoint = Calc3Point(map->point[8], map->point[0], DistanceOf(map->point[8], map->point[7]), 'R');
PointF vPoint2;
@@ -329,6 +307,6 @@
}
free(carBody.point);
- free(map2.point);
+ free(map2.point);*/
return ret;
}
--
Gitblit v1.8.0