| | |
| | | #include "../driver_test.h" |
| | | #include "../common/apptimer.h" |
| | | #include "../utils/xconvert.h" |
| | | #include "../master/comm_if.h" |
| | | #include "../native-lib.h" |
| | | |
| | | #include <vector> |
| | | #include <cstdlib> |
| | |
| | | const uint32_t STOP_CAR_TIME = D_SEC(2); |
| | | |
| | | static bool testing = false; |
| | | static int mapIndex = 0; |
| | | static uint32_t stopTimepoint = 0; |
| | | |
| | | static bool reportStopCarTimeout; |
| | |
| | | |
| | | static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect); |
| | | static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect); |
| | | static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone); |
| | | static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who); |
| | | |
| | | void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime) |
| | | void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime) |
| | | { |
| | | DEBUG("进入曲线行驶场地"); |
| | | |
| | | testing = true; |
| | | mapIndex = index; |
| | | |
| | | prevMoveDirect = moveDirect; |
| | | if (moveDirect == 0) { |
| | |
| | | crashRedLine = false; |
| | | |
| | | scanWindow.leftStart = scanWindow.leftEnd = scanWindow.rightStart = scanWindow.rightEnd = 0; |
| | | |
| | | MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 1); |
| | | } |
| | | |
| | | int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime) |
| | | { |
| | | Polygon tireRect; |
| | | |
| | | int who = 0; |
| | | MakePolygon(&tireRect, {car->carXY[car->left_front_tire[TIRE_OUTSIDE]], |
| | | car->carXY[car->right_front_tire[TIRE_OUTSIDE]], |
| | | car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], |
| | |
| | | |
| | | DEBUG("scanWindow leftStart %d leftEnd %d rightStart %d rightEnd %d", scanWindow.leftStart, scanWindow.leftEnd, scanWindow.rightStart, scanWindow.rightEnd); |
| | | |
| | | if (CrashRedLine(map, map2, car, &scanWindow)) { |
| | | if (CrashRedLine(map, map2, car, &scanWindow, who)) { |
| | | if (!crashRedLine) { |
| | | crashRedLine = true; |
| | | // 车轮压边线,不合格 |
| | | AddExamFault(27, rtkTime); |
| | | DEBUG("车轮压边线"); |
| | | if (who == 1) { |
| | | PlayTTS("压左曲线"); |
| | | } else if (who == 2) { |
| | | PlayTTS("压右曲线"); |
| | | } |
| | | } |
| | | } else { |
| | | crashRedLine = false; |
| | |
| | | |
| | | TEST_END: |
| | | CleanPolygon(&tireRect); |
| | | |
| | | if (!testing) { |
| | | MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 0); |
| | | } |
| | | |
| | | return testing ? 1 : 0; |
| | | } |
| | | |
| | |
| | | } |
| | | |
| | | // 车轮是否压边线 |
| | | static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone) |
| | | static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who) |
| | | { |
| | | bool ret = false; |
| | | |
| | |
| | | for (int e = zone->leftStart - 1; e >= zone->leftEnd; --e) { |
| | | MakeLine(&redLine, &map->point[s], &map->point[e]); |
| | | if (IntersectionOf(redLine, frontTireAxial) != GM_None) { |
| | | who = 1; |
| | | return true; |
| | | } |
| | | if (IntersectionOf(redLine, rearTireAxial) != GM_None) { |
| | | who = 1; |
| | | return true; |
| | | } |
| | | s = e; |
| | |
| | | for (int e = zone->rightStart - 1; e >= zone->rightEnd; --e) { |
| | | MakeLine(&redLine, &map2->point[s], &map2->point[e]); |
| | | if (IntersectionOf(redLine, frontTireAxial) != GM_None) { |
| | | who = 2; |
| | | return true; |
| | | } |
| | | if (IntersectionOf(redLine, rearTireAxial) != GM_None) { |
| | | who = 2; |
| | | return true; |
| | | } |
| | | s = e; |