From 03ca970f73af542456326b2fa277090ce0ea532f Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期二, 11 八月 2020 18:33:51 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_common/odo_graph.cpp | 46 +++++
lib/src/main/cpp/master/comm_if.cpp | 115 ++++++--------
lib/src/main/cpp/test_items2/road_exam.cpp | 269 +++++++-------------------------
lib/src/main/cpp/CMakeLists.txt | 1
lib/src/main/cpp/driver_test.h | 2
lib/src/main/cpp/test_common/odo_graph.h | 12 +
6 files changed, 173 insertions(+), 272 deletions(-)
diff --git a/lib/src/main/cpp/CMakeLists.txt b/lib/src/main/cpp/CMakeLists.txt
index d6861f1..7fb8102 100644
--- a/lib/src/main/cpp/CMakeLists.txt
+++ b/lib/src/main/cpp/CMakeLists.txt
@@ -29,6 +29,7 @@
mcu/mcu_if.cpp
test_common/car_sensor.cpp
+ test_common/odo_graph.cpp
test_items/error_list.cpp
test_items/park_edge.cpp
test_items/park_bottom.cpp
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index c82fc11..7c9e0b3 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -128,8 +128,6 @@
typedef struct {
int character; // 灞炴�с�婂疄绾裤�佽櫄绾裤�佸崐瀹炲崐铏氱嚎銆�
- int left_lane_direct;
- int right_lane_direct;
std::vector<PointF> points;
} segment_t;
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index 57cd155..3712f33 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -808,23 +808,6 @@
MakeLine(&road.startLine, &p1, &p2);
}
- /*if (itr->HasMember("stop_line")) {
- const Value &a2 = (*itr)["stop_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.stopLine, &p1, &p2);
- }*/
if (itr->HasMember("crossing") && itr->IsArray()) {
const Value &a2 = (*itr)["crossing"];
@@ -838,14 +821,17 @@
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());
}
if (itr2->HasMember("line")) {
const Value &s = (*itr2)["line"];
@@ -870,20 +856,6 @@
road.stopLine.assign(crossing.begin(), crossing.end());
}
- if (itr->HasMember("active")) {
- const Value &s = (*itr)["active"];
- DEBUG("璺彛鍔ㄤ綔 %d", s.GetInt());
-
- road.active = s.GetInt();
- }
- if (itr->HasMember("tts")) {
- const Value &s = (*itr)["tts"];
- road.tts = s.GetString();
- }
- if (itr->HasMember("stop_flag")) {
- const Value &s = (*itr)["stop_flag"];
- road.stopFlag = s.GetInt();
- }
if (itr->HasMember("next_road")) {
const Value &s = (*itr)["next_road"];
road.targetRoad = s.GetInt();
@@ -947,49 +919,64 @@
DEBUG("娈垫暟閲� %d", a2.Size());
for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2) {
- DEBUG("\t绾挎暟閲� %d", (*itr2).Size());
separate_t sep;
+ if (!itr2->IsObject())
+ break;
+ if (itr2->HasMember("lane_guide") && itr2->IsArray()) {
+ const Value &a3 = (*itr2)["lane_guide"];
- for (Value::ConstValueIterator itr3 = (*itr2).Begin(); itr3 != (*itr2).End(); ++itr3) {
- DEBUG("\t\t鑺傛暟閲� %d", (*itr3).Size());
- vector<segment_t> sline;
+ for (Value::ConstValueIterator itr3 = a3.Begin(); itr3 != a3.End(); ++itr3) {
+ lane_direct_t temp;
- for (Value::ConstValueIterator itr4 = (*itr3).Begin(); itr4 != (*itr3).End(); ++itr4) {
- const Value &type = (*itr4)["type"];
- const Value &line = (*itr4)["line"];
+ if (itr3->HasMember("head_tail")) {
+ const Value &a4 = (*itr3)["head_tail"];
+ int n = 0;
- segment_t seg;
+ for (Value::ConstValueIterator itr4 = a4.Begin(); itr4 != a4.End(); ++itr4, ++n) {
+ if (n == 0)
+ temp.start = mapPoints[(*itr4).GetInt()];
+ else if (n == 1)
+ temp.end = mapPoints[(*itr4).GetInt()];
+ }
+ }
+ if (itr3->HasMember("guide")) {
+ const Value &a4 = (*itr3)["guide"];
- DEBUG("\t\t\t鑺傜被鍨� = %d", type.GetInt());
- seg.character = type.GetInt();
-
- if ((*itr4).HasMember("left_lane_direct")) {
- const Value &dir = (*itr4)["left_lane_direct"];
- DEBUG("\t\t\t宸﹁溅閬撴柟鍚� %d", dir.GetInt());
- seg.left_lane_direct = dir.GetInt();
- } else {
- seg.left_lane_direct = 0;
+ for (Value::ConstValueIterator itr4 = a4.Begin(); itr4 != a4.End(); ++itr4) {
+ temp.direct.push_back((*itr4).GetInt());
+ }
}
- if ((*itr4).HasMember("right_lane_direct")) {
- const Value &dir = (*itr4)["right_lane_direct"];
- DEBUG("\t\t\t鍙宠溅閬撴柟鍚� %d", dir.GetInt());
- seg.right_lane_direct = dir.GetInt();
- } else {
- seg.right_lane_direct = 0;
- }
-
- for (Value::ConstValueIterator itr5 = line.Begin(); itr5 != line.End(); ++itr5) {
- DEBUG("\t\t\t鐐� = %d", (*itr5).GetInt());
-
- seg.points.push_back(mapPoints[(*itr5).GetInt()]);
- }
-
- sline.push_back(seg);
+ sep.lane_direct.push_back(temp);
}
- sep.lines.push_back(sline);
}
+ if (itr2->HasMember("lane_line") && itr2->IsArray() ) {
+ const Value &a3 = (*itr2)["lane_line"];
+ DEBUG("\t绾挎暟閲� %d", a3.Size());
+ for (Value::ConstValueIterator itr3 = a3.Begin(); itr3 != a3.End(); ++itr3) {
+ DEBUG("\t\t鑺傛暟閲� %d", (*itr3).Size());
+ vector<segment_t> sline;
+ for (Value::ConstValueIterator itr4 = (*itr3).Begin(); itr4 != (*itr3).End(); ++itr4) {
+ const Value &type = (*itr4)["type"];
+ const Value &line = (*itr4)["line"];
+
+ segment_t seg;
+
+ DEBUG("\t\t\t鑺傜被鍨� = %d", type.GetInt());
+ seg.character = type.GetInt();
+
+ for (Value::ConstValueIterator itr5 = line.Begin(); itr5 != line.End(); ++itr5) {
+ DEBUG("\t\t\t鐐� = %d", (*itr5).GetInt());
+
+ seg.points.push_back(mapPoints[(*itr5).GetInt()]);
+ }
+
+ sline.push_back(seg);
+ }
+ sep.lines.push_back(sline);
+ }
+ }
road.separate.push_back(sep);
}
}
diff --git a/lib/src/main/cpp/test_common/odo_graph.cpp b/lib/src/main/cpp/test_common/odo_graph.cpp
new file mode 100644
index 0000000..1266a1b
--- /dev/null
+++ b/lib/src/main/cpp/test_common/odo_graph.cpp
@@ -0,0 +1,46 @@
+//
+// Created by YY on 2020/8/11.
+//
+
+#include "odo_graph.h"
+#include "../test_items2/road_exam.h"
+#include "../common/apptimer.h"
+#include "../utils/xconvert.h"
+
+static double odoGraph;
+static struct drive_timer odoTimer;
+static double odoPrevSpeed;
+static int odoCnt;
+
+void ResetOdo(void)
+{
+ odoCnt = 0;
+ odoGraph = 0;
+}
+
+double ReadOdo(void)
+{
+ return odoGraph;
+}
+
+void UpdataOdo(double speed, int moveDirect, const struct RtkTime *rtkTime) {
+ // 琛岄┒璺濈锛屼笉鍖呭惈鍊掕溅
+ if (odoCnt == 0 && moveDirect == 1) {
+ odoPrevSpeed = speed;
+ odoCnt = 1;
+ Rtk2DriveTimer(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);
+ if (tm >= D_SEC(1)) {
+ odoGraph += ((double) tm) * (odoPrevSpeed + speed) / 2.0 / 1000.0;
+ Rtk2DriveTimer(odoTimer, rtkTime);
+ odoPrevSpeed = speed;
+ }
+ } else {
+ odoCnt = 0;
+ }
+ }
+}
\ No newline at end of file
diff --git a/lib/src/main/cpp/test_common/odo_graph.h b/lib/src/main/cpp/test_common/odo_graph.h
new file mode 100644
index 0000000..37f548a
--- /dev/null
+++ b/lib/src/main/cpp/test_common/odo_graph.h
@@ -0,0 +1,12 @@
+//
+// Created by YY on 2020/8/11.
+//
+
+#ifndef MYAPPLICATION2_ODO_GRAPH_H
+#define MYAPPLICATION2_ODO_GRAPH_H
+
+void ResetOdo(void);
+double ReadOdo(void);
+void UpdataOdo(double speed, int moveDirect, const struct RtkTime *rtkTime);
+
+#endif //MYAPPLICATION2_ODO_GRAPH_H
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 3d6a4e1..674f248 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -15,6 +15,7 @@
#include "drive_straight.h"
#include "stop_car.h"
#include "operate_gear.h"
+#include "../test_common/odo_graph.h"
#include <cmath>
#include <vector>
@@ -115,15 +116,13 @@
static bool laneChanging;
static int changeLaneDirect;
-static double odoGraph;
-static struct drive_timer odoTimer;
-static double odoPrevSpeed;
-static int odoCnt;
+
typedef struct {
int road;
int sep;
int lane;
+ int guide;
} lane_t;
typedef struct {
@@ -161,8 +160,6 @@
static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime);
static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car);
static int CrashRoadLine(road_t &road, const car_model *car);
-static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2);
-static bool LaneIsValid(struct car_on_lane lane);
static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList);
static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
static void ClearAll(road_exam_map &map);
@@ -199,7 +196,7 @@
prevGearNSlide = false;
gearNSlideTime = 0;
- currExamMapIndex = FIND_POSITION;
+ currExamMapIndex = -1;
startCar = START_CAR_NOT_DO;
stopCar = STOP_CAR_NOT_DO;
@@ -222,8 +219,7 @@
checkTurn = false;
ClearAll(RoadMap);
- odoGraph = 0.0;
- odoCnt = 0;
+ ResetOdo();
// 鍒濆鍖栬�冮」
}
@@ -426,7 +422,7 @@
/***************************************************************
* 杞﹁締鎵�鍦ㄩ亾璺紝鏍规嵁杞﹁締鐨勪腑杞村墠鐐�
- * @param currRoadIndex
+ * @param currRoadIndex, 浼樺厛妫�娴嬪綋鍓嶉亾璺�
* @param RoadMap
* @param car
* @return
@@ -493,7 +489,7 @@
/************************************************
* 杞﹁疆鍘嬪疄绾匡紝鍓嶅悗杞翠氦鍙夛紝鍓嶅悗杞抗浜ゅ弶
- * @param mode
+ * @param mode 1 - 閬撹矾瀹炵嚎/鍒嗛亾瀹炵嚎 2 - 鍒嗛亾铏氱嚎/鍗婂疄鍗婅櫄绾�
* @param RoadMap
* @param car
* @param CarModelList
@@ -505,6 +501,8 @@
Line frontLeftTireTrack, frontRightTireTrack;
Line rearLeftTireTrack, rearRightTireTrack;
bool track = false;
+ int lineType = -1;
+
MakeLine(&frontTireAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
MakeLine(&rearTireAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
@@ -542,6 +540,7 @@
PointF p1, p2;
Line redLine;
+ // 宸﹀彸璺竟瀹炵嚎
for (int n = 0; n < RoadMap.roads.size(); ++n) {
for (int m = 0; m < RoadMap.roads[n].leftEdge.size(); ++m) {
if (RoadMap.roads[n].leftEdge[m].character == LINE_SOLID && RoadMap.roads[n].leftEdge[m].points.size() >= 2) {
@@ -553,7 +552,8 @@
if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
- return true;
+ lineType = LINE_SOLID;
+ goto CRASH_LINE_CONFIRM;
}
if (track &&
@@ -561,7 +561,8 @@
IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
- return true;
+ lineType = LINE_SOLID;
+ goto CRASH_LINE_CONFIRM;
}
p1 = p2;
}
@@ -578,7 +579,8 @@
if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
- return true;
+ lineType = LINE_SOLID;
+ goto CRASH_LINE_CONFIRM;
}
if (track &&
@@ -586,13 +588,15 @@
IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
- return true;
+ lineType = LINE_SOLID;
+ goto CRASH_LINE_CONFIRM;
}
p1 = p2;
}
}
}
+ // 鍒嗛亾瀹炵嚎
for (int m = 0; m < RoadMap.roads[n].separate.size(); ++m) {
// 涓�缁勮溅閬�
for (int l = 0; l < RoadMap.roads[n].separate[m].lines.size(); ++l) {
@@ -601,38 +605,54 @@
// 涓�鏍瑰垎閬撶嚎涓嚎鍨嬬浉鍚岀殑
int character = RoadMap.roads[n].separate[m].lines[l][a].character;
- if (character == LINE_SOLID && RoadMap.roads[n].separate[m].lines[l][a].points.size() >= 2) {
- p1 = RoadMap.roads[n].separate[m].lines[l][a].points[0];
+ if (RoadMap.roads[n].separate[m].lines[l][a].points.size() < 2)
+ continue;
- for (int b = 1; b < RoadMap.roads[n].separate[m].lines[l][a].points.size(); ++b) {
- p2 = RoadMap.roads[n].separate[m].lines[l][a].points[b];
- MakeLine(&redLine, &p1, &p2);
- if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
- IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
- return true;
- }
- if (track &&
- (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
- IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
- return true;
- }
- p1 = p2;
+ p1 = RoadMap.roads[n].separate[m].lines[l][a].points[0];
+
+ for (int b = 1; b < RoadMap.roads[n].separate[m].lines[l][a].points.size(); ++b) {
+ p2 = RoadMap.roads[n].separate[m].lines[l][a].points[b];
+ MakeLine(&redLine, &p1, &p2);
+ if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
+ IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
+ lineType = character;
+ goto CRASH_LINE_CONFIRM;
}
+ if (track &&
+ (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
+ IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
+ IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
+ IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
+ lineType = character;
+ goto CRASH_LINE_CONFIRM;
+ }
+ p1 = p2;
}
- // 姣旇緝鏈�杩戠殑杩炵画鐨刵涓乏渚х偣鍜屼笂涓�涓彸渚х偣
- if (character == LINE_HALF_SOLID_LEFT) {
- // 涓嶈兘浠庡乏绉诲姩鍒板彸
- }
}
+ }
+ }
+ }
+
+CRASH_LINE_CONFIRM:
+ if (lineType == LINE_HALF_SOLID_LEFT) {
+ if (currCrashLineType != LINE_HALF_SOLID_LEFT && track) {
+ if (IntersectionOfLine(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], redLine) == -1) {
+ // 闈炴硶璺ㄧ嚎
+ }
+ }
+ } else if (lineType == LINE_HALF_SOLID_RIGHT) {
+ if (currCrashLineType != LINE_HALF_SOLID_RIGHT && track) {
+ if (IntersectionOfLine(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], redLine) == 1) {
+ // 闈炴硶璺ㄧ嚎
}
}
}
return false;
}
+
+
static int GetGuideDirect(road_exam_map &RoadMap, PointF point, int roadIndex, int sepIndex, int laneNo)
{
@@ -657,13 +677,13 @@
{
Line leftProjLine, rightProjLine;
Line sep;
- PointF p1, p2;
+ PointF p0, p1, p2;
if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
return false;
p1 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge, point);
- p2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, point);
+ p0 = p2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, point);
MakeLine(&leftProjLine, &point, &p1);
MakeLine(&rightProjLine, &point, &p2);
@@ -760,7 +780,7 @@
break;
}
}
-
+ theLane.guide = GetGuideDirect(RoadMap, p0, theLane.road, theLane.sep, theLane.lane);
return true;
}
}
@@ -877,7 +897,7 @@
{
uint32_t cts = AppTimer_GetTickCount();
int ri = CalcRoadIndex(-1, RoadMap, car);
- bool crash = CrashRedLine(0, RoadMap, car, CarModelList);
+ bool crash = CrashRedLine(0, 0, RoadMap, car, CarModelList);
lane_t laneInfo;
double redist = -1;
@@ -901,24 +921,7 @@
DEBUG("褰撳墠閬撹矾绱㈠紩 %d, 瑙﹀彂绾㈢嚎 %d lane %d 璺濈 %f %ld", ri, crash, laneInfo.lane, redist, AppTimer_GetTickCount() - cts);
- // 琛岄┒璺濈锛屼笉鍖呭惈鍊掕溅
- if (odoCnt == 0 && moveDirect == 1) {
- odoPrevSpeed = speed;
- odoCnt = 1;
- Rtk2DriveTimer(odoTimer, rtkTime);
- } else if (odoCnt == 1) {
- uint32_t tm = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
- odoTimer.hour, odoTimer.min, odoTimer.sec, odoTimer.msec*10);
- if (tm >= D_SEC(1)) {
- odoGraph += ((double)tm)*(odoPrevSpeed + speed)/2.0/1000.0;
- if (moveDirect == 1) {
- Rtk2DriveTimer(odoTimer, rtkTime);
- odoPrevSpeed = speed;
- } else {
- odoCnt = 0;
- }
- }
- }
+ UpdataOdo(speed, moveDirect, rtkTime);
// 瓒呴�熸娴�
if (moveDirect != 0 && speed > MAX_SPEED) {
@@ -1097,137 +1100,7 @@
speed, moveDirect, rtkTime);
// 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐�
- if (currExamMapIndex >= 0) {
- Polygon area;
- int n = 0;
-
- area.num = 0;
-
- for (int j = 0; j < RoadMap.roads[currExamMapIndex].leftEdge.size(); ++j) {
- if (j > 0) {
- area.num += RoadMap.roads[currExamMapIndex].leftEdge[j].points.size() - 1;
- } else {
- area.num += RoadMap.roads[currExamMapIndex].leftEdge[j].points.size();
- }
- }
- for (int j = 0; j < RoadMap.roads[currExamMapIndex].rightEdge.size(); ++j) {
- if (j > 0) {
- area.num += RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 1;
- } else {
- area.num += RoadMap.roads[currExamMapIndex].rightEdge[j].points.size();
- }
- }
-
- area.point = (PointF *) malloc(area.num * sizeof(PointF));
-
- for (int j = 0; j < RoadMap.roads[currExamMapIndex].leftEdge.size(); ++j) {
- for (int k = (j>0?1:0); k < RoadMap.roads[currExamMapIndex].leftEdge[j].points.size(); ++k) {
- area.point[n++] = RoadMap.roads[currExamMapIndex].leftEdge[j].points[k];
- }
- }
-
- for (int j = RoadMap.roads[currExamMapIndex].rightEdge.size() - 1; j >= 0; --j) {
- if (j == RoadMap.roads[currExamMapIndex].rightEdge.size() - 1) {
- for (int k = RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 1; k >= 0; --k) {
- area.point[n++] = RoadMap.roads[currExamMapIndex].rightEdge[j].points[k];
- }
- } else {
- for (int k = RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 2; k >= 0; --k) {
- area.point[n++] = RoadMap.roads[currExamMapIndex].rightEdge[j].points[k];
- }
- }
- }
-
- // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
- Polygon carBody;
-
- carBody.num = car->bodyNum;
- carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
- for (int i = 0; i < carBody.num; ++i) {
- carBody.point[i] = car->carXY[car->body[i]];
- }
-
- if (IntersectionOf(&carBody, &area) == GM_None) {
- DEBUG("绂诲紑璺 id = %d", RoadMap.roads[currExamMapIndex].id);
-
- RoadMap.roads[currExamMapIndex].arrivedTail = false;
-
- currExamMapIndex = FIND_POSITION;
- }
-
- free(carBody.point);
- free(area.point);
- }
- if (currExamMapIndex == FIND_POSITION) {
- DEBUG("鎼滅储閬撹矾");
- for (int i = 0; i < RoadMap.roads.size(); ++i) {
- Polygon area;
- int n = 0;
-
- area.num = 0;
-
- for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) {
- if (j > 0) {
- area.num += RoadMap.roads[i].leftEdge[j].points.size() - 1;
- } else {
- area.num += RoadMap.roads[i].leftEdge[j].points.size();
- }
- }
- for (int j = 0; j < RoadMap.roads[i].rightEdge.size(); ++j) {
- if (j > 0) {
- area.num += RoadMap.roads[i].rightEdge[j].points.size() - 1;
- } else {
- area.num += RoadMap.roads[i].rightEdge[j].points.size();
- }
- }
-
- area.point = (PointF *) malloc(area.num * sizeof(PointF));
-
- for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) {
- for (int k = (j>0?1:0); k < RoadMap.roads[i].leftEdge[j].points.size(); ++k) {
- area.point[n++] = RoadMap.roads[i].leftEdge[j].points[k];
- }
- }
-
- for (int j = RoadMap.roads[i].rightEdge.size() - 1; j >= 0; --j) {
- if (j == RoadMap.roads[i].rightEdge.size() - 1) {
- for (int k = RoadMap.roads[i].rightEdge[j].points.size() - 1; k >= 0; --k) {
- area.point[n++] = RoadMap.roads[i].rightEdge[j].points[k];
- }
- } else {
- for (int k = RoadMap.roads[i].rightEdge[j].points.size() - 2; k >= 0; --k) {
- area.point[n++] = RoadMap.roads[i].rightEdge[j].points[k];
- }
- }
- }
-
- if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &area) == GM_Containment) {
- currExamMapIndex = i;
- DEBUG("杩涘叆閬撹矾 id = %d", RoadMap.roads[i].id);
- if (nextRoadId >= 0 && RoadMap.roads[i].id != nextRoadId) {
- DEBUG("涓嶆寜瑙勭煩琛岄┒锛岃繘鍏ラ敊璇矾娈�");
- AddExamFault(3, rtkTime);
- }
- nextRoadId = -1;
- checkTurn = false;
- break;
- }
-
- free(area.point);
- }
- if (currExamMapIndex < 0) {
- currExamMapIndex = FIND_POSITION;//INVALID_POSITION;
- DEBUG("鎼滃鏈灉");
- }
- } else if (currExamMapIndex == INVALID_POSITION) {
- for (int i = 0; i < RoadMap.roads.size(); ++i) {
- if (CrashTheLine(RoadMap.roads[i].startLine, car, CarModelList)) {
- currExamMapIndex = i;
- DEBUG("杩涘叆閬撹矾 id = %d", RoadMap.roads[i].id);
- break;
- }
- }
- }
+ currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);
// 妫�娴嬪帇绾跨姸鎬�
bool crashRedLineNow = false;
@@ -1497,7 +1370,7 @@
if (startCar != START_CAR_DONE)
return;
- if (odoGraph > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) {
+ if (ReadOdo() > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) {
// 鍦ㄥ悎閫傛潯浠朵笅鍋滆溅缁撴潫鑰冭瘯
StartStopCarExam("璇烽潬杈瑰仠杞�");
stopCar = STOP_CAR_DOING;
@@ -1827,22 +1700,6 @@
}
return CRL_NONE;
-}
-
-static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2)
-{
- if (lane1.road == lane2.road && lane1.separate == lane2.separate && lane1.lane == lane2.lane) {
- return true;
- }
- return false;
-}
-
-static bool LaneIsValid(struct car_on_lane lane)
-{
- if (lane.road >= 0 && lane.separate >= 0 && lane.lane >= 0) {
- return true;
- }
- return false;
}
void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
--
Gitblit v1.8.0