From 6b136af26a51b3d53051cd3e0e7d467ed2ab8a6b Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 05 三月 2020 13:33:21 +0800
Subject: [PATCH] 更新地图
---
lib/src/main/cpp/test_items/driving_curve.cpp | 192 +++++++++++++++++++++++---------------
lib/src/main/cpp/driver_test.cpp | 57 +++++------
lib/src/main/cpp/rtk_module/rtk.cpp | 2
3 files changed, 143 insertions(+), 108 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 321f3c5..443a303 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -878,40 +878,16 @@
// }
// }
if (mapList[i].type == MAP_TYPE_CURVE) {
- const int step = 10;
- Polygon map;
- bool enter = false;
+ Line startLine;
+ Line carAxial;
- map.num = 1 + mapList[i].map.num / step + ((mapList[i].map.num % step == 0) ? 0 : 1);
- map.num += 1 + mapList[i].map2.num / step + ((mapList[i].map2.num % step == 0) ? 0 : 1);
- map.point = (PointF *) malloc(map.num * sizeof(PointF));
+ MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]);
+ MakeLine(&carAxial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
- int n = 0, m = 0;
- while (n < mapList[i].map.num - 1) {
- map.point[m++] = mapList[i].map.point[n];
- n += step;
+ if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
+ if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], startLine) > 0.1)
+ return i;
}
- map.point[m++] = mapList[i].map.point[mapList[i].map.num - 1];
-
- n = mapList[i].map2.num - 1;
- while (n > 0) {
- map.point[m++] = mapList[i].map2.point[n];
- n -= step;
- }
- map.point[m++] = mapList[i].map2.point[0];
-
- if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) {
- Line enterLine1;
-
- MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map2.point[0]));
-
- if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
- enter = true;
- }
-
- free(map.point);
-
- if (enter) return i;
}
}
return -1;
@@ -979,6 +955,25 @@
free(carBody.point);
free(map.point);
}
+ if (mapList[index].type == MAP_TYPE_CURVE) {
+ Line carAxial;
+ Line scanLine;
+
+ MakeLine(&carAxial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
+
+ int startL = 0, startR = 0;
+ while (startL < mapList[index].map.num || startR < mapList[index].map2.num) {
+ MakeLine(&scanLine, &mapList[index].map.point[startL], &mapList[index].map.point[startR]);
+ if (IntersectionOf(scanLine, carAxial) == GM_Intersection) {
+ break;
+ }
+ if (startL < mapList[index].map.num)
+ startL++;
+ if (startR < mapList[index].map2.num)
+ startR++;
+ }
+ ret = true;
+ }
return ret;
}
diff --git a/lib/src/main/cpp/rtk_module/rtk.cpp b/lib/src/main/cpp/rtk_module/rtk.cpp
index cda71de..bb5e332 100644
--- a/lib/src/main/cpp/rtk_module/rtk.cpp
+++ b/lib/src/main/cpp/rtk_module/rtk.cpp
@@ -202,7 +202,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_items/driving_curve.cpp b/lib/src/main/cpp/test_items/driving_curve.cpp
index b5294a1..3a3cd49 100644
--- a/lib/src/main/cpp/test_items/driving_curve.cpp
+++ b/lib/src/main/cpp/test_items/driving_curve.cpp
@@ -26,15 +26,14 @@
static bool reportStopCarTimeout;
static int prevMoveDirect;
static bool crashRedLine;
-static struct {
+static struct calc_zone_t {
int leftStart;
int leftEnd;
int rightStart;
int rightEnd;
} calcZone;
-static bool CrashRedLine(const Polygon *left, const Polygon *right, const car_model_cache_t *car);
-static bool ExitTestArea(const Polygon *left, const Polygon *right, const car_model_cache_t *car);
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone);
void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime)
{
@@ -49,104 +48,145 @@
reportStopCarTimeout = false;
crashRedLine = false;
- calcZone.leftStart = calcZone.leftEnd = calcZone.rightStart = calcZone.rightEnd = -1;
+ calcZone.leftStart = calcZone.leftEnd = calcZone.rightStart = calcZone.rightEnd = 0;
}
int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
Line start, end;
- Polygon zone;
+ Line axial;
- if (calcZone.leftStart == -1) {
- calcZone.leftStart = 0;
- }
- if (calcZone.rightStart == -1) {
- calcZone.rightStart = 0;
+ MakeLine(&axial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
+
+ MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
+ MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
+
+ if (IntersectionOf(start, axial) == GM_None) {
+ // 鍚戣捣鐐规煡鎵�
+ do {
+ if (calcZone.leftStart == 0 && calcZone.rightStart == 0) {
+ break;
+ }
+ if (calcZone.leftStart > 0)
+ calcZone.leftStart--;
+ if (calcZone.rightStart > 0)
+ calcZone.rightStart--;
+ MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
+ } while (IntersectionOf(start, axial) == GM_None);
+ } else {
+ // 鍚戠粓鐐规煡鎵�
+ do {
+ if (calcZone.leftStart >= map->num && calcZone.rightStart >= map2->num) {
+ break;
+ }
+ if (calcZone.leftStart < map->num)
+ calcZone.leftStart++;
+ if (calcZone.rightStart < map2->num)
+ calcZone.rightStart++;
+ MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
+ } while (IntersectionOf(start, axial) != GM_None);
}
- start.X1 = map->point[calcZone.leftStart].X;
- start.Y1 = map->point[calcZone.leftStart].Y;
-
- start.X2 = map->point[calcZone.rightStart].X;
- start.Y2 = map->point[calcZone.rightStart].Y;
-
- zone.num = 4;
- zone.point = (PointF *) malloc(zone.num * sizeof(PointF));
- zone.point[0] = car->carXY[car->left_front_tire[TIRE_OUTSIDE]];
- zone.point[1] = car->carXY[car->right_front_tire[TIRE_OUTSIDE]];
- zone.point[2] = car->carXY[car->right_rear_tire[TIRE_OUTSIDE]];
- zone.point[3] = car->carXY[car->left_rear_tire[TIRE_OUTSIDE]];
-
- if (IntersectionOf(start, &zone) == GM_None) {
-
+ if (IntersectionOf(end, axial) == GM_None) {
+ // 鍚戠粓鐐规煡鎵�
+ do {
+ if (calcZone.leftEnd >= map->num && calcZone.rightEnd >= map2->num) {
+ break;
+ }
+ if (calcZone.leftEnd < map->num)
+ calcZone.leftEnd++;
+ if (calcZone.rightEnd < map2->num)
+ calcZone.rightEnd++;
+ MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
+ } while (IntersectionOf(end, axial) == GM_None);
+ } else {
+ // 鍚戣捣鐐规煡鎵�
+ do {
+ if (calcZone.leftEnd == 0 && calcZone.rightEnd == 0) {
+ break;
+ }
+ if (calcZone.leftEnd > 0)
+ calcZone.leftEnd--;
+ if (calcZone.rightEnd > 0)
+ calcZone.rightEnd--;
+ MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
+ } while (IntersectionOf(end, axial) == GM_None);
}
- free(zone.point);
+ if (calcZone.leftStart <= calcZone.leftEnd || calcZone.rightStart <= calcZone.rightEnd) {
+ // 绂诲紑鍦哄湴
+ testing = false;
+ }
+
+ if (CrashRedLine(map, map2, car, &calcZone)) {
+ if (!crashRedLine) {
+ crashRedLine = true;
+ // 杞﹁疆鍘嬭竟绾匡紝涓嶅悎鏍�
+ AddExamFault(27, rtkTime);
+ DEBUG("杞﹁疆鍘嬭竟绾�");
+ }
+ } else {
+ crashRedLine = false;
+ }
+
+ if (moveDirect != prevMoveDirect) {
+ if (moveDirect == 0) {
+ stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
+ reportStopCarTimeout = false;
+
+ DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+ } else {
+
+ }
+ prevMoveDirect = moveDirect;
+ } else if (moveDirect == 0) {
+ uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
+
+ if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarTimeout) {
+ // 鍋滆溅瓒�2绉掞紝涓嶅悎鏍�
+ AddExamFault(28, rtkTime);
+ DEBUG("涓�斿仠杞�");
+ reportStopCarTimeout = true;
+ }
+ }
return testing ? 1 : 0;
}
// 杞﹁疆鏄惁鍘嬭竟绾�
-static bool CrashRedLine(const Polygon *left, const Polygon *right, const car_model_cache_t *car)
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone)
{
bool ret = false;
- car_model_cache_t *prev_car = GetCarModelCache(1);
- if (prev_car == NULL)
- return false;
+ Line frontTireAxial, rearTireAxial;
+ Line redLine;
- // 鎸�4涓疆瀛愮殑澶栦晶璁＄畻
- Line front_left_tire_track, front_right_tire_track, rear_left_tire_track, rear_right_tire_track;
- MakeLine(&front_left_tire_track, &car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], &prev_car->points[car->desc->front_left_tire[TIRE_OUTSIDE]]);
- MakeLine(&front_right_tire_track, &car->points[car->desc->front_right_tire[TIRE_OUTSIDE]], &prev_car->points[car->desc->front_right_tire[TIRE_OUTSIDE]]);
- MakeLine(&rear_left_tire_track, &car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]], &prev_car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]]);
- MakeLine(&rear_right_tire_track, &car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]], &prev_car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]]);
+ MakeLine(&frontTireAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
+ MakeLine(&rearTireAxial, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
- Line line;
-
- line.X1 = left->point[0].X;
- line.Y1 = left->point[0].Y;
- for (int i = 1; i < left->num; ++i) {
- line.X2 = left->point[i].X;
- line.Y2 = left->point[i].Y;
-
- if (IntersectionOf(line, front_left_tire_track) == GM_Intersection ||
- IntersectionOf(line, front_right_tire_track) == GM_Intersection ||
- IntersectionOf(line, rear_left_tire_track) == GM_Intersection ||
- IntersectionOf(line, rear_right_tire_track) == GM_Intersection) {
- ret = true;
- break;
+ int s = zone->leftStart;
+ for (int e = zone->leftStart + 1; e < zone->leftEnd; ++e) {
+ MakeLine(&redLine, &map->point[s], &map->point[e]);
+ if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
+ return true;
}
- line.X1 = line.X2;
- line.Y1 = line.Y2;
+ if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
+ return true;
+ }
+ s = e;
}
- line.X1 = right->point[0].X;
- line.Y1 = right->point[0].Y;
- for (int i = 1; !ret && i < right->num; ++i) {
- line.X2 = right->point[i].X;
- line.Y2 = right->point[i].Y;
-
- if (IntersectionOf(line, front_left_tire_track) == GM_Intersection ||
- IntersectionOf(line, front_right_tire_track) == GM_Intersection ||
- IntersectionOf(line, rear_left_tire_track) == GM_Intersection ||
- IntersectionOf(line, rear_right_tire_track) == GM_Intersection) {
- ret = true;
- break;
+ s = zone->rightStart;
+ for (int e = zone->rightStart + 1; e < zone->rightEnd; ++e) {
+ MakeLine(&redLine, &map2->point[s], &map2->point[e]);
+ if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
+ return true;
}
- line.X1 = line.X2;
- line.Y1 = line.Y2;
+ if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
+ return true;
+ }
+ s = e;
}
return ret;
-}
-
-// 鏁翠釜杞﹁締閮借椹剁璇ユ祴璇曞尯鍩�
-static bool ExitTestArea(const Polygon *left, const Polygon *right, const car_model_cache_t *car)
-{
- for (int i = 0; i < car->point_num; ++i) {
- if (IntersectionOfLine(left->point[0], right->point[0], car->points[i]) != 1)
- return false;
- }
- return true;
}
--
Gitblit v1.8.0