//
|
// Created by YY on 2019/10/31.
|
//
|
|
// |9
|
// |
|
// |
|
// |
|
// |
|
// 7-----------------|8
|
// 6_________________|5
|
// 3_________________|4
|
// |
|
// 2-----------------|1
|
// |
|
// |
|
// |
|
// |0
|
|
#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"
|
#include "../defs.h"
|
|
#define DEBUG(fmt, args...) LOGD("<stop_and_start> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
|
|
|
const double EPSILON = 1e-3;
|
|
static bool testing = false;
|
|
static PointF stopPoint;
|
|
static bool check1 = false;
|
|
static bool stopConfirm;
|
static bool restartComplete = false;
|
static bool occurCrashRedLine = false;
|
|
static bool slideNormalDistance = false;
|
static bool reportSlideFault = false;
|
static bool handBreakActive = false;
|
|
static bool CrashRedLine(prime_t &prime);
|
static double DistanceOfHead2Stopline(prime_t &prime);
|
static double DistanceOfTire2Edge(prime_t &prime);
|
static bool ExitTestArea(prime_t &prime);
|
|
void StartSAS(prime_t &prime)
|
{
|
DEBUG("进入坡起项目");
|
|
testing = true;
|
stopConfirm = false;
|
restartComplete = false;
|
occurCrashRedLine = false;
|
check1 = false;
|
slideNormalDistance = false;
|
reportSlideFault = false;
|
handBreakActive = false;
|
|
MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 1);
|
}
|
|
static void StoppingTimeout(apptimer_var_t val) {
|
// 起步时间超过30秒，不合格
|
AddExamFault(20303);
|
DEBUG("起步时间超过30秒");
|
}
|
|
static void StopConfirm(apptimer_var_t val) {
|
stopConfirm = true;
|
}
|
|
void MotionChange(move_status_t mv)
|
{
|
AppTimer_delete(StopConfirm);
|
|
if (mv == STOP) {
|
AppTimer_add(StopConfirm, D_SEC(1));
|
} else {
|
|
}
|
}
|
|
int TestSAS(prime_t &prime)
|
{
|
static double distanceToStopLine = 0, distanceToEdge = 0;
|
|
if (CrashRedLine(prime)) {
|
// 车轮压线，不合格
|
if (!occurCrashRedLine) {
|
AddExamFault(10116);
|
DEBUG("车轮压线");
|
}
|
occurCrashRedLine = true;
|
} else {
|
occurCrashRedLine = false;
|
}
|
|
// 检测到停车
|
if (prime.pMotion->move == STOP && stopConfirm) {
|
if (!check1) {
|
check1 = true;
|
// 存储停止点
|
stopPoint = prime.pModeling->base_point;
|
// 开始停车计时
|
AppTimer_delete(StoppingTimeout);
|
AppTimer_add(StoppingTimeout, examParam.ramp_start_car_limit_time);
|
|
// 检查车头和停车带的距离
|
distanceToStopLine = DistanceOfHead2Stopline(prime);
|
distanceToEdge = DistanceOfTire2Edge(prime);
|
|
DEBUG("DIS1 = %f DIS2 = %f", distanceToStopLine, distanceToEdge);
|
|
if (distanceToStopLine > examParam.ramp_stoppoint_red_distance) {
|
// 距离停止线前后超出50厘米
|
AddExamFault(20301);
|
DEBUG("距离停止线前后超出50厘米，不合格");
|
} else if (fabs(distanceToStopLine) > EPSILON) {
|
// 前保险没有位于停止带内，但没有超出50厘米，扣10分
|
AddExamFault(20304);
|
DEBUG("前保险没有位于停止带内，但没有超出50厘米");
|
}
|
|
if (distanceToEdge > examParam.ramp_edge_red_distance) {
|
// 距离边线超出50厘米,不合格
|
AddExamFault(20302);
|
DEBUG("距离边线超出50厘米");
|
} else if (distanceToEdge > examParam.ramp_edge_yellow_distance) {
|
// 距离边线超出30厘米，扣10分
|
AddExamFault(20305);
|
DEBUG("距离边线超出30厘米");
|
}
|
}
|
|
// 停车后，检查手刹拉起情况
|
if (!handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
|
handBreakActive = true;
|
}
|
}
|
|
if (prime.pMotion->move != STOP && stopConfirm) {
|
// 车辆从停止状态再次移动
|
double juli = DistanceOf(prime.pModeling->base_point, stopPoint);
|
|
if (juli > examParam.ramp_slide_yellow_distance) {
|
double deg = YawOf(stopPoint, prime.pModeling->base_point);
|
deg = fabs(prime.pModeling->yaw - deg);
|
if (deg > 180) {
|
deg = 360 - deg;
|
}
|
if (deg < 90) {
|
// 车辆上坡
|
if (!restartComplete) {
|
restartComplete = true;
|
AppTimer_delete(StoppingTimeout);
|
if (slideNormalDistance && !reportSlideFault) {
|
// 后滑超过10厘米，但没超过30厘米
|
DEBUG("后滑超过10厘米，但没超过30厘米");
|
reportSlideFault = true;
|
AddExamFault(10204);
|
}
|
if (!handBreakActive) {
|
// 检查是否拉住手刹
|
DEBUG("没拉手刹");
|
handBreakActive = true;
|
AddExamFault(20306);
|
}
|
}
|
} else {
|
// 车辆后滑
|
slideNormalDistance = true;
|
if (juli > examParam.ramp_slide_red_distance && !reportSlideFault) {
|
// 后滑超过30厘米, 不合格
|
DEBUG("后滑超过30厘米");
|
reportSlideFault = true;
|
AddExamFault(10106);
|
}
|
}
|
}
|
}
|
|
if (ExitTestArea(prime)) {
|
// 驶离测试区
|
if (!stopConfirm) {
|
// 不停车直接离开
|
AddExamFault(10103);
|
}
|
MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 0);
|
}
|
|
return testing ? 1 : 0;
|
}
|
|
// 车轮是否压边线
|
static bool CrashRedLine(prime_t &prime)
|
{
|
bool ret = false;
|
|
int red_lines[][2] = {{0, 1}, {1, 4}, {4, 5}, {5, 8}, {8, 9}};
|
|
|
Line frontAxle, rearAxle;
|
|
// 仅看车轮外侧
|
MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
|
prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
|
MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
|
prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);
|
|
for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
|
Line red_line;
|
MAKE_LINE(red_line, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[red_lines[i][0]],
|
prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[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(prime_t &prime)
|
{
|
double dis = 0.0;
|
|
Line upper_edge, lower_edge;
|
|
MAKE_LINE(upper_edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[5],
|
prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[6]);
|
MAKE_LINE(lower_edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[4],
|
prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[3]);
|
|
if (IntersectionOfLine(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge) == REL_POS_RIGHT) {
|
dis = DistanceOf(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge);
|
} else if (IntersectionOfLine(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], lower_edge) == REL_POS_LEFT) {
|
dis = DistanceOf(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], lower_edge);
|
}
|
|
DEBUG("DistanceOfHead2Stopline dis %f", dis);
|
|
return dis;
|
}
|
|
static double DistanceOfTire2Edge(prime_t &prime)
|
{
|
Line edge;
|
|
MAKE_LINE(edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[0],
|
prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[8]);
|
|
double l1 = DistanceOf(prime.pModeling->points[prime.pModel->right_front_tire[TIRE_OUTSIDE]], edge);
|
|
double l2 = DistanceOf(prime.pModeling->points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]], edge);
|
|
return MAX(l1, l2); // 取最远的
|
}
|
|
// 整个车辆都要驶离该测试区域
|
static bool ExitTestArea(prime_t &prime)
|
{
|
// 全车都需不在地图中
|
bool ret = false;
|
|
/*Polygon carBody, map2;
|
PointF vPoint = Calc3Point(map->point[8], map->point[0], DistanceOf(map->point[8], map->point[7]), 'R');
|
PointF vPoint2;
|
|
if (map->num == 10) {
|
vPoint2 = Calc3Point(map->point[8], map->point[9], DistanceOf(map->point[8], map->point[7]), 'L');
|
}
|
|
map2.num = 4;
|
map2.point = (PointF *)malloc(map2.num * sizeof(PointF));
|
|
if (map->num == 9)
|
MakePolygon(&map2, {vPoint, map->point[0], map->point[8], map->point[7]});
|
else
|
MakePolygon(&map2, {vPoint, map->point[0], map->point[9], vPoint2});
|
|
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;
|
}
|
