// // Created by YY on 2019/11/4. // // 4----------------------|5 // | // | // 3----------|2 | // | | // | | // | | // | | // | | // |1 |0 #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 enterAreaHeading; static bool turnLeftFinished; static bool crashRedLine; static bool CrashRedLine(prime_t &prime); static bool ExitTestArea(prime_t &prime); void StartTurnA90(prime_t &prime) { DEBUG("进入直角转弯场地"); testing = true; enterAreaHeading = (int) prime.pModeling->yaw; crashRedLine = false; turnLeftFinished = false; MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_TURN_90, 1); } static void StoppingTimeout(apptimer_var_t val) { DEBUG("中途停车"); AddExamFault(20703); } void MotionChange(move_status_t mv) { if (!testing) return; AppTimer_delete(StoppingTimeout); if (mv == STOP) { AppTimer_add(StoppingTimeout, D_SEC(2)); } else { } } void TestTurnA90(prime_t &prime) { int az = (int) prime.pModeling->yaw; vector dtox; vector line_set; Line distance_line; if (CrashRedLine(prime)) { if (!crashRedLine) { crashRedLine = true; // 碾压道路边缘,不合格 AddExamFault(20701); DEBUG("碾压道路边缘"); } } else { crashRedLine = false; } // 检查转向状态 if (ABS(az - enterAreaHeading) > 180) { az = 360 - ABS(az-enterAreaHeading); } else { az = ABS(az - enterAreaHeading); } if (az >= 30) { if (!turnLeftFinished) { char turn_direct; if((( az + 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(20702); } } turnLeftFinished = true; } if (ExitTestArea(prime)) { testing = false; MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_TURN_90, 0); } } // 车轮是否压边线 static bool CrashRedLine(prime_t &prime) { bool ret = false; const int red_lines[][2] = {{0, 5}, {5, 4}, {1, 2}, {2, 3}}; Line frontAxle, rearAxle; // 仅看车轮外侧 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 < sizeof(red_lines) / sizeof(red_lines[0]); ++i) { Line red_line; MAKE_LINE(red_line, prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[red_lines[i][0]], prime.pMap->turn_a90_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) { ret = true; break; } } return ret; } // 4个车轮和车头点不在场地中 static bool ExitTestArea(prime_t &prime) { Polygon polygon; polygon.num = prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map.size(); polygon.point = new PointF[polygon.num]; for (int i = 0; i < polygon.num; ++i) { polygon.point[i] = prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[i]; } int num = 0; if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], &polygon) == GM_None) { num++; } if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]], &polygon) == GM_None) { num++; } if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], &polygon) == GM_None) { num++; } if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]], &polygon) == GM_None) { num++; } if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]], &polygon) == GM_None) { num++; } delete []polygon.point; return num == 5? true : false; }