//
|
// Created by YY on 2019/11/4.
|
//
|
|
#include "turn_a90.h"
|
#include "../Geometry.h"
|
#include "../driver_test.h"
|
#include "../common/apptimer.h"
|
#include "../jni_log.h"
|
|
#include <vector>
|
#include <cstdlib>
|
|
#define DEBUG(fmt, args...) LOGD("<turn_a90> <%s>: " fmt, __func__, ##args)
|
|
using namespace std;
|
|
enum {
|
TURN_ANGLE_90,
|
TURN_ANGLE_90_CMP
|
};
|
|
const uint32_t STOP_CAR_TIME = D_SEC(2);
|
|
static bool TA90Testing;
|
static bool carStopEvent;
|
static bool checked;
|
static int currTarget;
|
static int azimuth;
|
static uint32_t stopTimepoint = 0;
|
|
static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car);
|
static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car);
|
static bool Turned(const Polygon *map, const car_model_cache_t *car);
|
|
void StartTurnA90(double heading)
|
{
|
azimuth = (int) heading;
|
checked = false;
|
TA90Testing = true;
|
carStopEvent = false;
|
|
currTarget = TURN_ANGLE_90;
|
}
|
|
void StopTurnA90(void)
|
{
|
TA90Testing = false;
|
}
|
|
int TestTurnA90(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status, double heading)
|
{
|
int status = 0;
|
|
if (!TA90Testing)
|
return -2;
|
|
DEBUG("TestTurnA90 %d", run_status);
|
|
if (CrashRedLine(map, car)) {
|
// 压线了
|
err.push_back(29);
|
status = -1;
|
DEBUG("错误 压线");
|
}
|
|
if (run_status != 0) {
|
if (carStopEvent)
|
DEBUG("TURN_ANGLE_90 停车时间 %ld", AppTimer_GetTickCount() - stopTimepoint);
|
|
if (carStopEvent && AppTimer_GetTickCount() - stopTimepoint > STOP_CAR_TIME) {
|
// 中途停车
|
err.push_back(31);
|
DEBUG("错误 停车1");
|
}
|
carStopEvent = false;
|
} else if (!carStopEvent){
|
carStopEvent = true;
|
stopTimepoint = AppTimer_GetTickCount();
|
}
|
|
if (ExitTestArea(map, car)) {
|
// 测试结束
|
status = 1;
|
}
|
|
if (currTarget == TURN_ANGLE_90) {
|
// 转向灯开启
|
int az = (int) heading;
|
|
if (abs(az - azimuth) > 180) {
|
az = 360 - abs(az-azimuth);
|
} else {
|
az = abs(az - azimuth);
|
}
|
|
if (az >= 10 && !checked) {
|
checked = true;
|
// 转向灯未开启
|
err.push_back(30);
|
DEBUG("错误 灯没看");
|
}
|
|
if (Turned(map, car)) {
|
currTarget = TURN_ANGLE_90_CMP;
|
checked = false;
|
}
|
DEBUG("TURN_ANGLE_90 %d %d",run_status, az);
|
} else {
|
// 关闭转向灯
|
Line line;
|
|
MakeLine(&line, &map->point[1], &map->point[2]);
|
|
// 大于2.5米后检查车灯
|
if (!checked && DistanceOf(car->points[0], line) >= 2.5) {
|
checked = true;
|
// 转向灯未关闭
|
err.push_back(30);
|
DEBUG("错误 灯没管");
|
}
|
DEBUG("TURN_ANGLE_90_CMP");
|
}
|
|
if (status != 0) {
|
StopTurnA90();
|
}
|
|
return status;
|
}
|
|
// 车轮是否压边线
|
static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car)
|
{
|
bool ret = false;
|
|
Line red_line;
|
const int red_lines[][2] = {{0, 5}, {5, 4}, {1, 2}, {2, 3}};
|
|
Line frontAxle, rearAxle;
|
|
// 仅看车轮外侧
|
MakeLine(&frontAxle, &car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->front_right_tire[TIRE_OUTSIDE]]);
|
MakeLine(&rearAxle, &car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->rear_right_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 bool ExitTestArea(const Polygon *map, const car_model_cache_t *car)
|
{
|
for (int i = 0; i < car->point_num; ++i) {
|
if (IntersectionOfLine(map->point[3], map->point[4], car->points[i]) != 1)
|
return false;
|
}
|
return true;
|
}
|
|
static bool Turned(const Polygon *map, const car_model_cache_t *car)
|
{
|
for (int i = 0; i < car->point_num; ++i) {
|
if (IntersectionOfLine(map->point[1], map->point[2], car->points[i]) != 1)
|
return false;
|
}
|
return true;
|
}
|
