From 27fc91fbe8f88b6885356e68828cfe1ce1db7601 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 28 二月 2024 09:53:13 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items/uphill.cpp | 75 ++++++++++++++++++++++---------------
1 files changed, 44 insertions(+), 31 deletions(-)
diff --git a/lib/src/main/cpp/test_items/stop_and_start.cpp b/lib/src/main/cpp/test_items/uphill.cpp
similarity index 78%
rename from lib/src/main/cpp/test_items/stop_and_start.cpp
rename to lib/src/main/cpp/test_items/uphill.cpp
index 0037350..6d0c532 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/uphill.cpp
@@ -2,7 +2,7 @@
// Created by YY on 2019/10/31.
//
-// |9
+// 11 |9
// |
// |
// |
@@ -15,13 +15,14 @@
// |
// |
// |
-// |0
+// 10 |0
#include <cstdlib>
#include <vector>
#include <cmath>
+#include <tuple>
-#include "stop_and_start.h"
+#include "uphill.h"
#include "../driver_test.h"
#include "../jni_log.h"
#include "../common/apptimer.h"
@@ -35,13 +36,9 @@
using namespace std;
-
-
-
const double EPSILON = 1e-3;
-static bool testing = false;
-
+static int handle;
static PointF stopPoint;
static bool check1 = false;
@@ -58,12 +55,12 @@
static double DistanceOfHead2Stopline(prime_t &prime);
static double DistanceOfTire2Edge(prime_t &prime);
static bool ExitTestArea(prime_t &prime);
+static void MotionChange(move_status_t mv, move_status_t prev, double distance);
-void StartSAS(prime_t &prime)
+void StartUphill(prime_t &prime)
{
DEBUG("杩涘叆鍧¤捣椤圭洰");
- testing = true;
stopConfirm = false;
restartComplete = false;
occurCrashRedLine = false;
@@ -71,8 +68,24 @@
slideNormalDistance = false;
reportSlideFault = false;
handBreakActive = false;
+ handle = RegisterCarMoveObserver(MotionChange);
+ MA_EnterMap(prime.examing_area.idx, MAP_TYPE_UPHILL, 1);
+}
- MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 1);
+void StopUphill(prime_t &prime)
+{
+ if (prime.examing_area.type != MAP_TYPE_UPHILL)
+ return;
+ DEBUG("閫�鍑哄潯璧烽」鐩�");
+
+ if (!check1) {
+ DEBUG("鏈繘琛屽仠杞﹁捣姝ュ姩浣�");
+ AddExamFault(10103);
+ }
+
+ UnregisterCarMoveObserver(handle);
+
+ prime.examing_area.type = MAP_TYPE_NONE;
}
static void StoppingTimeout(apptimer_var_t val) {
@@ -85,7 +98,7 @@
stopConfirm = true;
}
-void MotionChange(move_status_t mv)
+static void MotionChange(move_status_t mv, move_status_t prev, double distance)
{
AppTimer_delete(StopConfirm);
@@ -96,8 +109,10 @@
}
}
-int TestSAS(prime_t &prime)
+void TestUphill(prime_t &prime)
{
+ if (prime.examing_area.type != MAP_TYPE_UPHILL)
+ return;
static double distanceToStopLine = 0, distanceToEdge = 0;
if (CrashRedLine(prime)) {
@@ -119,7 +134,7 @@
stopPoint = prime.pModeling->base_point;
// 寮�濮嬪仠杞﹁鏃�
AppTimer_delete(StoppingTimeout);
- AppTimer_add(StoppingTimeout, examParam.ramp_start_car_limit_time);
+ AppTimer_add(StoppingTimeout, prime.examParam.ramp_start_car_limit_time);
// 妫�鏌ヨ溅澶村拰鍋滆溅甯︾殑璺濈
distanceToStopLine = DistanceOfHead2Stopline(prime);
@@ -127,7 +142,7 @@
DEBUG("DIS1 = %f DIS2 = %f", distanceToStopLine, distanceToEdge);
- if (distanceToStopLine > examParam.ramp_stoppoint_red_distance) {
+ if (distanceToStopLine > prime.examParam.ramp_stoppoint_red_distance) {
// 璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背
AddExamFault(20301);
DEBUG("璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背锛屼笉鍚堟牸");
@@ -137,11 +152,11 @@
DEBUG("鍓嶄繚闄╂病鏈変綅浜庡仠姝㈠甫鍐咃紝浣嗘病鏈夎秴鍑�50鍘樼背");
}
- if (distanceToEdge > examParam.ramp_edge_red_distance) {
+ if (distanceToEdge > prime.examParam.ramp_edge_red_distance) {
// 璺濈杈圭嚎瓒呭嚭50鍘樼背,涓嶅悎鏍�
AddExamFault(20302);
DEBUG("璺濈杈圭嚎瓒呭嚭50鍘樼背");
- } else if (distanceToEdge > examParam.ramp_edge_yellow_distance) {
+ } else if (distanceToEdge > prime.examParam.ramp_edge_yellow_distance) {
// 璺濈杈圭嚎瓒呭嚭30鍘樼背锛屾墸10鍒�
AddExamFault(20305);
DEBUG("璺濈杈圭嚎瓒呭嚭30鍘樼背");
@@ -149,7 +164,7 @@
}
// 鍋滆溅鍚庯紝妫�鏌ユ墜鍒规媺璧锋儏鍐�
- if (!handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
+ if (!handBreakActive && prime.sensor.hand_brake == ACTIVE) {
handBreakActive = true;
}
}
@@ -158,7 +173,7 @@
// 杞﹁締浠庡仠姝㈢姸鎬佸啀娆＄Щ鍔�
double juli = DistanceOf(prime.pModeling->base_point, stopPoint);
- if (juli > examParam.ramp_slide_yellow_distance) {
+ if (juli > prime.examParam.ramp_slide_yellow_distance) {
double deg = YawOf(stopPoint, prime.pModeling->base_point);
deg = fabs(prime.pModeling->yaw - deg);
if (deg > 180) {
@@ -185,7 +200,7 @@
} else {
// 杞﹁締鍚庢粦
slideNormalDistance = true;
- if (juli > examParam.ramp_slide_red_distance && !reportSlideFault) {
+ if (juli > prime.examParam.ramp_slide_red_distance && !reportSlideFault) {
// 鍚庢粦瓒呰繃30鍘樼背, 涓嶅悎鏍�
DEBUG("鍚庢粦瓒呰繃30鍘樼背");
reportSlideFault = true;
@@ -201,10 +216,8 @@
// 涓嶅仠杞︾洿鎺ョ寮�
AddExamFault(10103);
}
- MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 0);
+ MA_EnterMap(prime.examing_area.idx, MAP_TYPE_UPHILL, 0);
}
-
- return testing ? 1 : 0;
}
// 杞﹁疆鏄惁鍘嬭竟绾�
@@ -225,8 +238,8 @@
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]]);
+ MAKE_LINE(red_line, std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]],
+ std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[red_lines[i][1]]);
if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
IntersectionOf(red_line, rearAxle) == GM_Intersection) {
@@ -244,10 +257,10 @@
Line upper_edge, lower_edge;
- 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]);
+ MAKE_LINE(upper_edge, std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[5],
+ std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[6]);
+ MAKE_LINE(lower_edge, std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[4],
+ std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[3]);
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);
@@ -264,8 +277,8 @@
{
Line edge;
- 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]);
+ MAKE_LINE(edge, std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[0],
+ std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[8]);
double l1 = DistanceOf(prime.pModeling->points[prime.pModel->right_front_tire[TIRE_OUTSIDE]], edge);
--
Gitblit v1.8.0