// // Created by YY on 2020/3/24. // #include "stop_car.h" #include "../driver_test.h" #include "../native-lib.h" #include "../jni_log.h" #include "road_exam.h" #include "../utils/xconvert.h" #include "../common/apptimer.h" #include "../test_common/car_sensor.h" #include "../defs.h" #define DEBUG(fmt, args...) LOGD(" <%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; const int ENGINE_MIN_ROTATE = 200; 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); void StartStopCarExam(std::string tts) { 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()); } else { examTtsSeq = PlayTTS("请靠边停车"); } AppTimer_delete(PlayTTSTimeout); AppTimer_add(PlayTTSTimeout, D_SEC(8)); } void StopCarTTSDone(int id) { // 等语音播报完毕后计时 if (id == examTtsSeq) { DEBUG("StopCarTTSDone %d", id); ttsPlayEnd = 1; } } void TerminateStopCarExam(void) { AppTimer_delete(PlayTTSTimeout); } int ExecuteStopCarExam(int index, 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 index; 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米内未停车,不合格 DEBUG("停车距离超标,靠边停车结束"); AddExamFault(33, rtkTime); return -1; } if (moveDirect != prevMoveDirect) { 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); } 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) { // 停车超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; // 检测道路边缘线 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) { DEBUG("停车超出路边0.5米"); // 停车距离超过50厘米,不合格 AddExamFault(36, rtkTime); } else if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_2) { DEBUG("停车超出路边0.3米"); // 停车距离超过30厘米,扣10分 AddExamFault(37, rtkTime); } } } if (!inactiveBreakHandbreakTogether && ReadCarStatus(BREAK) == BREAK_INACTIVE && ReadCarStatus(HAND_BREAK) == BREAK_INACTIVE) { // 拉手刹前,松脚刹,扣10分 DEBUG("拉手刹前,松脚刹"); AddExamFault(39, rtkTime); inactiveBreakHandbreakTogether = true; } if (!notCloseEngine && ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE && ReadCarStatus(DOOR) == DOOR_OPEN) { // 下车前,不熄火,扣5分 DEBUG("下车前,不熄火"); AddExamFault(40, rtkTime); 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) { // 开门时间超过15秒,不合格 DEBUG("开门时间超过15秒"); AddExamFault(35, rtkTime); doorNotClose = true; } } } 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; } } return index; } static void PlayTTSTimeout(union sigval sig) { AppTimer_delete(PlayTTSTimeout); ttsPlayEnd = 1; }