//
|
// 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("<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;
|
|
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;
|
}
|
