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/turn_a90.cpp | 170 +++++++++++++++++++++++++++-----------------------------
1 files changed, 83 insertions(+), 87 deletions(-)
diff --git a/lib/src/main/cpp/test_items/turn_a90.cpp b/lib/src/main/cpp/test_items/turn_a90.cpp
index a2eafb1..724c69d 100644
--- a/lib/src/main/cpp/test_items/turn_a90.cpp
+++ b/lib/src/main/cpp/test_items/turn_a90.cpp
@@ -2,6 +2,17 @@
// Created by YY on 2019/11/4.
//
+// 4----------------------|5
+// |
+// |
+// 3----------|2 |
+// | |
+// | |
+// | |
+// | |
+// | |
+// |1 |0
+
#include "turn_a90.h"
#include "../test_common/Geometry.h"
#include "../driver_test.h"
@@ -21,90 +32,60 @@
using namespace std;
static bool testing;
-static int mapIndex;
static int enterAreaHeading;
static bool turnLeftFinished;
-static uint32_t stopTimepoint = 0;
-
-static bool reportStopCarTimeout;
-static int prevMoveDirect;
static bool crashRedLine;
-static bool CrashRedLine(const Polygon *map, const car_model *car);
-static bool ExitTestArea(const Polygon *map, const car_model *car);
+static bool CrashRedLine(prime_t &prime);
+static bool ExitTestArea(prime_t &prime);
-void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime)
+void StartTurnA90(prime_t &prime)
{
DEBUG("杩涘叆鐩磋杞集鍦哄湴");
testing = true;
- enterAreaHeading = (int) heading;
- prevMoveDirect = moveDirect;
- if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- }
- reportStopCarTimeout = false;
+ enterAreaHeading = (int) prime.pModeling->yaw;
crashRedLine = false;
turnLeftFinished = false;
- mapIndex = index;
- MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 1);
+ MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_TURN_90, 1);
}
-int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime)
+static void StoppingTimeout(apptimer_var_t val) {
+ DEBUG("涓�斿仠杞�");
+ AddExamFault(20703);
+}
+
+void MotionChange(move_status_t mv)
{
- int az = (int) heading;
+ if (!testing)
+ return;
+
+ AppTimer_delete(StoppingTimeout);
+
+ if (mv == STOP) {
+ AppTimer_add(StoppingTimeout, D_SEC(2));
+ } else {
+
+ }
+}
+
+void TestTurnA90(prime_t &prime)
+{
+ int az = (int) prime.pModeling->yaw;
vector<double> dtox;
vector<Line> line_set;
Line distance_line;
- if (ExitTestArea(map, car)) {
- testing = false;
- goto TEST_END;
- }
-
- // 璺濈妫�娴�
- MakeLine(&distance_line, &map->point[0], &map->point[5]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[5], &map->point[4]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[1], &map->point[2]);
- line_set.push_back(distance_line);
- MakeLine(&distance_line, &map->point[2], &map->point[3]);
- line_set.push_back(distance_line);
- DistanceOfTire2X(dtox, car, line_set);
- MA_SendDistance(dtox[0], dtox[1]);
-
- if (CrashRedLine(map, car)) {
+ if (CrashRedLine(prime)) {
if (!crashRedLine) {
crashRedLine = true;
// 纰惧帇閬撹矾杈圭紭,涓嶅悎鏍�
- AddExamFault(20701, rtkTime);
+ AddExamFault(20701);
DEBUG("纰惧帇閬撹矾杈圭紭");
}
} else {
crashRedLine = false;
- }
-
- if (moveDirect != prevMoveDirect) {
- if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- reportStopCarTimeout = false;
-
- DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
- } else {
-
- }
- prevMoveDirect = moveDirect;
- } else if (moveDirect == 0) {
- uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
-
- if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.turn_a90_pause_criteria) && !reportStopCarTimeout) {
- // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
- AddExamFault(20703, rtkTime);
- DEBUG("涓�斿仠杞�");
- reportStopCarTimeout = true;
- }
}
// 妫�鏌ヨ浆鍚戠姸鎬�
@@ -117,7 +98,7 @@
if (az >= 30) {
if (!turnLeftFinished) {
char turn_direct;
- if((( ((int)heading) + 360 - enterAreaHeading) % 360) < 180) {
+ if((( az + 360 - enterAreaHeading) % 360) < 180) {
DEBUG("鍙宠浆");
turn_direct = 'R';
} else {
@@ -128,39 +109,38 @@
if ((turn_direct == 'R' && ReadCarStatus(TURN_SIGNAL_LAMP) != RIGHT_TURN_LIGHT) ||
(turn_direct == 'L' && ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT)) {
DEBUG("杞悜鐏湭寮�鍚�");
- AddExamFault(20702, rtkTime);
+ AddExamFault(20702);
}
}
turnLeftFinished = true;
}
- if (turnLeftFinished) {
-
+ if (ExitTestArea(prime)) {
+ testing = false;
+ MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_TURN_90, 0);
}
-
-TEST_END:
- if (!testing) {
- MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 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;
const int red_lines[][2] = {{0, 5}, {5, 4}, {1, 2}, {2, 3}};
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 < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
- MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
+ Line red_line;
+ MAKE_LINE(red_line, prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[red_lines[i][0]],
+ prime.pMap->turn_a90_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) {
ret = true;
@@ -171,25 +151,41 @@
return ret;
}
-// 鏁翠釜杞﹁締閮借椹剁璇ユ祴璇曞尯鍩�
-static bool ExitTestArea(const Polygon *map, const car_model *car)
+// 4涓溅杞拰杞﹀ご鐐逛笉鍦ㄥ満鍦颁腑
+static bool ExitTestArea(prime_t &prime)
{
- bool ret = false;
+ Polygon polygon;
- // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
- Polygon carBody;
-
- 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]];
+ polygon.num = prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map.size();
+ polygon.point = new PointF[polygon.num];
+ for (int i = 0; i < polygon.num; ++i) {
+ polygon.point[i] = prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[i];
}
- if (IntersectionOf(&carBody, map) == GM_None) {
- ret = true;
+ int num = 0;
+
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
+ &polygon) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
+ &polygon) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
+ &polygon) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]],
+ &polygon) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]],
+ &polygon) == GM_None) {
+ num++;
}
- free(carBody.point);
+ delete []polygon.point;
- return ret;
+ return num == 5? true : false;
}
--
Gitblit v1.8.0