// // Created by YY on 2019/11/4. // #include "turn_a90.h" #include "../test_common/Geometry.h" #include "../driver_test.h" #include "../common/apptimer.h" #include "../jni_log.h" #include "../utils/xconvert.h" #include "../defs.h" #include "../test_common/car_sensor.h" #include "../master/comm_if.h" #include "area_exam.h" #include #include #define DEBUG(fmt, args...) LOGD(" <%s>: " fmt, __func__, ##args) using namespace std; static bool testing; static int mapIndex; static int enterAreaHeading; static bool turnLeftFinished; static uint32_t stopTimepoint = 0; static bool reportStopCarTimeout; static int prevMoveDirect; static bool crashRedLine; static bool CrashRedLine(const Polygon *map, const car_model *car); static bool ExitTestArea(const Polygon *map, const car_model *car); void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime) { DEBUG("进入直角转弯场地"); testing = true; enterAreaHeading = (int) heading; prevMoveDirect = moveDirect; if (moveDirect == 0) { stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); } reportStopCarTimeout = false; crashRedLine = false; turnLeftFinished = false; mapIndex = index; MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 1); } int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime) { int az = (int) heading; vector dtox; vector line_set; Line distance_line; if (ExitTestArea(map, car)) { testing = false; goto TEST_END; } // 距离检测 MakeLine(&distance_line, &map->point[0], &map->point[5]); line_set.push_back(distance_line); MakeLine(&distance_line, &map->point[5], &map->point[4]); line_set.push_back(distance_line); MakeLine(&distance_line, &map->point[1], &map->point[2]); line_set.push_back(distance_line); MakeLine(&distance_line, &map->point[2], &map->point[3]); line_set.push_back(distance_line); DistanceOfTire2X(dtox, car, line_set); MA_SendDistance(dtox[0], dtox[1]); if (CrashRedLine(map, car)) { if (!crashRedLine) { crashRedLine = true; // 碾压道路边缘,不合格 AddExamFault(29, 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 >= CorrectPauseCriteria(examParam.turn_a90_pause_criteria) && !reportStopCarTimeout) { // 停车超2秒,每次扣5分 AddExamFault(31, rtkTime); DEBUG("中途停车"); reportStopCarTimeout = true; } } // 检查转向状态 if (ABS(az - enterAreaHeading) > 180) { az = 360 - ABS(az-enterAreaHeading); } else { az = ABS(az - enterAreaHeading); } if (az >= 30) { if (!turnLeftFinished) { char turn_direct; if((( ((int)heading) + 360 - enterAreaHeading) % 360) < 180) { DEBUG("右转"); turn_direct = 'R'; } else { DEBUG("左转"); turn_direct = 'L'; } // 转向灯未开启,扣10分 if ((turn_direct == 'R' && ReadCarStatus(TURN_SIGNAL_LAMP) != RIGHT_TURN_LIGHT) || (turn_direct == 'L' && ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT)) { DEBUG("转向灯未开启"); AddExamFault(30, rtkTime); } } turnLeftFinished = true; } if (turnLeftFinished) { } if (!testing) { MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 0); } TEST_END: return testing? 1:0; } // 车轮是否压边线 static bool CrashRedLine(const Polygon *map, const car_model *car) { bool ret = false; Line red_line; const int red_lines[][2] = {{0, 5}, {5, 4}, {1, 2}, {2, 3}}; Line frontAxle, rearAxle; // 仅看车轮外侧 MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]); MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]); for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) { MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]); if (IntersectionOf(red_line, frontAxle) == GM_Intersection || IntersectionOf(red_line, rearAxle) == GM_Intersection) { ret = true; break; } } return ret; } // 整个车辆都要驶离该测试区域 static bool ExitTestArea(const Polygon *map, const car_model *car) { bool ret = false; // 全车都需不在地图中 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, map) == GM_None) { ret = true; } free(carBody.point); return ret; }