From 682b17ff66dff23e03c6a57de276ea0c3e670c0e Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期一, 23 三月 2020 18:44:47 +0800
Subject: [PATCH] 起步和直线
---
lib/src/main/cpp/test_items2/road_exam.cpp | 127 ++++++++++++++++++++++++++++++++++++------
1 files changed, 109 insertions(+), 18 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index ae82561..b158dd5 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -12,6 +12,7 @@
#include "../native-lib.h"
#include "through_something.h"
#include "../master/comm_if.h"
+#include "drive_straight.h"
#include <vector>
#include <list>
@@ -23,6 +24,12 @@
using namespace std;
#define TURN_CHECK_CNT 4
+
+enum {
+ START_CAR_NOT_DO,
+ START_CAR_DOING,
+ START_CAR_DONE
+};
static const int TURN_THRESHOLD = 5;
static const int TURN_CHECK_INTERVAL = 500;
@@ -40,7 +47,7 @@
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool reportStopCarOnRedArea;
-static PointF stopPoint;
+static PointF stopPoint, startPoint;
static bool prevGearError = false;
static bool prevGearNSlide = false;
@@ -61,8 +68,12 @@
static int gearErrorTime;
static int gearNSlideTime;
+static int startCar;
static int currExamMapIndex;
+static const int MAX_ENGINE_RPM = 2500;
+static const double START_CAR_MOVE_DISTANCE = 0.5;//10.0;
+static const double START_CAR_CHECK_DOOR_DISTANCE = 0.1;//1.0;
static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(5);
static const uint32_t GEAR_ERROR_TIMEOUT = D_SEC(15);
static const uint32_t STOP_CAR_TIME = D_SEC(2);
@@ -73,8 +84,9 @@
static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0;
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};
+static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
-static char isTurn(int currYaw, int prevYaw);
+static char isTurn(int currYaw, int prevYaw, int &ang);
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);
static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car);
static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car);
@@ -107,10 +119,69 @@
gearNSlideTime = 0;
currExamMapIndex = -1;
+
+ startCar = START_CAR_NOT_DO;
+}
+
+static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
+{
+ double moveDistance;
+ static bool checkDoor = false;
+ static bool handBreakActive = false;
+ static bool reportRPMOver = false;
+
+ if (startCar == START_CAR_NOT_DO) {
+ startPoint = car->basePoint;
+ reportRPMOver = false;
+ startCar = START_CAR_DOING;
+
+ PlayTTS("璇疯捣姝�");
+ } else if (startCar == START_CAR_DOING) {
+ moveDistance = DistanceOf(startPoint, car->basePoint);
+ if (moveDistance > START_CAR_MOVE_DISTANCE) {
+
+ if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
+ DEBUG("Handbreak active move over 10m");
+ // 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡10绫充互涓婏紝涓嶅悎鏍�
+ AddExamFault(25, rtkTime);
+ } else if (handBreakActive) {
+ // 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡1绫充互涓婏紝鎵�10鍒�
+ DEBUG("Handbreak active move over 1M");
+ AddExamFault(26, rtkTime);
+ }
+ startCar = START_CAR_DONE;
+ } else if (moveDistance >= START_CAR_CHECK_DOOR_DISTANCE) {
+ if (!checkDoor) {
+ checkDoor = true;
+
+ if (ReadCarStatus(DOOR) == DOOR_OPEN) {
+ // 杞﹂棬鏈畬鍏ㄥ叧闂紝涓嶅悎鏍�
+ DEBUG("杞﹂棬鏈叧闂�");
+ AddExamFault(23, rtkTime);
+ }
+
+ if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
+ handBreakActive = true;
+ }
+ }
+ }
+
+ if (ReadCarStatus(ENGINE_RPM) > MAX_ENGINE_RPM && !reportRPMOver) {
+ // 杞�熻秴鏍囷紝涓嶅悎鏍�
+ DEBUG("杞�熻秴鏍�");
+ AddExamFault(29, rtkTime);
+ reportRPMOver = true;
+ }
+ } else {
+
+ }
}
void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
+ // 璧锋妫�娴�
+ TestRoadStartCar(car, speed, moveDirect, rtkTime);
+
// 瓒呴�熸娴�
if (moveDirect != 0 && speed > MAX_SPEED) {
if (!occurOverSpeed) {
@@ -266,7 +337,7 @@
// 妫�鏌ユ槸鍚︽寔缁浆鍚�
char turnDirect = CheckCarTurn(CarModelList);
if (turnDirect == 'L') {
- PlayTTS("宸�1", 5692);
+// PlayTTS("宸�1");
if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
if (!reportTurnSignalError) {
DEBUG("娌℃墦宸﹁浆鐏�");
@@ -284,7 +355,7 @@
}
}
} else if (turnDirect == 'R') {
- PlayTTS("鍙�1", 5692);
+// PlayTTS("鍙�1");
if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
if (!reportTurnSignalError) {
DEBUG("娌℃墦鍙宠浆鐏�");
@@ -386,7 +457,7 @@
// 妫�鏌ュ彉閬撳墠锛屾槸鍚︽彁鍓嶈浆鍚戠伅
if (inter == 1) {
- PlayTTS("宸�2", 5698);
+// PlayTTS("宸�2");
// 鍚戝乏渚у彉閬�
DEBUG("鍚戝乏渚у彉閬�");
if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
@@ -395,7 +466,7 @@
AddExamFault(13, rtkTime);
}
} else {
- PlayTTS("鍙�2", 5698);
+// PlayTTS("鍙�2");
// 鍚戝彸渚у彉閬�
DEBUG("鍚戝彸渚у彉閬�");
if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
@@ -410,20 +481,32 @@
checkCrashGreenTimeout = 0;
}
- // 瑙﹀彂绾挎娴�
- if (currExamMapIndex == -1) {
+ // 瀹屾垚璧锋鍚庯紝瑙﹀彂绾挎娴�
+ if (currExamMapIndex == -1 && startCar == START_CAR_DONE) {
currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList);
if (currExamMapIndex != -1) {
DEBUG("纰版挒瑙﹀彂绾�");
MA_EnterMap(RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type, 1);
- StartThroughExam(currExamMapIndex, RoadMapList);
+
+ if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP &&
+ RoadMapList[currExamMapIndex].type <= TURN_AROUND_MAP) {
+ StartThroughExam(currExamMapIndex, RoadMapList);
+ } else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
+ StartDriveStraightExam(currExamMapIndex, RoadMapList);
+ }
}
- } else {
+ } else if (startCar == START_CAR_DONE) {
int prevIdx = currExamMapIndex;
- currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car,
- CarModelList, speed, moveDirect, rtkTime);
+ if (currExamMapIndex >= THROUGH_INTERSECTION_MAP && currExamMapIndex <= TURN_AROUND_MAP) {
+ currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car,
+ CarModelList, speed, moveDirect, rtkTime);
+ }
+ else if (currExamMapIndex == DRIVE_STRAIGHT_MAP) {
+ currExamMapIndex = ExecuteDriveStraightExam(currExamMapIndex, RoadMapList, car,
+ CarModelList, speed, moveDirect, rtkTime);
+ }
if (currExamMapIndex == -1) {
MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 1);
@@ -521,10 +604,9 @@
tm.msec = rtkTime->mss;
}
-static char isTurn(int currYaw, int prevYaw)
+static char isTurn(int currYaw, int prevYaw, int &ang)
{
// DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw);
-
int deltaAng = 0;
if (ABS(currYaw - prevYaw) > 180) {
@@ -532,6 +614,8 @@
} else {
deltaAng = ABS(currYaw - prevYaw);
}
+
+ ang = deltaAng;
// DEBUG("瑙掑害宸�� %d", deltaAng);
@@ -545,12 +629,12 @@
}
}
- return 0;
+ return 'N';
}
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)
{
- // 鏈�杩�3绉掑唴锛屾瘡绉掔殑瑙掑害宸ぇ浜�10搴︼紝涓旀柟鍚戠浉鍚岋紝杩炵画3绉掞紝璁や负杞悜
+ // 鏈�杩�2绉掑唴锛屾瘡0.5绉掔殑瑙掑害宸ぇ浜�5搴︼紝涓旀柟鍚戠浉鍚岋紝杩炵画4娆★紱鎴栫獊鐜拌秴30搴︾殑杞悜锛涜涓鸿浆鍚戙��
if (CarModelList.size() < 1)
return false;
@@ -571,10 +655,15 @@
uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
if (tdiff >= TURN_CHECK_INTERVAL) {
- turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw);
+ int ang = 0;
+ turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw, ang);
// DEBUG("%c 瑙掑害姣旇緝 %02d:%02d:%02d.%03d %02d:%02d:%02d.%03d", turn[checkCnt], c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
- if (turn[checkCnt] == 0) {
+
+ if (turn[checkCnt] == 'N') {
break;
+ } else if (ang >= 30) {
+ DEBUG("宸﹀彸杞‘璁� %c", turn[checkCnt]);
+ return turn[checkCnt];
}
c1 = c2;
@@ -790,3 +879,5 @@
}
return -1;
}
+
+
--
Gitblit v1.8.0