From cff87234c496f0a4522cd2ac57941f14c729b6ad Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期五, 28 八月 2020 01:05:09 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/master/comm_if.h | 2
lib/src/main/cpp/test_items2/road_exam.h | 9 -
lib/src/main/cpp/test_common/car_sensor.h | 2
lib/src/main/cpp/test_common/odo_graph.cpp | 10 -
lib/src/main/cpp/master/comm_if.cpp | 2
lib/src/main/cpp/native-lib.cpp | 2
lib/src/main/cpp/test_items2/road_exam.cpp | 173 ++++++++++++++++++----------------
lib/src/main/cpp/test_common/car_sensor.cpp | 65 ++++++------
lib/src/main/cpp/test_items2/operate_gear.cpp | 11 +
lib/src/main/cpp/mcu/mcu_if.cpp | 7
lib/src/main/cpp/rtk_platform/platform.cpp | 4
11 files changed, 147 insertions(+), 140 deletions(-)
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index 56ef347..c81177e 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -304,7 +304,7 @@
writer.Key("aps");
writer.Int(brief->aps);
writer.Key("lock");
- writer.Int(brief->lock);
+ writer.Int(brief->door);
writer.Key("seat_belt");
writer.Int(brief->seatBelt);
writer.Key("clutch");
diff --git a/lib/src/main/cpp/master/comm_if.h b/lib/src/main/cpp/master/comm_if.h
index 78750ee..d06868d 100644
--- a/lib/src/main/cpp/master/comm_if.h
+++ b/lib/src/main/cpp/master/comm_if.h
@@ -28,7 +28,7 @@
int key;
int gear;
int aps;
- int lock;
+ int door;
int seatBelt;
int clutch;
int horn;
diff --git a/lib/src/main/cpp/mcu/mcu_if.cpp b/lib/src/main/cpp/mcu/mcu_if.cpp
index 5d2ff23..732db69 100644
--- a/lib/src/main/cpp/mcu/mcu_if.cpp
+++ b/lib/src/main/cpp/mcu/mcu_if.cpp
@@ -123,7 +123,7 @@
}*/
if (RxBufLen > 0) {
- DEBUG("RECV LEN %d", RxBufLen);
+// DEBUG("RECV LEN %d", RxBufLen);
ParseMcu(RxBuf, RxBufLen);
RxBufLen = 0;
}
@@ -213,7 +213,7 @@
uint16_t crc16 = CRCCCITT(McuPkt.buffer, McuPkt.length + 4, 0, 0);
- DEBUG("mcuif crc16 but 0x%04X exp 0x%04X", McuPkt.crc16, crc16);
+// DEBUG("mcuif crc16 but 0x%04X exp 0x%04X", McuPkt.crc16, crc16);
if (McuPkt.crc16 == crc16) {
McuCommandEntry(McuPkt.id, McuPkt.buffer + 4, McuPkt.length);
@@ -358,12 +358,13 @@
case ID_MC_MCU_DFU_RSP:
break;
case ID_MC_CAR_INFO2:
+// DEBUG("ID_MC_CAR_INFO2 %d", lenth);
if (lenth > 0) {
PlatformStatusChanged(CAR_SENSOR_UPDATE_EVT, data, lenth);
}
break;
case ID_MC_CAR_INFO: {
- DEBUG("ID_MC_CAR_INFO %d", lenth);
+// DEBUG("ID_MC_CAR_INFO %d", lenth);
if (lenth > 0)
PlatformStatusChanged(MCU_UPDATE_EVT, data, lenth);
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index 6259c16..2858a67 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.4";
+const char *VIRTUAL_RTK_IP = "192.168.1.6";
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 4a6bd58..e63c642 100644
--- a/lib/src/main/cpp/rtk_platform/platform.cpp
+++ b/lib/src/main/cpp/rtk_platform/platform.cpp
@@ -436,7 +436,7 @@
brief.key = data[x++];
brief.gear = data[x++];
brief.aps = data[x++];
- brief.lock = data[x++];
+ brief.door = data[x++];
brief.seatBelt = data[x++];
brief.clutch = data[x++];
brief.horn = data[x++];
@@ -470,7 +470,7 @@
sensor.key = data[x++];
sensor.gear = data[x++];
sensor.aps = data[x++];
- sensor.lock = data[x++];
+ sensor.door = data[x++];
sensor.seatBelt = data[x++];
sensor.clutch = data[x++];
sensor.horn = data[x++];
diff --git a/lib/src/main/cpp/test_common/car_sensor.cpp b/lib/src/main/cpp/test_common/car_sensor.cpp
index b3a524c..d6f2a86 100644
--- a/lib/src/main/cpp/test_common/car_sensor.cpp
+++ b/lib/src/main/cpp/test_common/car_sensor.cpp
@@ -76,6 +76,7 @@
memset(SensorConfig, 0, sizeof(SensorConfig));
memset(CarStatus, 0, sizeof(CarStatus));
+ CarStatus[DOOR] = DOOR_CLOSE;
memset(&Sensor, 0, sizeof(Sensor));
@@ -88,36 +89,36 @@
void SetSensorCfg(int (*sensor)[3], int sensorNum)
{
- DEBUG("鍔犲叆浼犳劅鍣ㄩ厤缃� sensorNum %d", sensorNum);
-
- pthread_mutex_lock(&sonser_mutex);
-
- SensorNum = sensorNum;
- for (int i = 0; i < sensorNum; ++i) {
- DEBUG(" 閰嶇疆<%d>: id %d - fun %d - lvl %d", i, sensor[i][0], sensor[i][1], sensor[i][2]);
-
- SensorConfig[i].gpioId = sensor[i][0];
- SensorConfig[i].funId = sensor[i][1];
- if (sensor[i][2] > 0) {
- SensorConfig[i].validLvl = 1;
- } else {
- SensorConfig[i].validLvl = 0;
- }
-
- int level = 0;
-
- if (gpioStore & BV(SensorConfig[i].gpioId)) {
- level = 1;
- }
-
- if (level == SensorConfig[i].validLvl) {
- SensorChanged(SensorConfig[i].funId, 1);
- } else {
- SensorChanged(SensorConfig[i].funId, 0);
- }
- }
-
- pthread_mutex_unlock(&sonser_mutex);
+// DEBUG("鍔犲叆浼犳劅鍣ㄩ厤缃� sensorNum %d", sensorNum);
+//
+// pthread_mutex_lock(&sonser_mutex);
+//
+// SensorNum = sensorNum;
+// for (int i = 0; i < sensorNum; ++i) {
+// DEBUG(" 閰嶇疆<%d>: id %d - fun %d - lvl %d", i, sensor[i][0], sensor[i][1], sensor[i][2]);
+//
+// SensorConfig[i].gpioId = sensor[i][0];
+// SensorConfig[i].funId = sensor[i][1];
+// if (sensor[i][2] > 0) {
+// SensorConfig[i].validLvl = 1;
+// } else {
+// SensorConfig[i].validLvl = 0;
+// }
+//
+// int level = 0;
+//
+// if (gpioStore & BV(SensorConfig[i].gpioId)) {
+// level = 1;
+// }
+//
+// if (level == SensorConfig[i].validLvl) {
+// SensorChanged(SensorConfig[i].funId, 1);
+// } else {
+// SensorChanged(SensorConfig[i].funId, 0);
+// }
+// }
+//
+// pthread_mutex_unlock(&sonser_mutex);
}
void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t rpm)
@@ -187,8 +188,8 @@
if (s->handBreak != Sensor.handBreak && s->handBreak != '#') {
SensorChanged(SENSOR_HANDBREAK, BX(s->handBreak));
}
- if (s->lock != Sensor.lock && s->lock != '#') {
- SensorChanged(SENSOR_DOOR, BX(s->lock));
+ if (s->door != Sensor.door && s->door != '#') {
+ SensorChanged(SENSOR_DOOR, BX(s->door));
}
if (s->surround1 != Sensor.surround1 && s->surround1 != '#') {
SensorChanged(SENSOR_SURROUND_CAR_1, BX(s->surround1));
diff --git a/lib/src/main/cpp/test_common/car_sensor.h b/lib/src/main/cpp/test_common/car_sensor.h
index 2818c27..e15b183 100644
--- a/lib/src/main/cpp/test_common/car_sensor.h
+++ b/lib/src/main/cpp/test_common/car_sensor.h
@@ -73,7 +73,7 @@
int key;
int gear;
int aps;
- int lock;
+ int door;
int seatBelt;
int clutch;
int horn;
diff --git a/lib/src/main/cpp/test_common/odo_graph.cpp b/lib/src/main/cpp/test_common/odo_graph.cpp
index 1266a1b..977fc23 100644
--- a/lib/src/main/cpp/test_common/odo_graph.cpp
+++ b/lib/src/main/cpp/test_common/odo_graph.cpp
@@ -8,7 +8,7 @@
#include "../utils/xconvert.h"
static double odoGraph;
-static struct drive_timer odoTimer;
+static struct RtkTime odoTimer;
static double odoPrevSpeed;
static int odoCnt;
@@ -28,15 +28,13 @@
if (odoCnt == 0 && moveDirect == 1) {
odoPrevSpeed = speed;
odoCnt = 1;
- Rtk2DriveTimer(odoTimer, rtkTime);
+ odoTimer = *rtkTime;
} else if (odoCnt == 1) {
if (moveDirect == 1) {
- uint32_t tm = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss * 10,
- odoTimer.hour, odoTimer.min, odoTimer.sec,
- odoTimer.msec * 10);
+ uint32_t tm = TimeGetDiff(rtkTime, &odoTimer);
if (tm >= D_SEC(1)) {
odoGraph += ((double) tm) * (odoPrevSpeed + speed) / 2.0 / 1000.0;
- Rtk2DriveTimer(odoTimer, rtkTime);
+ odoTimer = *rtkTime;
odoPrevSpeed = speed;
}
} else {
diff --git a/lib/src/main/cpp/test_items2/operate_gear.cpp b/lib/src/main/cpp/test_items2/operate_gear.cpp
index 5a81d6b..73de9c3 100644
--- a/lib/src/main/cpp/test_items2/operate_gear.cpp
+++ b/lib/src/main/cpp/test_items2/operate_gear.cpp
@@ -15,6 +15,7 @@
static int expectGear;
static int upDownShift;
+static int oldGear;
static int setup;
@@ -46,6 +47,9 @@
if (setup == 0) {
return true;
} else if (setup == 1) {
+ if (sensor.value != GEAR_N)
+ oldGear = sensor.value;
+
switch (sensor.value) {
case GEAR_1: {
expectGear = GEAR_2;
@@ -84,7 +88,7 @@
}
}
} else if (setup == 2) {
- if (sensor.value == GEAR_N) {
+ if (sensor.value == GEAR_N || sensor.value == oldGear) {
} else if (sensor.value != expectGear) {
// 鏈寜鎸囦护鎿嶄綔鎸′綅锛屼笉鍚堟牸
@@ -93,11 +97,12 @@
return false;
} else {
// 鍦ㄦ鎸′綅琛岄┒涓�瀹氳窛绂伙紝鍐嶆墽琛屼笅涓�涓�
+ oldGear = expectGear;
shiftTime = *rtkTime;
setup = 3;
}
} else if (setup == 3) {
- if (TimeGetDiff(&shiftTime, rtkTime) >= D_SEC(5)) {
+ if (TimeGetDiff(&shiftTime, rtkTime) >= D_SEC(4)) {
setup = 4;
char buff[128];
expectGear += 0 - upDownShift;
@@ -105,7 +110,7 @@
}
}
else if (setup == 4) {
- if (sensor.value == GEAR_N) {
+ if (sensor.value == GEAR_N || sensor.value == oldGear) {
} else if (sensor.value != expectGear) {
// 鏈寜鎸囦护鎿嶄綔鎸′綅锛屼笉鍚堟牸
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 8c56d03..c9fc0d2 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -83,7 +83,7 @@
static bool StopCarOnRedArea;
static PointF stopPoint;
static bool prevGearError = false;
-static bool prevGearNSlide = false;
+static int GearNSlideStatus = 0;
static bool slideLongDistance;
static bool slideNormalDistance;
@@ -91,11 +91,10 @@
static struct RtkTime crashGreenRunTime, crashGreenStartTime, stopTimepoint;
-static struct drive_timer gearErrorTimePoint;
-static struct drive_timer gearNSlideTimePoint;
+static struct RtkTime gearErrorTimePoint;
+static struct RtkTime gearNSlideTimePoint;
static int gearErrorTime;
-static int gearNSlideTime;
static int currExamMapIndex;
@@ -147,6 +146,7 @@
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};
static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end);
+static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList);
static int isTurn(int currYaw, int prevYaw, int thres);
static void ResetTurnDetect(const car_model *car);
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
@@ -154,7 +154,7 @@
static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);
-static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
+static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);
@@ -176,8 +176,7 @@
slideNormalDistance = false;
prevGearError = false;
gearErrorTime = 0;
- prevGearNSlide = false;
- gearNSlideTime = 0;
+ GearNSlideStatus = 0;
currExamMapIndex = -1;
@@ -880,6 +879,8 @@
checkCrashGreenTimeout = 0;
// 璁板綍寮�濮嬪帇绾跨殑鏃堕棿锛屼笉纭畾鏄惁鏈夊彉閬撴剰鍥撅紝寰呯‘璁ゅ彉閬撳悗鍐嶅鐞嗕箣
crashGreenStartTime = *rtkTime;
+
+ DEBUG("寮�濮嬪帇铏氱嚎 %02d-%02d-%02d %02d:%02d:%02d", crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10);
}
crashDottedLine = true;
} else {
@@ -939,8 +940,11 @@
DEBUG("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
- DEBUG("杞悜鐏椂闂翠笉瓒�");
+ } 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);
}
@@ -949,8 +953,10 @@
DEBUG("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
- DEBUG("杞悜鐏椂闂翠笉瓒�");
+ } 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);
}
@@ -1298,27 +1304,26 @@
default:break;
}
// 绌烘。婊戣瓒呮椂
- if (currGearNSlide && prevGearNSlide) {
- gearNSlideTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
- gearNSlideTimePoint.hour, gearNSlideTimePoint.min, gearNSlideTimePoint.sec, gearNSlideTimePoint.msec*10);
+ if (currGearNSlide) {
+ if (GearNSlideStatus == 0) {
+ DEBUG("妫�娴嬪埌绌烘尅婊戣");
+ GearNSlideStatus = 1;
+ gearNSlideTimePoint = *rtkTime;
+ }
+ if (GearNSlideStatus == 1 && TimeGetDiff(rtkTime, &gearNSlideTimePoint) > GEAR_N_SLIDE_TIMEOUT) {
+ // 绌烘。婊戣瓒�5绉掞紝涓嶅悎鏍�
+ DEBUG("鎸′綅婊戣锛岃秴杩�5绉�");
+ AddExamFault(8, rtkTime);
+ GearNSlideStatus = 2;
+ }
+ } else if (GearNSlideStatus != 0) {
+ GearNSlideStatus = 0;
+ DEBUG("绌烘尅婊戣缁撴潫");
}
- if (gearNSlideTime > GEAR_N_SLIDE_TIMEOUT) {
- // 绌烘。婊戣瓒�5绉掞紝涓嶅悎鏍�
- DEBUG("鎸′綅婊戣锛岃秴杩�5绉�");
- AddExamFault(8, rtkTime);
- gearNSlideTime = 0;
- }
-
- prevGearNSlide = currGearNSlide;
- if (prevGearNSlide) {
- Rtk2DriveTimer(gearNSlideTimePoint, rtkTime);
- } else {
- gearNSlideTime = 0;
- }
+
// 鎸′綅涓嶅尮閰嶈秴鏃�
if (currGearError && prevGearError) {
- gearErrorTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
- gearErrorTimePoint.hour, gearErrorTimePoint.min, gearErrorTimePoint.sec, gearErrorTimePoint.msec*10);
+ gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
}
if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
// 绱15绉掞紝鎸′綅-杞﹂�熶笉鍖归厤锛屼笉鍚堟牸
@@ -1329,7 +1334,7 @@
prevGearError = currGearError;
if (prevGearError) {
- Rtk2DriveTimer(gearErrorTimePoint, rtkTime);
+ gearErrorTimePoint = *rtkTime;
}
// 璧锋鍚庢粦
@@ -1395,6 +1400,7 @@
ResetCrossingStatus(oldid);
ResetErrorLaneRpt(oldid);
}
+ Lane.guide = 0;
}
if (currExamMapIndex >= 0) {
@@ -1422,7 +1428,7 @@
if (!(NearbyCrossingGuide(stop_line_index, currExamMapIndex, RoadMap.roads[currExamMapIndex], car) & Lane.guide)) {
if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
- DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒");
+ DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒ %d: %d guide = %d", currExamMapIndex, stop_line_index, Lane.guide);
AddExamFault(9, rtkTime);
SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
}
@@ -1456,6 +1462,7 @@
DetectTurn(car, moveDirect, rtkTime);
ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, TargetFree > RoadCrossingFree? RoadCrossingFree : TargetFree);
+ ItemExam2(RoadMap, currExamMapIndex, car, CarModelList);
}
static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end)
@@ -1474,41 +1481,41 @@
if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_NOT_EXEC) {
not_complete = true;
- if (straight > 170 && road_end > 170) {
- StartDriveStraightExam();
- RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
- return;
- }
+// if (straight > 170 && road_end > 170) {
+// StartDriveStraightExam();
+// RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
+// return;
+// }
}
if (RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] == ROAD_EXAM_ITEM_NOT_EXEC) {
not_complete = true;
- if (road_end > 170) {
- StartOperateGearExam();
- RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
- return;
- }
+// if (road_end > 170) {
+// StartOperateGearExam();
+// RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
+// return;
+// }
}
if (RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] == ROAD_EXAM_ITEM_NOT_EXEC) {
not_complete = true;
- if (road_end > 150 && Lane.total > 1) {
- StartChaneLaneExam(Lane.no);
- RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
- return;
- }
+// if (road_end > 150 && Lane.total > 1) {
+// StartChaneLaneExam(Lane.no);
+// RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
+// return;
+// }
}
if (RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] == ROAD_EXAM_ITEM_NOT_EXEC) {
not_complete = true;
- if (road_end > 200 && Lane.total > 1) {
- if (Lane.no == 0) {
- // 宸插湪鏈�宸﹁溅閬擄紝鍏堝彉鏇磋溅閬擄紵
- StartChaneLaneExam(Lane.no);
- RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
- } else {
- StartOvertakeExam(Lane.no);
- RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
- }
- return;
- }
+// if (road_end > 200 && Lane.total > 1) {
+// if (Lane.no == 0) {
+// // 宸插湪鏈�宸﹁溅閬擄紝鍏堝彉鏇磋溅閬擄紵
+// StartChaneLaneExam(Lane.no);
+// RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
+// } else {
+// StartOvertakeExam(Lane.no);
+// RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
+// }
+// return;
+// }
}
if (!not_complete) {
@@ -1522,7 +1529,6 @@
} else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) {
if (ReadOdo() - freeRunDistance > freeRunExceptDistance) {
RoadExamStatus = ROAD_EXAM_READY_NEXT;
-
}
} else {
bool testing = false;
@@ -1554,7 +1560,7 @@
if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_START) {
freeRunExceptDistance = 60.0;
} else {
- freeRunExceptDistance = 250.0;
+ freeRunExceptDistance = 2;//250.0;
}
RoadExamStatus = ROAD_EXAM_FREE_RUN;
@@ -1563,12 +1569,23 @@
}
}
-void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
+static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
- tm.hour = rtkTime->hh;
- tm.min = rtkTime->mm;
- tm.sec = rtkTime->ss;
- tm.msec = rtkTime->mss;
+ int item = EntryItem(roadIndex, RoadMap, car, CarModelList);
+
+ if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 3) {
+ StartDriveStraightExam();
+ RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
+ } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 4) {
+ StartOperateGearExam();
+ RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
+ } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 1) {
+ StartChaneLaneExam(Lane.no);
+ RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
+ } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 2) {
+ StartOvertakeExam(Lane.no);
+ RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
+ }
}
void CrossRoadCallback(int road, int stop_line, int active, const car_model *car)
@@ -1666,7 +1683,7 @@
DEBUG("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
+ } else if (TimeGetDiff(&beginTurnTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
DEBUG("杞悜鐏椂闂翠笉瓒�");
// 涓嶈冻3绉掞紝涓嶅悎鏍�
AddExamFault(14, rtkTime);
@@ -1676,7 +1693,7 @@
DEBUG("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
+ } else if (TimeGetDiff(&beginTurnTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
DEBUG("杞悜鐏椂闂翠笉瓒�");
// 涓嶈冻3绉掞紝涓嶅悎鏍�
AddExamFault(14, rtkTime);
@@ -1784,35 +1801,27 @@
return false;
}
-static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
+static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
+ if (index < 0 || index >= RoadMap.roads.size())
+ return -1;
+
for (int i = 0; i < RoadMap.triggerLines.size(); ++i) {
if (RoadMap.triggerLines[i].road == RoadMap.roads[index].id) {
Line triggerLine;
- if (RoadMap.triggerLines[i].leftPoints.size() != RoadMap.triggerLines[i].points.size()) {
- RoadMap.triggerLines[i].leftPoints.clear();
+ PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[0]);
- for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) {
- RoadMap.triggerLines[i].leftPoints.push_back(CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[j]));
- }
-
- for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) {
- DEBUG("瑙﹀彂绾胯ˉ榻� road %d id %d type %d (%0.4f, %0.4f)-(%0.4f, %0.4f)", RoadMap.roads[index].id, RoadMap.triggerLines[i].id, RoadMap.triggerLines[i].active,
- RoadMap.triggerLines[i].points[0].X, RoadMap.triggerLines[i].points[0].Y, RoadMap.triggerLines[i].leftPoints[0].X, RoadMap.triggerLines[i].leftPoints[0].Y);
- }
- }
-
- MakeLine(&triggerLine, &RoadMap.triggerLines[i].points[0], &RoadMap.triggerLines[i].leftPoints[0]);
+ MakeLine(&triggerLine, &RoadMap.triggerLines[i].points[0], &p2);
if (CrashTheLine(triggerLine, car, CarModelList)) {
DEBUG("瑙﹀彂椤圭洰 %d %s (%0.4f, %0.4f)-(%0.4f, %0.4f)", RoadMap.triggerLines[i].active, RoadMap.triggerLines[i].tts.c_str(), triggerLine.X1, triggerLine.Y1, triggerLine.X2, triggerLine.Y2);
- return &RoadMap.triggerLines[i];
+ return RoadMap.triggerLines[i].active;
}
}
}
- return NULL;
+ return -1;
}
static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car)
diff --git a/lib/src/main/cpp/test_items2/road_exam.h b/lib/src/main/cpp/test_items2/road_exam.h
index a36f8a9..39626b1 100644
--- a/lib/src/main/cpp/test_items2/road_exam.h
+++ b/lib/src/main/cpp/test_items2/road_exam.h
@@ -20,13 +20,6 @@
#define THROUGH_BUS_STATION_MAP 109
#define TURN_AROUND_MAP 110
-struct drive_timer {
- int hour;
- int min;
- int sec;
- int msec;
-};
-
typedef struct
{
int name;
@@ -37,7 +30,7 @@
car_sensor_value_t ReadCarSensorValue(int name);
void CrossRoadCallback(int road, int stop_line, int active, const car_model *car);
-void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
+
void InitRoadExam(road_exam_map &RoadMap);
void TerminateRoadExam(void);
void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
--
Gitblit v1.8.0