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 +++++++++++++++-----------------
app/src/main/java/safeluck/drive/evaluation/MainActivity.java | 2
lib/src/main/cpp/driver_test.cpp | 61 +++++++++
lib/src/main/cpp/test_items/park_edge.cpp | 36 ++++-
lib/src/main/cpp/test_items/stop_and_start.cpp | 5
lib/src/main/cpp/test_items/turn_a90.h | 5
.idea/misc.xml | 2
7 files changed, 183 insertions(+), 123 deletions(-)
diff --git a/.idea/misc.xml b/.idea/misc.xml
index dfd2c79..37a7509 100644
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
- <component name="ProjectRootManager" version="2" languageLevel="JDK_1_7" project-jdk-name="JDK" project-jdk-type="JavaSDK">
+ <component name="ProjectRootManager" version="2" languageLevel="JDK_1_7" project-jdk-name="1.8" project-jdk-type="JavaSDK">
<output url="file://$PROJECT_DIR$/build/classes" />
</component>
<component name="ProjectType">
diff --git a/app/src/main/java/safeluck/drive/evaluation/MainActivity.java b/app/src/main/java/safeluck/drive/evaluation/MainActivity.java
index e2cb43d..afb7b18 100644
--- a/app/src/main/java/safeluck/drive/evaluation/MainActivity.java
+++ b/app/src/main/java/safeluck/drive/evaluation/MainActivity.java
@@ -98,7 +98,7 @@
String strs = new String(datas, Charset.forName("ISO-8859-1"));
MyLog.i(TAG, "mcu upgrade datas.legnth=="+strs.getBytes("ISO-8859-1").length);
AYSdk.getInstance().sendCmd(Constant.UPGRADE_MCU_CONTENT_FILE, strs);
- FileUtil.deleteFile("",Constant.MCU_FILE_NAME);
+// FileUtil.deleteFile("",Constant.MCU_FILE_NAME);
}else{
MyLog.i(TAG,"mcu鍗囩骇鏂囦欢涓嶅瓨鍦�");
}
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 3608fbf..64c63c9 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -148,8 +148,6 @@
MapList[MapNum].id = id;
- if (type == 1) type = 3; //////////////////////////////////////
-
MapList[MapNum].type = type;
MapList[MapNum].map.num = pointNum;
@@ -347,7 +345,7 @@
RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
brief.qf = RtkBuffer[index].qf;
- brief.map_id = 863;//GetMapId(CurrExamMapIndex, MapList, MapNum);
+ brief.map_id = 865;//GetMapId(CurrExamMapIndex, MapList, MapNum);
brief.move = move;
brief.speed = speed * 3.6;
brief.heading = RtkBuffer[index].heading;
@@ -384,6 +382,7 @@
free(brief.point);
struct RtkTime rtkTime;
+ double azimuth = RtkBuffer[index].heading;
rtkTime.YY = RtkBuffer[index].YY;
rtkTime.MM = RtkBuffer[index].MM;
@@ -409,6 +408,10 @@
CurrExamStatus = 0;
break;
case MAP_TYPE_STOP_START:
+ DEBUG("杩涘叆涓婂潯璧锋鍦哄湴");
+ MA_SendDebugInfo("杩涘叆涓婂潯璧锋鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ StartSAS(move, &rtkTime);
+ CurrExamStatus = 0;
break;
case MAP_TYPE_PART_EDGE:
DEBUG("杩涘叆渚ф柟浣嶅仠杞﹀満鍦�");
@@ -420,6 +423,10 @@
case MAP_TYPE_CURVE:
break;
case MAP_TYPE_TURN_90:
+ DEBUG("杩涘叆鐩磋杞集鍦哄湴");
+ MA_SendDebugInfo("杩涘叆鐩磋杞集鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ StartTurnA90(move, azimuth, &rtkTime);
+ CurrExamStatus = 0;
break;
default:break;
}
@@ -435,6 +442,7 @@
CarModel, CarModelPrev, speed, move, &rtkTime);
break;
case MAP_TYPE_STOP_START:
+ CurrExamStatus = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, &rtkTime);
break;
case MAP_TYPE_PART_EDGE:
CurrExamStatus = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, &rtkTime);
@@ -442,6 +450,7 @@
case MAP_TYPE_CURVE:
break;
case MAP_TYPE_TURN_90:
+ CurrExamStatus = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, &rtkTime);
break;
default:
break;
@@ -678,8 +687,41 @@
// 杞︾殑鏈�鍓嶇偣鏄惁杩涘叆鍦板浘
for (int i = 0; i < mapNum && car != NULL; ++i) {
if (mapList[i].type == MAP_TYPE_STOP_START) {
+ // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
+ double x9, y9, xo, yo;
- } else if (mapList[i].type == MAP_TYPE_CURVE) {
+ bool enter = false;
+
+ xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
+ yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;
+
+ x9 = 2*xo - mapList[i].map.point[8].X;
+ y9 = 2*yo - mapList[i].map.point[8].Y;
+
+ Polygon map;
+
+ map.num = 4;
+ map.point = (PointF *) malloc(map.num * sizeof(PointF));
+
+ map.point[0] = mapList[i].map.point[0];
+ map.point[1] = mapList[i].map.point[8];
+ map.point[2] = mapList[i].map.point[7];
+ map.point[3].X = x9;
+ map.point[3].Y = y9;
+
+ if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) {
+ Line enterLine1;
+
+ MakeLine(&enterLine1, &(map.point[0]), &(map.point[3]));
+
+ if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
+ enter = true;
+ }
+
+ free(map.point);
+
+ if (enter) return i;
+ } /*else if (mapList[i].type == MAP_TYPE_CURVE) {
} else if (mapList[i].type == MAP_TYPE_PARK_BUTTOM || mapList[i].type == MAP_TYPE_PART_EDGE) {
if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
@@ -692,7 +734,16 @@
DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
return i;
}
- }
+ } else if (mapList[i].type == MAP_TYPE_TURN_90) {
+ if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
+ Line enterLine1;
+
+ MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+
+ if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
+ return i;
+ }
+ }*/
}
return -1;
}
diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp
index 87993ff..105bd16 100644
--- a/lib/src/main/cpp/test_items/park_edge.cpp
+++ b/lib/src/main/cpp/test_items/park_edge.cpp
@@ -20,6 +20,7 @@
const int PARK_TIMEOUT = 90;
const uint32_t STOP_CAR_TIME = D_SEC(2);
+static bool reportExamTimeout;
static uint64_t stopTimepoint = 0;
static bool occurCrashRedLine1, occurCrashRedLine2;
static int prevMoveStatus, storeMoveStatusBeforeStop;
@@ -31,10 +32,12 @@
static bool CrashRedLine2(const Polygon *map, const car_model *car);
static bool EnterParking(const Polygon *map, const car_model *car);
static bool ExitParkArea(const Polygon *map, const car_model *car);
+static bool ExitParkArea2(const Polygon *map, const car_model *car);
void StartParkEdge(int moveStatus, const struct RtkTime *rtkTime)
{
occurCrashRedLine1 = occurCrashRedLine2 = false; // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑
+ reportExamTimeout = false;
prevMoveStatus = moveStatus;
parkSuccess = false;
@@ -74,25 +77,23 @@
occurCrashRedLine2 = false;
}
- if (ExitParkArea(map, car)) {
+ if (ExitParkArea(map, car) || ExitParkArea2(map, car)) {
if (!parkSuccess || !occurMoveBack) {
// 鐩存帴椹剁娴嬭瘯鍖猴紝璁や负绉诲簱涓嶅叆
AddExamFault(21, rtkTime);
DEBUG("鐩存帴椹剁娴嬭瘯鍖�,绉诲簱涓嶅叆");
- status = -1;
- } else {
- status = 1;
}
+ status = 1;
}
if (occurMoveBack) {
uint32_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
- if (tp - moveBackTimePoint >= PARK_TIMEOUT) {
+ if (!reportExamTimeout && tp - moveBackTimePoint >= PARK_TIMEOUT) {
// 瓒呮椂90绉掞紝涓嶅悎鏍�
AddExamFault(22, rtkTime);
+ reportExamTimeout = true;
DEBUG("绉诲簱90绉掕秴鏃�");
- status = -1;
}
}
@@ -129,7 +130,6 @@
// 鍋滄鍚庯紝杞﹁韩鍑虹嚎锛屼笉鍚堟牸
AddExamFault(21, rtkTime);
DEBUG("绉诲簱涓嶅叆");
- status = -1;
}
// 鍦ㄨ繖閲屾鏌ヨ浆鍚戠伅鐘舵��
@@ -254,3 +254,25 @@
}
return true;
}
+
+static bool ExitParkArea2(const Polygon *map, const car_model *car)
+{
+// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
+ bool ret = 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]];
+ }
+
+ if (IntersectionOf(&carBody, map) == GM_None) {
+ ret = true;
+ }
+
+ free(carBody.point);
+
+ return ret;
+}
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 266a8e3..1398ee5 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/stop_and_start.cpp
@@ -118,7 +118,6 @@
// 璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背
AddExamFault(12, rtkTime);
DEBUG("璺濈鍋滄绾垮墠鍚庤秴鍑�50鍘樼背锛屼笉鍚堟牸");
- status = -1;
} else if (fabs(dis1) > EPSILON) {
// 鍓嶄繚闄╂病鏈変綅浜庡仠姝㈠甫鍐咃紝浣嗘病鏈夎秴鍑�50鍘樼背锛屾墸10鍒�
AddExamFault(17, rtkTime);
@@ -129,7 +128,6 @@
// 璺濈杈圭嚎瓒呭嚭50鍘樼背,涓嶅悎鏍�
AddExamFault(14, rtkTime);
DEBUG("璺濈杈圭嚎瓒呭嚭50鍘樼背");
- status = -1;
} else if (dis2 > EDGE_DISTANCE_THRESHOLD_YELLOW) {
// 璺濈杈圭嚎瓒呭嚭30鍘樼背锛屼笉鍚堟牸
AddExamFault(18, rtkTime);
@@ -183,6 +181,8 @@
}
}
}
+
+ return status;
}
// 杞﹁疆鏄惁鍘嬭竟绾�
@@ -250,6 +250,7 @@
// 鏁翠釜杞﹁締閮借椹剁璇ユ祴璇曞尯鍩�
static bool ExitTestArea(const Polygon *map, const car_model *car)
{
+ // 鍦�8->7绾跨殑鍙充晶
for (int i = 0; i < car->bodyNum; ++i) {
if (IntersectionOfLine(map->point[8], map->point[7], car->carXY[car->body[i]]) != -1)
return false;
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;
}
diff --git a/lib/src/main/cpp/test_items/turn_a90.h b/lib/src/main/cpp/test_items/turn_a90.h
index 66ccb66..f6e88b2 100644
--- a/lib/src/main/cpp/test_items/turn_a90.h
+++ b/lib/src/main/cpp/test_items/turn_a90.h
@@ -10,8 +10,7 @@
using namespace std;
-void StartTurnA90(double heading);
-void StopTurnA90(void);
-int TestTurnA90(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status, double heading);
+void StartTurnA90(int moveDirect, double heading, const struct RtkTime *rtkTime);
+int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime);
#endif //RTKDRIVERTEST_TURN_A90_H
--
Gitblit v1.8.0