//
|
// Created by YY on 2019/10/23.
|
//
|
|
#include "park_bottom.h"
|
#include "../common/apptimer.h"
|
#include "../test_common/Geometry.h"
|
#include "../native-lib.h"
|
#include "../jni_log.h"
|
#include "../driver_test.h"
|
#include "../utils/xconvert.h"
|
#include "../master/comm_if.h"
|
#include "area_exam.h"
|
#include "../test_common/car_sensor.h"
|
#include "../test_common/odo_graph.h"
|
#include <vector>
|
#include <cstdlib>
|
|
#define DEBUG(fmt, args...) LOGD("<park_bottom> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
enum {
|
NONE,
|
FIRST_TOUCH_CTRL_LINE,
|
FIRST_PARK,
|
SECOND_TOUCH_CTRL_LINE,
|
SECOND_PARK,
|
THIRD_TOUCH_CTRL_LINE
|
};
|
|
enum {
|
TESTING,
|
TEST_FAIL, // 因触发某些规则，在车身未完全立场情况下，提前终止部分测试
|
TEST_FINISH
|
};
|
|
static int testStatus;
|
static bool reverseCar = false;
|
static int mapIndex = 0;
|
|
const uint32_t CHECK_PARK_DELAY = 400;
|
|
static uint32_t stopTimepoint;
|
static int prevMoveDirect, storeMoveDirectBeforeStop;
|
static bool occurCrashRedLine;
|
static bool checkPartStatus;
|
static uint32_t firstReverseTimepoint;
|
static bool reportExamTimeout, reportParkFail;
|
static bool crossCtrlLineSw;
|
static int parkCount;
|
static char carray[3];
|
static int darray[3];
|
static int parkStatus[3];
|
|
static int gearAtStop;
|
static int currGear;
|
static char prevCrossedCtrlLine;
|
static double odo;
|
static int exitAreaCfm;
|
static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car);
|
static bool EnterParking(const Polygon *map, const car_model *car);
|
static bool CrashRedLine(const Polygon *map, const car_model *car, int &who);
|
static bool ExitParkArea(const Polygon *map, const car_model *car);
|
static bool AllTireExitParkArea(const Polygon *map, const car_model *car);
|
|
void StartParkBottom(int index, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
DEBUG("StartParkBottom");
|
testStatus = TESTING;
|
|
reverseCar = false;
|
mapIndex = index;
|
memset(carray, 0, sizeof(carray));
|
memset(darray, 0, sizeof(darray));
|
memset(parkStatus, 0, sizeof(parkStatus));
|
prevMoveDirect = moveDirect;
|
|
checkPartStatus = false;
|
firstReverseTimepoint = 0;
|
reportExamTimeout = false;
|
parkCount = 0;
|
crossCtrlLineSw = false;
|
reportParkFail = false;
|
occurCrashRedLine = false;
|
|
currGear = ReadCarStatus(GEAR);
|
prevCrossedCtrlLine = 0;
|
stopTimepoint = 0;
|
exitAreaCfm = 0;
|
odo = ReadOdo();
|
|
PlayTTS("您已进入倒车入库区域", NULL);
|
}
|
|
int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
|
{
|
char crossCtrlLine = 0;
|
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
|
int who = 0;
|
vector<double> dtox;
|
vector<Line> line_set;
|
Line distance_line;
|
|
bool gear_change = false;
|
int gear = ReadCarStatus(GEAR);
|
|
if (gear == GEAR_R) {
|
if (currGear != GEAR_R) {
|
gear_change = true;
|
currGear = GEAR_R;
|
}
|
} else if (currGear == GEAR_R) {
|
gear_change = true;
|
currGear = gear;
|
}
|
|
if (gear_change) {
|
// 检查上一次挡位的行驶距离，过小就放弃，避开学员原地挂挡重试
|
double run_distance = ReadOdo() - odo;
|
|
DEBUG("2次挡位运行距离 %f", run_distance);
|
|
if (run_distance < 1) {
|
gear_change = false;
|
DEBUG("2次挡位运行距离过小，忽略");
|
}
|
odo = ReadOdo();
|
}
|
|
if (testStatus == TESTING && gear_change) {
|
if (currGear == GEAR_R) {
|
// 挂倒挡,检测是否过控制线
|
DEBUG("开始挂倒挡");
|
if (!reverseCar) {
|
DEBUG("开始首轮入库");
|
reverseCar = true;
|
MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
|
firstReverseTimepoint = tp; // 开始210秒计时
|
}
|
crossCtrlLine = CrossCtrlLine(map, car, carPrev);
|
|
if (parkCount >= 2) {
|
DEBUG("开始次轮入库");
|
parkCount = 0;
|
MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
|
firstReverseTimepoint = tp; // 开始210秒计时
|
}
|
|
if (crossCtrlLine == 0) {
|
// 倒车前，前轮未驶过控制线
|
AddExamFault(20104, rtkTime);
|
DEBUG("倒车前，前轮未驶过控制线");
|
} else if (crossCtrlLine == prevCrossedCtrlLine) {
|
// 重复跨越同一控制线，不按规定线路，顺序形式，不合格
|
AddExamFault(20101, rtkTime);
|
DEBUG("不按规定线路，顺序形式， 同 %c 侧", prevCrossedCtrlLine);
|
} else {
|
prevCrossedCtrlLine = crossCtrlLine;
|
DEBUG("开始 %c 侧 倒库", prevCrossedCtrlLine);
|
}
|
} else {
|
// 从倒挡移出，检测是否入库
|
DEBUG("从倒挡移出");
|
|
parkCount++;
|
|
DEBUG("库位检查次数 = %d", parkCount);
|
|
if (EnterParking(map, car)) {
|
DEBUG("倒库成功");
|
} else {
|
AddExamFault(20103, rtkTime);
|
DEBUG("倒库不入");
|
}
|
}
|
}
|
|
if (testStatus == TESTING && parkCount < 2 && AllTireExitParkArea(map, car)) {
|
if (++exitAreaCfm >= 4) { // 避免信号漂移造成的误判
|
testStatus = TEST_FAIL;
|
AddExamFault(10103, rtkTime);
|
DEBUG("直接驶离测试区,不按考试员指令驾驶");
|
}
|
} else {
|
exitAreaCfm = 0;
|
}
|
|
if (ExitParkArea(map, car)) {
|
DEBUG("离开场地");
|
// 离开场地
|
testStatus = TEST_FINISH;
|
/*if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail && reverseCar) {
|
// 倒库不入，不合格
|
reportParkFail = true;
|
AddExamFault(20103, rtkTime);
|
DEBUG("倒库不入");
|
}*/
|
goto TEST_END;
|
}
|
|
// 距离检测
|
MakeLine(&distance_line, &map->point[0], &map->point[7]);
|
line_set.push_back(distance_line);
|
MakeLine(&distance_line, &map->point[1], &map->point[2]);
|
line_set.push_back(distance_line);
|
MakeLine(&distance_line, &map->point[2], &map->point[3]);
|
line_set.push_back(distance_line);
|
MakeLine(&distance_line, &map->point[3], &map->point[4]);
|
line_set.push_back(distance_line);
|
MakeLine(&distance_line, &map->point[4], &map->point[5]);
|
line_set.push_back(distance_line);
|
MakeLine(&distance_line, &map->point[5], &map->point[6]);
|
line_set.push_back(distance_line);
|
DistanceOfTire2X(dtox, car, line_set);
|
MA_SendDistance(dtox[0], dtox[1]);
|
|
if (CrashRedLine(map, car, who)) {
|
if (!occurCrashRedLine /*&& reverseCar*/) {
|
occurCrashRedLine = true;
|
// 车身出线，不合格
|
AddExamFault(10116, rtkTime);
|
DEBUG("车轮压线");
|
/*if (who == 1) {
|
PlayTTS("压左库位线", NULL);
|
} else if (who == 2) {
|
PlayTTS("压右库位线", NULL);
|
}*/
|
}
|
} else {
|
occurCrashRedLine = false;
|
}
|
|
if (moveDirect != prevMoveDirect) {
|
if (moveDirect == 0) {
|
stopTimepoint = tp;
|
gearAtStop = (currGear == GEAR_R ? 1 : 0);
|
DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
|
DEBUG("停车时挡位 = %d", gearAtStop);
|
} else if (prevMoveDirect == 0 && stopTimepoint > 0) {
|
DEBUG("继续行驶 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
|
|
DEBUG("停车时间 %ld", tp - stopTimepoint);
|
DEBUG("再次移动时挡位 = %d", currGear == GEAR_R ? 1 : 0);
|
|
if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_bottom_pause_criteria)
|
&& gearAtStop == (currGear == GEAR_R ? 1 : 0)) {
|
// 停车超2秒，每次扣5分
|
AddExamFault(20106, rtkTime);
|
DEBUG("中途停车");
|
}
|
}
|
|
prevMoveDirect = moveDirect;
|
}
|
|
/* crossCtrlLine = CrossCtrlLine(map, car, carPrev);
|
if (crossCtrlLine > 0 && !crossCtrlLineSw) {
|
crossCtrlLineSw = true;
|
if (parkCount == 0) {
|
carray[0] = crossCtrlLine;
|
} else if (parkCount == 1) {
|
if (carray[0] == crossCtrlLine) {
|
// 不按规定线路，顺序形式，不合格
|
AddExamFault(20101, rtkTime);
|
DEBUG("不按规定线路，顺序形式");
|
}
|
carray[1] = crossCtrlLine;
|
} else if (parkCount == 2) {
|
if (carray[0] != crossCtrlLine) {
|
// 不按规定线路，顺序形式，不合格
|
AddExamFault(20101, rtkTime);
|
DEBUG("不按规定线路，顺序形式");
|
} else {
|
// 离开测试区，停止计时
|
DEBUG("离开测试区，停止计时");
|
testStatus = false;
|
goto TEST_END;
|
}
|
carray[2] = crossCtrlLine;
|
}
|
}
|
|
if (testStatus && darray[0] > 0 && tp - firstReverseTimepoint >= examParam.park_bottom_limit_time) {
|
// 完成超时，不合格
|
if (!reportExamTimeout) {
|
reportExamTimeout = true;
|
AddExamFault(20105, rtkTime);
|
DEBUG("项目超时");
|
}
|
}
|
|
if (moveDirect != prevMoveDirect) {
|
if (moveDirect == 0) {
|
stopTimepoint = tp;
|
storeMoveDirectBeforeStop = prevMoveDirect;
|
if (prevMoveDirect == -1) {
|
checkPartStatus = true; // 每次倒车停止，触发入库检查
|
}
|
|
DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
|
} else {
|
DEBUG("继续行驶 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
|
|
DEBUG("停车时间 %ld", tp - stopTimepoint);
|
|
if (moveDirect == storeMoveDirectBeforeStop) {
|
// 同方向再启动，继续判断是否停车超时
|
if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_bottom_pause_criteria) && reverseCar) {
|
// 停车超2秒，每次扣5分
|
AddExamFault(20106, rtkTime);
|
DEBUG("中途停车");
|
}
|
} else if (moveDirect == -1) {
|
// 切换为倒车
|
if (!reverseCar) {
|
reverseCar = true;
|
MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
|
}
|
if (darray[parkCount] == 0) {
|
if (!crossCtrlLineSw) {
|
// 倒车前，前轮未驶过控制线
|
AddExamFault(20104, rtkTime);
|
DEBUG("倒车前，前轮未驶过控制线");
|
}
|
darray[parkCount] = 1;
|
firstReverseTimepoint = tp; // 首次倒车，才意味着此项目开始
|
}
|
} else {
|
// 切换为前进
|
DEBUG("切换为前进");
|
|
if (tp - stopTimepoint >= CHECK_PARK_DELAY) {
|
if (crossCtrlLineSw) {
|
if (parkStatus[parkCount] != 1) {
|
// 倒库不入，不合格
|
reportParkFail = true;
|
AddExamFault(20103, rtkTime);
|
DEBUG("倒库不入");
|
}
|
}
|
|
crossCtrlLineSw = false;
|
|
if (parkCount < 2)
|
parkCount++;
|
}
|
}
|
}
|
prevMoveDirect = moveDirect;
|
} else if (moveDirect == -1) {
|
if (darray[parkCount] == 0) {
|
// 切换为倒车
|
if (!crossCtrlLineSw) {
|
// 倒车前，前轮未驶过控制线
|
AddExamFault(20104, rtkTime);
|
DEBUG("倒车前，前轮未驶过控制线");
|
}
|
darray[parkCount] = 1;
|
firstReverseTimepoint = tp;
|
}
|
} else if (moveDirect == 0 && crossCtrlLineSw) {
|
if (tp - stopTimepoint >= CHECK_PARK_DELAY && checkPartStatus) {
|
if (EnterParking(map, car)) {
|
parkStatus[parkCount] = 1;
|
}
|
checkPartStatus = false;
|
}
|
}*/
|
|
TEST_END:
|
if (testStatus == TEST_FINISH) {
|
DEBUG("倒库结束");
|
MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 0);
|
return 0;
|
}
|
return 1;
|
}
|
|
// 检测2前轮是否正向越过左右控制线
|
static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car)
|
{
|
// 过右控制线
|
if ((IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) &&
|
(IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == -1) &&
|
(IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->axial[AXIAL_REAR]]) == 1)) {
|
return 'R';
|
}
|
|
// 过左控制线
|
if ((IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == 1) &&
|
(IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == 1) &&
|
(IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->axial[AXIAL_REAR]]) == -1)) {
|
return 'L';
|
}
|
|
return 0;
|
}
|
|
static bool EnterParking(const Polygon *map, const car_model *car) {
|
bool succ = false;
|
|
Polygon parking;
|
Polygon car_body;
|
|
car_body.num = car->bodyNum;
|
car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
|
for (int i = 0; i < car_body.num; ++i) {
|
car_body.point[i] = car->carXY[car->body[i]];
|
}
|
|
MakePolygon(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]});
|
|
if (IntersectionOf(&car_body, &parking) == GM_Containment) {
|
succ = true;
|
}
|
|
CleanPolygon(&parking);
|
free(car_body.point);
|
|
DEBUG("检查倒库状态 %s", succ ? "成功" : "失败");
|
|
return succ;
|
}
|
|
static bool CrashRedLine(const Polygon *map, const car_model *car, int &who)
|
{
|
bool ret = false;
|
|
Line red_line;
|
const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}};
|
|
Polygon car_body;
|
|
car_body.num = car->bodyNum;
|
car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
|
for (int i = 0; i < car_body.num; ++i) {
|
car_body.point[i] = car->carXY[car->body[i]];
|
}
|
|
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, &car_body) != GM_None) {
|
if (i == 2 || i == 1) {
|
who = 1;
|
} else if (i == 4 || i == 5) {
|
who = 2;
|
}
|
|
ret = true;
|
break;
|
}
|
}
|
|
free(car_body.point);
|
return ret;
|
}
|
|
static bool ExitParkArea(const Polygon *map, 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]];
|
}
|
|
if (IntersectionOf(&carBody, map) == GM_None) {
|
ret = true;
|
}
|
|
free(carBody.point);
|
|
return ret;
|
}
|
|
// 双前轮和双后轮不在区域
|
static bool AllTireExitParkArea(const Polygon *map, const car_model *car)
|
{
|
int tireExitNum = 0;
|
|
if (IntersectionOf(car->carXY[ car->left_front_tire[TIRE_OUTSIDE] ], map) == GM_None) {
|
tireExitNum++;
|
}
|
|
if (IntersectionOf(car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ], map) == GM_None) {
|
tireExitNum++;
|
}
|
|
if (IntersectionOf(car->carXY[ car->left_rear_tire[TIRE_OUTSIDE] ], map) == GM_None) {
|
tireExitNum++;
|
}
|
|
if (IntersectionOf(car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ], map) == GM_None) {
|
tireExitNum++;
|
}
|
|
if (tireExitNum >= 4) {
|
return true;
|
}
|
return false;
|
}
|
