| | |
| | | const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1; |
| | | const double CHANGE_LANE_RANGE = 100.0; |
| | | const double OVERTAKE_RANGE = 150.0; |
| | | const int OVERTAKE_HOLD_TIME = D_SEC(11); // 在超车道行驶的一段时间 |
| | | const int OVERTAKE_HOLD_TIME = D_SEC(3); // 在超车道行驶的一段时间 |
| | | const double EXAM_RANGE = 3000.0; // 至少驾驶距离 |
| | | |
| | | static const double LASTEST_BREAK_POINT = 30.0; |
| | |
| | | int type; // 实线,虚线 |
| | | } CurrentLane; |
| | | static bool laneChanging; |
| | | static int changeLaneDirect; |
| | | |
| | | static double odoGraph; |
| | | static struct drive_timer odoTimer; |
| | | static double odoPrevSpeed; |
| | |
| | | |
| | | CurrentLane.road = CurrentLane.separate = CurrentLane.lane = -1; |
| | | laneChanging = false; |
| | | changeLaneDirect = 0; |
| | | |
| | | nextRoadId = -1; |
| | | checkTurn = false; |
| | |
| | | crashGreenCmpTime.min, |
| | | crashGreenCmpTime.sec, |
| | | crashGreenCmpTime.msec * 10); |
| | | int laneDirect = 0; |
| | | |
| | | if (diff < CHANGE_LANE_MIN_INTERVAL) { |
| | | if (CurrentLane.lane > lane.lane) { |
| | | laneDirect = 1; |
| | | } else { |
| | | laneDirect = -1; |
| | | } |
| | | |
| | | if (diff < CHANGE_LANE_MIN_INTERVAL && laneDirect == changeLaneDirect) { |
| | | DEBUG("===================== 连续变道 ============!!"); |
| | | // 连续变道,不合格 |
| | | AddExamFault(15, rtkTime); |
| | |
| | | if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_CHANGE_LANE) { |
| | | DEBUG("变更车道项目完成"); |
| | | currRoadItem = NULL; |
| | | PlayTTS("完成变道"); |
| | | } else if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_OVERTAKE) { |
| | | if (CurrentLane.lane > lane.lane) { |
| | | DEBUG("超车变道完成"); |
| | | overtake = true; |
| | | Rtk2DriveTimer(overTakeCmpTime, rtkTime); |
| | | PlayTTS("完成超车"); |
| | | } else { |
| | | DEBUG("右道超车,错误"); |
| | | AddExamFault(3, rtkTime); |
| | | currRoadItem = NULL; |
| | | } |
| | | } |
| | | |
| | | if (CurrentLane.lane > lane.lane) { |
| | | changeLaneDirect = -1; |
| | | } else { |
| | | changeLaneDirect = 1; |
| | | } |
| | | |
| | | CurrentLane = lane; |
| | |
| | | } |
| | | return false; |
| | | } |
| | | |
| | | #if 0 |
| | | void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) |
| | | { |
| | | // 起步检测 |
| | | TestRoadStartCar(car, speed, moveDirect, rtkTime); |
| | | |
| | | // 超速检测 |
| | | if (moveDirect != 0 && speed > MAX_SPEED) { |
| | | if (!occurOverSpeed) { |
| | | occurOverSpeed = true; |
| | | // 超速,不合格 |
| | | AddExamFault(10, rtkTime); |
| | | } |
| | | } else { |
| | | occurOverSpeed = false; |
| | | } |
| | | |
| | | // 副刹车检测 |
| | | if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) { |
| | | // 副刹车踩下,不合格 |
| | | if (!occurSecondBreak) { |
| | | DEBUG("副刹车动作了"); |
| | | occurSecondBreak = true; |
| | | AddExamFault(17, rtkTime); |
| | | } |
| | | } else { |
| | | occurSecondBreak = false; |
| | | } |
| | | |
| | | // 挡位匹配检测 |
| | | bool currGearError = false; |
| | | bool currGearNSlide = false; |
| | | |
| | | switch (ReadCarStatus(GEAR)) { |
| | | case GEAR_N: |
| | | if (moveDirect != 0) { |
| | | // 空档滑行 |
| | | currGearNSlide = true; |
| | | } |
| | | break; |
| | | case GEAR_1: |
| | | if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) { |
| | | currGearError = true; |
| | | } |
| | | break; |
| | | case GEAR_2: |
| | | if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) { |
| | | currGearError = true; |
| | | } |
| | | break; |
| | | case GEAR_3: |
| | | if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) { |
| | | currGearError = true; |
| | | } |
| | | break; |
| | | case GEAR_4: |
| | | if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) { |
| | | currGearError = true; |
| | | } |
| | | break; |
| | | case GEAR_5: |
| | | if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) { |
| | | currGearError = true; |
| | | } |
| | | break; |
| | | default:break; |
| | | } |
| | | // 空档滑行超时 |
| | | if (currGearNSlide && prevGearNSlide) { |
| | | gearNSlideTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, |
| | | gearNSlideTimePoint.hour, gearNSlideTimePoint.min, gearNSlideTimePoint.sec, gearNSlideTimePoint.msec*10); |
| | | } |
| | | if (gearNSlideTime > GEAR_N_SLIDE_TIMEOUT) { |
| | | // 空档滑行超5秒,不合格 |
| | | DEBUG("挡位滑行,超过5秒"); |
| | | AddExamFault(8, rtkTime); |
| | | gearNSlideTime = 0; |
| | | } |
| | | |
| | | prevGearNSlide = currGearNSlide; |
| | | if (prevGearNSlide) { |
| | | Rtk2DriveTimer(gearNSlideTimePoint, rtkTime); |
| | | } else { |
| | | gearNSlideTime = 0; |
| | | } |
| | | // 挡位不匹配超时 |
| | | if (currGearError && prevGearError) { |
| | | gearErrorTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, |
| | | gearErrorTimePoint.hour, gearErrorTimePoint.min, gearErrorTimePoint.sec, gearErrorTimePoint.msec*10); |
| | | } |
| | | if (gearErrorTime > GEAR_ERROR_TIMEOUT) { |
| | | // 累计15秒,挡位-车速不匹配,不合格 |
| | | DEBUG("挡位错误超过15秒"); |
| | | AddExamFault(6, rtkTime); |
| | | gearErrorTime = 0; |
| | | } |
| | | |
| | | prevGearError = currGearError; |
| | | if (prevGearError) { |
| | | Rtk2DriveTimer(gearErrorTimePoint, rtkTime); |
| | | } |
| | | |
| | | // 起步后滑 |
| | | if (moveDirect != prevMoveDirect) { |
| | | if (moveDirect == 0) { |
| | | stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | reportStopCarOnRedArea = false; |
| | | |
| | | DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss); |
| | | } else if (moveDirect == -1 && prevMoveDirect == 0) { |
| | | DEBUG("开始后滑"); |
| | | stopPoint = car->basePoint; |
| | | occurSlide = true; |
| | | } |
| | | 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 && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) { |
| | | // 停车超2秒,停在红区,不合格 |
| | | AddExamFault(16, rtkTime); |
| | | DEBUG("禁停区停车"); |
| | | reportStopCarOnRedArea = true; |
| | | } |
| | | } else if (moveDirect == -1) { |
| | | if (occurSlide) { |
| | | double slideDistance = DistanceOf(stopPoint, car->basePoint); |
| | | |
| | | if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) { |
| | | slideNormalDistance = true; |
| | | } |
| | | |
| | | if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) { |
| | | // 后滑超过30厘米, 不合格 |
| | | AddExamFault(5, rtkTime); |
| | | DEBUG("后滑超过30厘米"); |
| | | slideLongDistance = true; |
| | | } |
| | | } |
| | | } else { |
| | | if (slideNormalDistance) { |
| | | // 后滑,扣10分 |
| | | AddExamFault(18, rtkTime); |
| | | DEBUG("后滑超过10厘米, 但不超过30厘米"); |
| | | } |
| | | |
| | | slideNormalDistance = false; |
| | | slideLongDistance = false; |
| | | occurSlide = false; |
| | | } |
| | | |
| | | switch (ReadCarStatus(TURN_SIGNAL_LAMP)) { |
| | | case LEFT_TURN_LIGHT: |
| | | if (currTurnSignalStatus != LEFT_TURN_LIGHT) { |
| | | currTurnSignalStatus = LEFT_TURN_LIGHT; |
| | | Rtk2DriveTimer(turnSignalChangeTime, rtkTime); |
| | | } |
| | | break; |
| | | case RIGHT_TURN_LIGHT: |
| | | if (currTurnSignalStatus != RIGHT_TURN_LIGHT) { |
| | | currTurnSignalStatus = RIGHT_TURN_LIGHT; |
| | | Rtk2DriveTimer(turnSignalChangeTime, rtkTime); |
| | | } |
| | | break; |
| | | default: |
| | | currTurnSignalStatus = ReadCarStatus(TURN_SIGNAL_LAMP); |
| | | break; |
| | | } |
| | | |
| | | // 检查是否持续转向 |
| | | char turnDirect = CheckCarTurn(CarModelList); |
| | | if (turnDirect == 'L') { |
| | | // PlayTTS("左1"); |
| | | if (currTurnSignalStatus != LEFT_TURN_LIGHT) { |
| | | if (!reportTurnSignalError) { |
| | | DEBUG("没打左转灯"); |
| | | // 没打左转灯,不合格 |
| | | reportTurnSignalError = true; |
| | | ReportTurnSignalError(13, rtkTime); |
| | | } |
| | | } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, |
| | | turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) { |
| | | if (!reportTurnSignalError) { |
| | | DEBUG("转向灯时间不足"); |
| | | // 不足3秒,不合格 |
| | | reportTurnSignalError = true; |
| | | ReportTurnSignalError(14, rtkTime); |
| | | } |
| | | } |
| | | } else if (turnDirect == 'R') { |
| | | // PlayTTS("右1"); |
| | | if (currTurnSignalStatus != RIGHT_TURN_LIGHT) { |
| | | if (!reportTurnSignalError) { |
| | | DEBUG("没打右转灯"); |
| | | // 没打右转灯,不合格 |
| | | reportTurnSignalError = true; |
| | | ReportTurnSignalError(13, rtkTime); |
| | | } |
| | | } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, |
| | | turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) { |
| | | if (!reportTurnSignalError) { |
| | | DEBUG("转向灯时间不足"); |
| | | // 不足3秒,不合格 |
| | | reportTurnSignalError = true; |
| | | ReportTurnSignalError(14, rtkTime); |
| | | } |
| | | } |
| | | } else { |
| | | reportTurnSignalError = false; |
| | | } |
| | | |
| | | // 撞红线 |
| | | if (CrashRedLine(RoadMapList, car)) { |
| | | if (!occurCrashRedLine) { |
| | | // 车辆行驶中骑轧车道中心实线或者车道边缘实线,不合格 |
| | | DEBUG("撞道路边缘线"); |
| | | AddExamFault(11, rtkTime); |
| | | occurCrashRedLine = true; |
| | | } |
| | | } else { |
| | | occurCrashRedLine = false; |
| | | } |
| | | |
| | | // 撞绿线 |
| | | static PointF p1, p2; |
| | | if (CrashGreenLine(RoadMapList, car, p1, p2)) { |
| | | // 压虚线 |
| | | if (moveDirect != 0) { |
| | | if (checkCrashGreenTimeout == 0) { |
| | | checkCrashGreenTimeout = 1; |
| | | Rtk2DriveTimer(crashGreenRunTime, rtkTime); // 运动中压虚线的开始时间点 |
| | | } else if (checkCrashGreenTimeout == 1) { |
| | | uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, |
| | | crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10); |
| | | |
| | | if (diff >= CRASH_DOTTED_LINE_TIMEOUT) { |
| | | DEBUG("长时间压虚线"); |
| | | checkCrashGreenTimeout = 2; |
| | | // 长时间骑轧车道分界线行驶,不合格 |
| | | AddExamFault(12, rtkTime); |
| | | } |
| | | } |
| | | } else { |
| | | // 停车就不计时了 |
| | | checkCrashGreenTimeout = 0; |
| | | } |
| | | |
| | | // 检测当前车辆于虚线的位置,做变道检测; |
| | | // 检测是否3秒前有开启对应之转向灯 |
| | | if (!occurCrashGreenLine) { |
| | | occurCrashGreenLine = true; |
| | | // 记录开始压线的时间,不确定是否有变道意图,待确认变道后再处理之 |
| | | Rtk2DriveTimer(crashGreenStartTime, rtkTime); |
| | | turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus; |
| | | } |
| | | |
| | | // p1 ---------------> p2 |
| | | double angle = car->yaw - YawOf(p2, p1); |
| | | if (angle < 0 || angle > 180) { |
| | | // 右侧 |
| | | carIntersectionOfGreenLine = 'R'; |
| | | } else { |
| | | // 左侧 |
| | | carIntersectionOfGreenLine = 'L'; |
| | | } |
| | | } else { |
| | | // 不再压虚线 |
| | | if (occurCrashGreenLine) { |
| | | int inter = IntersectionOfLine(p1, p2, car->basePoint); |
| | | |
| | | // 完成跨线动作 |
| | | if ((inter == 1 && carIntersectionOfGreenLine == 'R') || |
| | | (inter == -1 && carIntersectionOfGreenLine == 'L')) { |
| | | // 比较上次跨线时间 |
| | | if (crashGreenCmpTime.hour >= 0) { |
| | | uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, |
| | | rtkTime->mss * 10, |
| | | crashGreenCmpTime.hour, crashGreenCmpTime.min, |
| | | crashGreenCmpTime.sec, crashGreenCmpTime.msec * 10); |
| | | |
| | | if (diff < CHANGE_LANE_MIN_INTERVAL) { |
| | | DEBUG("===================== 连续变道 ============!!"); |
| | | // 连续变道,不合格 |
| | | AddExamFault(15, rtkTime); |
| | | } |
| | | } |
| | | |
| | | // 记录本次变道时间点 |
| | | Rtk2DriveTimer(crashGreenCmpTime, rtkTime); |
| | | |
| | | // 检查变道前,是否提前转向灯 |
| | | if (inter == 1) { |
| | | // PlayTTS("左2"); |
| | | // 向左侧变道 |
| | | DEBUG("向左侧变道"); |
| | | if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) { |
| | | DEBUG("变调未打灯!!"); |
| | | // 没打灯,不合格 |
| | | ReportTurnSignalError(13, rtkTime); |
| | | } |
| | | } else { |
| | | // PlayTTS("右2"); |
| | | // 向右侧变道 |
| | | DEBUG("向右侧变道"); |
| | | if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) { |
| | | DEBUG("变调未打灯!!"); |
| | | // 没打灯,不合格 |
| | | ReportTurnSignalError(14, rtkTime); |
| | | } |
| | | } |
| | | } |
| | | } |
| | | occurCrashGreenLine = false; |
| | | checkCrashGreenTimeout = 0; |
| | | } |
| | | |
| | | // 完成起步后,触发线检测 |
| | | if (currExamMapIndex == -1 && startCar == START_CAR_DONE) { |
| | | currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList); |
| | | if (currExamMapIndex != -1) { |
| | | DEBUG("碰撞触发线"); |
| | | |
| | | MA_EnterMap(RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type, 1); |
| | | |
| | | if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP && |
| | | RoadMapList[currExamMapIndex].type <= TURN_AROUND_MAP) { |
| | | StartThroughExam(currExamMapIndex, RoadMapList); |
| | | } else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) { |
| | | |
| | | } else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) { |
| | | |
| | | } else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) { |
| | | |
| | | } |
| | | } |
| | | } else if (startCar == START_CAR_DONE) { |
| | | int prevIdx = currExamMapIndex; |
| | | |
| | | if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP && currExamMapIndex <= TURN_AROUND_MAP) { |
| | | currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car, |
| | | CarModelList, speed, moveDirect, rtkTime); |
| | | } |
| | | else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) { |
| | | |
| | | } else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) { |
| | | |
| | | } else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) { |
| | | |
| | | } |
| | | |
| | | if (currExamMapIndex == -1) { |
| | | DEBUG("离开区域 index %d id %d type %d", prevIdx, RoadMapList[prevIdx].id, RoadMapList[prevIdx].type); |
| | | |
| | | MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 0); |
| | | } |
| | | } |
| | | } |
| | | |
| | | |
| | | bool ExitSonArea(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car) |
| | | { |
| | | bool ret = false; |
| | | |
| | | if (index < 0 || index > RoadMapList.size()) |
| | | return true; |
| | | |
| | | if (RoadMapList[index].area.point != NULL) { |
| | | // 需要车身全部离开 |
| | | 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, &RoadMapList[index].area) == GM_None) { |
| | | ret = true; |
| | | } |
| | | |
| | | free(carBody.point); |
| | | } |
| | | |
| | | return ret; |
| | | } |
| | | |
| | | bool CrashSonRedLine(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList) |
| | | { |
| | | bool ret = false; |
| | | |
| | | if (CarModelList.size() < 5 || index < 0 || index >= RoadMapList.size()) |
| | | return ret; |
| | | |
| | | Polygon trace; |
| | | |
| | | trace.num = 5; // 最近5个轮轨迹 |
| | | trace.point = (PointF *) malloc(sizeof(PointF) * trace.num); |
| | | |
| | | list<car_model *>::iterator iter = CarModelList.begin(); |
| | | |
| | | int pn = 0; |
| | | while (iter != CarModelList.end() && pn < trace.num) { |
| | | trace.point[pn++] = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]]; |
| | | ++iter; |
| | | } |
| | | |
| | | // 每条线都检测 |
| | | for (int j = 0; j < RoadMapList[index].redLineNum; ++j) { |
| | | Line red_line; |
| | | |
| | | int kp = 0; |
| | | |
| | | // 触发线一般应该只有首尾2点(id, p1, p2) |
| | | for (int k = 1; k < RoadMapList[index].redLine[j].num; ++k) { |
| | | MakeLine(&red_line, &RoadMapList[index].redLine[j].point[kp], |
| | | &RoadMapList[index].redLine[j].point[k]); |
| | | |
| | | int pp = 0; |
| | | for (int p = 1; p < pn; ++p) { |
| | | Line trace_line; |
| | | MakeLine(&trace_line, &trace.point[pp], &trace.point[p]); |
| | | |
| | | if (IntersectionOf(trace_line, red_line) == GM_Intersection) { |
| | | // 碰到触发线 |
| | | ret = true; |
| | | goto SEARCH_SON_RED_LINE_END; |
| | | } |
| | | |
| | | pp = p; |
| | | } |
| | | |
| | | kp = k; |
| | | } |
| | | } |
| | | |
| | | SEARCH_SON_RED_LINE_END: |
| | | free(trace.point); |
| | | |
| | | return ret; |
| | | } |
| | | #endif |
| | | |
| | | void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime) |
| | | { |