| | |
| | | test_items2/operate_gear.cpp |
| | | test_items2/smart_item.cpp |
| | | test_items2/car_start.cpp |
| | | test_items2/overtake.cpp |
| | | test_items2/change_lane.cpp |
| | | |
| | | rtk_module/rtk.cpp |
| | | rtk_module/virtual_rtk.cpp |
| | |
| | | } |
| | | case SENSOR_DOOR: { |
| | | if (value == 0) { |
| | | WriteCarStatus(DOOR, DOOR_OPEN); |
| | | } else { |
| | | WriteCarStatus(DOOR, DOOR_CLOSE); |
| | | } else { |
| | | WriteCarStatus(DOOR, DOOR_OPEN); |
| | | } |
| | | break; |
| | | } |
New file |
| | |
| | | // |
| | | // Created by YY on 2020/8/21. |
| | | // |
| | | |
| | | #include "change_lane.h" |
| | | #include "../test_common/odo_graph.h" |
| | | #include "../native-lib.h" |
| | | #include "../jni_log.h" |
| | | #include "../driver_test.h" |
| | | |
| | | #define DEBUG(fmt, args...) LOGD("<change_lane> <%s>: " fmt, __func__, ##args) |
| | | |
| | | static double maxMoveDistance; |
| | | static int originalLane; |
| | | static bool start; |
| | | |
| | | static void TtsBack(int seq) |
| | | { |
| | | maxMoveDistance = ReadOdo(); |
| | | start = true; |
| | | } |
| | | |
| | | void StartChaneLaneExam(int ori_lane) |
| | | { |
| | | DEBUG("变更车道"); |
| | | |
| | | start = false; |
| | | originalLane = ori_lane; |
| | | PlayTTS("前方请变更车道", TtsBack); |
| | | } |
| | | |
| | | bool TestChangeLane(int currLane, const struct RtkTime *rtkTime) |
| | | { |
| | | if (!start) |
| | | return true; |
| | | if (originalLane != currLane) { |
| | | DEBUG("完成变道"); |
| | | PlayTTS("完成变道", NULL); |
| | | return false; |
| | | } |
| | | |
| | | if (ReadOdo() - maxMoveDistance > 100) { |
| | | // 超车未完成 |
| | | DEBUG("超车固定距离内未完成"); |
| | | AddExamFault(3, rtkTime); |
| | | return false; |
| | | } |
| | | |
| | | return true; |
| | | } |
New file |
| | |
| | | // |
| | | // Created by YY on 2020/8/21. |
| | | // |
| | | |
| | | #ifndef MYAPPLICATION2_CHANGE_LANE_H |
| | | #define MYAPPLICATION2_CHANGE_LANE_H |
| | | |
| | | void StartChaneLaneExam(int ori_lane); |
| | | |
| | | bool TestChangeLane(int currLane, const struct RtkTime *rtkTime); |
| | | |
| | | #endif //MYAPPLICATION2_CHANGE_LANE_H |
| | |
| | | #include "road_exam.h" |
| | | #include "../master/comm_if.h" |
| | | #include "../common/apptimer.h" |
| | | #include "../test_common/odo_graph.h" |
| | | #include <cmath> |
| | | |
| | | #define DEBUG(fmt, args...) LOGD("<drive_straight> <%s>: " fmt, __func__, ##args) |
| | |
| | | static const double CHECK_STAGE_DISTANCE = 100.0; |
| | | static const double MAX_OFFSET_DISTANCE = 0.3; |
| | | |
| | | static int ttsPlayEnd; |
| | | static bool crossStartLine; |
| | | static bool reportOffsetOver; |
| | | static double edgeDistance; |
| | | static double distanceToStartSum; |
| | | static int examTtsSeq; |
| | | static int setup; |
| | | static double beginOdo; |
| | | static int yaw_stat; |
| | | |
| | | static double CalcDistance2Edge(road_t &road, const car_model *car); |
| | | static void PlayTTSTimeout(union sigval sig); |
| | | static void TtsBack(int seq) |
| | | { |
| | | setup = 1; |
| | | } |
| | | |
| | | void StartDriveStraightExam(std::string tts) { |
| | | void StartDriveStraightExam(void) { |
| | | DEBUG("开始直线行驶"); |
| | | |
| | | ttsPlayEnd = 0; |
| | | if (!tts.empty()) { |
| | | examTtsSeq = PlayTTS(tts.c_str(), NULL); |
| | | } else { |
| | | examTtsSeq = PlayTTS("请保持直线行驶", NULL); |
| | | } |
| | | |
| | | distanceToStartSum = 0; |
| | | reportOffsetOver = false; |
| | | |
| | | AppTimer_delete(PlayTTSTimeout); |
| | | AppTimer_add(PlayTTSTimeout, D_SEC(5)); |
| | | setup = 0; |
| | | yaw_stat = 0; |
| | | PlayTTS("请保持直线行驶", TtsBack); |
| | | } |
| | | |
| | | int ExecuteDriveStraightExam(road_t &road, const car_model *car, |
| | | const trigger_line_t *item, const struct RtkTime *rtkTime) { |
| | | static PointF startPoint; |
| | | bool TestDriveStraight(const car_model *car, const struct RtkTime *rtkTime) { |
| | | static double yaw; |
| | | static Line baseLine; |
| | | |
| | | double dis2roadEdge = 0; |
| | | if (setup == 1) { |
| | | // 偏航角平均值 |
| | | if (yaw_stat == 0) { |
| | | yaw = car->yaw; |
| | | } else { |
| | | yaw += car->yaw; |
| | | } |
| | | yaw_stat++; |
| | | if (yaw_stat == 5) { |
| | | yaw = fmod(yaw, 360) / 5; |
| | | |
| | | if (ttsPlayEnd == 1) { |
| | | ttsPlayEnd = 2; |
| | | startPoint = car->basePoint; |
| | | edgeDistance = CalcDistance2Edge(road, car); // 基准边距 |
| | | |
| | | DEBUG("当前基准路边间距 %f", edgeDistance); |
| | | PointF extPoint = PointExtend(car->basePoint, 100, yaw); |
| | | MakeLine(&baseLine, &car->basePoint, &extPoint); |
| | | beginOdo = ReadOdo(); |
| | | setup = 2; |
| | | } |
| | | } else if (setup == 2) { |
| | | if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine) > MAX_OFFSET_DISTANCE) { |
| | | DEBUG("直线偏移大于30厘米"); |
| | | // 偏移大于30厘米,不合格 |
| | | AddExamFault(30, rtkTime); |
| | | return false; |
| | | } |
| | | } |
| | | |
| | | if (ttsPlayEnd != 2) |
| | | return 1; |
| | | |
| | | double distanceToStart = DistanceOf(car->basePoint, startPoint); |
| | | dis2roadEdge = CalcDistance2Edge(road, car); |
| | | |
| | | DEBUG("路边间距 %f --- %f", dis2roadEdge, edgeDistance); |
| | | |
| | | if (!reportOffsetOver && fabs(dis2roadEdge - edgeDistance) > MAX_OFFSET_DISTANCE) { |
| | | DEBUG("直线偏移大于30厘米"); |
| | | // 偏移大于30厘米,不合格 |
| | | AddExamFault(30, rtkTime); |
| | | reportOffsetOver = true; |
| | | |
| | | ////////////////////////////////////////////// |
| | | // startPoint = car->basePoint; |
| | | // edgeDistance = dis2roadEdge; |
| | | // reportOffsetOver = false; |
| | | } |
| | | |
| | | if (distanceToStart > CHECK_STAGE_DISTANCE) { |
| | | DEBUG("复位边距偏移量"); |
| | | startPoint = car->basePoint; |
| | | edgeDistance = dis2roadEdge; |
| | | reportOffsetOver = false; |
| | | distanceToStartSum += distanceToStart; |
| | | distanceToStart = 0; |
| | | } |
| | | |
| | | if (distanceToStart + distanceToStartSum > 105) { |
| | | DEBUG("离开直线行驶区域"); |
| | | if (setup == 2 && ReadOdo() - beginOdo > CHECK_STAGE_DISTANCE) { |
| | | DEBUG("直线行驶结束"); |
| | | PlayTTS("直线行驶结束", NULL); |
| | | return -1; |
| | | } |
| | | return 1; |
| | | } |
| | | |
| | | void DriveStraightTTSDone(int id) |
| | | { |
| | | // 等语音播报完毕后计时 |
| | | if (id == examTtsSeq) { |
| | | DEBUG("StopCarTTSDone %d", id); |
| | | ttsPlayEnd = 1; |
| | | AppTimer_delete(PlayTTSTimeout); |
| | | } |
| | | } |
| | | |
| | | void TerminateDriveStraightExam(void) |
| | | { |
| | | AppTimer_delete(PlayTTSTimeout); |
| | | } |
| | | |
| | | static void PlayTTSTimeout(union sigval sig) |
| | | { |
| | | AppTimer_delete(PlayTTSTimeout); |
| | | |
| | | ttsPlayEnd = 1; |
| | | } |
| | | |
| | | static double CalcDistance2Edge(road_t &road, const car_model *car) |
| | | { |
| | | PointF vp; |
| | | bool get_vp = false; |
| | | double distance = 0; |
| | | |
| | | // 检测道路边缘线 |
| | | for (int i = 0; i < road.leftEdge.size(); ++i) { |
| | | PointF p1, p2; |
| | | Line edge; |
| | | |
| | | p1 = road.leftEdge[i].points[0]; |
| | | for (int j = 1; j < road.leftEdge[i].points.size(); ++j) { |
| | | p2 = road.leftEdge[i].points[j]; |
| | | MakeLine(&edge, &p1, &p2); |
| | | |
| | | if (VerticalPointOnLine(car->basePoint, edge, vp)) { |
| | | get_vp = true; |
| | | goto FIND_VP_END; |
| | | } |
| | | |
| | | p1 = p2; |
| | | } |
| | | return false; |
| | | } |
| | | |
| | | FIND_VP_END: |
| | | if (get_vp) { |
| | | DEBUG("得到垂点 %d: %f, %f -- %f, %f", road.id, car->basePoint.X, car->basePoint.Y, vp.X, vp.Y); |
| | | distance = DistanceOf(car->basePoint, vp); |
| | | } else { |
| | | // 没有找到匹配线端,按最小距离顶点计算 |
| | | DEBUG("无垂点"); |
| | | distance = 100; |
| | | for (int i = 0; i < road.leftEdge.size(); ++i) { |
| | | for (int j = 0; j < road.leftEdge[i].points.size(); ++j) { |
| | | double x; |
| | | if (distance > (x = DistanceOf(car->basePoint, road.leftEdge[i].points[j]))) { |
| | | distance = x; |
| | | } |
| | | } |
| | | } |
| | | } |
| | | |
| | | return distance; |
| | | return true; |
| | | } |
| | |
| | | |
| | | #include "../driver_test.h" |
| | | |
| | | void StartDriveStraightExam(std::string tts); |
| | | int ExecuteDriveStraightExam(road_t &road, const car_model *car, |
| | | const trigger_line_t *item, const struct RtkTime *rtkTime); |
| | | void DriveStraightTTSDone(int id); |
| | | void TerminateDriveStraightExam(void); |
| | | void StartDriveStraightExam(void); |
| | | bool TestDriveStraight(const car_model *car, const struct RtkTime *rtkTime); |
| | | |
| | | #endif //MYAPPLICATION2_DRIVE_STRAIGHT_H |
| | |
| | | static int setup; |
| | | |
| | | static double maxMoveDistance; |
| | | static double gearMoveDistance; |
| | | |
| | | static void TtsBack(int seq) |
| | | { |
| | |
| | | |
| | | bool TestOperateGear(const struct RtkTime *rtkTime) |
| | | { |
| | | static struct RtkTime shiftTime; |
| | | |
| | | car_sensor_value_t sensor = ReadCarSensorValue(GEAR); |
| | | |
| | | if (sensor.name != GEAR) |
| | |
| | | return false; |
| | | } else { |
| | | // 在此挡位行驶一定距离,再执行下一个 |
| | | gearMoveDistance = ReadOdo(); |
| | | shiftTime = *rtkTime; |
| | | setup = 3; |
| | | } |
| | | } else if (setup == 3) { |
| | | if (ReadOdo() - gearMoveDistance > 10) { |
| | | if (TimeGetDiff(&shiftTime, rtkTime) >= D_SEC(5)) { |
| | | setup = 4; |
| | | char buff[128]; |
| | | expectGear += 0 - upDownShift; |
| | |
| | | return false; |
| | | } else { |
| | | // 在此挡位行驶一定距离,再执行下一个 |
| | | gearMoveDistance = ReadOdo(); |
| | | shiftTime = *rtkTime; |
| | | setup = 5; |
| | | } |
| | | } else if (setup == 5) { |
| | | if (ReadOdo() - gearMoveDistance > 10) { |
| | | if (TimeGetDiff(&shiftTime, rtkTime) >= D_SEC(5)) { |
| | | PlayTTS("加减挡位操作结束", NULL); |
| | | return false; |
| | | } |
New file |
| | |
| | | // |
| | | // Created by YY on 2020/8/21. |
| | | // |
| | | |
| | | #include "overtake.h" |
| | | #include "../native-lib.h" |
| | | #include "../jni_log.h" |
| | | #include "../test_common/odo_graph.h" |
| | | #include "../driver_test.h" |
| | | |
| | | #define DEBUG(fmt, args...) LOGD("<overtake> <%s>: " fmt, __func__, ##args) |
| | | |
| | | static double maxMoveDistance; |
| | | static int setup; |
| | | static int originalLane; |
| | | |
| | | static void TtsBack(int seq) |
| | | { |
| | | maxMoveDistance = ReadOdo(); |
| | | setup = 1; |
| | | } |
| | | |
| | | void StartOvertakeExam(int ori_lane) |
| | | { |
| | | DEBUG("超越前方车辆"); |
| | | |
| | | PlayTTS("请超越前方车辆", TtsBack); |
| | | |
| | | setup = 0; |
| | | originalLane = ori_lane; |
| | | } |
| | | |
| | | bool TestOvertake(int currLane, const struct RtkTime *rtkTime) |
| | | { |
| | | if (setup == 0) { |
| | | return true; |
| | | } |
| | | if (setup == 1) { |
| | | if (originalLane == currLane) { |
| | | |
| | | } else if (currLane + 1 == originalLane) { |
| | | setup = 2; |
| | | originalLane = currLane; |
| | | } else { |
| | | DEBUG("右侧超车"); |
| | | AddExamFault(3, rtkTime); |
| | | return false; |
| | | } |
| | | } else if (setup == 2) { |
| | | if (originalLane == currLane) { |
| | | |
| | | } else if (currLane == originalLane + 1) { |
| | | PlayTTS("完成超车", NULL); |
| | | return false; |
| | | } else { |
| | | DEBUG("超车违规变道"); |
| | | AddExamFault(3, rtkTime); |
| | | return false; |
| | | } |
| | | } |
| | | |
| | | if (ReadOdo() - maxMoveDistance > 150) { |
| | | // 超车未完成 |
| | | DEBUG("超车固定距离内未完成"); |
| | | AddExamFault(3, rtkTime); |
| | | return false; |
| | | } |
| | | |
| | | return true; |
| | | } |
New file |
| | |
| | | // |
| | | // Created by YY on 2020/8/21. |
| | | // |
| | | |
| | | #ifndef MYAPPLICATION2_OVERTAKE_H |
| | | #define MYAPPLICATION2_OVERTAKE_H |
| | | |
| | | void StartOvertakeExam(int ori_lane); |
| | | |
| | | bool TestOvertake(int currLane, const struct RtkTime *rtkTime); |
| | | |
| | | #endif //MYAPPLICATION2_OVERTAKE_H |
| | |
| | | #include "operate_gear.h" |
| | | #include "../test_common/odo_graph.h" |
| | | #include "car_start.h" |
| | | #include "overtake.h" |
| | | #include "change_lane.h" |
| | | |
| | | #include <cmath> |
| | | #include <vector> |
| | |
| | | |
| | | using namespace std; |
| | | |
| | | #define TURN_CHECK_CNT 6 |
| | | |
| | | enum { |
| | | START_CAR_NOT_DO, |
| | | START_CAR_DOING, |
| | | START_CAR_DONE |
| | | }; |
| | | |
| | | enum { |
| | | STOP_CAR_NOT_DO, |
| | | STOP_CAR_DOING, |
| | | STOP_CAR_DONE |
| | | }; |
| | | |
| | | enum { |
| | | CROSSING_NOT_HINT, |
| | |
| | | |
| | | static const int CROSSING_TURN_THRESHOLD = 45; |
| | | static const int TURN_THRESHOLD = 3; |
| | | static const int TURN_CHECK_INTERVAL = 500; |
| | | |
| | | const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3; |
| | | 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(3); // 在超车道行驶的一段时间 |
| | | |
| | | const double EXAM_RANGE = 3000.0; // 至少驾驶距离 |
| | | |
| | | static const double NEXT_ROAD_TIP = 100.0; // 到达路口前提示下一个怎么走 |
| | | |
| | | |
| | | static bool occurCrashRedLine; |
| | | static bool occurCrashGreenLine; |
| | | static bool occurOverSpeed; |
| | | static bool occurSecondBreak; |
| | | |
| | | static char carIntersectionOfGreenLine; |
| | | static int currTurnSignalStatus; |
| | | static int turnSignalStatusWhenCrashGreenLine; |
| | | |
| | | static int prevMoveDirect; |
| | | static uint32_t stopTimepoint = 0; |
| | |
| | | |
| | | static struct drive_timer gearErrorTimePoint; |
| | | static struct drive_timer gearNSlideTimePoint; |
| | | static struct drive_timer startCarLeftTurnSignalTime; |
| | | static struct drive_timer overTakeCmpTime; |
| | | |
| | | static int gearErrorTime; |
| | | static int gearNSlideTime; |
| | | |
| | | static int stopCar; |
| | | static int currExamMapIndex; |
| | | static trigger_line_t *currRoadItem; |
| | | static PointF roadItemStartPoint; |
| | | static struct drive_timer roadItemStartTime; |
| | | static bool overtake = false; |
| | | static bool checkTurn = false; |
| | | |
| | | static lane_t Lane; |
| | | static change_lane_t ChangeLane; |
| | |
| | | static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10); |
| | | static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3); |
| | | |
| | | static const int CRL_NONE = 0; |
| | | static const int CRL_SEP_DOTTED = 1; |
| | | static const int CRL_SEP_SOLID = 2; |
| | | static const int CRL_EDGE_DOTTED = 3; |
| | | static const int CRL_EDGE_SOLID = 4; |
| | | |
| | | static const double MAX_SPEED = 60.0 * 1000.0 / 3600.0; |
| | | static const double MAX_SPEED = 60.0 * 1000.0 / 3600.0; // 超速确认 |
| | | static const double DEC_MAX_SPEED = 55.0 * 1000.0 / 3600.0; // 超速取消 |
| | | static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}}; |
| | | |
| | | static void ItemExam(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end); |
| | | static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end); |
| | | static int isTurn(int currYaw, int prevYaw, int thres); |
| | | static void ResetTurnDetect(const car_model *car); |
| | | static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime); |
| | |
| | | static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car); |
| | | |
| | | static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList); |
| | | |
| | | static bool AllCmp(road_exam_map &map); |
| | | |
| | | static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car); |
| | | |
| | |
| | | { |
| | | DEBUG("Start road_exam"); |
| | | |
| | | occurCrashRedLine = false; |
| | | occurCrashGreenLine = false; |
| | | occurOverSpeed = false; |
| | | |
| | | occurSecondBreak = false; |
| | | |
| | | carIntersectionOfGreenLine = 0; |
| | | |
| | | currTurnSignalStatus = OFF_LIGHT; |
| | | prevMoveDirect = 0; |
| | | |
| | | reportStopCarOnRedArea = false; |
| | |
| | | |
| | | currExamMapIndex = -1; |
| | | |
| | | stopCar = STOP_CAR_NOT_DO; |
| | | |
| | | currRoadItem = NULL; |
| | | |
| | | Lane.road = Lane.sep = Lane.no = -1; |
| | | Lane.guide = 0; |
| | | |
| | | checkTurn = false; |
| | | |
| | | CrashLineType = -1; |
| | | turnCnt = -1; |
| | |
| | | DEBUG("变调未打灯!!"); |
| | | // 没打灯,不合格 |
| | | AddExamFault(13, rtkTime); |
| | | } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) { |
| | | } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) { |
| | | DEBUG("转向灯时间不足"); |
| | | // 不足3秒,不合格 |
| | | AddExamFault(14, rtkTime); |
| | |
| | | DEBUG("变调未打灯!!"); |
| | | // 没打灯,不合格 |
| | | AddExamFault(13, rtkTime); |
| | | } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) { |
| | | } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) { |
| | | DEBUG("转向灯时间不足"); |
| | | // 不足3秒,不合格 |
| | | AddExamFault(14, rtkTime); |
| | |
| | | return guide; |
| | | } |
| | | |
| | | static uint8_t itemExec[4] = {0}; |
| | | static double odo; |
| | | |
| | | void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) |
| | | { |
| | | double BigStraightRoadFree = 0, RoadCrossingFree = 0; |
| | |
| | | UpdataOdo(speed, moveDirect, rtkTime); |
| | | |
| | | // 超速检测 |
| | | if (moveDirect != 0 && speed > MAX_SPEED) { |
| | | if (speed > MAX_SPEED) { |
| | | if (!occurOverSpeed) { |
| | | occurOverSpeed = true; |
| | | // 超速,不合格 |
| | | DEBUG("超速 %f", speed); |
| | | DEBUG("超速 %f", ConvertMs2KMh(speed)); |
| | | AddExamFault(10, rtkTime); |
| | | } |
| | | } else { |
| | | } else if (speed < DEC_MAX_SPEED ) { |
| | | occurOverSpeed = false; |
| | | } |
| | | |
| | |
| | | |
| | | RoadCrossingFree = freeSepDis; |
| | | |
| | | DEBUG("直道剩余距离 %f, 车道剩余距离 %f", BigStraightRoadFree, RoadCrossingFree); |
| | | // DEBUG("累计行驶距离 %f, 直道剩余距离 %f, 车道剩余距离 %f", ReadOdo(), BigStraightRoadFree, RoadCrossingFree); |
| | | } |
| | | |
| | | // 检测压线状态 |
| | |
| | | // 额外的转向检测 |
| | | DetectTurn(car, moveDirect, rtkTime); |
| | | |
| | | ItemExam(RoadMap, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, RoadCrossingFree); |
| | | |
| | | if (ReadOdo() > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) { |
| | | // 在合适条件下停车结束考试 |
| | | StartStopCarExam("请靠边停车"); |
| | | stopCar = STOP_CAR_DOING; |
| | | } else if (stopCar == STOP_CAR_DOING) { |
| | | if (ExecuteStopCarExam(RoadMap.roads[currExamMapIndex], car, CarModelList, speed, moveDirect, rtkTime) < 0) |
| | | stopCar = STOP_CAR_DONE; |
| | | } |
| | | |
| | | // 执行某个项目 |
| | | /*if (currRoadItem != NULL) { |
| | | if (currRoadItem->active == ROAD_ITEM_CHANGE_LANE) { |
| | | if (DistanceOf(car->basePoint, roadItemStartPoint) > CHANGE_LANE_RANGE) { |
| | | DEBUG("变道距离超标"); |
| | | AddExamFault(3, rtkTime); |
| | | currRoadItem = NULL; |
| | | } |
| | | } else if (currRoadItem->active == ROAD_ITEM_OVERTAKE) { |
| | | if (!overtake && DistanceOf(car->basePoint, roadItemStartPoint) > OVERTAKE_RANGE) { |
| | | DEBUG("超车距离超标"); |
| | | AddExamFault(3, rtkTime); |
| | | currRoadItem = NULL; |
| | | } else if (overtake && TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, |
| | | rtkTime->mss * 10, |
| | | overTakeCmpTime.hour, overTakeCmpTime.min, |
| | | overTakeCmpTime.sec, overTakeCmpTime.msec * 10) > OVERTAKE_HOLD_TIME) { |
| | | DEBUG("回原车道"); |
| | | PlayTTS("请返回原车道"); |
| | | currRoadItem = NULL; |
| | | } |
| | | } else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) { |
| | | if (ExecuteOperateGearExam(rtkTime) < 0) { |
| | | currRoadItem = NULL; |
| | | } |
| | | } else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) { |
| | | if (ExecuteDriveStraightExam(RoadMap.roads[currExamMapIndex], car, currRoadItem, rtkTime) < 0) { |
| | | currRoadItem = NULL; |
| | | } |
| | | } |
| | | } // 检测由触发线控制的项目 |
| | | else if (currExamMapIndex >= 0) { |
| | | trigger_line_t *new_item = EntryItem(currExamMapIndex, RoadMap, car, CarModelList); |
| | | |
| | | if (new_item != NULL && !new_item->cmp) { |
| | | currRoadItem = new_item; |
| | | if (!currRoadItem->tts.empty()) |
| | | PlayTTS(currRoadItem->tts.c_str()); |
| | | |
| | | // 初始时间和距离限制 |
| | | roadItemStartPoint = car->basePoint; |
| | | Rtk2DriveTimer(roadItemStartTime, rtkTime); |
| | | |
| | | if (currRoadItem->active == ROAD_ITEM_OVERTAKE) { |
| | | overtake = false; |
| | | } else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) { |
| | | StartOperateGearExam(rtkTime); |
| | | } else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) { |
| | | StartDriveStraightExam(currRoadItem->tts); |
| | | } |
| | | } |
| | | }*/ |
| | | ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, RoadCrossingFree); |
| | | } |
| | | |
| | | static void ItemExam(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end) |
| | | static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end) |
| | | { |
| | | static double freeRunDistance; |
| | | static double totalRunDistance; |
| | | static double freeRunExceptDistance = 50.0; |
| | | |
| | | if (RoadExamStatus == ROAD_EXAM_READY_NEXT) { |
| | | if (RoadExamItem[ROAD_EXAM_ITEM_CAR_START] == ROAD_EXAM_ITEM_NOT_EXEC) { |
| | |
| | | totalRunDistance = ReadOdo(); |
| | | RoadExamStatus = ROAD_EXAM_ITEM_CAR_START; |
| | | } else { |
| | | if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_NOT_EXEC) { |
| | | if (straight > 170 && road_end > 170) { |
| | | bool not_complete = false; |
| | | |
| | | if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_NOT_EXEC) { |
| | | not_complete = true; |
| | | if (straight > 170 && road_end > 170) { |
| | | StartDriveStraightExam(); |
| | | RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT; |
| | | return; |
| | | } |
| | | } |
| | | if (RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] == ROAD_EXAM_ITEM_NOT_EXEC) { |
| | | if (straight > 150 && road_end > 150) { |
| | | not_complete = true; |
| | | if (road_end > 170) { |
| | | StartOperateGearExam(); |
| | | RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR; |
| | | return; |
| | | } |
| | | } |
| | | if (RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] == ROAD_EXAM_ITEM_NOT_EXEC) { |
| | | |
| | | not_complete = true; |
| | | if (road_end > 150 && Lane.total > 1) { |
| | | StartChaneLaneExam(Lane.no); |
| | | RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE; |
| | | return; |
| | | } |
| | | } |
| | | if (RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] == ROAD_EXAM_ITEM_NOT_EXEC) { |
| | | |
| | | not_complete = true; |
| | | if (road_end > 200 && Lane.total > 1) { |
| | | if (Lane.no == 0) { |
| | | // 已在最左车道,先变更车道? |
| | | StartChaneLaneExam(Lane.no); |
| | | RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE; |
| | | } else { |
| | | StartOvertakeExam(Lane.no); |
| | | RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE; |
| | | } |
| | | return; |
| | | } |
| | | } |
| | | |
| | | if (!not_complete) { |
| | | if (road_end > 200 && ReadOdo() > EXAM_RANGE) { |
| | | RoadExamStatus = ROAD_EXAM_ITEM_CAR_STOP; |
| | | StartStopCarExam(); |
| | | return; |
| | | } |
| | | } |
| | | } |
| | | } else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) { |
| | | if (ReadOdo() - freeRunDistance > 300) { |
| | | if (ReadOdo() - freeRunDistance > freeRunExceptDistance) { |
| | | RoadExamStatus = ROAD_EXAM_READY_NEXT; |
| | | |
| | | } |
| | | } else { |
| | | bool testing = false; |
| | |
| | | case ROAD_EXAM_ITEM_OP_GEAR: |
| | | testing = TestOperateGear(rtkTime); |
| | | break; |
| | | case ROAD_EXAM_ITEM_CHANGE_LANE: |
| | | testing = TestChangeLane(Lane.no, rtkTime); |
| | | break; |
| | | case ROAD_EXAM_ITEM_OVER_TAKE: |
| | | testing = TestOvertake(Lane.no, rtkTime); |
| | | break; |
| | | case ROAD_EXAM_ITEM_STRAIGHT: |
| | | testing = TestDriveStraight(car, rtkTime); |
| | | break; |
| | | case ROAD_EXAM_ITEM_CAR_STOP: |
| | | testing = TestStopCar(RoadMap, roadIndex, car, moveDirect, rtkTime); |
| | | break; |
| | | default:break; |
| | | } |
| | | |
| | | if (!testing) { |
| | | RoadExamItem[RoadExamStatus] = ROAD_EXAM_ITEM_EXECED; |
| | | if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_START) { |
| | | freeRunExceptDistance = 60.0; |
| | | } else { |
| | | freeRunExceptDistance = 200.0; |
| | | } |
| | | |
| | | RoadExamStatus = ROAD_EXAM_FREE_RUN; |
| | | freeRunDistance = ReadOdo(); |
| | | } |
| | | } |
| | | |
| | | } |
| | | |
| | | void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime) |
| | |
| | | DEBUG("变调未打灯!!"); |
| | | // 没打灯,不合格 |
| | | AddExamFault(13, rtkTime); |
| | | } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) { |
| | | } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) { |
| | | DEBUG("转向灯时间不足"); |
| | | // 不足3秒,不合格 |
| | | AddExamFault(14, rtkTime); |
| | |
| | | DEBUG("变调未打灯!!"); |
| | | // 没打灯,不合格 |
| | | AddExamFault(13, rtkTime); |
| | | } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) { |
| | | } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) { |
| | | DEBUG("转向灯时间不足"); |
| | | // 不足3秒,不合格 |
| | | AddExamFault(14, rtkTime); |
| | |
| | | } |
| | | |
| | | return NULL; |
| | | } |
| | | |
| | | static bool AllCmp(road_exam_map &map) |
| | | { |
| | | for (int i = 0; i < map.triggerLines.size(); ++i) { |
| | | if (!map.triggerLines[i].cmp) |
| | | return false; |
| | | } |
| | | return true; |
| | | } |
| | | |
| | | static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car) |
| | |
| | | #include "../common/apptimer.h" |
| | | #include "../test_common/car_sensor.h" |
| | | #include "../defs.h" |
| | | #include "../test_common/odo_graph.h" |
| | | |
| | | #define DEBUG(fmt, args...) LOGD("<stop_car> <%s>: " fmt, __func__, ##args) |
| | | |
| | | static bool inactiveBreakHandbreakTogether, notCloseEngine, inactiveHandBreakAfterOpenDoor, occurOpenDoor, doorNotClose, checkRoadDistance; |
| | | static int examTtsSeq = 0; |
| | | static int ttsPlayEnd; |
| | | static double moveDistance; |
| | | static double prevSpeed; |
| | | static struct drive_timer prevPointTime; |
| | | static int prevMoveDirect; |
| | | static uint32_t stopTimepoint = 0; |
| | | static uint32_t openDoorTimepoint; |
| | | static bool BreakHandbreakReleaseSametime, OpenDoor; |
| | | |
| | | const int ENGINE_MIN_ROTATE = 200; |
| | | static const int ENGINE_MIN_ROTATE = 200; |
| | | static const double MAX_STOP_DISTANCE = 150; |
| | | |
| | | static const uint32_t STOP_CAR_TIME = D_SEC(2); |
| | | static const uint32_t OPEN_DOOR_TIMEOUT = D_SEC(15); |
| | | static const double DISTANCE_TO_ROAD_EDGE_1 = 0.5; |
| | | static const double DISTANCE_TO_ROAD_EDGE_2 = 0.3; |
| | | |
| | | static void PlayTTSTimeout(union sigval sig); |
| | | static double beginOdo; |
| | | static int setup; |
| | | |
| | | void StartStopCarExam(std::string tts) { |
| | | static void TtsBack(int seq) |
| | | { |
| | | setup = 1; |
| | | } |
| | | |
| | | void StartStopCarExam(void) { |
| | | DEBUG("靠边停车"); |
| | | ttsPlayEnd = 0; |
| | | moveDistance = 0; |
| | | prevMoveDirect = 0; |
| | | inactiveBreakHandbreakTogether = false; |
| | | notCloseEngine = false; |
| | | inactiveHandBreakAfterOpenDoor = false; |
| | | occurOpenDoor = false; |
| | | doorNotClose = false; |
| | | checkRoadDistance = false; |
| | | |
| | | if (!tts.empty()) { |
| | | examTtsSeq = PlayTTS(tts.c_str(), NULL); |
| | | } else { |
| | | examTtsSeq = PlayTTS("请靠边停车", NULL); |
| | | } |
| | | |
| | | AppTimer_delete(PlayTTSTimeout); |
| | | AppTimer_add(PlayTTSTimeout, D_SEC(8)); |
| | | BreakHandbreakReleaseSametime = false; |
| | | setup = 0; |
| | | OpenDoor = false; |
| | | PlayTTS("请靠边停车", TtsBack); |
| | | } |
| | | |
| | | void StopCarTTSDone(int id) |
| | | { |
| | | // 等语音播报完毕后计时 |
| | | if (id == examTtsSeq) { |
| | | DEBUG("StopCarTTSDone %d", id); |
| | | ttsPlayEnd = 1; |
| | | } |
| | | } |
| | | bool TestStopCar(road_exam_map &RoadMap, int roadIndex, const car_model *car, int moveDirect, const struct RtkTime *rtkTime) { |
| | | struct RtkTime time; |
| | | |
| | | void TerminateStopCarExam(void) |
| | | { |
| | | AppTimer_delete(PlayTTSTimeout); |
| | | } |
| | | if (setup == 0) |
| | | return true; |
| | | |
| | | int ExecuteStopCarExam(road_t &road, const car_model *car, |
| | | LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) { |
| | | if (ttsPlayEnd == 1) { |
| | | ttsPlayEnd = 2; |
| | | prevSpeed = speed; |
| | | Rtk2DriveTimer(prevPointTime, rtkTime); |
| | | } |
| | | |
| | | if (ttsPlayEnd != 2) |
| | | return 1; |
| | | |
| | | uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, |
| | | rtkTime->mss * 10, |
| | | prevPointTime.hour, prevPointTime.min, |
| | | prevPointTime.sec, prevPointTime.msec * 10); |
| | | if (diff >= D_SEC(1)) { |
| | | moveDistance += (double)diff * (speed + prevSpeed) / 2.0 / 1000.0; |
| | | prevSpeed = speed; |
| | | Rtk2DriveTimer(prevPointTime, rtkTime); |
| | | DEBUG("停车已行驶距离 %f", moveDistance); |
| | | } |
| | | |
| | | if (moveDistance > 150) { |
| | | // 150米内未停车,不合格 |
| | | if (roadIndex < 0) { |
| | | DEBUG("停车距离超标,靠边停车结束"); |
| | | AddExamFault(33, rtkTime); |
| | | return -1; |
| | | return false; |
| | | } |
| | | |
| | | if (moveDirect != prevMoveDirect) { |
| | | if (setup == 1) { |
| | | beginOdo = ReadOdo(); |
| | | setup = 2; |
| | | } else if (setup == 2) { |
| | | if (moveDirect == 0) { |
| | | stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | DEBUG("####### 靠边停车,停车了 %d %d %d %d %d %d %d ######", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss); |
| | | time = *rtkTime; |
| | | setup = 3; |
| | | } |
| | | 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 && !checkRoadDistance) { |
| | | } else if (setup == 3) { |
| | | if (moveDirect != 0) { |
| | | setup = 2; |
| | | } else if (TimeGetDiff(rtkTime, &time) >= D_SEC(2)) { |
| | | // 停车超2秒,开始判断 |
| | | DEBUG("检测和路边的距离"); |
| | | PointF pc, vp; |
| | | bool get_vp = false; |
| | | double dis2roadEdge = 0; |
| | | |
| | | checkRoadDistance = true; |
| | | // 前后轮的中点 |
| | | pc.X = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].X) / 2; |
| | | pc.Y = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].Y) / 2; |
| | | PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]); |
| | | PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]); |
| | | |
| | | // 检测道路边缘线 |
| | | for (int i = 0; i < road.rightEdge.size(); ++i) { |
| | | PointF p1, p2; |
| | | Line edge; |
| | | |
| | | p1 = road.rightEdge[i].points[0]; |
| | | for (int j = 1; j < road.rightEdge[i].points.size(); ++j) { |
| | | p2 = road.rightEdge[i].points[j]; |
| | | MakeLine(&edge, &p1, &p2); |
| | | |
| | | |
| | | if (VerticalPointOnLine(pc, edge, vp)) { |
| | | get_vp = true; |
| | | goto FIND_VP_END; |
| | | } |
| | | |
| | | p1 = p2; |
| | | } |
| | | } |
| | | |
| | | FIND_VP_END: |
| | | if (get_vp) { |
| | | dis2roadEdge = DistanceOf(pc, vp); |
| | | } else { |
| | | // 没有找到匹配线端,按最小距离顶点计算 |
| | | dis2roadEdge = 100; |
| | | for (int i = 0; i < road.rightEdge.size(); ++i) { |
| | | for (int j = 0; j < road.rightEdge[i].points.size(); ++j) { |
| | | double dis; |
| | | if (dis2roadEdge > (dis = DistanceOf(pc, road.rightEdge[i].points[j]))) { |
| | | dis2roadEdge = dis; |
| | | } |
| | | } |
| | | } |
| | | } |
| | | |
| | | DEBUG("停车距路边距离 %f", dis2roadEdge); |
| | | |
| | | if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_1) { |
| | | if (DistanceOf(p1, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]) > DISTANCE_TO_ROAD_EDGE_1 || |
| | | DistanceOf(p2, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]) > DISTANCE_TO_ROAD_EDGE_1) { |
| | | DEBUG("停车超出路边0.5米"); |
| | | // 停车距离超过50厘米,不合格 |
| | | AddExamFault(36, rtkTime); |
| | | } else if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_2) { |
| | | return false; |
| | | } else if (DistanceOf(p1, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]) > DISTANCE_TO_ROAD_EDGE_2 || |
| | | DistanceOf(p2, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]) > DISTANCE_TO_ROAD_EDGE_2) { |
| | | DEBUG("停车超出路边0.3米"); |
| | | // 停车距离超过30厘米,扣10分 |
| | | AddExamFault(37, rtkTime); |
| | | return false; |
| | | } |
| | | |
| | | setup = 4; |
| | | } |
| | | } |
| | | } else if (setup == 4) { |
| | | car_sensor_value_t brk = ReadCarSensorValue(BREAK); |
| | | car_sensor_value_t hbrk = ReadCarSensorValue(HAND_BREAK); |
| | | car_sensor_value_t door = ReadCarSensorValue(DOOR); |
| | | car_sensor_value_t rpm = ReadCarSensorValue(ENGINE_RPM); |
| | | |
| | | if (!inactiveBreakHandbreakTogether && ReadCarStatus(BREAK) == BREAK_INACTIVE && ReadCarStatus(HAND_BREAK) == BREAK_INACTIVE) { |
| | | // 拉手刹前,松脚刹,扣10分 |
| | | DEBUG("拉手刹前,松脚刹"); |
| | | AddExamFault(39, rtkTime); |
| | | inactiveBreakHandbreakTogether = true; |
| | | } |
| | | if (!BreakHandbreakReleaseSametime && brk.value == BREAK_INACTIVE && hbrk.value == BREAK_INACTIVE) { |
| | | // 拉手刹前,松脚刹,扣10分 |
| | | DEBUG("拉手刹前,松脚刹"); |
| | | AddExamFault(39, rtkTime); |
| | | BreakHandbreakReleaseSametime = true; |
| | | } |
| | | |
| | | if (!notCloseEngine && ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE && ReadCarStatus(DOOR) == DOOR_OPEN) { |
| | | // 下车前,不熄火,扣5分 |
| | | DEBUG("下车前,不熄火"); |
| | | AddExamFault(40, rtkTime); |
| | | if (door.value == DOOR_OPEN) { |
| | | if (!OpenDoor) { |
| | | if (rpm.value < ENGINE_MIN_ROTATE) { |
| | | // 下车前,不熄火,扣5分 |
| | | DEBUG("下车前,不熄火"); |
| | | AddExamFault(40, rtkTime); |
| | | } |
| | | if (hbrk.value == BREAK_INACTIVE) { |
| | | // 开门前,未拉手刹, 扣10分 |
| | | DEBUG("开门前,未拉手刹"); |
| | | AddExamFault(38, rtkTime); |
| | | } |
| | | time = *rtkTime; |
| | | OpenDoor = true; |
| | | } |
| | | |
| | | notCloseEngine = true; |
| | | } |
| | | |
| | | if (!inactiveHandBreakAfterOpenDoor && ReadCarStatus(DOOR) == DOOR_OPEN && ReadCarStatus(HAND_BREAK) == BREAK_INACTIVE) { |
| | | // 开门前,未拉手刹, 扣10分 |
| | | DEBUG("开门前,未拉手刹"); |
| | | AddExamFault(38, rtkTime); |
| | | |
| | | inactiveHandBreakAfterOpenDoor = true; |
| | | } |
| | | |
| | | if (ReadCarStatus(DOOR) == DOOR_OPEN) { |
| | | if (!occurOpenDoor) { |
| | | occurOpenDoor = true; |
| | | openDoorTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | } else { |
| | | uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | |
| | | if (!doorNotClose && tp - openDoorTimepoint >= OPEN_DOOR_TIMEOUT) { |
| | | if (TimeGetDiff(rtkTime, &time) > OPEN_DOOR_TIMEOUT) { |
| | | // 开门时间超过15秒,不合格 |
| | | DEBUG("开门时间超过15秒"); |
| | | AddExamFault(35, rtkTime); |
| | | doorNotClose = true; |
| | | return false; |
| | | } |
| | | } |
| | | } else if (occurOpenDoor) { |
| | | DEBUG("开车门,靠边停车结束"); |
| | | return -1; |
| | | } |
| | | |
| | | if (occurOpenDoor) { |
| | | uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | |
| | | if (tp - openDoorTimepoint >= OPEN_DOOR_TIMEOUT + D_SEC(5)) { |
| | | DEBUG("开门时间大于20秒,靠边停车结束"); |
| | | return -1; |
| | | if (OpenDoor && door.value == DOOR_CLOSE) { |
| | | DEBUG("完成停车"); |
| | | PlayTTS("靠边停车结束", NULL); |
| | | return false; |
| | | } |
| | | } |
| | | |
| | | return 1; |
| | | } |
| | | if (ReadOdo() - beginOdo > MAX_STOP_DISTANCE) { |
| | | // 150米内未停车,不合格 |
| | | DEBUG("停车距离超标,靠边停车结束"); |
| | | AddExamFault(33, rtkTime); |
| | | return false; |
| | | } |
| | | |
| | | static void PlayTTSTimeout(union sigval sig) |
| | | { |
| | | AppTimer_delete(PlayTTSTimeout); |
| | | |
| | | ttsPlayEnd = 1; |
| | | return true; |
| | | } |
| | |
| | | |
| | | #include "../driver_test.h" |
| | | |
| | | void StartStopCarExam(std::string tts); |
| | | int ExecuteStopCarExam(road_t &road, const car_model *car, |
| | | LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime); |
| | | void TerminateStopCarExam(void); |
| | | void StopCarTTSDone(int id); |
| | | void StartStopCarExam(void); |
| | | bool TestStopCar(road_exam_map &RoadMap, int roadIndex, const car_model *car, int moveDirect, const struct RtkTime *rtkTime); |
| | | |
| | | #endif //MYAPPLICATION2_STOP_CAR_H |