//
|
// Created by YY on 2019/10/31.
|
//
|
|
#include <cstdlib>
|
#include <vector>
|
#include <cmath>
|
|
#include "stop_and_start.h"
|
#include "../driver_test.h"
|
#include "../jni_log.h"
|
#include "../common/apptimer.h"
|
#include "../utils/xconvert.h"
|
#include "../test_common/car_sensor.h"
|
#include "../master/comm_if.h"
|
#include "area_exam.h"
|
|
#define DEBUG(fmt, args...) LOGD("<stop_and_start> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
|
const uint32_t STOP_CAR_TIME = D_SEC(1);
|
|
const double EPSILON = 1e-3;
|
|
static bool testing = false;
|
|
static PointF stopPoint;
|
|
static int mapIndex = 0;
|
static int prevMoveDirect;
|
static uint32_t stopTimepoint = 0;
|
static bool stopCar = false;
|
static uint32_t stopCarTime;
|
static bool occurCrashRedLine = false;
|
static bool slideLongDistance = false;
|
static bool slideNormalDistance = false;
|
static bool reportSlideFault = false;
|
static bool reportStartTimeout = false;
|
static bool handBreakActive = false;
|
static bool CrashRedLine(const Polygon *map, const car_model *car);
|
static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car);
|
static double DistanceOfTire2Edge(const Polygon *map, const car_model *car);
|
static bool ExitTestArea(const Polygon *map, const car_model *car);
|
|
void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
DEBUG("进入坡起项目");
|
testing = true;
|
mapIndex = index;
|
prevMoveDirect = moveDirect;
|
|
if (moveDirect == 0) {
|
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
}
|
|
occurCrashRedLine = false;
|
stopCar = false;
|
slideLongDistance = false;
|
slideNormalDistance = false;
|
reportSlideFault = false;
|
reportStartTimeout = false;
|
handBreakActive = false;
|
|
MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 1);
|
}
|
|
int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
if (CrashRedLine(map, car)) {
|
// 车轮压线，不合格
|
if (!occurCrashRedLine) {
|
AddExamFault(13, rtkTime);
|
DEBUG("车轮压线");
|
}
|
occurCrashRedLine = true;
|
} else {
|
occurCrashRedLine = false;
|
}
|
|
if (ExitTestArea(map, car)) {
|
// 驶离测试区
|
testing = false;
|
}
|
|
// 距离检测
|
{
|
vector<double> dtox;
|
vector<Line> line_set;
|
Line distance_line;
|
|
MakeLine(&distance_line, &map->point[0], &map->point[8]);
|
line_set.push_back(distance_line);
|
DistanceOfTire2X(dtox, car, line_set);
|
MA_SendDistance(dtox[0], dtox[1]);
|
}
|
|
if (prevMoveDirect != moveDirect) {
|
if (moveDirect == 0) {
|
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
} else if (stopCar && !handBreakActive) {
|
// 检查是否拉住手刹
|
handBreakActive = true;
|
AddExamFault(19, rtkTime);
|
DEBUG("没拉手刹");
|
}
|
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 && !stopCar) {
|
// 这里判断停车状态
|
stopCar = true;
|
stopCarTime = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
|
stopPoint = car->carXY[car->body[0]];
|
|
double dis1 = DistanceOfHead2Stopline(map, car);
|
double dis2 = DistanceOfTire2Edge(map, car);
|
|
DEBUG("DIS1 = %f DIS2 = %f", dis1, dis2);
|
|
if (dis1 > examParam.ramp_stoppoint_red_distance) {
|
// 距离停止线前后超出50厘米
|
AddExamFault(12, rtkTime);
|
DEBUG("距离停止线前后超出50厘米，不合格");
|
} else if (fabs(dis1) > EPSILON) {
|
// 前保险没有位于停止带内，但没有超出50厘米，扣10分
|
AddExamFault(17, rtkTime);
|
DEBUG("前保险没有位于停止带内，但没有超出50厘米");
|
}
|
|
if (dis2 > examParam.ramp_edge_red_distance) {
|
// 距离边线超出50厘米,不合格
|
AddExamFault(14, rtkTime);
|
DEBUG("距离边线超出50厘米");
|
} else if (dis2 > examParam.ramp_edge_yellow_distance) {
|
// 距离边线超出30厘米，扣10分
|
AddExamFault(18, rtkTime);
|
DEBUG("距离边线超出30厘米");
|
}
|
}
|
|
if (stopCar && !handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
|
handBreakActive = true;
|
}
|
}
|
|
// 判断起步后滑状态
|
if (stopCar) {
|
if (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) == 1) {
|
// 发生后滑
|
double slideDistance = DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]);
|
|
if (slideDistance > examParam.ramp_slide_yellow_distance) {
|
slideNormalDistance = true;
|
}
|
|
if (slideDistance > examParam.ramp_slide_red_distance && !slideLongDistance && !reportSlideFault) {
|
// 后滑超过30厘米, 不合格
|
AddExamFault(16, rtkTime);
|
DEBUG("后滑超过30厘米");
|
slideLongDistance = true;
|
reportSlideFault = true;
|
}
|
}
|
|
|
if (!reportStartTimeout && (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) != -1 ||
|
DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) < 0.1)) {
|
if (TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10) - stopCarTime > examParam.ramp_start_car_limit_time) {
|
// 起步时间超过30秒，不合格
|
AddExamFault(15, rtkTime);
|
DEBUG("起步时间超过30秒");
|
reportStartTimeout = true;
|
}
|
}
|
|
if (IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->axial[AXIAL_REAR]]) == -1) {
|
// 车尾驶过停止杆
|
if (slideNormalDistance && !slideLongDistance && !reportSlideFault) {
|
reportSlideFault = true;
|
// 后滑超过10厘米，但没超过30厘米
|
AddExamFault(20, rtkTime);
|
DEBUG("后滑超过10厘米，但没超过30厘米");
|
}
|
}
|
}
|
|
if (!testing) {
|
MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 0);
|
}
|
|
return testing ? 1 : 0;
|
}
|
|
// 车轮是否压边线
|
static bool CrashRedLine(const Polygon *map, const car_model *car)
|
{
|
bool ret = false;
|
|
Line red_line;
|
const int red_lines[][2] = {{0, 8}};
|
|
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 < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
|
MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
|
if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
|
IntersectionOf(red_line, rearAxle) == GM_Intersection) {
|
ret = true;
|
break;
|
}
|
}
|
|
return ret;
|
}
|
|
static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car)
|
{
|
double dis = 0.0;
|
|
int rel1 = IntersectionOfLine(map->point[4], map->point[3], car->carXY[car->body[0]]);
|
int rel2 = IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->body[0]]);
|
|
DEBUG("%d %d %f, %f", car->body[0], car->axial[AXIAL_FRONT], car->carXY[car->body[0]].X, car->carXY[car->body[0]].Y);
|
DEBUG("rel1 %d rel2 %d", rel1, rel2);
|
|
if (rel1 == 1) {
|
Line line1;
|
|
MakeLine(&line1, &map->point[4], &map->point[3]);
|
|
dis = DistanceOf(car->carXY[car->body[0]], line1);
|
} else if (rel2 == -1) {
|
Line line2;
|
|
MakeLine(&line2, &map->point[5], &map->point[6]);
|
|
dis = DistanceOf(car->carXY[car->body[0]], line2);
|
}
|
|
DEBUG("DistanceOfHead2Stopline dis %f", dis);
|
|
return dis;
|
}
|
|
static double DistanceOfTire2Edge(const Polygon *map, const car_model *car)
|
{
|
Line edge;
|
|
MakeLine(&edge, &map->point[0], &map->point[8]);
|
|
double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], edge);
|
|
double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], edge);
|
|
return (l1+l2)/2.0;
|
}
|
|
// 整个车辆都要驶离该测试区域
|
static bool ExitTestArea(const Polygon *map, const car_model *car)
|
{
|
// 全车都需不在地图中
|
bool ret = false;
|
|
Polygon carBody, map2;
|
PointF vPoint = Calc3Point(map->point[8], map->point[0], DistanceOf(map->point[8], map->point[7]), 'R');
|
|
map2.num = 4;
|
map2.point = (PointF *)malloc(map2.num * sizeof(PointF));
|
|
MakePolygon(&map2, {vPoint, map->point[0], map->point[7], map->point[8]});
|
|
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, &map2) == GM_None) {
|
ret = true;
|
}
|
|
free(carBody.point);
|
free(map2.point);
|
return ret;
|
}
|
