//
|
// Created by YY on 2020/3/17.
|
//
|
|
#include "road_exam.h"
|
#include "../driver_test.h"
|
#include "../utils/xconvert.h"
|
#include "../common/apptimer.h"
|
#include "../jni_log.h"
|
#include "../defs.h"
|
#include "../test_common/car_sensor.h"
|
|
#include <vector>
|
#include <list>
|
#include <string>
|
|
#define DEBUG(fmt, args...) LOGD("<road_exam> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
#define TURN_CHECK_CNT 5
|
|
static const int TURN_THRESHOLD = 10;
|
static const int TURN_CHECK_INTERVAL = D_SEC(1);
|
|
static bool occurCrashRedLine;
|
static bool occurCrashGreenLine;
|
static bool occurOverSpeed;
|
static int checkCrashGreenTimeout;
|
static char carIntersectionOfGreenLine;
|
static int currTurnSignalStatus;
|
static int turnSignalStatusWhenCrashGreenLine;
|
static bool reportTurnSignalError;
|
static int prevMoveDirect;
|
static uint32_t stopTimepoint = 0;
|
static bool reportStopCarOnRedArea;
|
static PointF stopPoint;
|
|
static struct drive_timer {
|
int hour;
|
int min;
|
int sec;
|
int msec;
|
} crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;
|
|
static const uint32_t STOP_CAR_TIME = D_SEC(2);
|
static const uint32_t CHANGE_ROAD_MIN_INTERVAL = D_SEC(10);
|
static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10);
|
static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3);
|
|
static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0;
|
|
static void Rtk2DirveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
|
static char isTurn(int currYaw, int prevYaw);
|
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);
|
static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car);
|
static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car);
|
static bool CrashGreenLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, PointF &p1, PointF &p2);
|
|
void Init(void)
|
{
|
crashGreenCmpTime.hour = -1;
|
occurCrashRedLine = false;
|
occurCrashGreenLine = false;
|
occurOverSpeed = false;
|
checkCrashGreenTimeout = 0;
|
carIntersectionOfGreenLine = 0;
|
|
reportTurnSignalError = false;
|
currTurnSignalStatus = OFF_LIGHT;
|
prevMoveDirect = 0;
|
|
reportStopCarOnRedArea = false;
|
}
|
|
void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
if (moveDirect != 0 && speed > MAX_SPEED) {
|
if (!occurOverSpeed) {
|
occurOverSpeed = true;
|
// 超速，不合格
|
AddExamFault(10, rtkTime);
|
}
|
} else {
|
occurOverSpeed = false;
|
}
|
|
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 = 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;
|
}
|
|
stopPoint = car->basePoint;
|
}
|
|
switch (ReadCarStatus(TURN_SIGNAL_LAMP)) {
|
case LEFT_TURN_LIGHT:
|
if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
|
currTurnSignalStatus = LEFT_TURN_LIGHT;
|
Rtk2DirveTimer(turnSignalChangeTime, rtkTime);
|
}
|
break;
|
case RIGHT_TURN_LIGHT:
|
if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
|
currTurnSignalStatus = RIGHT_TURN_LIGHT;
|
Rtk2DirveTimer(turnSignalChangeTime, rtkTime);
|
}
|
break;
|
default:
|
currTurnSignalStatus = ReadCarStatus(TURN_SIGNAL_LAMP);
|
break;
|
}
|
|
|
// 检查是否持续转向
|
char turnDirect = CheckCarTurn(CarModelList);
|
if (turnDirect == 'L') {
|
if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
|
if (!reportTurnSignalError) {
|
DEBUG("没打左转灯");
|
// 没打左转灯，不合格
|
AddExamFault(13, rtkTime);
|
reportTurnSignalError = true;
|
}
|
} 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秒，不合格
|
AddExamFault(14, rtkTime);
|
reportTurnSignalError = true;
|
}
|
}
|
} else if (turnDirect == 'R') {
|
if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
|
if (!reportTurnSignalError) {
|
DEBUG("没打右转灯");
|
// 没打右转灯，不合格
|
AddExamFault(13, rtkTime);
|
reportTurnSignalError = true;
|
}
|
} 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秒，不合格
|
AddExamFault(14, rtkTime);
|
reportTurnSignalError = true;
|
}
|
}
|
} else {
|
reportTurnSignalError = false;
|
}
|
|
|
if (CrashRedLine(RoadMapList, car)) {
|
if (!occurCrashRedLine) {
|
// 车辆行驶中骑轧车道中心实线或者车道边缘实线，不合格
|
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;
|
Rtk2DirveTimer(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) {
|
checkCrashGreenTimeout = 2;
|
// 长时间骑轧车道分界线行驶，不合格
|
AddExamFault(12, rtkTime);
|
}
|
}
|
} else {
|
// 停车就不计时了
|
checkCrashGreenTimeout = 0;
|
}
|
|
// 检测当前车辆于虚线的位置，做变道检测;
|
// 检测是否3秒前有开启对应之转向灯
|
if (!occurCrashGreenLine) {
|
occurCrashGreenLine = true;
|
// 记录开始压线的时间，不确定是否有变道意图，待确认变道后再处理之
|
Rtk2DirveTimer(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_ROAD_MIN_INTERVAL) {
|
// 连续变道，不合格
|
AddExamFault(15, rtkTime);
|
}
|
}
|
|
// 记录本次变道时间点
|
Rtk2DirveTimer(crashGreenCmpTime, rtkTime);
|
|
// 检查变道前，是否提前转向灯
|
if (inter == 1) {
|
// 向左侧变道
|
DEBUG("向左侧变道");
|
if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
|
// 没打灯，不合格
|
AddExamFault(13, rtkTime);
|
}
|
} else {
|
// 向右侧变道
|
DEBUG("向右侧变道");
|
if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
|
// 没打灯，不合格
|
AddExamFault(13, rtkTime);
|
}
|
}
|
}
|
}
|
occurCrashGreenLine = false;
|
checkCrashGreenTimeout = 0;
|
}
|
}
|
|
static void Rtk2DirveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
|
{
|
tm.hour = rtkTime->hh;
|
tm.min = rtkTime->mm;
|
tm.sec = rtkTime->ss;
|
tm.msec = rtkTime->mss;
|
}
|
|
static char isTurn(int currYaw, int prevYaw)
|
{
|
if (ABS(currYaw - prevYaw) > 180) {
|
currYaw = 360 - ABS(currYaw-prevYaw);
|
} else {
|
currYaw = ABS(currYaw - prevYaw);
|
}
|
|
if (currYaw >= TURN_THRESHOLD) {
|
if((( currYaw + 360 - prevYaw) % 360) < 180) {
|
DEBUG("右转");
|
return 'R';
|
} else {
|
DEBUG("左转");
|
return 'L';
|
}
|
}
|
|
return 0;
|
}
|
|
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)
|
{
|
// 最近3秒内，每秒的角度差大于10度，且方向相同，连续3秒，认为转向
|
if (CarModelList.size() < 1)
|
return false;
|
|
list<car_model *>::iterator iter = CarModelList.begin();
|
|
car_model *c1 = *iter, *c2;
|
|
++iter;
|
|
char turn[TURN_CHECK_CNT] = {0};
|
int checkCnt = 0;
|
|
while (iter != CarModelList.end()) {
|
c2 = *iter;
|
|
uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
|
|
if (tdiff >= TURN_CHECK_INTERVAL) {
|
turn[checkCnt++] = isTurn((int)c1->yaw, (int)c2->yaw);
|
c1 = c2;
|
|
if (checkCnt == TURN_CHECK_CNT)
|
break;
|
}
|
|
++iter;
|
}
|
|
int i = 0;
|
for (; checkCnt == TURN_CHECK_CNT && i < TURN_CHECK_CNT-1; ++i) {
|
if (turn[i] != turn[i+1])
|
break;
|
}
|
|
if (i == TURN_CHECK_CNT-1)
|
return turn[0];
|
|
return 0;
|
}
|
|
/*****************************************************
|
* CrashRedLine 整个考试区域的道路边缘线，实线等。
|
* 按车轮碰触计算，车身不计
|
*/
|
static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car)
|
{
|
Line frontAxle, rearAxle;
|
|
MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
|
MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
|
|
for (int i = 0; i < RoadMapList.size(); ++i) {
|
if (RoadMapList[i].type == 100) {
|
|
// 每条线都检测
|
for (int j = 0; j < RoadMapList[i].redLineNum; ++j) {
|
Line red_line;
|
int kp = 0;
|
|
for (int k = 1; k < RoadMapList[i].redLine[j].num; ++k) {
|
MakeLine(&red_line, &RoadMapList[i].redLine[j].point[kp], &RoadMapList[i].redLine[j].point[k]);
|
|
if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
|
IntersectionOf(red_line, rearAxle) == GM_Intersection) {
|
return true;
|
}
|
kp = k;
|
}
|
}
|
break;
|
}
|
}
|
return false;
|
}
|
|
/**********************************************************
|
* 按整个车身是否覆盖计算
|
* @param RoadMapList
|
* @param car
|
* @return
|
*/
|
static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car)
|
{
|
bool ret = false;
|
|
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]];
|
}
|
|
for (int i = 0; i < RoadMapList.size(); ++i) {
|
if (RoadMapList[i].type == 100) {
|
|
// 每条红区都检测
|
for (int j = 0; j < RoadMapList[i].redAreaNum; ++j) {
|
if (IntersectionOf(&carBody, &RoadMapList[i].redArea[j]) != GM_None) {
|
ret = true;
|
}
|
}
|
break;
|
}
|
}
|
|
free(carBody.point);
|
|
return ret;
|
}
|
|
/**************************************************
|
* 车轮触碰道路虚线。检测行驶时间超长；变道情况；
|
* @param RoadMapList
|
* @param car
|
*/
|
static bool CrashGreenLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, PointF &p1, PointF &p2)
|
{
|
Line frontAxle, rearAxle;
|
|
MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
|
MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
|
|
for (int i = 0; i < RoadMapList.size(); ++i) {
|
if (RoadMapList[i].type == 100) {
|
|
// 每条线都检测
|
for (int j = 0; j < RoadMapList[i].greenLineNum; ++j) {
|
Line green_line;
|
int kp = 0;
|
|
for (int k = 1; k < RoadMapList[i].greenLine[j].num; ++k) {
|
MakeLine(&green_line, &RoadMapList[i].greenLine[j].point[kp], &RoadMapList[i].greenLine[j].point[k]);
|
|
if (IntersectionOf(green_line, frontAxle) == GM_Intersection ||
|
IntersectionOf(green_line, rearAxle) == GM_Intersection) {
|
|
p1 = RoadMapList[i].greenLine[j].point[kp];
|
p2 = RoadMapList[i].greenLine[j].point[k];
|
|
return true;
|
}
|
kp = k;
|
}
|
}
|
break;
|
}
|
}
|
return false;
|
}
|
