From d05c25aa4823e9d2aa812e9c0e3d1d66bb9861dd Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期六, 29 八月 2020 10:47:17 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/drive_straight.cpp | 33 ++++++-
lib/src/main/cpp/test_items2/drive_straight.h | 2
lib/src/main/cpp/driver_test.cpp | 3
lib/src/main/cpp/test_items2/overtake.cpp | 3
lib/src/main/cpp/native-lib.cpp | 2
lib/src/main/cpp/test_items2/road_exam.cpp | 130 ++++++++++++++++++-------------
lib/src/main/cpp/test_common/car_sensor.cpp | 3
lib/src/main/cpp/test_items2/change_lane.cpp | 3
lib/src/main/cpp/test_items2/operate_gear.cpp | 26 ++++--
lib/src/main/cpp/rtk_platform/platform.cpp | 3
10 files changed, 132 insertions(+), 76 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 08fbfbd..e15aaa6 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -513,7 +513,8 @@
if (TimeGetDiff(rtkTime, &cTime) >= D_SEC(3)) {
cTime = *rtkTime;
- DEBUG("ENGINE_RPM %d OBD_SPEED %f SPEED %f",
+ DEBUG("GEAR %d RPM %d OBD_SPEED %f SPEED %f",
+ ReadCarStatus(GEAR) - GEAR_N,
ReadCarStatus(ENGINE_RPM),
((double)ReadCarStatus(OBD_SPEED)) / 10.0,
speed * 3.6);
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index 2858a67..4738ab8 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/lib/src/main/cpp/native-lib.cpp
@@ -27,7 +27,7 @@
const int RTK_PLATFORM_PORT = 12125;
const uint8_t phone[] = {0x20,0x19,0x10,0x15,0x00,0x00,0x00,0x01};
-const char *VIRTUAL_RTK_IP = "192.168.1.6";
+const char *VIRTUAL_RTK_IP = "192.168.1.7";
const int VIRTUAL_RTK_PORT = 9001;
static pthread_mutex_t tts_mutex = PTHREAD_MUTEX_INITIALIZER;
diff --git a/lib/src/main/cpp/rtk_platform/platform.cpp b/lib/src/main/cpp/rtk_platform/platform.cpp
index f0dd9a0..a343c04 100644
--- a/lib/src/main/cpp/rtk_platform/platform.cpp
+++ b/lib/src/main/cpp/rtk_platform/platform.cpp
@@ -489,6 +489,9 @@
sensor.surround3 = data[x++];
sensor.surround4 = data[x++];
+ if (sensor.clutch == 1)
+ sensor.gear = 0;
+
UpdateSensor(&sensor);
}
if (events & CARD_UPDATE_EVT) {
diff --git a/lib/src/main/cpp/test_common/car_sensor.cpp b/lib/src/main/cpp/test_common/car_sensor.cpp
index d6f2a86..5e98de8 100644
--- a/lib/src/main/cpp/test_common/car_sensor.cpp
+++ b/lib/src/main/cpp/test_common/car_sensor.cpp
@@ -77,6 +77,7 @@
memset(CarStatus, 0, sizeof(CarStatus));
CarStatus[DOOR] = DOOR_CLOSE;
+ CarStatus[GEAR] = GEAR_N;
memset(&Sensor, 0, sizeof(Sensor));
@@ -265,7 +266,7 @@
static void SensorChanged(int id, int value)
{
- DEBUG("鐘舵�佹敼鍙� %d = %d", id, value);
+// DEBUG("鐘舵�佹敼鍙� %d = %d", id, value);
switch (id) {
case SENSOR_LEFT_TURN_SIGNAL: {
diff --git a/lib/src/main/cpp/test_items2/change_lane.cpp b/lib/src/main/cpp/test_items2/change_lane.cpp
index e4d927a..83e5f5e 100644
--- a/lib/src/main/cpp/test_items2/change_lane.cpp
+++ b/lib/src/main/cpp/test_items2/change_lane.cpp
@@ -18,6 +18,7 @@
{
maxMoveDistance = ReadOdo();
start = true;
+ DEBUG("鍙橀亾寮�濮嬮噷绋� %ld", maxMoveDistance);
}
void StartChaneLaneExam(int ori_lane)
@@ -41,7 +42,7 @@
if (ReadOdo() - maxMoveDistance > 100) {
// 瓒呰溅鏈畬鎴�
- DEBUG("瓒呰溅鍥哄畾璺濈鍐呮湭瀹屾垚");
+ DEBUG("鍙橀亾鍥哄畾璺濈鍐呮湭瀹屾垚 褰撳墠閲岀▼ %ld", ReadOdo());
AddExamFault(3, rtkTime);
return false;
}
diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp
index d6a9db2..8b7d1cf 100644
--- a/lib/src/main/cpp/test_items2/drive_straight.cpp
+++ b/lib/src/main/cpp/test_items2/drive_straight.cpp
@@ -20,6 +20,7 @@
static int setup;
static double beginOdo;
static int yaw_stat;
+static double offsetBase;
static void TtsBack(int seq)
{
@@ -33,9 +34,13 @@
PlayTTS("璇蜂繚鎸佺洿绾胯椹�", TtsBack);
}
-bool TestDriveStraight(const car_model *car, const struct RtkTime *rtkTime) {
+bool TestDriveStraight(road_exam_map &RoadMap, int roadIndex, const car_model *car, const struct RtkTime *rtkTime) {
static double yaw;
static Line baseLine;
+ static double offset1, offset2;
+
+ if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
+ return false;
if (setup == 1) {
// 鍋忚埅瑙掑钩鍧囧��
@@ -47,7 +52,7 @@
yaws.push_back(car->yaw);
yaw_stat++;
- if (yaw_stat == 5) {
+ if (yaw_stat == 15) {
yaw = AvgYaw(yaws);
vector<double>().swap(yaws);
@@ -55,13 +60,28 @@
PointF extPoint = PointExtend(car->basePoint, 100, yaw);
MakeLine(&baseLine, &car->basePoint, &extPoint);
beginOdo = ReadOdo();
+
+ PointF px = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]);
+ offsetBase = DistanceOf(px, car->carXY[car->axial[AXIAL_FRONT]]);
+ DEBUG("璧风偣鍋忕 %f", offsetBase);
+
setup = 2;
}
} else if (setup == 2) {
- double offset = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine);
+ offset1 = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine);
- if (offset > MAX_OFFSET_DISTANCE) {
- DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背 offset = %f", offset);
+ if (offset1 > MAX_OFFSET_DISTANCE) {
+ DEBUG("铏氭嫙鐩寸嚎鍋忕Щ澶т簬30鍘樼背 offset1 = %f", offset1);
+// // 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸
+// AddExamFault(30, rtkTime);
+// return false;
+ }
+
+ PointF px = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]);
+ offset2 = DistanceOf(px, car->carXY[car->axial[AXIAL_FRONT]]);
+
+ if (fabs(offset2 - offsetBase) > MAX_OFFSET_DISTANCE) {
+ DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背 offset2 = %f", fabs(offset2 - offsetBase));
// 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸
AddExamFault(30, rtkTime);
return false;
@@ -69,7 +89,8 @@
}
if (setup == 2 && ReadOdo() - beginOdo > CHECK_STAGE_DISTANCE) {
- DEBUG("鐩寸嚎琛岄┒缁撴潫");
+ DEBUG("鐩寸嚎琛岄┒缁撴潫 offset1 = %f offset2 = %f", offset1, fabs(offset2 - offsetBase));
+
PlayTTS("鐩寸嚎琛岄┒缁撴潫", NULL);
return false;
}
diff --git a/lib/src/main/cpp/test_items2/drive_straight.h b/lib/src/main/cpp/test_items2/drive_straight.h
index cda3f2a..9d463f1 100644
--- a/lib/src/main/cpp/test_items2/drive_straight.h
+++ b/lib/src/main/cpp/test_items2/drive_straight.h
@@ -8,6 +8,6 @@
#include "../driver_test.h"
void StartDriveStraightExam(void);
-bool TestDriveStraight(const car_model *car, const struct RtkTime *rtkTime);
+bool TestDriveStraight(road_exam_map &RoadMap, int roadIndex, const car_model *car, const struct RtkTime *rtkTime);
#endif //MYAPPLICATION2_DRIVE_STRAIGHT_H
diff --git a/lib/src/main/cpp/test_items2/operate_gear.cpp b/lib/src/main/cpp/test_items2/operate_gear.cpp
index 157d2a7..d365cc7 100644
--- a/lib/src/main/cpp/test_items2/operate_gear.cpp
+++ b/lib/src/main/cpp/test_items2/operate_gear.cpp
@@ -47,9 +47,10 @@
if (setup == 0) {
return true;
} else if (setup == 1) {
- if (sensor.value != GEAR_N)
+ if (sensor.value != GEAR_N) {
oldGear = sensor.value;
-
+ DEBUG("鍑嗗棣栨鎹㈡尅 GEAR = %d", oldGear);
+ }
switch (sensor.value) {
case GEAR_1: {
expectGear = GEAR_2;
@@ -89,49 +90,56 @@
}
} else if (setup == 2) {
if (sensor.value == GEAR_N || sensor.value == oldGear) {
-
+ DEBUG("绛夊緟棣栨鎹㈡尅");
} else if (sensor.value != expectGear) {
// 鏈寜鎸囦护鎿嶄綔鎸′綅锛屼笉鍚堟牸
- DEBUG("鏈寜鎸囦护鎿嶄綔鎸′綅");
+ DEBUG("棣栨鎹㈡尅閿欒 GEAR %d 甯屾湜 %d", sensor.value, expectGear);
AddExamFault(31, rtkTime);
return false;
} else {
// 鍦ㄦ鎸′綅琛岄┒涓�瀹氳窛绂伙紝鍐嶆墽琛屼笅涓�涓�
+ DEBUG("棣栨鎹㈡尅鎴愬姛锛屼笅涓�涓�...");
oldGear = expectGear;
shiftTime = *rtkTime;
setup = 3;
}
} else if (setup == 3) {
- if (TimeGetDiff(&shiftTime, rtkTime) >= D_SEC(4)) {
+ if (TimeGetDiff(rtkTime, &shiftTime) >= D_SEC(3)) {
setup = 4;
char buff[128];
expectGear += 0 - upDownShift;
sprintf(buff, "璇�%s鍒�%d鎸�", upDownShift > 0 ? "鍑�": "鍔�", expectGear - GEAR_N);
+ PlayTTS(buff, NULL);
+
+ DEBUG("%s", buff);
+ DEBUG("鍑嗗浜屾鎹㈡尅 甯屾湜 %d", expectGear);
}
}
else if (setup == 4) {
if (sensor.value == GEAR_N || sensor.value == oldGear) {
-
+ DEBUG("绛夊緟浜屾鎹㈡尅");
} else if (sensor.value != expectGear) {
// 鏈寜鎸囦护鎿嶄綔鎸′綅锛屼笉鍚堟牸
- DEBUG("鏈寜鎸囦护鎿嶄綔鎸′綅");
+ DEBUG("浜屾鎹㈡尅閿欒 GEAR %d 甯屾湜 %d", sensor.value, expectGear);
AddExamFault(31, rtkTime);
return false;
} else {
// 鍦ㄦ鎸′綅琛岄┒涓�瀹氳窛绂伙紝鍐嶆墽琛屼笅涓�涓�
shiftTime = *rtkTime;
setup = 5;
+ DEBUG("浜屾鎹㈡尅鎴愬姛锛屼笅涓�涓�...");
}
} else if (setup == 5) {
- if (TimeGetDiff(&shiftTime, rtkTime) >= D_SEC(5)) {
+ if (TimeGetDiff(rtkTime, &shiftTime) >= D_SEC(3)) {
PlayTTS("鍔犲噺鎸′綅鎿嶄綔缁撴潫", NULL);
+ DEBUG("鍔犲噺鎸′綅鎿嶄綔缁撴潫");
return false;
}
}
if (ReadOdo() - maxMoveDistance > 120) {
// 鏈寜鎸囦护鎿嶄綔鎸′綅锛屼笉鍚堟牸
- DEBUG("鏈寜鎸囦护鎿嶄綔鎸′綅");
+ DEBUG("鏈寜鎸囦护鎿嶄綔鎸′綅锛岃秴鏃�");
AddExamFault(31, rtkTime);
return false;
}
diff --git a/lib/src/main/cpp/test_items2/overtake.cpp b/lib/src/main/cpp/test_items2/overtake.cpp
index c756867..e67105f 100644
--- a/lib/src/main/cpp/test_items2/overtake.cpp
+++ b/lib/src/main/cpp/test_items2/overtake.cpp
@@ -18,6 +18,7 @@
{
maxMoveDistance = ReadOdo();
setup = 1;
+ DEBUG("瓒呰溅寮�濮嬮噷绋� %ld", maxMoveDistance);
}
void StartOvertakeExam(int ori_lane)
@@ -61,7 +62,7 @@
if (ReadOdo() - maxMoveDistance > 150) {
// 瓒呰溅鏈畬鎴�
- DEBUG("瓒呰溅鍥哄畾璺濈鍐呮湭瀹屾垚");
+ DEBUG("瓒呰溅鍥哄畾璺濈鍐呮湭瀹屾垚 褰撳墠閲岀▼ %ld", ReadOdo());
AddExamFault(3, rtkTime);
return false;
}
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index b4e0483..53eea6a 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -72,7 +72,7 @@
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
-const double EXAM_RANGE = 3000.0; // 鑷冲皯椹鹃┒璺濈
+const double EXAM_RANGE = 2700.0; // 鑷冲皯椹鹃┒璺濈
static bool occurOverSpeed;
static bool occurSecondBreak;
@@ -93,6 +93,8 @@
static struct RtkTime gearErrorTimePoint;
static struct RtkTime gearNSlideTimePoint;
+
+static car_sensor_value_t turnSignalStatus;
static int gearErrorTime;
@@ -127,13 +129,14 @@
} RoadExamItem_t;
static map<int, int> RoadExamItem;
static int RoadExamStatus;
+static bool win;
static struct RtkTime beginTurnTime, prevDetectTurnTime;
static int startTurnYaw, prevYaw;
static int turnCnt, turnTimeCnt;
static int prevTurnWise;
-static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(5);
+static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(10);
static const uint32_t GEAR_ERROR_TIMEOUT = D_SEC(15);
static const uint32_t STOP_CAR_TIME = D_SEC(2);
static const uint32_t CHANGE_LANE_MIN_INTERVAL = D_SEC(10);
@@ -199,6 +202,8 @@
RoadExamItem[ROAD_EXAM_ITEM_CAR_STOP] = ROAD_EXAM_ITEM_NOT_EXEC;
RoadExamStatus = ROAD_EXAM_READY_NEXT;
+
+ win = false;
}
void TerminateRoadExam(void)
@@ -879,7 +884,7 @@
checkCrashGreenTimeout = 0;
// 璁板綍寮�濮嬪帇绾跨殑鏃堕棿锛屼笉纭畾鏄惁鏈夊彉閬撴剰鍥撅紝寰呯‘璁ゅ彉閬撳悗鍐嶅鐞嗕箣
crashGreenStartTime = *rtkTime;
-
+ turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
DEBUG("寮�濮嬪帇铏氱嚎 %02d-%02d-%02d %02d:%02d:%02d", crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10);
}
crashDottedLine = true;
@@ -926,40 +931,45 @@
if (newLane.road == Lane.road && newLane.sep == Lane.sep) {
gain = newLane.no - Lane.no;
} else {
- DEBUG("杞﹂亾鍒嗘鍒囨崲");
+ DEBUG("杞﹂亾鍒嗘鍒囨崲 Old Lane road = %d sep = %d no = %d New Lane road = %d sep = %d no = %d", Lane.road, Lane.sep, Lane.no, newLane.road, newLane.sep, newLane.no);
ChangeLane.gain = 0;
}
// 妫�鏌ヨ浆鍚戠伅
if (gain != 0) {
DEBUG("鍙橀亾 new lane %d, gain %d", newLane.no, gain);
- car_sensor_value_t lamp = ReadCarSensorValue(TURN_SIGNAL_LAMP);
- if (lamp.name == TURN_SIGNAL_LAMP) {
- if (gain < 0) {
- if (lamp.value != LEFT_TURN_LIGHT) {
- DEBUG("鍙樿皟鏈墦鐏�!!");
- // 娌℃墦鐏紝涓嶅悎鏍�
- AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
- DEBUG("杞悜鐏椂闂翠笉瓒� %02d-%02d-%02d %02d:%02d:%02d.%03d -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
- crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10,
- lamp.time.YY, lamp.time.MM, lamp.time.DD, lamp.time.hh, lamp.time.mm, lamp.time.ss, lamp.time.mss * 10);
+ if (gain < 0) {
+ if (turnSignalStatus.value != LEFT_TURN_LIGHT) {
+ DEBUG("鍙樿皟鏈墦鐏�!!");
+ // 娌℃墦鐏紝涓嶅悎鏍�
+ AddExamFault(13, rtkTime);
+ } else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.time) <
+ TURN_SIGNAL_LAMP_ADVANCE) {
+ DEBUG("杞悜鐏椂闂翠笉瓒� %02d-%02d-%02d %02d:%02d:%02d.%03d -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
+ crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD,
+ crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss,
+ crashGreenStartTime.mss * 10,
+ turnSignalStatus.time.YY, turnSignalStatus.time.MM, turnSignalStatus.time.DD, turnSignalStatus.time.hh,
+ turnSignalStatus.time.mm, turnSignalStatus.time.ss, turnSignalStatus.time.mss * 10);
- // 涓嶈冻3绉掞紝涓嶅悎鏍�
- AddExamFault(14, rtkTime);
- }
- } else {
- if (lamp.value != RIGHT_TURN_LIGHT) {
- DEBUG("鍙樿皟鏈墦鐏�!!");
- // 娌℃墦鐏紝涓嶅悎鏍�
- AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
- DEBUG("杞悜鐏椂闂翠笉瓒� %02d-%02d-%02d %02d:%02d:%02d.%03d -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
- crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10,
- lamp.time.YY, lamp.time.MM, lamp.time.DD, lamp.time.hh, lamp.time.mm, lamp.time.ss, lamp.time.mss * 10);
- // 涓嶈冻3绉掞紝涓嶅悎鏍�
- AddExamFault(14, rtkTime);
- }
+ // 涓嶈冻3绉掞紝涓嶅悎鏍�
+ AddExamFault(14, rtkTime);
+ }
+ } else {
+ if (turnSignalStatus.value != RIGHT_TURN_LIGHT) {
+ DEBUG("鍙樿皟鏈墦鐏�!!");
+ // 娌℃墦鐏紝涓嶅悎鏍�
+ AddExamFault(13, rtkTime);
+ } else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.time) <
+ TURN_SIGNAL_LAMP_ADVANCE) {
+ DEBUG("杞悜鐏椂闂翠笉瓒� %02d-%02d-%02d %02d:%02d:%02d.%03d -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
+ crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD,
+ crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss,
+ crashGreenStartTime.mss * 10,
+ turnSignalStatus.time.YY, turnSignalStatus.time.MM, turnSignalStatus.time.DD, turnSignalStatus.time.hh,
+ turnSignalStatus.time.mm, turnSignalStatus.time.ss, turnSignalStatus.time.mss * 10);
+ // 涓嶈冻3绉掞紝涓嶅悎鏍�
+ AddExamFault(14, rtkTime);
}
}
@@ -1238,6 +1248,11 @@
UpdataOdo(speed, moveDirect, rtkTime);
+ if (!win) {
+ win = true;
+ turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
+ }
+
// 鍒硅溅鎻愮ず闊�
RingBreak();
@@ -1323,6 +1338,7 @@
// 鎸′綅涓嶅尮閰嶈秴鏃�
if (currGearError && prevGearError) {
+ DEBUG("鎸′綅閿欒澧炲姞 %ld姣 褰撳墠鎸′綅 %d 鏃堕�� %f", TimeGetDiff(rtkTime, &gearErrorTimePoint), ReadCarStatus(GEAR), ConvertMs2KMh(speed));
gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
}
if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
@@ -1425,10 +1441,11 @@
// DEBUG("Lane淇℃伅 road %d sep %d total %d no %d guide %d", Lane.road, Lane.sep, Lane.total, Lane.no, Lane.guide);
if (Lane.guide > 0 && currExamMapIndex >= 0) {
int stop_line_index;
+ int act = NearbyCrossingGuide(stop_line_index, currExamMapIndex, RoadMap.roads[currExamMapIndex], car);
- if (!(NearbyCrossingGuide(stop_line_index, currExamMapIndex, RoadMap.roads[currExamMapIndex], car) & Lane.guide)) {
+ if (act != 0 && !(act & Lane.guide)) {
if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
- DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒ %d: %d guide = %d", currExamMapIndex, stop_line_index, Lane.guide);
+ DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒ %d: %d 鏈熸湜 = %d guide = %d", currExamMapIndex, stop_line_index, act, Lane.guide);
AddExamFault(9, rtkTime);
SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
}
@@ -1547,7 +1564,7 @@
testing = TestOvertake(Lane.no, rtkTime);
break;
case ROAD_EXAM_ITEM_STRAIGHT:
- testing = TestDriveStraight(car, rtkTime);
+ testing = TestDriveStraight(RoadMap, roadIndex, car, rtkTime);
break;
case ROAD_EXAM_ITEM_CAR_STOP:
testing = TestStopCar(RoadMap, roadIndex, car, moveDirect, rtkTime);
@@ -1650,6 +1667,8 @@
turnCnt++;
beginTurnTime = *rtkTime;
startTurnYaw = (int) car->yaw;
+
+ turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
}
} else if (turnCnt == 1) {
if (angle != 0 && moveDirect != 0) {
@@ -1661,6 +1680,8 @@
} else {
beginTurnTime = *rtkTime;
startTurnYaw = (int) car->yaw;
+
+ turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
}
} else {
turnCnt = 0;
@@ -1676,28 +1697,27 @@
// 纭杞集琛屼负锛屾娴嬪紑濮嬪垰杞集鏃惰浆鍚戠伅鎯呭喌
turnCnt = -1;
- car_sensor_value_t lamp = ReadCarSensorValue(TURN_SIGNAL_LAMP);
- if (lamp.name == TURN_SIGNAL_LAMP) {
- if (wise < 0) {
- if (lamp.value != LEFT_TURN_LIGHT) {
- DEBUG("鍙樿皟鏈墦鐏�!!");
- // 娌℃墦鐏紝涓嶅悎鏍�
- AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&beginTurnTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
- DEBUG("杞悜鐏椂闂翠笉瓒�");
- // 涓嶈冻3绉掞紝涓嶅悎鏍�
- AddExamFault(14, rtkTime);
- }
- } else {
- if (lamp.value != RIGHT_TURN_LIGHT) {
- DEBUG("鍙樿皟鏈墦鐏�!!");
- // 娌℃墦鐏紝涓嶅悎鏍�
- AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&beginTurnTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
- DEBUG("杞悜鐏椂闂翠笉瓒�");
- // 涓嶈冻3绉掞紝涓嶅悎鏍�
- AddExamFault(14, rtkTime);
- }
+ if (wise < 0) {
+ if (turnSignalStatus.value != LEFT_TURN_LIGHT) {
+ DEBUG("鍙樿皟鏈墦鐏�!!");
+ // 娌℃墦鐏紝涓嶅悎鏍�
+ AddExamFault(13, rtkTime);
+ } else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) <
+ TURN_SIGNAL_LAMP_ADVANCE) {
+ DEBUG("杞悜鐏椂闂翠笉瓒�");
+ // 涓嶈冻3绉掞紝涓嶅悎鏍�
+ AddExamFault(14, rtkTime);
+ }
+ } else {
+ if (turnSignalStatus.value != RIGHT_TURN_LIGHT) {
+ DEBUG("鍙樿皟鏈墦鐏�!!");
+ // 娌℃墦鐏紝涓嶅悎鏍�
+ AddExamFault(13, rtkTime);
+ } else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) <
+ TURN_SIGNAL_LAMP_ADVANCE) {
+ DEBUG("杞悜鐏椂闂翠笉瓒�");
+ // 涓嶈冻3绉掞紝涓嶅悎鏍�
+ AddExamFault(14, rtkTime);
}
}
} else if (turnTimeCnt > D_SEC(10)) {
--
Gitblit v1.8.0