From 53be5cac481ddc7927ca3f8fe75a5180f0d6567a Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期五, 27 三月 2020 00:16:33 +0800
Subject: [PATCH] 角度修正。
---
lib/src/main/cpp/test_items2/drive_straight.cpp | 29 +++++----
lib/src/main/cpp/test_items2/through_something.cpp | 2
lib/src/main/cpp/test_items2/dummy_light.cpp | 2
lib/src/main/cpp/test_items2/road_exam.cpp | 87 ++++++++++++++++++++++++++---
lib/src/main/cpp/test_items2/stop_car.cpp | 18 +++--
lib/src/main/cpp/test_items2/operate_gear.cpp | 2
6 files changed, 109 insertions(+), 31 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp
index ee94653..16a8832 100644
--- a/lib/src/main/cpp/test_items2/drive_straight.cpp
+++ b/lib/src/main/cpp/test_items2/drive_straight.cpp
@@ -56,23 +56,28 @@
crossStartLine = true;
startPoint = car->basePoint;
edgeDistance = DistanceOf(car->basePoint, road_edge);
+
+ DEBUG("褰撳墠鍩哄噯璺竟闂磋窛 %f", edgeDistance);
}
} else {
double distanceToStart = DistanceOf(car->basePoint, startPoint);
- if (distanceToStart > CHECK_STAGE_DISTANCE) {
- if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > MAX_OFFSET_DISTANCE) {
- DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背");
- // 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸
- AddExamFault(30, rtkTime);
- reportOffsetOver = true;
+ DEBUG("璺竟闂磋窛 %f --- %f", DistanceOf(car->basePoint, road_edge), edgeDistance);
- //////////////////////////////////////////////
- startPoint = car->basePoint;
- edgeDistance = DistanceOf(car->basePoint, road_edge);
- reportOffsetOver = false;
- }
- } else {
+ if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > MAX_OFFSET_DISTANCE) {
+ DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背");
+ // 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸
+ AddExamFault(30, rtkTime);
+ reportOffsetOver = true;
+
+ //////////////////////////////////////////////
+ startPoint = car->basePoint;
+ edgeDistance = DistanceOf(car->basePoint, road_edge);
+ reportOffsetOver = false;
+ }
+
+ if (distanceToStart > CHECK_STAGE_DISTANCE) {
+ DEBUG("澶嶄綅杈硅窛鍋忕Щ閲�");
startPoint = car->basePoint;
edgeDistance = DistanceOf(car->basePoint, road_edge);
reportOffsetOver = false;
diff --git a/lib/src/main/cpp/test_items2/dummy_light.cpp b/lib/src/main/cpp/test_items2/dummy_light.cpp
index c048150..81a3a02 100644
--- a/lib/src/main/cpp/test_items2/dummy_light.cpp
+++ b/lib/src/main/cpp/test_items2/dummy_light.cpp
@@ -58,9 +58,9 @@
void DummyLightTTSDone(int id)
{
- DEBUG("DummyLightTTSDone %d", id);
// 绛夎闊虫挱鎶ュ畬姣曞悗璁℃椂
if (id == examTtsSeq) {
+ DEBUG("DummyLightTTSDone %d", id);
for (int i = 0; i < contentNum; ++i) {
if (content[i].itemStatus == TTS_DOING) {
content[i].itemStatus = TTS_DONE;
diff --git a/lib/src/main/cpp/test_items2/operate_gear.cpp b/lib/src/main/cpp/test_items2/operate_gear.cpp
index 98ea7bf..1785dfb 100644
--- a/lib/src/main/cpp/test_items2/operate_gear.cpp
+++ b/lib/src/main/cpp/test_items2/operate_gear.cpp
@@ -156,9 +156,9 @@
void OperateGearTTSDone(int id)
{
- DEBUG("鍗囬檷鎸℃寚浠ゅ彂鍑哄畬姣� %d", id);
// 绛夎闊虫挱鎶ュ畬姣曞悗璁℃椂
if (id == examTtsSeq) {
+ DEBUG("鍗囬檷鎸℃寚浠ゅ彂鍑哄畬姣� %d", id);
opGear = CHECK_OP;
AppTimer_add(CheckGear, D_SEC(7));
}
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index e48b855..1a86093 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -25,7 +25,7 @@
using namespace std;
-#define TURN_CHECK_CNT 4
+#define TURN_CHECK_CNT 6
enum {
START_CAR_NOT_DO,
@@ -33,7 +33,7 @@
START_CAR_DONE
};
-static const int TURN_THRESHOLD = 5;
+static const int TURN_THRESHOLD = 1;
static const int TURN_CHECK_INTERVAL = 500;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
@@ -57,10 +57,12 @@
static bool slideLongDistance;
static bool slideNormalDistance;
static bool occurSlide;
+static bool startCarLeftTurnSignal, checkStartCarSignal;
static struct drive_timer crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;
static struct drive_timer gearErrorTimePoint;
static struct drive_timer gearNSlideTimePoint;
+static struct drive_timer startCarLeftTurnSignalTime;
static int gearErrorTime;
static int gearNSlideTime;
@@ -71,6 +73,10 @@
static bool checkDoor = false;
static bool handBreakActive = false;
static bool reportRPMOver = false;
+
+static const uint32_t TURN_ERROR_COLD_TIME = D_SEC(10);
+
+static bool turnError13Cold, turnError14Cold;
static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -94,6 +100,10 @@
static bool CrashGreenLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, PointF &p1, PointF &p2);
static int CrashTriggerLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList);
static int FindMapIndexById(int id, LIST_ROAD_MAP &RoadMapList);
+
+static void TurnSignalError13ColdTimer(union sigval sig);
+static void TurnSignalError14ColdTimer(union sigval sig);
+static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime);
void InitRoadExam(void)
{
@@ -127,6 +137,10 @@
checkDoor = false;
handBreakActive = false;
reportRPMOver = false;
+
+ checkStartCarSignal = startCarLeftTurnSignal = false;
+
+ turnError13Cold = turnError14Cold = true;
}
void TerminateRoadExam(void)
@@ -134,6 +148,42 @@
TerminateDummyLightExam();
TerminateStopCarExam();
TerminateOperateGearExam();
+
+ AppTimer_delete(TurnSignalError13ColdTimer);
+ AppTimer_delete(TurnSignalError14ColdTimer);
+}
+
+static void TurnSignalError13ColdTimer(union sigval sig)
+{
+ AppTimer_delete(TurnSignalError13ColdTimer);
+
+ turnError13Cold = true;
+}
+
+static void TurnSignalError14ColdTimer(union sigval sig)
+{
+ AppTimer_delete(TurnSignalError14ColdTimer);
+
+ turnError14Cold = true;
+}
+
+static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime)
+{
+ if (err == 13 && turnError13Cold) {
+ DEBUG("璧锋涓嶅紑杞悜鐏�");
+ AddExamFault(13, rtkTime);
+
+ turnError13Cold = false;
+ AppTimer_delete(TurnSignalError13ColdTimer);
+ AppTimer_add(TurnSignalError13ColdTimer, TURN_ERROR_COLD_TIME);
+ } else if (err == 14 && turnError14Cold) {
+ DEBUG("璧锋杞悜鐏笉瓒�3绉�");
+ AddExamFault(14, rtkTime);
+
+ turnError14Cold = false;
+ AppTimer_delete(TurnSignalError14ColdTimer);
+ AppTimer_add(TurnSignalError14ColdTimer, TURN_ERROR_COLD_TIME);
+ }
}
static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
@@ -148,8 +198,24 @@
PlayTTS("璇疯捣姝�");
} else if (startCar == START_CAR_DOING) {
moveDistance = DistanceOf(startPoint, car->basePoint);
+ DEBUG("璧锋琛岄┒璺濈 %f", moveDistance);
+
+ if (!startCarLeftTurnSignal && ReadCarStatus(TURN_SIGNAL_LAMP) == LEFT_TURN_LIGHT) {
+ startCarLeftTurnSignal = true;
+ Rtk2DriveTimer(startCarLeftTurnSignalTime, rtkTime);
+ }
+
+ if (!checkStartCarSignal && moveDirect == 1) {
+ checkStartCarSignal = true;
+ if (!startCarLeftTurnSignal) {
+ ReportTurnSignalError(13, rtkTime);
+ } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
+ startCarLeftTurnSignalTime.hour, startCarLeftTurnSignalTime.min, startCarLeftTurnSignalTime.sec, startCarLeftTurnSignalTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
+ ReportTurnSignalError(14, rtkTime);
+ }
+ }
+
if (moveDistance > START_CAR_MOVE_DISTANCE) {
-
if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
DEBUG("Handbreak active move over 10m");
// 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡10绫充互涓婏紝涓嶅悎鏍�
@@ -159,6 +225,9 @@
DEBUG("Handbreak active move over 1M");
AddExamFault(26, rtkTime);
}
+
+ PlayTTS("瀹屾垚璧锋");
+ DEBUG("############# 瀹屾垚璧锋 ############");
startCar = START_CAR_DONE;
} else if (moveDistance >= START_CAR_CHECK_DOOR_DISTANCE) {
if (!checkDoor) {
@@ -363,16 +432,16 @@
if (!reportTurnSignalError) {
DEBUG("娌℃墦宸﹁浆鐏�");
// 娌℃墦宸﹁浆鐏紝涓嶅悎鏍�
- AddExamFault(13, rtkTime);
reportTurnSignalError = true;
+ ReportTurnSignalError(13, rtkTime);
}
} else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
if (!reportTurnSignalError) {
DEBUG("杞悜鐏椂闂翠笉瓒�");
// 涓嶈冻3绉掞紝涓嶅悎鏍�
- AddExamFault(14, rtkTime);
reportTurnSignalError = true;
+ ReportTurnSignalError(14, rtkTime);
}
}
} else if (turnDirect == 'R') {
@@ -381,16 +450,16 @@
if (!reportTurnSignalError) {
DEBUG("娌℃墦鍙宠浆鐏�");
// 娌℃墦鍙宠浆鐏紝涓嶅悎鏍�
- AddExamFault(13, rtkTime);
reportTurnSignalError = true;
+ ReportTurnSignalError(13, rtkTime);
}
} else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
if (!reportTurnSignalError) {
DEBUG("杞悜鐏椂闂翠笉瓒�");
// 涓嶈冻3绉掞紝涓嶅悎鏍�
- AddExamFault(14, rtkTime);
reportTurnSignalError = true;
+ ReportTurnSignalError(14, rtkTime);
}
}
} else {
@@ -484,7 +553,7 @@
if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
DEBUG("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
- AddExamFault(13, rtkTime);
+ ReportTurnSignalError(13, rtkTime);
}
} else {
// PlayTTS("鍙�2");
@@ -493,7 +562,7 @@
if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
DEBUG("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
- AddExamFault(13, rtkTime);
+ ReportTurnSignalError(14, rtkTime);
}
}
}
diff --git a/lib/src/main/cpp/test_items2/stop_car.cpp b/lib/src/main/cpp/test_items2/stop_car.cpp
index 6629623..4e93662 100644
--- a/lib/src/main/cpp/test_items2/stop_car.cpp
+++ b/lib/src/main/cpp/test_items2/stop_car.cpp
@@ -61,9 +61,9 @@
void StopCarTTSDone(int id)
{
- DEBUG("StopCarTTSDone %d", id);
// 绛夎闊虫挱鎶ュ畬姣曞悗璁℃椂
if (id == examTtsSeq) {
+ DEBUG("StopCarTTSDone %d", id);
ttsPlayEnd = 1;
}
}
@@ -88,14 +88,16 @@
rtkTime->mss * 10,
prevPointTime.hour, prevPointTime.min,
prevPointTime.sec, prevPointTime.msec * 10);
-
- moveDistance += (double)diff * (speed + prevSpeed) / 2;
- prevSpeed = speed;
- Rtk2DriveTimer(prevPointTime, rtkTime);
+ if (diff >= D_SEC(1)) {
+ moveDistance += (double)diff * (speed + prevSpeed) / 2.0 / 1000.0;
+ prevSpeed = speed;
+ Rtk2DriveTimer(prevPointTime, rtkTime);
+ DEBUG("鍋滆溅宸茶椹惰窛绂� %f", moveDistance);
+ }
if (moveDistance > 150) {
// 150绫冲唴鏈仠杞︼紝涓嶅悎鏍�
- DEBUG("鍋滆溅璺濈瓒呮爣");
+ DEBUG("鍋滆溅璺濈瓒呮爣锛岄潬杈瑰仠杞︾粨鏉�");
AddExamFault(33, rtkTime);
return -1;
}
@@ -103,7 +105,7 @@
if (moveDirect != prevMoveDirect) {
if (moveDirect == 0) {
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+ DEBUG("####### 闈犺竟鍋滆溅锛屽仠杞︿簡 %d %d %d %d %d %d %d ######", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
}
prevMoveDirect = moveDirect;
} else if (moveDirect == 0) {
@@ -189,6 +191,7 @@
}
}
} else if (occurOpenDoor) {
+ DEBUG("寮�杞﹂棬锛岄潬杈瑰仠杞︾粨鏉�");
return -1;
}
@@ -196,6 +199,7 @@
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
if (tp - openDoorTimepoint >= OPEN_DOOR_TIMEOUT + D_SEC(5)) {
+ DEBUG("寮�闂ㄦ椂闂村ぇ浜�20绉掞紝闈犺竟鍋滆溅缁撴潫");
return -1;
}
}
diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp
index 913333b..613a362 100644
--- a/lib/src/main/cpp/test_items2/through_something.cpp
+++ b/lib/src/main/cpp/test_items2/through_something.cpp
@@ -25,7 +25,7 @@
return;
DEBUG("杩涘叆璺�冮�氳繃something鍦板浘 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
if (!RoadMapList[index].tts.empty()) {
- DEBUG("鎾斁TTS");
+ DEBUG("鎾斁TTS %s", RoadMapList[index].tts.c_str());
PlayTTS(RoadMapList[index].tts.c_str());
} else {
DEBUG("娌℃湁TTS");
--
Gitblit v1.8.0