From 3ce7d9cbccabf7f94d8203a98796599cd9dd5411 Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期二, 11 二月 2020 17:14:27 +0800
Subject: [PATCH] 修改了上坡起步。
---
lib/src/main/cpp/test_items/stop_and_start.cpp | 229 ++++++++++++++++++++++++++++++++------------------------
1 files changed, 130 insertions(+), 99 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 6b8fa0e..266a8e3 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/stop_and_start.cpp
@@ -10,6 +10,9 @@
#include "../driver_test.h"
#include "../jni_log.h"
#include "../common/apptimer.h"
+#include "../utils/xconvert.h"
+
+#define DEBUG(fmt, args...) LOGD("<stop_and_start> <%s>: " fmt, __func__, ##args)
using namespace std;
@@ -24,138 +27,166 @@
const double EDGE_DISTANCE_THRESHOLD_YELLOW = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
-const int CAR_START_TIMEOUT = 30;
+const uint32_t CAR_START_TIMEOUT = D_SEC(30);
+const uint32_t STOP_CAR_TIME = D_SEC(2);
+
const double EPSILON = 1e-3;
static bool SASTesting = false;
static double slideDistance;
-static bool startCarTimeout;
-static int currTarget;
static PointF stopPoint;
-static int startCarConfirm; // 璧锋鏃讹紝鎸佺画鍓嶈繘涓�灏忔鎵嶇畻
-static void StartCarTimeout(union sigval sig);
-static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car);
-static double DistanceOfHead2Stopline(const Polygon *map, const car_model_cache_t *car);
-static double DistanceOfTire2Edge(const Polygon *map, const car_model_cache_t *car);
-static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car);
+static int prevMoveDirect;
+static uint64_t stopTimepoint = 0;
+static bool stopCar = false;
+static uint32_t stopCarTime;
+static bool occurCrashRedLine = false;
+static bool slideLongDistance = false;
+static bool slideNormalDistance = false;
+static bool reportSlideFault = false;
+static bool reportStartTimeout = false;
-void StartSAS(void)
+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 moveDirect, const struct RtkTime *rtkTime)
{
+ DEBUG("杩涘叆鍧¤捣椤圭洰");
SASTesting = true;
slideDistance = 0.0;
- startCarTimeout = false;
- currTarget = STOP_CAR;
+
+ prevMoveDirect = moveDirect;
+
+ if (moveDirect == 0) {
+ stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh,
+ rtkTime->mm, rtkTime->ss);
+ stopTimepoint = stopTimepoint * 1000 + rtkTime->mss * 10;
+ }
+
+ occurCrashRedLine = false;
+ stopCar = false;
+ slideLongDistance = false;
+ slideNormalDistance = false;
+ reportSlideFault = false;
+ reportStartTimeout = false;
}
-void StopSAS(void)
-{
- SASTesting = false;
- AppTimer_delete(StartCarTimeout);
-}
-
-int TestSAS(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status)
+int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
int status = 0;
- if (!SASTesting)
- return -2;
-
- if (currTarget >= STOP_CAR) {
- if (CrashRedLine(map, car)) {
- // 杞﹁疆鍘嬬嚎
- err.push_back(13);
- status = -1;
+ if (CrashRedLine(map, car)) {
+ // 杞﹁疆鍘嬬嚎锛屼笉鍚堟牸
+ if (!occurCrashRedLine) {
+ AddExamFault(13, rtkTime);
+ DEBUG("杞﹁疆鍘嬬嚎");
}
+ occurCrashRedLine = true;
+ } else {
+ occurCrashRedLine = false;
}
- if (currTarget == STOP_CAR) {
- if (run_status == 0) {
+ if (ExitTestArea(map, car)) {
+ // 椹剁娴嬭瘯鍖�
+ status = 1;
+ }
+
+ if (prevMoveDirect != moveDirect) {
+ if (moveDirect == 0) {
+ stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
+ stopTimepoint = stopTimepoint * 1000 + rtkTime->mss*10;
+ }
+ prevMoveDirect = moveDirect;
+ } else if (moveDirect == 0) {
+ uint64_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
+ tp = tp * 1000 + rtkTime->mss * 10;
+
+ if (tp - stopTimepoint >= STOP_CAR_TIME && !stopCar) {
+ // 杩欓噷鍒ゆ柇鍋滆溅鐘舵��
+ stopCar = true;
+ stopCarTime = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
+
+ stopPoint = car->carXY[car->body[0]];
+
double dis1 = DistanceOfHead2Stopline(map, car);
double dis2 = DistanceOfTire2Edge(map, car);
if (dis1 > STOP_DISTANCE_THRESHOLD_RED) {
// 璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背
- err.push_back(12);
+ AddExamFault(12, rtkTime);
+ DEBUG("璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背锛屼笉鍚堟牸");
status = -1;
} else if (fabs(dis1) > EPSILON) {
- // 鍓嶄繚闄╂病鏈変綅浜庡仠姝㈠甫鍐咃紝浣嗘病鏈夎秴鍑�50鍘樼背
- err.push_back(17);
+ // 鍓嶄繚闄╂病鏈変綅浜庡仠姝㈠甫鍐咃紝浣嗘病鏈夎秴鍑�50鍘樼背锛屾墸10鍒�
+ AddExamFault(17, rtkTime);
+ DEBUG("鍓嶄繚闄╂病鏈変綅浜庡仠姝㈠甫鍐咃紝浣嗘病鏈夎秴鍑�50鍘樼背");
}
if (dis2 > EDGE_DISTANCE_THRESHOLD_RED) {
- // 璺濈杈圭嚎瓒呭嚭50鍘樼背
- err.push_back(14);
+ // 璺濈杈圭嚎瓒呭嚭50鍘樼背,涓嶅悎鏍�
+ AddExamFault(14, rtkTime);
+ DEBUG("璺濈杈圭嚎瓒呭嚭50鍘樼背");
status = -1;
} else if (dis2 > EDGE_DISTANCE_THRESHOLD_YELLOW) {
- // 璺濈杈圭嚎瓒呭嚭30鍘樼背
- err.push_back(18);
+ // 璺濈杈圭嚎瓒呭嚭30鍘樼背锛屼笉鍚堟牸
+ AddExamFault(18, rtkTime);
+ DEBUG("璺濈杈圭嚎瓒呭嚭30鍘樼背");
}
// 妫�鏌ユ槸鍚︽媺浣忔墜鍒�
-
- AppTimer_delete(StartCarTimeout);
- AppTimer_add(StartCarTimeout, D_SEC(CAR_START_TIMEOUT));
- slideDistance = 0.0;
- stopPoint = car->points[0];
- startCarConfirm = 0;
- currTarget = START_CAR;
- } else if (run_status > 0) {
- if (ExitTestArea(map, car)) {
- // 杞﹁締鐩存帴椹剁娴嬭瘯鍖猴紝鐩存帴娣樻卑
- err.push_back(12);
- status = -1;
+ if (true) {
+ AddExamFault(19, rtkTime);
+ DEBUG("娌℃媺鎵嬪埞");
}
}
- } else if (currTarget == START_CAR) {
- if (startCarTimeout) {
- startCarTimeout = false;
- //璧锋鏃堕棿瓒呰繃30绉�
- err.push_back(15);
- status = -1;
- }
+ }
- if (run_status > 0) {
- startCarConfirm++;
- if (startCarConfirm == 2) {
- AppTimer_delete(StartCarTimeout); // 璧锋瀹屾垚
+ // 鍒ゆ柇璧锋鍚庢粦鐘舵��
+ if (stopCar) {
+ if (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) == 1) {
+ // 鍙戠敓鍚庢粦
+ slideDistance = DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]);
+
+ if (slideLongDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
+ slideNormalDistance = true;
}
- if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
+ if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance && !reportSlideFault) {
+ // 鍚庢粦瓒呰繃30鍘樼背, 涓嶅悎鏍�
+ AddExamFault(16, 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(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss) - stopCarTime > CAR_START_TIMEOUT) {
+ // 璧锋鏃堕棿瓒呰繃30绉掞紝涓嶅悎鏍�
+ AddExamFault(15, 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鍘樼背
- err.push_back(20);
- }
- } else if (run_status < 0) {
- // 鍚庢粦浜�
- slideDistance = DistanceOf(stopPoint, car->points[0]);
- if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED) {
- // 鍚庢粦瓒呰繃30鍘樼背
- err.push_back(16);
- status = -1;
+ AddExamFault(20, rtkTime);
+ DEBUG("鍚庢粦瓒呰繃10鍘樼背锛屼絾娌¤秴杩�30鍘樼背");
}
}
-
- if (ExitTestArea(map, car)) {
- // 娴嬭瘯瀹屾垚浜�
- status = 1;
- }
}
-
- if (status != 0) {
- StopSAS();
- }
-
- return status;
-}
-
-static void StartCarTimeout(union sigval sig) {
- AppTimer_delete(StartCarTimeout);
- startCarTimeout = true;
}
// 杞﹁疆鏄惁鍘嬭竟绾�
-static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car)
+static bool CrashRedLine(const Polygon *map, const car_model *car)
{
bool ret = false;
@@ -164,8 +195,8 @@
Line frontAxle, rearAxle;
- MakeLine(&frontAxle, &car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->front_right_tire[TIRE_OUTSIDE]]);
- MakeLine(&rearAxle, &car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]]);
+ 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]]);
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]]);
@@ -179,48 +210,48 @@
return ret;
}
-static double DistanceOfHead2Stopline(const Polygon *map, const car_model_cache_t *car)
+static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car)
{
double dis = 0.0;
- int rel1 = IntersectionOfLine(map->point[4], map->point[3], car->points[0]);
- int rel2 = IntersectionOfLine(map->point[5], map->point[6], car->points[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]]);
if (rel1 == 1) {
Line line1;
MakeLine(&line1, &map->point[4], &map->point[3]);
- dis = DistanceOf(car->points[0], line1);
+ 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->points[0], line2);
+ dis = DistanceOf(car->carXY[car->body[0]], line2);
}
return dis;
}
-static double DistanceOfTire2Edge(const Polygon *map, const car_model_cache_t *car)
+static double DistanceOfTire2Edge(const Polygon *map, const car_model *car)
{
Line edge;
MakeLine(&edge, &map->point[0], &map->point[8]);
- double l1 = DistanceOf(car->points[car->desc->front_right_tire[TIRE_OUTSIDE]], edge);
+ double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], edge);
- double l2 = DistanceOf(car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]], edge);
+ double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], edge);
return (l1+l2)/2.0;
}
// 鏁翠釜杞﹁締閮借椹剁璇ユ祴璇曞尯鍩�
-static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car)
+static bool ExitTestArea(const Polygon *map, const car_model *car)
{
- for (int i = 0; i < car->point_num; ++i) {
- if (IntersectionOfLine(map->point[8], map->point[7], car->points[i]) != -1)
+ for (int i = 0; i < car->bodyNum; ++i) {
+ if (IntersectionOfLine(map->point[8], map->point[7], car->carXY[car->body[i]]) != -1)
return false;
}
return true;
--
Gitblit v1.8.0