From 7b6dd5603277ac8fddba735c456f99d4c07e2e9b Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期三, 12 二月 2020 17:43:36 +0800
Subject: [PATCH] 修改了上坡起步。
---
lib/src/main/cpp/test_items/turn_a90.cpp | 195 ++++++++++++++++++++++--------------------------
1 files changed, 91 insertions(+), 104 deletions(-)
diff --git a/lib/src/main/cpp/test_items/turn_a90.cpp b/lib/src/main/cpp/test_items/turn_a90.cpp
index 68d1a43..d03e269 100644
--- a/lib/src/main/cpp/test_items/turn_a90.cpp
+++ b/lib/src/main/cpp/test_items/turn_a90.cpp
@@ -7,6 +7,8 @@
#include "../driver_test.h"
#include "../common/apptimer.h"
#include "../jni_log.h"
+#include "../utils/xconvert.h"
+#include "../defs.h"
#include <vector>
#include <cstdlib>
@@ -15,122 +17,103 @@
using namespace std;
-enum {
- TURN_ANGLE_90,
- TURN_ANGLE_90_CMP
-};
-
const uint32_t STOP_CAR_TIME = D_SEC(2);
static bool TA90Testing;
-static bool carStopEvent;
-static bool checked;
-static int currTarget;
+
static int azimuth;
-static uint32_t stopTimepoint = 0;
+static bool turnLeftFinished;
+static uint64_t stopTimepoint = 0;
-static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car);
-static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car);
-static bool Turned(const Polygon *map, const car_model_cache_t *car);
+static bool reportStopCarTimeout;
+static int prevMoveDirect;
+static bool crashRedLine;
-void StartTurnA90(double heading)
+static bool CrashRedLine(const Polygon *map, const car_model *car);
+static bool ExitTestArea(const Polygon *map, const car_model *car);
+
+void StartTurnA90(int moveDirect, double heading, const struct RtkTime *rtkTime)
{
azimuth = (int) heading;
- checked = false;
- TA90Testing = true;
- carStopEvent = false;
-
- currTarget = TURN_ANGLE_90;
+ 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;
+ }
+ reportStopCarTimeout = false;
+ crashRedLine = false;
+ turnLeftFinished = false;
}
-void StopTurnA90(void)
-{
- TA90Testing = false;
-}
-
-int TestTurnA90(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status, double heading)
+int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
int status = 0;
- if (!TA90Testing)
- return -2;
-
- DEBUG("TestTurnA90 %d", run_status);
-
- if (CrashRedLine(map, car)) {
- // 鍘嬬嚎浜�
- err.push_back(29);
- status = -1;
- DEBUG("閿欒 鍘嬬嚎");
- }
-
- if (run_status != 0) {
- if (carStopEvent)
- DEBUG("TURN_ANGLE_90 鍋滆溅鏃堕棿 %ld", AppTimer_GetTickCount() - stopTimepoint);
-
- if (carStopEvent && AppTimer_GetTickCount() - stopTimepoint > STOP_CAR_TIME) {
- // 涓�斿仠杞�
- err.push_back(31);
- DEBUG("閿欒 鍋滆溅1");
- }
- carStopEvent = false;
- } else if (!carStopEvent){
- carStopEvent = true;
- stopTimepoint = AppTimer_GetTickCount();
- }
-
if (ExitTestArea(map, car)) {
- // 娴嬭瘯缁撴潫
status = 1;
}
- if (currTarget == TURN_ANGLE_90) {
- // 杞悜鐏紑鍚�
- int az = (int) heading;
-
- if (abs(az - azimuth) > 180) {
- az = 360 - abs(az-azimuth);
- } else {
- az = abs(az - azimuth);
+ if (CrashRedLine(map, car)) {
+ if (!crashRedLine) {
+ crashRedLine = true;
+ // 纰惧帇閬撹矾杈圭紭,涓嶅悎鏍�
+ AddExamFault(29, rtkTime);
+ DEBUG("纰惧帇閬撹矾杈圭紭");
}
-
- if (az >= 10 && !checked) {
- checked = true;
- // 杞悜鐏湭寮�鍚�
- err.push_back(30);
- DEBUG("閿欒 鐏病鐪�");
- }
-
- if (Turned(map, car)) {
- currTarget = TURN_ANGLE_90_CMP;
- checked = false;
- }
- DEBUG("TURN_ANGLE_90 %d %d",run_status, az);
} else {
- // 鍏抽棴杞悜鐏�
- Line line;
-
- MakeLine(&line, &map->point[1], &map->point[2]);
-
- // 澶т簬2.5绫冲悗妫�鏌ヨ溅鐏�
- if (!checked && DistanceOf(car->points[0], line) >= 2.5) {
- checked = true;
- // 杞悜鐏湭鍏抽棴
- err.push_back(30);
- DEBUG("閿欒 鐏病绠�");
- }
- DEBUG("TURN_ANGLE_90_CMP");
+ crashRedLine = false;
}
- if (status != 0) {
- StopTurnA90();
+ if (moveDirect != prevMoveDirect) {
+ if (moveDirect == 0) {
+ stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
+ stopTimepoint = stopTimepoint * 1000 + 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) {
+ 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 && !reportStopCarTimeout) {
+ // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
+ AddExamFault(31, rtkTime);
+ DEBUG("涓�斿仠杞�");
+ reportStopCarTimeout = true;
+ }
+ }
+
+ // 妫�鏌ヨ浆鍚戠姸鎬�
+ int az = (int) heading;
+
+ if (ABS(az - azimuth) > 180) {
+ az = 360 - ABS(az-azimuth);
+ } else {
+ az = ABS(az - azimuth);
+ }
+
+ if (az >= 30) {
+ if (!turnLeftFinished) {
+ // 杞悜鐏湭寮�鍚紝鎵�10鍒�
+ AddExamFault(30, rtkTime);
+ DEBUG("杞悜鐏湭寮�鍚�");
+ }
+ turnLeftFinished = true;
+ }
+
+ if (turnLeftFinished) {
+
}
return status;
}
// 杞﹁疆鏄惁鍘嬭竟绾�
-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;
@@ -140,8 +123,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]]);
@@ -156,20 +139,24 @@
}
// 鏁翠釜杞﹁締閮借椹剁璇ユ祴璇曞尯鍩�
-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[3], map->point[4], car->points[i]) != 1)
- return false;
- }
- return true;
-}
+ bool ret = false;
-static bool Turned(const Polygon *map, const car_model_cache_t *car)
-{
- for (int i = 0; i < car->point_num; ++i) {
- if (IntersectionOfLine(map->point[1], map->point[2], car->points[i]) != 1)
- return false;
+ // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
+ 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]];
}
- return true;
+
+ if (IntersectionOf(&carBody, map) == GM_None) {
+ ret = true;
+ }
+
+ free(carBody.point);
+
+ return ret;
}
--
Gitblit v1.8.0