| | |
| | | // Created by YY on 2019/10/31. |
| | | // |
| | | |
| | | // |9 |
| | | // | |
| | | // | |
| | | // | |
| | | // | |
| | | // 7-----------------|8 |
| | | // 6_________________|5 |
| | | // 3_________________|4 |
| | | // | |
| | | // 2-----------------|1 |
| | | // | |
| | | // | |
| | | // | |
| | | // |0 |
| | | |
| | | #include <cstdlib> |
| | | #include <vector> |
| | | #include <cmath> |
| | |
| | | using namespace std; |
| | | |
| | | |
| | | const uint32_t STOP_CAR_TIME = D_SEC(1); |
| | | |
| | | |
| | | const double EPSILON = 1e-3; |
| | | |
| | |
| | | |
| | | static PointF stopPoint; |
| | | |
| | | static int mapIndex = 0; |
| | | static int prevMoveDirect; |
| | | static uint32_t stopTimepoint = 0; |
| | | static bool stopCar = false; |
| | | static uint32_t stopCarTime; |
| | | static bool check1 = false; |
| | | |
| | | static bool stopConfirm; |
| | | static bool restartComplete = false; |
| | | static bool occurCrashRedLine = false; |
| | | static bool slideLongDistance = false; |
| | | |
| | | static bool slideNormalDistance = false; |
| | | static bool reportSlideFault = false; |
| | | static bool reportStartTimeout = false; |
| | | static bool handBreakActive = false; |
| | | static bool CrashRedLine(const Polygon *map, const car_model *car); |
| | | static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car); |
| | | static double DistanceOfTire2Edge(const Polygon *map, const car_model *car); |
| | | static bool ExitTestArea(const Polygon *map, const car_model *car); |
| | | |
| | | void StartSAS(int index, const Polygon *map, const car_model *car, int moveDirect, const struct RtkTime *rtkTime) |
| | | static bool CrashRedLine(prime_t &prime); |
| | | static double DistanceOfHead2Stopline(prime_t &prime); |
| | | static double DistanceOfTire2Edge(prime_t &prime); |
| | | static bool ExitTestArea(prime_t &prime); |
| | | |
| | | void StartSAS(prime_t &prime) |
| | | { |
| | | double yawEdge = YawOf(map->point[8], map->point[0]); |
| | | |
| | | if (moveDirect < 0 || DeltaYaw(car->yaw, yawEdge) >= 90.0) { |
| | | testing = false; |
| | | return; |
| | | } |
| | | |
| | | DEBUG("进入坡起项目"); |
| | | |
| | | testing = true; |
| | | mapIndex = index; |
| | | prevMoveDirect = moveDirect; |
| | | |
| | | if (moveDirect == 0) { |
| | | stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | } |
| | | |
| | | stopConfirm = false; |
| | | restartComplete = false; |
| | | occurCrashRedLine = false; |
| | | stopCar = false; |
| | | slideLongDistance = false; |
| | | check1 = false; |
| | | slideNormalDistance = false; |
| | | reportSlideFault = false; |
| | | reportStartTimeout = false; |
| | | handBreakActive = false; |
| | | |
| | | MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 1); |
| | | MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 1); |
| | | } |
| | | |
| | | int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime) |
| | | static void StoppingTimeout(apptimer_var_t val) { |
| | | // 起步时间超过30秒,不合格 |
| | | AddExamFault(20303); |
| | | DEBUG("起步时间超过30秒"); |
| | | } |
| | | |
| | | static void StopConfirm(apptimer_var_t val) { |
| | | stopConfirm = true; |
| | | } |
| | | |
| | | void MotionChange(move_status_t mv) |
| | | { |
| | | AppTimer_delete(StopConfirm); |
| | | |
| | | if (mv == STOP) { |
| | | AppTimer_add(StopConfirm, D_SEC(1)); |
| | | } else { |
| | | |
| | | } |
| | | } |
| | | |
| | | int TestSAS(prime_t &prime) |
| | | { |
| | | static double distanceToStopLine = 0, distanceToEdge = 0; |
| | | if (!testing) |
| | | return 0; |
| | | |
| | | if (CrashRedLine(map, car)) { |
| | | if (CrashRedLine(prime)) { |
| | | // 车轮压线,不合格 |
| | | if (!occurCrashRedLine) { |
| | | AddExamFault(10116, rtkTime); |
| | | AddExamFault(10116); |
| | | DEBUG("车轮压线"); |
| | | } |
| | | occurCrashRedLine = true; |
| | |
| | | occurCrashRedLine = false; |
| | | } |
| | | |
| | | if (ExitTestArea(map, car)) { |
| | | // 驶离测试区 |
| | | if (!stopCar) { |
| | | // 不停车直接离开 |
| | | AddExamFault(10103, rtkTime); |
| | | } |
| | | testing = false; |
| | | } |
| | | // 检测到停车 |
| | | if (prime.pMotion->move == STOP && stopConfirm) { |
| | | if (!check1) { |
| | | check1 = true; |
| | | // 存储停止点 |
| | | stopPoint = prime.pModeling->base_point; |
| | | // 开始停车计时 |
| | | AppTimer_delete(StoppingTimeout); |
| | | AppTimer_add(StoppingTimeout, examParam.ramp_start_car_limit_time); |
| | | |
| | | if (prevMoveDirect != moveDirect) { |
| | | if (moveDirect == 0) { |
| | | stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | } else if (stopCar && !handBreakActive) { |
| | | // 检查是否拉住手刹 |
| | | handBreakActive = true; |
| | | AddExamFault(20306, rtkTime); |
| | | DEBUG("没拉手刹"); |
| | | } |
| | | 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 && !stopCar) { |
| | | // 这里判断停车状态 |
| | | stopCar = true; |
| | | stopCarTime = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | |
| | | stopPoint = car->carXY[car->body[0]]; |
| | | |
| | | distanceToStopLine = DistanceOfHead2Stopline(map, car); |
| | | distanceToEdge = DistanceOfTire2Edge(map, car); |
| | | // 检查车头和停车带的距离 |
| | | distanceToStopLine = DistanceOfHead2Stopline(prime); |
| | | distanceToEdge = DistanceOfTire2Edge(prime); |
| | | |
| | | DEBUG("DIS1 = %f DIS2 = %f", distanceToStopLine, distanceToEdge); |
| | | |
| | | if (distanceToStopLine > examParam.ramp_stoppoint_red_distance) { |
| | | // 距离停止线前后超出50厘米 |
| | | AddExamFault(20301, rtkTime); |
| | | AddExamFault(20301); |
| | | DEBUG("距离停止线前后超出50厘米,不合格"); |
| | | } else if (fabs(distanceToStopLine) > EPSILON) { |
| | | // 前保险没有位于停止带内,但没有超出50厘米,扣10分 |
| | | AddExamFault(20304, rtkTime); |
| | | AddExamFault(20304); |
| | | DEBUG("前保险没有位于停止带内,但没有超出50厘米"); |
| | | } |
| | | |
| | | if (distanceToEdge > examParam.ramp_edge_red_distance) { |
| | | // 距离边线超出50厘米,不合格 |
| | | AddExamFault(20302, rtkTime); |
| | | AddExamFault(20302); |
| | | DEBUG("距离边线超出50厘米"); |
| | | } else if (distanceToEdge > examParam.ramp_edge_yellow_distance) { |
| | | // 距离边线超出30厘米,扣10分 |
| | | AddExamFault(20305, rtkTime); |
| | | AddExamFault(20305); |
| | | DEBUG("距离边线超出30厘米"); |
| | | } |
| | | } |
| | | |
| | | if (stopCar && !handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) { |
| | | // 停车后,检查手刹拉起情况 |
| | | if (!handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) { |
| | | handBreakActive = true; |
| | | } |
| | | } |
| | | |
| | | if (!stopCar) { |
| | | // 距离检测 |
| | | vector<double> dtox; |
| | | vector<Line> line_set; |
| | | Line distance_line; |
| | | if (prime.pMotion->move != STOP && stopConfirm) { |
| | | // 车辆从停止状态再次移动 |
| | | double juli = DistanceOf(prime.pModeling->base_point, stopPoint); |
| | | |
| | | MakeLine(&distance_line, &map->point[0], &map->point[8]); |
| | | line_set.push_back(distance_line); |
| | | DistanceOfTire2X(dtox, car, line_set); |
| | | MA_SendDistance(dtox[0], dtox[1]); |
| | | } else { |
| | | MA_SendDistance(distanceToStopLine, distanceToEdge); |
| | | } |
| | | |
| | | // 判断起步后滑状态 |
| | | if (stopCar) { |
| | | if (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) == 1) { |
| | | // 发生后滑 |
| | | double slideDistance = DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]); |
| | | |
| | | if (slideDistance > examParam.ramp_slide_yellow_distance) { |
| | | if (juli > examParam.ramp_slide_yellow_distance) { |
| | | double deg = YawOf(stopPoint, prime.pModeling->base_point); |
| | | deg = fabs(prime.pModeling->yaw - deg); |
| | | if (deg > 180) { |
| | | deg = 360 - deg; |
| | | } |
| | | if (deg < 90) { |
| | | // 车辆上坡 |
| | | if (!restartComplete) { |
| | | restartComplete = true; |
| | | AppTimer_delete(StoppingTimeout); |
| | | if (slideNormalDistance && !reportSlideFault) { |
| | | // 后滑超过10厘米,但没超过30厘米 |
| | | DEBUG("后滑超过10厘米,但没超过30厘米"); |
| | | reportSlideFault = true; |
| | | AddExamFault(10204); |
| | | } |
| | | if (!handBreakActive) { |
| | | // 检查是否拉住手刹 |
| | | DEBUG("没拉手刹"); |
| | | handBreakActive = true; |
| | | AddExamFault(20306); |
| | | } |
| | | } |
| | | } else { |
| | | // 车辆后滑 |
| | | slideNormalDistance = true; |
| | | } |
| | | |
| | | if (slideDistance > examParam.ramp_slide_red_distance && !slideLongDistance && !reportSlideFault) { |
| | | // 后滑超过30厘米, 不合格 |
| | | AddExamFault(10106, rtkTime); |
| | | DEBUG("后滑超过30厘米"); |
| | | slideLongDistance = true; |
| | | reportSlideFault = true; |
| | | } |
| | | } |
| | | |
| | | |
| | | if (!reportStartTimeout && (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) != -1 || |
| | | DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) < 0.1)) { |
| | | if (TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10) - stopCarTime > examParam.ramp_start_car_limit_time) { |
| | | // 起步时间超过30秒,不合格 |
| | | AddExamFault(20303, rtkTime); |
| | | DEBUG("起步时间超过30秒"); |
| | | reportStartTimeout = true; |
| | | } |
| | | } |
| | | |
| | | if (IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->axial[AXIAL_REAR]]) == -1) { |
| | | // 车尾驶过停止杆 |
| | | if (slideNormalDistance && !slideLongDistance && !reportSlideFault) { |
| | | reportSlideFault = true; |
| | | // 后滑超过10厘米,但没超过30厘米 |
| | | AddExamFault(10204, rtkTime); |
| | | DEBUG("后滑超过10厘米,但没超过30厘米"); |
| | | if (juli > examParam.ramp_slide_red_distance && !reportSlideFault) { |
| | | // 后滑超过30厘米, 不合格 |
| | | DEBUG("后滑超过30厘米"); |
| | | reportSlideFault = true; |
| | | AddExamFault(10106); |
| | | } |
| | | } |
| | | } |
| | | } |
| | | |
| | | if (!testing) { |
| | | MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 0); |
| | | if (ExitTestArea(prime)) { |
| | | // 驶离测试区 |
| | | if (!stopConfirm) { |
| | | // 不停车直接离开 |
| | | AddExamFault(10103); |
| | | } |
| | | MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 0); |
| | | } |
| | | |
| | | return testing ? 1 : 0; |
| | | } |
| | | |
| | | // 车轮是否压边线 |
| | | static bool CrashRedLine(const Polygon *map, const car_model *car) |
| | | static bool CrashRedLine(prime_t &prime) |
| | | { |
| | | bool ret = false; |
| | | |
| | | Line red_line; |
| | | int red_lines[2][2]; |
| | | int red_line_num = 0; |
| | | int red_lines[][2] = {{0, 1}, {1, 4}, {4, 5}, {5, 8}, {8, 9}}; |
| | | |
| | | if (map->num == 10) { |
| | | red_lines[0][0] = 0; |
| | | red_lines[0][1] = 8; |
| | | red_lines[1][0] = 8; |
| | | red_lines[1][1] = 9; |
| | | red_line_num = 2; |
| | | } else { |
| | | red_lines[0][0] = 0; |
| | | red_lines[0][1] = 8; |
| | | red_line_num = 1; |
| | | } |
| | | |
| | | 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]]); |
| | | // 仅看车轮外侧 |
| | | MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], |
| | | prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]); |
| | | MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], |
| | | prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]); |
| | | |
| | | for (int i = 0; i < red_line_num; ++i) { |
| | | MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]); |
| | | for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) { |
| | | Line red_line; |
| | | MAKE_LINE(red_line, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[red_lines[i][0]], |
| | | prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[red_lines[i][1]]); |
| | | |
| | | if (IntersectionOf(red_line, frontAxle) == GM_Intersection || |
| | | IntersectionOf(red_line, rearAxle) == GM_Intersection) { |
| | | IntersectionOf(red_line, rearAxle) == GM_Intersection) { |
| | | ret = true; |
| | | break; |
| | | } |
| | |
| | | return ret; |
| | | } |
| | | |
| | | static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car) |
| | | static double DistanceOfHead2Stopline(prime_t &prime) |
| | | { |
| | | double dis = 0.0; |
| | | |
| | | int rel1 = IntersectionOfLine(map->point[4], map->point[3], car->carXY[car->body[0]]); |
| | | int rel2 = IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->body[0]]); |
| | | Line upper_edge, lower_edge; |
| | | |
| | | DEBUG("%d %d %f, %f", car->body[0], car->axial[AXIAL_FRONT], car->carXY[car->body[0]].X, car->carXY[car->body[0]].Y); |
| | | DEBUG("rel1 %d rel2 %d", rel1, rel2); |
| | | MAKE_LINE(upper_edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[5], |
| | | prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[6]); |
| | | MAKE_LINE(lower_edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[4], |
| | | prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[3]); |
| | | |
| | | if (rel1 == 1) { |
| | | Line line1; |
| | | |
| | | MakeLine(&line1, &map->point[4], &map->point[3]); |
| | | |
| | | dis = DistanceOf(car->carXY[car->body[0]], line1); |
| | | } else if (rel2 == -1) { |
| | | Line line2; |
| | | |
| | | MakeLine(&line2, &map->point[5], &map->point[6]); |
| | | |
| | | dis = DistanceOf(car->carXY[car->body[0]], line2); |
| | | if (IntersectionOfLine(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge) == REL_POS_RIGHT) { |
| | | dis = DistanceOf(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge); |
| | | } else if (IntersectionOfLine(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], lower_edge) == REL_POS_LEFT) { |
| | | dis = DistanceOf(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], lower_edge); |
| | | } |
| | | |
| | | DEBUG("DistanceOfHead2Stopline dis %f", dis); |
| | |
| | | return dis; |
| | | } |
| | | |
| | | static double DistanceOfTire2Edge(const Polygon *map, const car_model *car) |
| | | static double DistanceOfTire2Edge(prime_t &prime) |
| | | { |
| | | Line edge; |
| | | |
| | | MakeLine(&edge, &map->point[0], &map->point[8]); |
| | | MAKE_LINE(edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[0], |
| | | prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[8]); |
| | | |
| | | double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], edge); |
| | | double l1 = DistanceOf(prime.pModeling->points[prime.pModel->right_front_tire[TIRE_OUTSIDE]], edge); |
| | | |
| | | double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], edge); |
| | | |
| | | // return (l1+l2)/2.0; |
| | | double l2 = DistanceOf(prime.pModeling->points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]], edge); |
| | | |
| | | return MAX(l1, l2); // 取最远的 |
| | | } |
| | | |
| | | // 整个车辆都要驶离该测试区域 |
| | | static bool ExitTestArea(const Polygon *map, const car_model *car) |
| | | static bool ExitTestArea(prime_t &prime) |
| | | { |
| | | // 全车都需不在地图中 |
| | | bool ret = false; |
| | | |
| | | Polygon carBody, map2; |
| | | /*Polygon carBody, map2; |
| | | PointF vPoint = Calc3Point(map->point[8], map->point[0], DistanceOf(map->point[8], map->point[7]), 'R'); |
| | | PointF vPoint2; |
| | | |
| | |
| | | } |
| | | |
| | | free(carBody.point); |
| | | free(map2.point); |
| | | free(map2.point);*/ |
| | | return ret; |
| | | } |