//
|
// Created by YY on 2019/11/4.
|
//
|
|
// 4----------------------|5
|
// |
|
// |
|
// 3----------|2 |
|
// | |
|
// | |
|
// | |
|
// | |
|
// | |
|
// |1 |0
|
|
#include "turn_a90.h"
|
#include "../test_common/Geometry.h"
|
#include "../driver_test.h"
|
#include "../common/apptimer.h"
|
#include "../jni_log.h"
|
#include "../utils/xconvert.h"
|
#include "../defs.h"
|
#include "../test_common/car_sensor.h"
|
#include "../master/comm_if.h"
|
#include "area_exam.h"
|
|
#include <vector>
|
#include <cstdlib>
|
|
#define DEBUG(fmt, args...) LOGD("<turn_a90> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
static bool testing;
|
|
static int enterAreaHeading;
|
static bool turnLeftFinished;
|
static bool crashRedLine;
|
|
static bool CrashRedLine(prime_t &prime);
|
static bool ExitTestArea(prime_t &prime);
|
|
void StartTurnA90(prime_t &prime)
|
{
|
DEBUG("进入直角转弯场地");
|
testing = true;
|
enterAreaHeading = (int) prime.pModeling->yaw;
|
crashRedLine = false;
|
turnLeftFinished = false;
|
|
MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_TURN_90, 1);
|
}
|
|
static void StoppingTimeout(apptimer_var_t val) {
|
DEBUG("中途停车");
|
AddExamFault(20703);
|
}
|
|
void MotionChange(move_status_t mv)
|
{
|
if (!testing)
|
return;
|
|
AppTimer_delete(StoppingTimeout);
|
|
if (mv == STOP) {
|
AppTimer_add(StoppingTimeout, D_SEC(2));
|
} else {
|
|
}
|
}
|
|
void TestTurnA90(prime_t &prime)
|
{
|
int az = (int) prime.pModeling->yaw;
|
vector<double> dtox;
|
vector<Line> line_set;
|
Line distance_line;
|
|
if (CrashRedLine(prime)) {
|
if (!crashRedLine) {
|
crashRedLine = true;
|
// 碾压道路边缘,不合格
|
AddExamFault(20701);
|
DEBUG("碾压道路边缘");
|
}
|
} else {
|
crashRedLine = false;
|
}
|
|
// 检查转向状态
|
if (ABS(az - enterAreaHeading) > 180) {
|
az = 360 - ABS(az-enterAreaHeading);
|
} else {
|
az = ABS(az - enterAreaHeading);
|
}
|
|
if (az >= 30) {
|
if (!turnLeftFinished) {
|
char turn_direct;
|
if((( az + 360 - enterAreaHeading) % 360) < 180) {
|
DEBUG("右转");
|
turn_direct = 'R';
|
} else {
|
DEBUG("左转");
|
turn_direct = 'L';
|
}
|
// 转向灯未开启，扣10分
|
if ((turn_direct == 'R' && ReadCarStatus(TURN_SIGNAL_LAMP) != RIGHT_TURN_LIGHT) ||
|
(turn_direct == 'L' && ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT)) {
|
DEBUG("转向灯未开启");
|
AddExamFault(20702);
|
}
|
}
|
turnLeftFinished = true;
|
}
|
|
if (ExitTestArea(prime)) {
|
testing = false;
|
MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_TURN_90, 0);
|
}
|
}
|
|
// 车轮是否压边线
|
static bool CrashRedLine(prime_t &prime)
|
{
|
bool ret = false;
|
|
const int red_lines[][2] = {{0, 5}, {5, 4}, {1, 2}, {2, 3}};
|
|
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->turn_a90_map[prime.curr_exam_map.map_idx].map[red_lines[i][0]],
|
prime.pMap->turn_a90_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;
|
}
|
|
// 4个车轮和车头点不在场地中
|
static bool ExitTestArea(prime_t &prime)
|
{
|
Polygon polygon;
|
|
polygon.num = prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map.size();
|
polygon.point = new PointF[polygon.num];
|
for (int i = 0; i < polygon.num; ++i) {
|
polygon.point[i] = prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[i];
|
}
|
|
int num = 0;
|
|
if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
|
&polygon) == GM_None) {
|
num++;
|
}
|
if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
|
&polygon) == GM_None) {
|
num++;
|
}
|
if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
|
&polygon) == GM_None) {
|
num++;
|
}
|
if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]],
|
&polygon) == GM_None) {
|
num++;
|
}
|
if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]],
|
&polygon) == GM_None) {
|
num++;
|
}
|
|
delete []polygon.point;
|
|
return num == 5? true : false;
|
}
|
