From 611f51ed073de48e83f46d47c82cd5ebe61258d7 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 16 九月 2020 18:14:27 +0800
Subject: [PATCH] 加入地图标定模式
---
lib/src/main/cpp/master/comm_if.h | 14 +
lib/src/main/cpp/test_items2/road_exam.h | 3
lib/src/main/cpp/driver_test.cpp | 45 ++
lib/src/main/cpp/master/comm_if.cpp | 154 ++++++++--
lib/src/main/cpp/native-lib.cpp | 4
lib/src/main/cpp/test_items2/through_something.cpp | 113 ++++---
lib/src/main/cpp/test_items2/road_exam.cpp | 440 +++++++++++++++++++------------
lib/src/main/cpp/rtk_module/rtk.cpp | 2
lib/src/main/cpp/driver_test.h | 30 +
9 files changed, 524 insertions(+), 281 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index e15aaa6..e596990 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -54,7 +54,8 @@
enum {
TEST_TYPE_AREA = 2,
TEST_TYPE_ROAD_DUMMY_LIGHT,
- TEST_TYPE_ROAD_TRUE_LIGHT
+ TEST_TYPE_ROAD_TRUE_LIGHT,
+ TEST_TYPE_ROAD_CALIBRATE
};
static bool ExamStart = false;
@@ -109,7 +110,8 @@
RoadMap.roads.clear();
RoadMap.specialAreas.clear();
- RoadMap.triggerLines.clear();
+ RoadMap.forbidLines.clear();
+ RoadMap.examScheme.clear();
CarSensorInit();
@@ -192,18 +194,22 @@
vector<road_t>().swap(RoadMap.roads);
vector<special_area_t>().swap(RoadMap.specialAreas);
- vector<trigger_line_t>().swap(RoadMap.triggerLines);
+ vector<forbid_line_t>().swap(RoadMap.forbidLines);
+ vector<scheme_t>().swap(RoadMap.examScheme);
}
-void SetRoadMap(road_exam_map &map)
+void SetRoadMap(road_exam_map &map, vector<scheme_t> &scheme)
{
if (ExamStart) return;
RoadMap.roads.assign(map.roads.begin(), map.roads.end());
RoadMap.specialAreas.assign(map.specialAreas.begin(), map.specialAreas.end());
- RoadMap.triggerLines.assign(map.triggerLines.begin(), map.triggerLines.end());
+// RoadMap.triggerLines.assign(map.triggerLines.begin(), map.triggerLines.end());
RoadMap.forbidLines.assign(map.forbidLines.begin(), map.forbidLines.end());
- DEBUG("寰楀埌鏂扮殑璺�冨湴鍥� 璺暟閲� %d 鐗规畩鍖哄煙鏁伴噺 %d 瑙﹀彂绾挎暟閲� %d 鍏朵粬绂佹绾挎暟閲� %d", RoadMap.roads.size(), RoadMap.specialAreas.size(), RoadMap.triggerLines.size(), RoadMap.forbidLines.size());
+
+ RoadMap.examScheme.assign(scheme.begin(), scheme.end());
+
+ DEBUG("寰楀埌鏂扮殑璺�冨湴鍥� 璺暟閲� %d 鐗规畩鍖哄煙鏁伴噺 %d 鍏朵粬绂佹绾挎暟閲� %d 椤圭洰鏁伴噺 %d", RoadMap.roads.size(), RoadMap.specialAreas.size(), RoadMap.forbidLines.size(), RoadMap.examScheme.size());
}
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
@@ -402,12 +408,18 @@
if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
exam_dummy_light = 0; //0
+ RoadMap.calibrate = 0;
}
if (type == TEST_TYPE_ROAD_TRUE_LIGHT) {
+ RoadMap.calibrate = 0;
InitRoadExam(RoadMap);
}
if (type == TEST_TYPE_AREA) {
InitAreaExam();
+ }
+ if (type == TEST_TYPE_ROAD_CALIBRATE) {
+ RoadMap.calibrate = 1;
+ InitRoadExam(RoadMap);
}
}
MA_SendExamStatus(1, 0);
@@ -865,6 +877,27 @@
return true;
}
+void RoadChange(int road, int status)
+{
+ struct roadStatusBrief brief;
+
+ brief.road_id = road;
+ brief.status = status;
+
+ MA_SendRoadStatus(&brief);
+}
+
+void CrossingChange(int road, int crossing, int status)
+{
+ struct crossingStatusBrief brief;
+
+ brief.road_id = road;
+ brief.crossing_index = crossing;
+ brief.status = status;
+
+ MA_SendCrossingStatus(&brief);
+}
+
void AddExamFault(int wrong, const struct RtkTime *rtkTime)
{
struct ExamFault fault;
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 524fccc..d19c35b 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -137,16 +137,14 @@
} separate_t;
typedef struct {
- int active;
- string tts;
bool stopFlag;
Line line;
+ bool centrePointValid;
PointF centrePoint;
} stop_line_t;
typedef struct {
int id;
- Line startLine;
// Line stopLine;
// int active; // 鍒拌揪璺彛灏鹃儴鐨勮杩涙柟鍚�
bool activeBreak; // 璺彛鍒硅溅鍑忛��
@@ -175,15 +173,9 @@
} special_area_t;
typedef struct {
- int id;
int road;
int active;
- int time; // 椤圭洰鏈�澶у畬鎴愭椂闂�
- int distance; // 椤圭洰鏈�澶у畬鎴愯窛绂�
- string tts;
- bool cmp;
std::vector<PointF> points;
- std::vector<PointF> leftPoints; // 瀵瑰簲鍒伴亾璺乏渚х殑鐐�
} trigger_line_t;
typedef struct {
@@ -192,11 +184,25 @@
std::vector<PointF> points;
} forbid_line_t;
+typedef struct {
+ int road_id;
+ int index;
+ int active;
+} crossing_active_t;
+
+typedef struct {
+ std::string name;
+ std::vector<crossing_active_t> crossingActive;
+ std::vector<trigger_line_t> triggerLines;
+} scheme_t;
+
struct road_exam_map {
+ int calibrate;
std::vector<road_t> roads;
std::vector<special_area_t> specialAreas;
- std::vector<trigger_line_t> triggerLines;
+// std::vector<trigger_line_t> triggerLines;
std::vector<forbid_line_t> forbidLines;
+ std::vector<scheme_t> examScheme;
};
struct area_exam_map {
@@ -218,7 +224,7 @@
void CleanRoadMap(void);
-void SetRoadMap(road_exam_map &map);
+void SetRoadMap(road_exam_map &map, vector<scheme_t> &scheme);
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
@@ -228,6 +234,8 @@
void UpdateRTKInfo(const rtk_info *s);
void AddExamFault(int wrong, const struct RtkTime *rtkTime);
+void RoadChange(int road, int status);
+void CrossingChange(int road, int crossing, int status);
void SystemShutdown(int event, int timeout);
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index c81177e..0468b02 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -64,6 +64,9 @@
#define ID_SM_DISTANCE 0x0020
#define ID_SM_CARSENSOR 0x0013
+#define ID_MS_ROAD_BRIEF 0x0015 // 绉戠洰涓夐」鐩爣瀹氾紝杩涘嚭鏌愭潯璺殑鎻愮ず
+#define ID_MS_CROSSING_BRIEF 0x0016 // 绉戠洰涓夐」鐩爣瀹氾紝鎺ヨ繎/椹剁鏌愬悇璺彛鐨勬彁绀�
+
#define MA_OUT_GPS_BRIEF 0x0001
#define MA_OUT_RTK_BRIEF 0x0002
#define MA_OUT_CAR_BRIEF 0x0004
@@ -438,6 +441,38 @@
writer.EndObject();
SendMsgToMainProcIndep(ID_SM_DISTANCE, sb.GetString());
+}
+
+void MA_SendRoadStatus(const struct roadStatusBrief *brief)
+{
+ StringBuffer sb;
+ Writer<StringBuffer> writer(sb);
+
+ writer.StartObject();
+ writer.Key("road_id");
+ writer.Int(brief->road_id);
+ writer.Key("status");
+ writer.Int(brief->status);
+ writer.EndObject();
+
+ SendMsgToMainProcIndep(ID_MS_ROAD_BRIEF, sb.GetString());
+}
+
+void MA_SendCrossingStatus(const struct crossingStatusBrief *brief)
+{
+ StringBuffer sb;
+ Writer<StringBuffer> writer(sb);
+
+ writer.StartObject();
+ writer.Key("road_id");
+ writer.Int(brief->road_id);
+ writer.Key("crossing_index");
+ writer.Int(brief->crossing_index);
+ writer.Key("status");
+ writer.Int(brief->status);
+ writer.EndObject();
+
+ SendMsgToMainProcIndep(ID_MS_CROSSING_BRIEF, sb.GetString());
}
void MA_SendExamWrong(vector<ExamFault> &ExamFaultList)
@@ -844,10 +879,6 @@
road_exam_map map;
- map.roads.clear();
- map.specialAreas.clear();
- map.triggerLines.clear();
-
if (doc.HasMember("road")) {
const Value &a = doc["road"];
@@ -860,23 +891,6 @@
DEBUG("璺痠d %d", s.GetInt());
road.id = s.GetInt();
}
- if (itr->HasMember("start_line")) {
- const Value &a2 = (*itr)["start_line"];
-
- PointF p1, p2;
- int n = 0;
-
- for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2, ++n) {
- DEBUG("璧峰绾� %d", (*itr2).GetInt());
- if (n == 0) {
- p1 = mapPoints[(*itr2).GetInt()];
- } else if (n == 1) {
- p2 = mapPoints[(*itr2).GetInt()];
- }
- }
-
- MakeLine(&road.startLine, &p1, &p2);
- }
if (itr->HasMember("crossing")) {
const Value &a2 = (*itr)["crossing"];
@@ -887,20 +901,12 @@
if (!itr2->IsObject()) {
break;
}
- if (itr2->HasMember("active")) {
- const Value &s = (*itr2)["active"];
- temp.active = s.GetInt();
- DEBUG("璺彛鍔ㄤ綔 %d", temp.active);
- }
if (itr2->HasMember("stop_flag")) {
const Value &s = (*itr2)["stop_flag"];
temp.stopFlag = s.GetInt();
DEBUG("璺彛鍋滆溅 %d", temp.stopFlag);
- }
- if (itr2->HasMember("tts")) {
- const Value &s = (*itr2)["tts"];
- temp.tts = s.GetString();
- DEBUG("璺彛鎻愮ず %s", temp.tts.c_str());
+ } else {
+ temp.stopFlag = false;
}
if (itr2->HasMember("line")) {
const Value &s = (*itr2)["line"];
@@ -920,8 +926,10 @@
}
if (itr2->HasMember("center_point")) {
const Value &s = (*itr2)["center_point"];
-
temp.centrePoint = mapPoints[s.GetInt()];
+ temp.centrePointValid = true;
+ } else {
+ temp.centrePointValid = false;
}
crossing.push_back(temp);
}
@@ -1092,7 +1100,7 @@
map.specialAreas.push_back(specialArea);
}
}
- if (doc.HasMember("trigger_line")) {
+ /*if (doc.HasMember("trigger_line")) {
const Value &a = doc["trigger_line"];
for (Value::ConstValueIterator itr = a.Begin();
@@ -1139,7 +1147,7 @@
map.triggerLines.push_back(trigger);
}
- }
+ }*/
if (doc.HasMember("red_line")) {
const Value &a = doc["red_line"];
@@ -1169,10 +1177,86 @@
}
}
+ vector<scheme_t> schemes;
+
+ if (doc.HasMember("scheme")) {
+ const Value &a = doc["scheme"];
+
+ for (Value::ConstValueIterator itr = a.Begin();
+ itr != a.End(); ++itr) {
+ scheme_t scheme;
+ if (itr->HasMember("name")) {
+ const Value &s = (*itr)["name"];
+ scheme.name = s.GetString();
+ }
+ if (itr->HasMember("crossing_active")) {
+ const Value &a2 = (*itr)["crossing_active"];
+
+ for (Value::ConstValueIterator itr2 = a2.Begin();
+ itr2 != a2.End(); ++itr2) {
+ crossing_active_t act;
+
+ if (itr2->HasMember("road")) {
+ const Value &s = (*itr2)["road"];
+ act.road_id = s.GetInt();
+ }
+ if (itr2->HasMember("idx")) {
+ const Value &s = (*itr2)["idx"];
+ act.index = s.GetInt();
+ }
+ if (itr2->HasMember("active")) {
+ const Value &s = (*itr2)["active"];
+ act.active = s.GetInt();
+ }
+
+ scheme.crossingActive.push_back(act);
+ }
+ }
+ if (itr->HasMember("trigger_line")) {
+ const Value &a2 = (*itr)["trigger_line"];
+
+ for (Value::ConstValueIterator itr2 = a2.Begin();
+ itr2 != a2.End(); ++itr2) {
+ trigger_line_t ins;
+
+ if (itr2->HasMember("x_y")) {
+ const Value &a3 = (*itr2)["x_y"];
+ if (a3.IsArray()) {
+ PointF point;
+ int n = 0;
+ for (Value::ConstValueIterator itr3 = a3.Begin();
+ itr3 != a3.End(); ++itr3) {
+ if (n == 0) {
+ point.X = (*itr3).GetDouble();
+ n = 1;
+ } else if (n == 1) {
+ point.Y = (*itr3).GetDouble();
+ ins.points.push_back(point);
+ n = 0;
+ }
+ }
+ }
+ }
+ if (itr2->HasMember("road")) {
+ const Value &s = (*itr2)["road"];
+ ins.road = s.GetInt();
+ }
+ if (itr2->HasMember("type")) {
+ const Value &s = (*itr2)["type"];
+ ins.active = s.GetInt();
+ }
+
+ scheme.triggerLines.push_back(ins);
+ }
+ }
+ schemes.push_back(scheme);
+ }
+ }
+
DEBUG("鍦板浘瑙ｆ瀽瀹屾瘯");
CleanRoadMap();
- SetRoadMap(map);
+ SetRoadMap(map, schemes);
} else {
DEBUG("############## 鍦板浘瑙ｆ瀽鍑洪敊###################");
}
diff --git a/lib/src/main/cpp/master/comm_if.h b/lib/src/main/cpp/master/comm_if.h
index d06868d..a5355e0 100644
--- a/lib/src/main/cpp/master/comm_if.h
+++ b/lib/src/main/cpp/master/comm_if.h
@@ -104,6 +104,17 @@
int pointNum;
};
+struct roadStatusBrief {
+ int road_id;
+ int status;
+};
+
+struct crossingStatusBrief {
+ int road_id;
+ int crossing_index;
+ int status;
+};
+
void MA_MainProcMsgEntry(int cmd, const char *value);
void MA_MainProcBinMsgEntry(int cmd, const uint8_t *value, int length);
@@ -131,4 +142,7 @@
void MA_SendDistance(double l, double r);
+void MA_SendRoadStatus(const struct roadStatusBrief *brief);
+void MA_SendCrossingStatus(const struct crossingStatusBrief *brief);
+
#endif //MYAPPLICATION2_COMM_IF_H
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index 4738ab8..b8e44e1 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/lib/src/main/cpp/native-lib.cpp
@@ -27,8 +27,8 @@
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.7";
-const int VIRTUAL_RTK_PORT = 9001;
+const char *VIRTUAL_RTK_IP = "192.168.16.100";
+const int VIRTUAL_RTK_PORT = 9002;
static pthread_mutex_t tts_mutex = PTHREAD_MUTEX_INITIALIZER;
diff --git a/lib/src/main/cpp/rtk_module/rtk.cpp b/lib/src/main/cpp/rtk_module/rtk.cpp
index a5c75d3..b7afde4 100644
--- a/lib/src/main/cpp/rtk_module/rtk.cpp
+++ b/lib/src/main/cpp/rtk_module/rtk.cpp
@@ -214,7 +214,7 @@
}*/
if (RxBufLen > 0) {
-#if 1
+#if 0
const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
if(ptr != RxBuf) {
memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 53eea6a..0ade5a9 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -155,7 +155,7 @@
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
static bool StopOnRedArea(road_exam_map &RoadMap, const car_model *car);
-static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);
+static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);
static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
@@ -1109,7 +1109,7 @@
int stop_line_index;
PointF nearbyStopPoint;
- NearbyCrossingGuide(stop_line_index, roadIndex, RoadMap.roads[roadIndex], car);
+ NearbyCrossingGuide(RoadMap, stop_line_index, roadIndex, RoadMap.roads[roadIndex], car);
nearbyStopPoint.X = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.X1;
nearbyStopPoint.Y = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.Y1;
@@ -1159,6 +1159,20 @@
return rec;
}
+int ExamSchemeCrossing(road_exam_map &RoadMap, int roadIndex, int crossIndex)
+{
+ if (roadIndex < 0 || roadIndex >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0) {
+ return -1;
+ }
+ for (int i = 0; i < RoadMap.examScheme[0].crossingActive.size(); ++i) {
+ if (RoadMap.examScheme[0].crossingActive[i].road_id == RoadMap.roads[roadIndex].id &&
+ RoadMap.examScheme[0].crossingActive[i].index == crossIndex) {
+ return RoadMap.examScheme[0].crossingActive[i].active;
+ }
+ }
+ return -1;
+}
+
static void HintCrossing(road_exam_map &RoadMap, int roadIndex, int stopIndex, double distance) {
if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
return;
@@ -1166,18 +1180,47 @@
if (distance > 5 && distance < 70) {
// 鎻愮ず璺彛鎬庝箞璧�
if (GetCrossingStatus(roadIndex, stopIndex) == CROSSING_NOT_HINT) {
- if (!RoadMap.roads[roadIndex].stopLine[stopIndex].tts.empty()) {
- DEBUG("璺彛鎻愮ず %s", RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str());
- PlayTTS(RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str(), NULL);
+ // 椤圭洰鏍囧畾妯″紡涓嬶紝杩欓噷鎻愮ず鏍囧畾鏈矾鍙ｅ姩浣�
+ if (RoadMap.calibrate) {
+ CrossingChange(RoadMap.roads[roadIndex].id, stopIndex, 1);
+ } else {
+ int act = ExamSchemeCrossing(RoadMap, roadIndex, stopIndex);
+
+ switch (act) {
+ case ROAD_ACTIVE_FORWARD:
+ DEBUG("璺彛鎻愮ず 鐩磋");
+ PlayTTS("鍓嶆柟璺彛璇风洿琛�", NULL);
+ break;
+ case ROAD_ACTIVE_TURN_LEFT:
+ DEBUG("璺彛鎻愮ず 宸﹁浆");
+ PlayTTS("鍓嶆柟璺彛璇峰乏杞�", NULL);
+ break;
+ case ROAD_ACTIVE_TURN_RIGHT:
+ DEBUG("璺彛鎻愮ず 鍙宠浆");
+ PlayTTS("鍓嶆柟璺彛璇峰彸杞�", NULL);
+ break;
+ case ROAD_ACTIVE_TURN_BACKWARD:
+ DEBUG("璺彛鎻愮ず 鎺夊ご");
+ PlayTTS("鍓嶆柟璺彛璇锋帀澶�", NULL);
+ break;
+ default:
+ DEBUG("璺彛鎻愮ず 鏈厤缃�");
+ PlayTTS("鍓嶆柟璺彛鍚暀缁冩寚浠�", NULL);
+ break;
+ }
}
ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_HAS_HINT);
}
} else if (distance > 75 && GetCrossingStatus(roadIndex, stopIndex) != CROSSING_NOT_HINT) {
ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_NOT_HINT);
+
+ if (RoadMap.calibrate) {
+ CrossingChange(RoadMap.roads[roadIndex].id, stopIndex, 0);
+ }
}
}
-static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car)
+static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car)
{
int guide = 0, stopLine = 0;
double distance;
@@ -1203,7 +1246,7 @@
struct nearby_crossing temp;
temp.stopLine = i;
- temp.guide = road.stopLine[i].active;
+ temp.guide = ExamSchemeCrossing(RoadMap, roadIndex, i);
temp.distance = distance;
set.push_back(temp);
@@ -1253,157 +1296,169 @@
turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
}
- // 鍒硅溅鎻愮ず闊�
- RingBreak();
+ if (RoadMap.calibrate == 0) {
+ // 鍒硅溅鎻愮ず闊�
+ RingBreak();
- // 瓒呴�熸娴�
- if (speed > MAX_SPEED) {
- if (!occurOverSpeed) {
- occurOverSpeed = true;
- // 瓒呴�燂紝涓嶅悎鏍�
- DEBUG("瓒呴�� %f", ConvertMs2KMh(speed));
- AddExamFault(10, rtkTime);
+ // 瓒呴�熸娴�
+ if (speed > MAX_SPEED) {
+ if (!occurOverSpeed) {
+ occurOverSpeed = true;
+ // 瓒呴�燂紝涓嶅悎鏍�
+ DEBUG("瓒呴�� %f", ConvertMs2KMh(speed));
+ AddExamFault(10, rtkTime);
+ }
+ } else if (speed < DEC_MAX_SPEED) {
+ occurOverSpeed = false;
}
- } else if (speed < DEC_MAX_SPEED ) {
- occurOverSpeed = false;
- }
- // 鍓埞杞︽娴�
- if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) {
- // 鍓埞杞﹁俯涓嬶紝涓嶅悎鏍�
- if (!occurSecondBreak) {
- DEBUG("鍓埞杞﹀姩浣滀簡");
- occurSecondBreak = true;
- AddExamFault(17, rtkTime);
+ // 鍓埞杞︽娴�
+ if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) {
+ // 鍓埞杞﹁俯涓嬶紝涓嶅悎鏍�
+ if (!occurSecondBreak) {
+ DEBUG("鍓埞杞﹀姩浣滀簡");
+ occurSecondBreak = true;
+ AddExamFault(17, rtkTime);
+ }
+ } else {
+ occurSecondBreak = false;
}
- } else {
- occurSecondBreak = false;
- }
- // 鎸′綅鍖归厤妫�娴�
- bool currGearError = false;
- bool currGearNSlide = false;
+ // 鎸′綅鍖归厤妫�娴�
+ bool currGearError = false;
+ bool currGearNSlide = false;
- switch (ReadCarStatus(GEAR)) {
- case GEAR_N:
- if (moveDirect != 0) {
- // 绌烘。婊戣
- currGearNSlide = true;
- }
- break;
- case GEAR_1:
- if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) {
- currGearError = true;
- }
- break;
- case GEAR_2:
- if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) {
- currGearError = true;
- }
- break;
- case GEAR_3:
- if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) {
- currGearError = true;
- }
- break;
- case GEAR_4:
- if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) {
- currGearError = true;
- }
- break;
- case GEAR_5:
- if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) {
- currGearError = true;
- }
- break;
- default:break;
- }
- // 绌烘。婊戣瓒呮椂
- if (currGearNSlide) {
- if (GearNSlideStatus == 0) {
- DEBUG("妫�娴嬪埌绌烘尅婊戣");
- GearNSlideStatus = 1;
- gearNSlideTimePoint = *rtkTime;
+ switch (ReadCarStatus(GEAR)) {
+ case GEAR_N:
+ if (moveDirect != 0) {
+ // 绌烘。婊戣
+ currGearNSlide = true;
+ }
+ break;
+ case GEAR_1:
+ if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] ||
+ ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) {
+ currGearError = true;
+ }
+ break;
+ case GEAR_2:
+ if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] ||
+ ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) {
+ currGearError = true;
+ }
+ break;
+ case GEAR_3:
+ if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] ||
+ ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) {
+ currGearError = true;
+ }
+ break;
+ case GEAR_4:
+ if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] ||
+ ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) {
+ currGearError = true;
+ }
+ break;
+ case GEAR_5:
+ if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] ||
+ ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) {
+ currGearError = true;
+ }
+ break;
+ default:
+ break;
}
- 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 (currGearError && prevGearError) {
- DEBUG("鎸′綅閿欒澧炲姞 %ld姣 褰撳墠鎸′綅 %d 鏃堕�� %f", TimeGetDiff(rtkTime, &gearErrorTimePoint), ReadCarStatus(GEAR), ConvertMs2KMh(speed));
- gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
- }
- if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
- // 绱15绉掞紝鎸′綅-杞﹂�熶笉鍖归厤锛屼笉鍚堟牸
- DEBUG("鎸′綅閿欒瓒呰繃15绉�");
- AddExamFault(6, rtkTime);
- gearErrorTime = 0;
- }
-
- prevGearError = currGearError;
- if (prevGearError) {
- gearErrorTimePoint = *rtkTime;
- }
-
- // 璧锋鍚庢粦
- if (moveDirect != prevMoveDirect) {
- if (moveDirect == 0) {
- stopTimepoint = *rtkTime;
- StopCarOnRedArea = false;
-
- DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
-
- if (slideNormalDistance) {
- // 鍚庢粦锛屾墸10鍒�
- AddExamFault(18, rtkTime);
- DEBUG("鍚庢粦瓒呰繃10鍘樼背, 浣嗕笉瓒呰繃30鍘樼背");
+ // 绌烘。婊戣瓒呮椂
+ if (currGearNSlide) {
+ if (GearNSlideStatus == 0) {
+ DEBUG("妫�娴嬪埌绌烘尅婊戣");
+ GearNSlideStatus = 1;
+ gearNSlideTimePoint = *rtkTime;
}
-
- slideNormalDistance = false;
- slideLongDistance = false;
- occurSlide = false;
- } else if (moveDirect == -1 && prevMoveDirect == 0) {
- DEBUG("寮�濮嬪悗婊�");
- stopPoint = car->basePoint;
- occurSlide = true;
- }
- prevMoveDirect = moveDirect;
- } else if (moveDirect == 0) {
- // 鎸佺画鍋滆溅
- if (TimeGetDiff(rtkTime, &stopTimepoint) >= STOP_CAR_TIME && !StopCarOnRedArea && StopOnRedArea(RoadMap, car)) {
- // 鍋滆溅瓒�2绉掞紝鍋滃湪绾㈠尯锛屼笉鍚堟牸
- AddExamFault(16, rtkTime);
- DEBUG("绂佸仠鍖哄仠杞�");
- StopCarOnRedArea = true;
- }
- } else if (moveDirect == -1) {
- // 鎸佺画鍚庢粦
- if (occurSlide) {
- double slideDistance = DistanceOf(stopPoint, car->basePoint);
-
- if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
- slideNormalDistance = true;
+ 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 (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) {
- // 鍚庢粦瓒呰繃30鍘樼背, 涓嶅悎鏍�
- AddExamFault(5, rtkTime);
- DEBUG("鍚庢粦瓒呰繃30鍘樼背");
- slideLongDistance = true;
+ // 鎸′綅涓嶅尮閰嶈秴鏃�
+ if (currGearError && prevGearError) {
+ DEBUG("鎸′綅閿欒澧炲姞 %ld姣 褰撳墠鎸′綅 %d 鏃堕�� %f", TimeGetDiff(rtkTime, &gearErrorTimePoint),
+ ReadCarStatus(GEAR), ConvertMs2KMh(speed));
+ gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
+ }
+ if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
+ // 绱15绉掞紝鎸′綅-杞﹂�熶笉鍖归厤锛屼笉鍚堟牸
+ DEBUG("鎸′綅閿欒瓒呰繃15绉�");
+ AddExamFault(6, rtkTime);
+ gearErrorTime = 0;
+ }
+
+ prevGearError = currGearError;
+ if (prevGearError) {
+ gearErrorTimePoint = *rtkTime;
+ }
+
+ // 璧锋鍚庢粦
+ if (moveDirect != prevMoveDirect) {
+ if (moveDirect == 0) {
+ stopTimepoint = *rtkTime;
+ StopCarOnRedArea = false;
+
+ DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD,
+ rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+
+ if (slideNormalDistance) {
+ // 鍚庢粦锛屾墸10鍒�
+ AddExamFault(18, rtkTime);
+ DEBUG("鍚庢粦瓒呰繃10鍘樼背, 浣嗕笉瓒呰繃30鍘樼背");
+ }
+
slideNormalDistance = false;
+ slideLongDistance = false;
occurSlide = false;
+ } else if (moveDirect == -1 && prevMoveDirect == 0) {
+ DEBUG("寮�濮嬪悗婊�");
+ stopPoint = car->basePoint;
+ occurSlide = true;
}
+ prevMoveDirect = moveDirect;
+ } else if (moveDirect == 0) {
+ // 鎸佺画鍋滆溅
+ if (TimeGetDiff(rtkTime, &stopTimepoint) >= STOP_CAR_TIME && !StopCarOnRedArea &&
+ StopOnRedArea(RoadMap, car)) {
+ // 鍋滆溅瓒�2绉掞紝鍋滃湪绾㈠尯锛屼笉鍚堟牸
+ AddExamFault(16, rtkTime);
+ DEBUG("绂佸仠鍖哄仠杞�");
+ StopCarOnRedArea = true;
+ }
+ } else if (moveDirect == -1) {
+ // 鎸佺画鍚庢粦
+ if (occurSlide) {
+ double slideDistance = DistanceOf(stopPoint, car->basePoint);
+
+ if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
+ slideNormalDistance = true;
+ }
+
+ if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) {
+ // 鍚庢粦瓒呰繃30鍘樼背, 涓嶅悎鏍�
+ AddExamFault(5, rtkTime);
+ DEBUG("鍚庢粦瓒呰繃30鍘樼背");
+ slideLongDistance = true;
+ slideNormalDistance = false;
+ occurSlide = false;
+ }
+ }
+ } else {
+ // 鍓嶈繘
}
- } else {
- // 鍓嶈繘
}
// 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐�
@@ -1415,6 +1470,15 @@
if (oldid >= 0) {
ResetCrossingStatus(oldid);
ResetErrorLaneRpt(oldid);
+
+ }
+
+ if (RoadMap.calibrate) {
+ if (currExamMapIndex < 0 && oldid >= 0) {
+ RoadChange(oldid, 0);
+ } else if (currExamMapIndex >= 0) {
+ RoadChange(currExamMapIndex, 1);
+ }
}
Lane.guide = 0;
}
@@ -1426,41 +1490,44 @@
}
ExitTarget(RoadMap, car, CarModelList, rtkTime);
- oldid = CrashLineType;
- // 妫�娴嬪帇绾跨姸鎬�
- DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);
+ if (RoadMap.calibrate == 0) {
+ oldid = CrashLineType;
+ // 妫�娴嬪帇绾跨姸鎬�
+ DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);
- if (oldid != CrashLineType) {
- DEBUG("鍘嬬嚎绫诲瀷鍒囨崲 %d", CrashLineType);
- }
+ if (oldid != CrashLineType) {
+ DEBUG("鍘嬬嚎绫诲瀷鍒囨崲 %d", CrashLineType);
+ }
-
- oldid = Lane.guide;
- DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
+ oldid = Lane.guide;
+ DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
// 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 (Lane.guide > 0 && currExamMapIndex >= 0) {
+ int stop_line_index;
+ int act = NearbyCrossingGuide(RoadMap, stop_line_index, currExamMapIndex,
+ RoadMap.roads[currExamMapIndex], car);
- if (act != 0 && !(act & Lane.guide)) {
- if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
- DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒ %d: %d 鏈熸湜 = %d guide = %d", currExamMapIndex, stop_line_index, act, Lane.guide);
- AddExamFault(9, rtkTime);
- SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
+ if (act != 0 && !(act & Lane.guide)) {
+ if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
+ DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒ %d: %d 鏈熸湜 = %d guide = %d", currExamMapIndex,
+ stop_line_index, act, Lane.guide);
+ AddExamFault(9, rtkTime);
+ SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
+ }
}
}
- }
- if (Lane.guide != oldid) {
- DEBUG("瀵煎悜绫诲瀷鍒囨崲 %d", Lane.guide);
+ if (Lane.guide != oldid) {
+ DEBUG("瀵煎悜绫诲瀷鍒囨崲 %d", Lane.guide);
+ }
}
if (currExamMapIndex >= 0 && Lane.guide == 0) {
BigStraightRoadFree = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);
road_end_point_t ep = NearbyRoadEndPoint(currExamMapIndex, RoadMap, car);
-
+ // 鎻愮ず璺彛鎬庝箞璧�
HintCrossing(RoadMap, ep.road_index, ep.stop_line_index, ep.distance);
double freeSepDis = SeparateLength(RoadMap, Lane, car);
@@ -1476,10 +1543,15 @@
}
// 棰濆鐨勮浆鍚戞娴�
- DetectTurn(car, moveDirect, rtkTime);
- ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, TargetFree > RoadCrossingFree? RoadCrossingFree : TargetFree);
- ItemExam2(RoadMap, currExamMapIndex, car, CarModelList);
+ if (RoadMap.calibrate == 0) {
+ 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)
@@ -1605,8 +1677,11 @@
}
}
-void CrossRoadCallback(int road, int stop_line, int active, const car_model *car)
+void CrossRoadCallback(road_exam_map &RoadMap, int road, int stop_line, int active, const car_model *car)
{
+ if (RoadMap.calibrate) {
+ CrossingChange(RoadMap.roads[road].id, stop_line, 0);
+ }
SetErrorLaneRpt(road, stop_line, false);
if (active != ROAD_ACTIVE_FORWARD) {
ResetTurnDetect(car);
@@ -1823,12 +1898,12 @@
static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
- if (index < 0 || index >= RoadMap.roads.size())
+ if (index < 0 || index >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0)
return -1;
- for (int i = 0; i < RoadMap.triggerLines.size(); ++i) {
- if (RoadMap.triggerLines[i].road == RoadMap.roads[index].id) {
- Line triggerLine;
+ for (int j = 0; j < RoadMap.examScheme[0].triggerLines.size(); ++j) {
+ if (RoadMap.examScheme[0].triggerLines[j].road == RoadMap.roads[index].id) {
+ /*Line triggerLine;
PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[0]);
@@ -1837,6 +1912,25 @@
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].active;
+ }*/
+
+ vector<double> vec;
+
+ PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
+ car->carXY[car->axial[AXIAL_FRONT]]);
+ PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
+ RoadMap.examScheme[0].triggerLines[j].points[0]);
+
+ vec.push_back(DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
+ RoadMap.examScheme[0].triggerLines[j].points[0]));
+ vec.push_back(DistanceOf(p2, RoadMap.examScheme[0].triggerLines[j].points[0]));
+ vec.push_back(DistanceOf(p1, car->carXY[car->axial[AXIAL_FRONT]]));
+
+ sort(vec.begin(), vec.end());
+
+ if (fabs(vec[0] + vec[1] - vec[2]) < 0.1) {
+ DEBUG("瑙﹀彂椤圭洰 %d", RoadMap.examScheme[0].triggerLines[j].active);
+ return RoadMap.examScheme[0].triggerLines[j].active;
}
}
}
diff --git a/lib/src/main/cpp/test_items2/road_exam.h b/lib/src/main/cpp/test_items2/road_exam.h
index 39626b1..eeea8e7 100644
--- a/lib/src/main/cpp/test_items2/road_exam.h
+++ b/lib/src/main/cpp/test_items2/road_exam.h
@@ -29,7 +29,8 @@
car_sensor_value_t ReadCarSensorValue(int name);
-void CrossRoadCallback(int road, int stop_line, int active, const car_model *car);
+void CrossRoadCallback(road_exam_map &RoadMap, int road, int stop_line, int active, const car_model *car);
+int ExamSchemeCrossing(road_exam_map &RoadMap, int roadIndex, int crossIndex);
void InitRoadExam(road_exam_map &RoadMap);
void TerminateRoadExam(void);
diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp
index 09f6318..1000b06 100644
--- a/lib/src/main/cpp/test_items2/through_something.cpp
+++ b/lib/src/main/cpp/test_items2/through_something.cpp
@@ -104,6 +104,10 @@
RemoveTargetReduceRec(TargetReduceRec, key);
}
}
+
+ if (RoadMap.calibrate == 1)
+ goto GET_DISTANCE_OF_NEARBY_TARGET;
+
// 浜鸿閬撱�佸叕浜ょ珯銆佸鏍�
for (int i = 0; i < RoadMap.specialAreas.size(); i++) {
if (RoadMap.specialAreas[i].type == GRID_AREA || RoadMap.specialAreas[i].road != RoadMap.roads[roadIndex].id)
@@ -165,6 +169,7 @@
}
}
+ GET_DISTANCE_OF_NEARBY_TARGET:
if (nearbyTarget.size() > 0) {
sort(nearbyTarget.begin(), nearbyTarget.end());
return nearbyTarget[0];
@@ -182,69 +187,73 @@
if (CrashTheLine(RoadMap.roads[road].stopLine[x].line, car, CarModelList)) {
DEBUG("瓒婅繃璺彛 road %d 璺彛 %d", road, x);
- CrossRoadCallback(road, x, RoadMap.roads[road].stopLine[x].active, car);
+ CrossRoadCallback(RoadMap, road, x, ExamSchemeCrossing(RoadMap, road, x), car);
- if (!(it->second & REDUCE_SPEED)) {
- // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍�
- DEBUG("涓嶆寜瑙勫畾鍑忛��");
+ if (RoadMap.calibrate == 0) {
+ if (!(it->second & REDUCE_SPEED)) {
+ // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍�
+ DEBUG("涓嶆寜瑙勫畾鍑忛��");
- if (RoadMap.roads[road].stopLine[x].active == ROAD_ACTIVE_FORWARD) {
- AddExamFault(41, rtkTime);
- } else if (RoadMap.roads[road].stopLine[x].active ==
- ROAD_ACTIVE_TURN_LEFT) {
- AddExamFault(43, rtkTime);
- } else if (RoadMap.roads[road].stopLine[x].active ==
- ROAD_ACTIVE_TURN_RIGHT) {
- AddExamFault(46, rtkTime);
+ if (ExamSchemeCrossing(RoadMap, road, x) == ROAD_ACTIVE_FORWARD) {
+ AddExamFault(41, rtkTime);
+ } else if (ExamSchemeCrossing(RoadMap, road, x) ==
+ ROAD_ACTIVE_TURN_LEFT) {
+ AddExamFault(43, rtkTime);
+ } else if (ExamSchemeCrossing(RoadMap, road, x) ==
+ ROAD_ACTIVE_TURN_RIGHT) {
+ AddExamFault(46, rtkTime);
+ }
+ }
+ if (!(it->second & STOP_CAR) && RoadMap.roads[road].stopLine[x].stopFlag) {
+ // 涓嶅仠杞︾灜鏈涳紝涓嶅悎鏍�
+ DEBUG("涓嶅仠杞︾灜鏈�");
+ if (ExamSchemeCrossing(RoadMap, road, x) == ROAD_ACTIVE_FORWARD) {
+ AddExamFault(42, rtkTime);
+ } else if (ExamSchemeCrossing(RoadMap, road, x) ==
+ ROAD_ACTIVE_TURN_LEFT) {
+ AddExamFault(44, rtkTime);
+ } else if (ExamSchemeCrossing(RoadMap, road, x) ==
+ ROAD_ACTIVE_TURN_RIGHT) {
+ AddExamFault(47, rtkTime);
+ }
}
}
- if (!(it->second & STOP_CAR) && RoadMap.roads[road].stopLine[x].stopFlag) {
- // 涓嶅仠杞︾灜鏈涳紝涓嶅悎鏍�
- DEBUG("涓嶅仠杞︾灜鏈�");
- if (RoadMap.roads[road].stopLine[x].active == ROAD_ACTIVE_FORWARD) {
- AddExamFault(42, rtkTime);
- } else if (RoadMap.roads[road].stopLine[x].active ==
- ROAD_ACTIVE_TURN_LEFT) {
- AddExamFault(44, rtkTime);
- } else if (RoadMap.roads[road].stopLine[x].active ==
- ROAD_ACTIVE_TURN_RIGHT) {
- AddExamFault(47, rtkTime);
- }
- }
-
RemoveTargetReduceRec(TargetReduceRec, it->first);
goto RECHECK;
}
}
+ if (RoadMap.calibrate == 1)
+ return;
+
RECHECK2:
for (auto it = TargetReduceRec2.begin(); it != TargetReduceRec2.end(); ++it) {
- int x = it->first;
- Line line;
- int roadIndex = 0;
+ int x = it->first;
+ Line line;
+ int roadIndex = 0;
- for (; roadIndex < RoadMap.roads.size(); ++roadIndex) {
- if (RoadMap.roads[roadIndex].id == RoadMap.specialAreas[x].road)
- break;
- }
-
- PointF point2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge,
- RoadMap.specialAreas[x].area[0]);
- MakeLine(&line, &RoadMap.specialAreas[x].area[0], &point2);
-
- if (CrashTheLine(line, car, CarModelList)) {
- if (RoadMap.specialAreas[x].type == ZEBRA_CROSSING &&
- !(it->second & REDUCE_SPEED)) {
- DEBUG("浜鸿閬� 涓嶆寜瑙勫畾鍑忛��");
- AddExamFault(48, rtkTime);
- }
- if (RoadMap.specialAreas[x].type == BUS_STATION_AREA &&
- !(it->second & REDUCE_SPEED)) {
- DEBUG("鍏氦绔� 涓嶆寜瑙勫畾鍑忛��");
- AddExamFault(50, rtkTime);
- }
- RemoveTargetReduceRec(TargetReduceRec2, it->first);
- goto RECHECK2;
- }
+ for (; roadIndex < RoadMap.roads.size(); ++roadIndex) {
+ if (RoadMap.roads[roadIndex].id == RoadMap.specialAreas[x].road)
+ break;
}
+
+ PointF point2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge,
+ RoadMap.specialAreas[x].area[0]);
+ MakeLine(&line, &RoadMap.specialAreas[x].area[0], &point2);
+
+ if (CrashTheLine(line, car, CarModelList)) {
+ if (RoadMap.specialAreas[x].type == ZEBRA_CROSSING &&
+ !(it->second & REDUCE_SPEED)) {
+ DEBUG("浜鸿閬� 涓嶆寜瑙勫畾鍑忛��");
+ AddExamFault(48, rtkTime);
+ }
+ if (RoadMap.specialAreas[x].type == BUS_STATION_AREA &&
+ !(it->second & REDUCE_SPEED)) {
+ DEBUG("鍏氦绔� 涓嶆寜瑙勫畾鍑忛��");
+ AddExamFault(50, rtkTime);
+ }
+ RemoveTargetReduceRec(TargetReduceRec2, it->first);
+ goto RECHECK2;
+ }
+ }
}
--
Gitblit v1.8.0